12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'develop' into feature/LPSolver

release/4.3a0
ivan 2016-06-12 19:44:19 -04:00
parent 2e4a94e2bb a40c797392
commit 6bafe9932e
107 changed files with 22287 additions and 1376 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
CMakeLists.txt
View File

@ -66,8 +66,9 @@ option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also
option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF)
option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON)
option(GTSAM_USE_VECTOR3_POINTS "Simply typdef Point3 to eigen::Vector3d" OFF)
option(GTSAM_TYPEDEF_POINTS_TO_VECTORS "Typdef Point2 and Point3 to Eigen::Vector equivalents" OFF)
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON)
option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON)
# Options relating to MATLAB wrapper
# TODO: Check for matlab mex binary before handling building of binaries
@ -90,8 +91,8 @@ if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4)
message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.")
endif()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_USE_VECTOR3_POINTS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_USE_VECTOR3_POINTS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.")
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_TYPEDEF_POINTS_TO_VECTORS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.")
endif()
# Flags for choosing default packaging tools
@ -485,13 +486,14 @@ message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}")
message(STATUS " CPack Generator : ${CPACK_GENERATOR}")
message(STATUS "GTSAM flags ")
print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ")
print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ")
print_config_flag(${GTSAM_USE_VECTOR3_POINTS} "Point3 is typedef to Vector3 ")
print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ")
print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ")
print_config_flag(${GTSAM_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ")
print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_config_flag(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration")
message(STATUS "MATLAB toolbox flags ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ")

134
README.md
View File

@ -1,59 +1,77 @@
README - Georgia Tech Smoothing and Mapping library
===================================================
What is GTSAM?
--------------
GTSAM is a library of C++ classes that implement smoothing and
mapping (SAM) in robotics and vision, using factor graphs and Bayes
networks as the underlying computing paradigm rather than sparse
matrices.
On top of the C++ library, GTSAM includes a MATLAB interface (enable
GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
is under development.
Quickstart
----------
In the root library folder execute:
```
#!bash
$ mkdir build
$ cd build
$ cmake ..
$ make check (optional, runs unit tests)
$ make install
```
Prerequisites:
- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
- A modern compiler, i.e., at least gcc 4.7.3 on Linux.
Optional prerequisites - used automatically if findable by CMake:
- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`)
- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
GTSAM 4 Compatibility
---------------------
GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag.
Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect.
Additional Information
----------------------
Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here.
See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
README - Georgia Tech Smoothing and Mapping library
===================================================
What is GTSAM?
--------------
GTSAM is a library of C++ classes that implement smoothing and
mapping (SAM) in robotics and vision, using factor graphs and Bayes
networks as the underlying computing paradigm rather than sparse
matrices.
On top of the C++ library, GTSAM includes a MATLAB interface (enable
GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
is under development.
Quickstart
----------
In the root library folder execute:
```
#!bash
$ mkdir build
$ cd build
$ cmake ..
$ make check (optional, runs unit tests)
$ make install
```
Prerequisites:
- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
- A modern compiler, i.e., at least gcc 4.7.3 on Linux.
Optional prerequisites - used automatically if findable by CMake:
- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`)
- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
GTSAM 4 Compatibility
---------------------
GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag.
Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect.
The Preintegrated IMU Factor
----------------------------
GTSAM includes a state of the art IMU handling scheme based on
- Todd Lupton and Salah Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
Our implementation improves on this using integration on the manifold, as detailed in
- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014.
- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015.
If you are using the factor in academic work, please cite the publications above.
In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in docs/ImuFactor.pdf, is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF.
Additional Information
----------------------
Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here.
See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS).

3
examples/CameraResectioning.cpp
View File

@ -48,8 +48,7 @@ public:
virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const {
SimpleCamera camera(pose, *K_);
Point2 reprojectionError(camera.project(P_, H, boost::none, boost::none) - p_);
return reprojectionError.vector();
return camera.project(P_, H, boost::none, boost::none) - p_;
}
};

20101
examples/Data/imuAndGPSdata.csv Normal file
View File

File diff suppressed because it is too large Load Diff

269
examples/ImuFactorsExample.cpp Normal file
View File

@ -0,0 +1,269 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file imuFactorsExample
* @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor navigation code.
* @author Garrett (ghemann@gmail.com), Luca Carlone
*/
/**
* Example of use of the imuFactors (imuFactor and combinedImuFactor) in conjunction with GPS
* - you can test imuFactor (resp. combinedImuFactor) by commenting (resp. uncommenting)
* the line #define USE_COMBINED (few lines below)
* - we read IMU and GPS data from a CSV file, with the following format:
* A row starting with "i" is the first initial position formatted with
* N, E, D, qx, qY, qZ, qW, velN, velE, velD
* A row starting with "0" is an imu measurement
* linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
* A row starting with "1" is a gps correction formatted with
* N, E, D, qX, qY, qZ, qW
* Note that for GPS correction, we're only using the position not the rotation. The
* rotation is provided in the file for ground truth comparison.
*/
// GTSAM related includes.
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <fstream>
#include <iostream>
// Uncomment line below to use the CombinedIMUFactor as opposed to the standard ImuFactor.
// #define USE_COMBINED
using namespace gtsam;
using namespace std;
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
const string output_filename = "imuFactorExampleResults.csv";
// This will either be PreintegratedImuMeasurements (for ImuFactor) or
// PreintegratedCombinedMeasurements (for CombinedImuFactor).
PreintegrationType *imu_preintegrated_;
int main(int argc, char* argv[])
{
string data_filename;
if (argc < 2) {
printf("using default CSV file\n");
data_filename = findExampleDataFile("imuAndGPSdata.csv");
} else {
data_filename = argv[1];
}
// Set up output file for plotting errors
FILE* fp_out = fopen(output_filename.c_str(), "w+");
fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,gt_qy,gt_qz,gt_qw\n");
// Begin parsing the CSV file. Input the first line for initialization.
// From there, we'll iterate through the file and we'll preintegrate the IMU
// or add in the GPS given the input.
ifstream file(data_filename.c_str());
string value;
// Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD)
Eigen::Matrix<double,10,1> initial_state = Eigen::Matrix<double,10,1>::Zero();
getline(file, value, ','); // i
for (int i=0; i<9; i++) {
getline(file, value, ',');
initial_state(i) = atof(value.c_str());
}
getline(file, value, '\n');
initial_state(9) = atof(value.c_str());
cout << "initial state:\n" << initial_state.transpose() << "\n\n";
// Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z);
Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3),
initial_state(4), initial_state(5));
Point3 prior_point(initial_state.head<3>());
Pose3 prior_pose(prior_rotation, prior_point);
Vector3 prior_velocity(initial_state.tail<3>());
imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
Values initial_values;
int correction_count = 0;
initial_values.insert(X(correction_count), prior_pose);
initial_values.insert(V(correction_count), prior_velocity);
initial_values.insert(B(correction_count), prior_imu_bias);
// Assemble prior noise model and add it the graph.
noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished()); // rad,rad,rad,m, m, m
noiseModel::Diagonal::shared_ptr velocity_noise_model = noiseModel::Isotropic::Sigma(3,0.1); // m/s
noiseModel::Diagonal::shared_ptr bias_noise_model = noiseModel::Isotropic::Sigma(6,1e-3);
// Add all prior factors (pose, velocity, bias) to the graph.
NonlinearFactorGraph *graph = new NonlinearFactorGraph();
graph->add(PriorFactor<Pose3>(X(correction_count), prior_pose, pose_noise_model));
graph->add(PriorFactor<Vector3>(V(correction_count), prior_velocity,velocity_noise_model));
graph->add(PriorFactor<imuBias::ConstantBias>(B(correction_count), prior_imu_bias,bias_noise_model));
// We use the sensor specs to build the noise model for the IMU factor.
double accel_noise_sigma = 0.0003924;
double gyro_noise_sigma = 0.000205689024915;
double accel_bias_rw_sigma = 0.004905;
double gyro_bias_rw_sigma = 0.000001454441043;
Matrix33 measured_acc_cov = Matrix33::Identity(3,3) * pow(accel_noise_sigma,2);
Matrix33 measured_omega_cov = Matrix33::Identity(3,3) * pow(gyro_noise_sigma,2);
Matrix33 integration_error_cov = Matrix33::Identity(3,3)*1e-8; // error committed in integrating position from velocities
Matrix33 bias_acc_cov = Matrix33::Identity(3,3) * pow(accel_bias_rw_sigma,2);
Matrix33 bias_omega_cov = Matrix33::Identity(3,3) * pow(gyro_bias_rw_sigma,2);
Matrix66 bias_acc_omega_int = Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration
boost::shared_ptr<PreintegratedCombinedMeasurements::Params> p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
// PreintegrationBase params:
p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
p->integrationCovariance = integration_error_cov; // integration uncertainty continuous
// should be using 2nd order integration
// PreintegratedRotation params:
p->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous
// PreintegrationCombinedMeasurements params:
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
p->biasAccOmegaInt = bias_acc_omega_int;
#ifdef USE_COMBINED
imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias);
#else
imu_preintegrated_ = new PreintegratedImuMeasurements(p, prior_imu_bias);
#endif
// Store previous state for the imu integration and the latest predicted outcome.
NavState prev_state(prior_pose, prior_velocity);
NavState prop_state = prev_state;
imuBias::ConstantBias prev_bias = prior_imu_bias;
// Keep track of the total error over the entire run for a simple performance metric.
double current_position_error = 0.0, current_orientation_error = 0.0;
double output_time = 0.0;
double dt = 0.005; // The real system has noise, but here, results are nearly
// exactly the same, so keeping this for simplicity.
// All priors have been set up, now iterate through the data file.
while (file.good()) {
// Parse out first value
getline(file, value, ',');
int type = atoi(value.c_str());
if (type == 0) { // IMU measurement
Eigen::Matrix<double,6,1> imu = Eigen::Matrix<double,6,1>::Zero();
for (int i=0; i<5; ++i) {
getline(file, value, ',');
imu(i) = atof(value.c_str());
}
getline(file, value, '\n');
imu(5) = atof(value.c_str());
// Adding the IMU preintegration.
imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
} else if (type == 1) { // GPS measurement
Eigen::Matrix<double,7,1> gps = Eigen::Matrix<double,7,1>::Zero();
for (int i=0; i<6; ++i) {
getline(file, value, ',');
gps(i) = atof(value.c_str());
}
getline(file, value, '\n');
gps(6) = atof(value.c_str());
correction_count++;
// Adding IMU factor and GPS factor and optimizing.
#ifdef USE_COMBINED
PreintegratedCombinedMeasurements *preint_imu_combined = dynamic_cast<PreintegratedCombinedMeasurements*>(imu_preintegrated_);
CombinedImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
X(correction_count ), V(correction_count ),
B(correction_count-1), B(correction_count ),
*preint_imu_combined);
graph->add(imu_factor);
#else
PreintegratedImuMeasurements *preint_imu = dynamic_cast<PreintegratedImuMeasurements*>(imu_preintegrated_);
ImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
X(correction_count ), V(correction_count ),
B(correction_count-1),
*preint_imu);
graph->add(imu_factor);
imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1),
B(correction_count ),
zero_bias, bias_noise_model));
#endif
noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(3,1.0);
GPSFactor gps_factor(X(correction_count),
Point3(gps(0), // N,
gps(1), // E,
gps(2)), // D,
correction_noise);
graph->add(gps_factor);
// Now optimize and compare results.
prop_state = imu_preintegrated_->predict(prev_state, prev_bias);
initial_values.insert(X(correction_count), prop_state.pose());
initial_values.insert(V(correction_count), prop_state.v());
initial_values.insert(B(correction_count), prev_bias);
LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
Values result = optimizer.optimize();
// Overwrite the beginning of the preintegration for the next step.
prev_state = NavState(result.at<Pose3>(X(correction_count)),
result.at<Vector3>(V(correction_count)));
prev_bias = result.at<imuBias::ConstantBias>(B(correction_count));
// Reset the preintegration object.
imu_preintegrated_->resetIntegrationAndSetBias(prev_bias);
// Print out the position and orientation error for comparison.
Vector3 gtsam_position = prev_state.pose().translation();
Vector3 position_error = gtsam_position - gps.head<3>();
current_position_error = position_error.norm();
Quaternion gtsam_quat = prev_state.pose().rotation().toQuaternion();
Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
Quaternion quat_error = gtsam_quat * gps_quat.inverse();
quat_error.normalize();
Vector3 euler_angle_error(quat_error.x()*2,
quat_error.y()*2,
quat_error.z()*2);
current_orientation_error = euler_angle_error.norm();
// display statistics
cout << "Position error:" << current_position_error << "\t " << "Angular error:" << current_orientation_error << "\n";
fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
output_time, gtsam_position(0), gtsam_position(1), gtsam_position(2),
gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), gtsam_quat.w(),
gps(0), gps(1), gps(2),
gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w());
output_time += 1.0;
} else {
cerr << "ERROR parsing file\n";
return 1;
}
}
fclose(fp_out);
cout << "Complete, results written to " << output_filename << "\n\n";;
return 0;
}

37
examples/README
View File

@ -1,37 +0,0 @@
This directory contains a number of examples that illustrate the use of GTSAM:
SimpleRotation: a super-simple example of optimizing a single rotation according to a single prior
Kalman Filter Examples
======================
elaboratePoint2KalmanFilter: simple linear Kalman filter on a moving 2D point, but done using factor graphs
easyPoint2KalmanFilter: uses the cool generic templated Kalman filter class to do the same
fullStateKalmanFilter: simple 1D example with a full-state filter
errorStateKalmanFilter: simple 1D example of a moving target measured by a accelerometer, incl. drift-rate bias
2D Pose SLAM
============
LocalizationExample.cpp: modeling robot motion
LocalizationExample2.cpp: example with GPS like measurements
Pose2SLAMExample: A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h
Pose2SLAMExample_advanced: same, but uses an Optimizer object
Pose2SLAMwSPCG: solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
Planar SLAM with landmarks
==========================
PlanarSLAMExample: simple robotics example using the pre-built planar SLAM domain
PlanarSLAMExample_selfcontained: simple robotics example with all typedefs internal to this script.
Visual SLAM
===========
CameraResectioning.cpp: An example of gtsam for solving the camera resectioning problem
The directory vSLAMexample includes 2 simple examples using GTSAM:
- vSFMexample using visualSLAM in for structure-from-motion (SFM), and
- vISAMexample using visualSLAM and ISAM for incremental SLAM updates
See the separate README file there.
Undirected Graphical Models (UGM)
=================================
The best representation for a Markov Random Field is a factor graph :-)
This is illustrated with some discrete examples from the UGM MATLAB toolbox, which
can be found at http://www.di.ens.fr/~mschmidt/Software/UGM

96
examples/README.md Normal file
View File

@ -0,0 +1,96 @@
# GTSAM Examples
This directory contains all GTSAM C++ examples GTSAM pertaining to SFM
## Basic Examples:
* **SimpleRotation**: a simple example of optimizing a single rotation according to a single prior
* **CameraResectioning**: resection camera from some known points
* **SFMExample**: basic structure from motion
* **SFMExample_bal**: same, but read data from read from BAL file
* **SelfCalibrationExample**: Do SFM while also optimizing for calibration
## Stereo Visual Odometry Examples
Visual odometry using a stereo rig:
* **StereoVOExample**: basic example of stereo VO
* **StereoVOExample_large**: larger, with a snippet of Kitti data
## More Advanced Examples
The following examples illustrate some concepts from Georgia Tech's research papers, listed in the references section at the end:
* **VisualISAMExample**: uses iSAM [TRO08]
* **VisualISAM2Example**: uses iSAM2 [IJRR12]
* **SFMExample_SmartFactor**: uses smartFactors [ICRA14]
## Kalman Filter Examples
* **elaboratePoint2KalmanFilter**: simple linear Kalman filter on a moving 2D point, but done using factor graphs
* **easyPoint2KalmanFilter**: uses the generic templated Kalman filter class to do the same
* **fullStateKalmanFilter**: simple 1D example with a full-state filter
* **errorStateKalmanFilter**: simple 1D example of a moving target measured by a accelerometer, incl. drift-rate bias
## 2D Pose SLAM
* **LocalizationExample.cpp**: modeling robot motion
* **LocalizationExample2.cpp**: example with GPS like measurements
* **Pose2SLAMExample**: A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h
* **Pose2SLAMExample_advanced**: same, but uses an Optimizer object
* **Pose2SLAMwSPCG**: solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
## Planar SLAM with landmarks
* **PlanarSLAMExample**: simple robotics example using the pre-built planar SLAM domain
* **PlanarSLAMExample_selfcontained**: simple robotics example with all typedefs internal to this script.
## Visual SLAM
The directory **vSLAMexample** includes 2 simple examples using GTSAM:
- **vSFMexample** using visual SLAM for structure-from-motion (SFM)
- **vISAMexample** using visual SLAM and ISAM for incremental SLAM updates
See the separate README file there.
##Undirected Graphical Models (UGM)
The best representation for a Markov Random Field is a factor graph :-) This is illustrated with some discrete examples from the UGM MATLAB toolbox, which
can be found at <http://www.di.ens.fr/~mschmidt/Software/UGM>
##Building and Running
To build, cd into the directory and do:
```
mkdir build
cd build
cmake ..
```
For each .cpp file in this directory two make targets are created, one to build the executable, and one to build and run it. For example, the file `CameraResectioning.cpp` contains simple example to resection a camera from 4 known points. You can build it using
```
make CameraResectioning
```
or build and run it immediately with
```
make CameraResectioning.run
```
which should output:
```
Final result:
Values with 1 values:
Value x1: R:
[
1, 0.0, 0.0,
0.0, -1, 0.0,
0.0, 0.0, -1,
];
t: [0, 0, 2]';
```
## References
- [TRO08]: [iSAM: Incremental Smoothing and Mapping, Michael Kaess](http://frank.dellaert.com/pub/Kaess08tro.pdf), Michael Kaess, Ananth Ranganathan, and Frank Dellaert, IEEE Transactions on Robotics, 2008
- [IJRR12]: [iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree](http://www.cc.gatech.edu/~dellaert/pub/Kaess12ijrr.pdf), Michael Kaess, Hordur Johannsson, Richard Roberts, Viorela Ila, John Leonard, and Frank Dellaert, International Journal of Robotics Research, 2012
- [ICRA14]: [Eliminating Conditionally Independent Sets in Factor Graphs: A Unifying Perspective based on Smart Factors](http://frank.dellaert.com/pub/Carlone14icra.pdf), Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, IEEE International Conference on Robotics and Automation (ICRA), 2014

6
examples/SFMExample_SmartFactor.cpp
View File

@ -82,13 +82,13 @@ int main(int argc, char* argv[]) {
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.push_back(PriorFactor<Camera>(0, Camera(poses[0],K), noise));
graph.push_back(PriorFactor<Pose3>(0, poses[0], noise));
// Because the structure-from-motion problem has a scale ambiguity, the problem is
// still under-constrained. Here we add a prior on the second pose x1, so this will
// fix the scale by indicating the distance between x0 and x1.
// Because these two are fixed, the rest of the poses will be also be fixed.
graph.push_back(PriorFactor<Camera>(1, Camera(poses[1],K), noise)); // add directly to graph
graph.push_back(PriorFactor<Pose3>(1, poses[1], noise)); // add directly to graph
graph.print("Factor Graph:\n");
@ -97,7 +97,7 @@ int main(int argc, char* argv[]) {
Values initialEstimate;
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(i, Camera(poses[i].compose(delta), K));
initialEstimate.insert(i, poses[i].compose(delta));
initialEstimate.print("Initial Estimates:\n");
// Optimize the graph and print results

6
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -74,16 +74,16 @@ int main(int argc, char* argv[]) {
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.push_back(PriorFactor<Camera>(0, Camera(poses[0],K), noise));
graph.push_back(PriorFactor<Pose3>(0, poses[0], noise));
// Fix the scale ambiguity by adding a prior
graph.push_back(PriorFactor<Camera>(1, Camera(poses[0],K), noise));
graph.push_back(PriorFactor<Pose3>(1, poses[0], noise));
// Create the initial estimate to the solution
Values initialEstimate;
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(i, Camera(poses[i].compose(delta),K));
initialEstimate.insert(i, poses[i].compose(delta));
// We will use LM in the outer optimization loop, but by specifying "Iterative" below
// We indicate that an iterative linear solver should be used.

12
examples/easyPoint2KalmanFilter.cpp
View File

@ -70,7 +70,7 @@ int main() {
// Predict the new value with the EKF class
Point2 x1_predict = ekf.predict(factor1);
x1_predict.print("X1 Predict");
traits<Point2>::Print(x1_predict, "X1 Predict");
@ -91,7 +91,7 @@ int main() {
// Update the Kalman Filter with the measurement
Point2 x1_update = ekf.update(factor2);
x1_update.print("X1 Update");
traits<Point2>::Print(x1_update, "X1 Update");
@ -101,13 +101,13 @@ int main() {
difference = Point2(1,0);
BetweenFactor<Point2> factor3(x1, x2, difference, Q);
Point2 x2_predict = ekf.predict(factor1);
x2_predict.print("X2 Predict");
traits<Point2>::Print(x2_predict, "X2 Predict");
// Update
Point2 z2(2.0, 0.0);
PriorFactor<Point2> factor4(x2, z2, R);
Point2 x2_update = ekf.update(factor4);
x2_update.print("X2 Update");
traits<Point2>::Print(x2_update, "X2 Update");
@ -117,13 +117,13 @@ int main() {
difference = Point2(1,0);
BetweenFactor<Point2> factor5(x2, x3, difference, Q);
Point2 x3_predict = ekf.predict(factor5);
x3_predict.print("X3 Predict");
traits<Point2>::Print(x3_predict, "X3 Predict");
// Update
Point2 z3(3.0, 0.0);
PriorFactor<Point2> factor6(x3, z3, R);
Point2 x3_update = ekf.update(factor6);
x3_update.print("X3 Update");
traits<Point2>::Print(x3_update, "X3 Update");
return 0;
}

53
gtsam.h
View File

@ -266,23 +266,12 @@ class Point2 {
// Group
static gtsam::Point2 identity();
gtsam::Point2 inverse() const;
gtsam::Point2 compose(const gtsam::Point2& p2) const;
gtsam::Point2 between(const gtsam::Point2& p2) const;
// Manifold
gtsam::Point2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Point2& p) const;
// Lie Group
static gtsam::Point2 Expmap(Vector v);
static Vector Logmap(const gtsam::Point2& p);
// Standard Interface
double x() const;
double y() const;
Vector vector() const;
double dist(const gtsam::Point2& p2) const;
double distance(const gtsam::Point2& p2) const;
double norm() const;
// enabling serialization functionality
@ -2506,30 +2495,8 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
bool getUse2ndOrderCoriolis() const;
};
#include <gtsam/navigation/PreintegrationBase.h>
virtual class PreintegrationBase {
// Constructors
PreintegrationBase(const gtsam::PreintegrationParams* params);
PreintegrationBase(const gtsam::PreintegrationParams* params,
const gtsam::imuBias::ConstantBias& bias);
// Testable
void print(string s) const;
bool equals(const gtsam::PreintegrationBase& expected, double tol);
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
// Standard Interface
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
};
#include <gtsam/navigation/ImuFactor.h>
virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase {
class PreintegratedImuMeasurements {
// Constructors
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params);
PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params,
@ -2544,6 +2511,13 @@ virtual class PreintegratedImuMeasurements: gtsam::PreintegrationBase {
double deltaT);
void resetIntegration();
Matrix preintMeasCov() const;
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
};
virtual class ImuFactor: gtsam::NonlinearFactor {
@ -2559,7 +2533,7 @@ virtual class ImuFactor: gtsam::NonlinearFactor {
};
#include <gtsam/navigation/CombinedImuFactor.h>
virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase {
class PreintegratedCombinedMeasurements {
// Testable
void print(string s) const;
bool equals(const gtsam::PreintegratedCombinedMeasurements& expected,
@ -2570,6 +2544,13 @@ virtual class PreintegratedCombinedMeasurements: gtsam::PreintegrationBase {
double deltaT);
void resetIntegration();
Matrix preintMeasCov() const;
double deltaTij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const;
};
virtual class CombinedImuFactor: gtsam::NonlinearFactor {

5
gtsam/config.h.in
View File

@ -64,7 +64,10 @@
#cmakedefine GTSAM_ALLOW_DEPRECATED_SINCE_V4
// Publish flag about Eigen typedef
#cmakedefine GTSAM_USE_VECTOR3_POINTS
#cmakedefine GTSAM_TYPEDEF_POINTS_TO_VECTORS
// Support Metis-based nested dissection
#cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION
// Support Metis-based nested dissection
#cmakedefine GTSAM_TANGENT_PREINTEGRATION

2
gtsam/geometry/Cal3Bundler.cpp
View File

@ -106,7 +106,7 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (uncalibrate(pn).distance(pi) <= tol)
if (distance2(uncalibrate(pn), pi) <= tol)
break;
const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y;
const double rr = xx + yy;

2
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -144,7 +144,7 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (uncalibrate(pn).distance(pi) <= tol) break;
if (distance2(uncalibrate(pn), pi) <= tol) break;
const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr);

2
gtsam/geometry/CalibratedCamera.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)

5
gtsam/geometry/CalibratedCamera.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -19,6 +19,7 @@
#pragma once
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/concepts.h>
#include <gtsam/base/Manifold.h>
@ -28,8 +29,6 @@
namespace gtsam {
class Point2;
class GTSAM_EXPORT CheiralityException: public ThreadsafeException<
CheiralityException> {
public:

8
gtsam/geometry/CameraSet.h
View File

@ -56,8 +56,7 @@ protected:
// Project and fill error vector
Vector b(ZDim * m);
for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
Z e = predicted[i] - measured[i];
b.segment<ZDim>(row) = e.vector();
b.segment<ZDim>(row) = traits<Z>::Local(measured[i], predicted[i]);
}
return b;
}
@ -107,7 +106,8 @@ public:
// Allocate result
size_t m = this->size();
std::vector<Z> z(m);
std::vector<Z> z;
z.reserve(m);
// Allocate derivatives
if (E) E->resize(ZDim * m, N);
@ -117,7 +117,7 @@ public:
for (size_t i = 0; i < m; i++) {
MatrixZD Fi;
Eigen::Matrix<double, ZDim, N> Ei;
z[i] = this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0);
z.emplace_back(this->at(i).project2(point, Fs ? &Fi : 0, E ? &Ei : 0));
if (Fs) (*Fs)[i] = Fi;
if (E) E->block<ZDim, N>(ZDim * i, 0) = Ei;
}

104
gtsam/geometry/Point2.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -23,21 +23,11 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
void Point2::print(const string& s) const {
cout << s << *this << endl;
}
/* ************************************************************************* */
bool Point2::equals(const Point2& q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol);
}
/* ************************************************************************* */
double Point2::norm(OptionalJacobian<1,2> H) const {
double r = sqrt(x_ * x_ + y_ * y_);
double norm2(const Point2& p, OptionalJacobian<1,2> H) {
double r = std::sqrt(p.x() * p.x() + p.y() * p.y());
if (H) {
if (fabs(r) > 1e-10)
*H << x_ / r, y_ / r;
*H << p.x() / r, p.y() / r;
else
*H << 1, 1; // really infinity, why 1 ?
}
@ -45,34 +35,66 @@ double Point2::norm(OptionalJacobian<1,2> H) const {
}
/* ************************************************************************* */
double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1,
OptionalJacobian<1,2> H2) const {
Point2 d = point - *this;
double distance2(const Point2& p, const Point2& q, OptionalJacobian<1, 2> H1,
OptionalJacobian<1, 2> H2) {
Point2 d = q - p;
if (H1 || H2) {
Matrix12 H;
double r = d.norm(H);
double r = norm2(d, H);
if (H1) *H1 = -H;
if (H2) *H2 = H;
return r;
} else
} else {
return d.norm();
}
}
/*
* Calculate f and h, respectively the parallel and perpendicular distance of
* the intersections of two circles along and from the line connecting the centers.
* Both are dimensionless fractions of the distance d between the circle centers.
* If the circles do not intersect or they are identical, returns boost::none.
* If one solution (touching circles, as determined by tol), h will be exactly zero.
* h is a good measure for how accurate the intersection will be, as when circles touch
* or nearly touch, the intersection is ill-defined with noisy radius measurements.
* @param R_d : R/d, ratio of radius of first circle to distance between centers
* @param r_d : r/d, ratio of radius of second circle to distance between centers
* @param tol: absolute tolerance below which we consider touching circles
*/
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
/* ************************************************************************* */
void Point2::print(const string& s) const {
cout << s << *this << endl;
}
/* ************************************************************************* */
bool Point2::equals(const Point2& q, double tol) const {
return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol);
}
/* ************************************************************************* */
double Point2::norm(OptionalJacobian<1,2> H) const {
return gtsam::norm2(*this, H);
}
/* ************************************************************************* */
double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1,
OptionalJacobian<1,2> H2) const {
return gtsam::distance2(*this, point, H1, H2);
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point2& p) {
os << '(' << p.x() << ", " << p.y() << ')';
return os;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d, double tol) {
return circleCircleIntersection(R_d, r_d, tol);
}
std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh) {
return circleCircleIntersection(c1, c2, fh);
}
std::list<Point2> CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol) {
return circleCircleIntersection(c1, r1, c2, r2, tol);
}
#endif
#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
/* ************************************************************************* */
// Math inspired by http://paulbourke.net/geometry/circlesphere/
boost::optional<Point2> Point2::CircleCircleIntersection(double R_d, double r_d,
boost::optional<Point2> circleCircleIntersection(double R_d, double r_d,
double tol) {
double R2_d2 = R_d*R_d; // Yes, RD-D2 !
@ -83,11 +105,11 @@ boost::optional<Point2> Point2::CircleCircleIntersection(double R_d, double r_d,
// Hence, there are only solutions if >=0
if (h2<-tol) return boost::none; // allow *slightly* negative
else if (h2<tol) return Point2(f,0.0); // one solution
else return Point2(f,sqrt(h2)); // two solutions
else return Point2(f,std::sqrt(h2)); // two solutions
}
/* ************************************************************************* */
list<Point2> Point2::CircleCircleIntersection(Point2 c1, Point2 c2,
list<Point2> circleCircleIntersection(Point2 c1, Point2 c2,
boost::optional<Point2> fh) {
list<Point2> solutions;
@ -116,27 +138,21 @@ list<Point2> Point2::CircleCircleIntersection(Point2 c1, Point2 c2,
}
/* ************************************************************************* */
list<Point2> Point2::CircleCircleIntersection(Point2 c1, double r1, Point2 c2,
list<Point2> circleCircleIntersection(Point2 c1, double r1, Point2 c2,
double r2, double tol) {
// distance between circle centers.
double d = c1.dist(c2);
double d = distance2(c1, c2);
// centers coincide, either no solution or infinite number of solutions.
if (d<1e-9) return list<Point2>();
// Calculate f and h given normalized radii
double _d = 1.0/d, R_d = r1*_d, r_d=r2*_d;
boost::optional<Point2> fh = CircleCircleIntersection(R_d,r_d);
boost::optional<Point2> fh = circleCircleIntersection(R_d,r_d);
// Call version that takes fh
return CircleCircleIntersection(c1, c2, fh);
}
/* ************************************************************************* */
ostream &operator<<(ostream &os, const Point2& p) {
os << '(' << p.x() << ", " << p.y() << ')';
return os;
return circleCircleIntersection(c1, c2, fh);
}
/* ************************************************************************* */

195
gtsam/geometry/Point2.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -22,6 +22,14 @@
namespace gtsam {
#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
/// As of GTSAM 4, in order to make GTSAM more lean,
/// it is now possible to just typedef Point2 to Vector2
typedef Vector2 Point2;
#else
/**
* A 2D point
* Complies with the Testable Concept
@ -29,69 +37,30 @@ namespace gtsam {
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT Point2 {
class GTSAM_EXPORT Point2 : public Vector2 {
private:
double x_, y_;
public:
enum { dimension = 2 };
/// @name Standard Constructors
/// @{
/// default constructor
Point2(): x_(0), y_(0) {}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
// Deprecated default constructor initializes to zero, in contrast to new behavior below
Point2() { setZero(); }
#else
Point2() {}
#endif
/// construct from doubles
Point2(double x, double y): x_(x), y_(y) {}
using Vector2::Vector2;
/// @}
/// @name Advanced Constructors
/// @{
/// construct from 2D vector
explicit Point2(const Vector2& v) {
x_ = v(0);
y_ = v(1);
}
/*
* @brief Circle-circle intersection, given normalized radii.
* Calculate f and h, respectively the parallel and perpendicular distance of
* the intersections of two circles along and from the line connecting the centers.
* Both are dimensionless fractions of the distance d between the circle centers.
* If the circles do not intersect or they are identical, returns boost::none.
* If one solution (touching circles, as determined by tol), h will be exactly zero.
* h is a good measure for how accurate the intersection will be, as when circles touch
* or nearly touch, the intersection is ill-defined with noisy radius measurements.
* @param R_d : R/d, ratio of radius of first circle to distance between centers
* @param r_d : r/d, ratio of radius of second circle to distance between centers
* @param tol: absolute tolerance below which we consider touching circles
* @return optional Point2 with f and h, boost::none if no solution.
*/
static boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d,
double tol = 1e-9);
/*
* @brief Circle-circle intersection, from the normalized radii solution.
* @param c1 center of first circle
* @param c2 center of second circle
* @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
*/
static std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2>);
/**
* @brief Intersect 2 circles
* @param c1 center of first circle
* @param r1 radius of first circle
* @param c2 center of second circle
* @param r2 radius of second circle
* @param tol: absolute tolerance below which we consider touching circles
* @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
*/
static std::list<Point2> CircleCircleIntersection(Point2 c1, double r1,
Point2 c2, double r2, double tol = 1e-9);
explicit Point2(const Vector2& v):Vector2(v) {}
/// @}
/// @name Testable
/// @{
@ -107,21 +76,7 @@ public:
/// @{
/// identity
inline static Point2 identity() {return Point2();}
/// inverse
inline Point2 operator- () const {return Point2(-x_,-y_);}
/// add vector on right
inline Point2 operator +(const Vector2& v) const {
return Point2(x_ + v[0], y_ + v[1]);
}
/// add
inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
/// subtract
inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);}
inline static Point2 identity() {return Point2(0,0);}
/// @}
/// @name Vector Space
@ -137,51 +92,44 @@ public:
double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none,
OptionalJacobian<1,2> H2 = boost::none) const;
/** @deprecated The following function has been deprecated, use distance above */
inline double dist(const Point2& p2) const {
return (p2 - *this).norm();
}
/// multiply with a scalar
inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);}
/// divide by a scalar
inline Point2 operator / (double q) const {return Point2(x_/q,y_/q);}
/// @}
/// @name Standard Interface
/// @{
/// equality
inline bool operator ==(const Point2& q) const {return x_==q.x_ && y_==q.y_;}
inline bool operator ==(const Point2& q) const {return x()==q.x() && y()==q.y();}
/// get x
double x() const {return x_;}
inline double x() const {return (*this)[0];}
/// get y
double y() const {return y_;}
inline double y() const {return (*this)[1];}
/// return vectorized form (column-wise). TODO: why does this function exist?
Vector2 vector() const { return Vector2(x_, y_); }
/// return vectorized form (column-wise).
const Vector2& vector() const { return *this; }
/// @}
/// @name Deprecated
/// @{
inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;}
inline void operator *= (double s) {x_*=s;y_*=s;}
Point2 inverse() const { return -(*this);}
Point2 compose(const Point2& q) const { return (*this)+q;}
Point2 between(const Point2& q) const { return q-(*this);}
Vector2 localCoordinates(const Point2& q) const { return between(q).vector();}
Point2 retract(const Vector2& v) const { return compose(Point2(v));}
static Vector2 Logmap(const Point2& p) { return p.vector();}
static Point2 Expmap(const Vector2& v) { return Point2(v);}
/// @}
/// Streaming
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Point2& p);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
Point2 inverse() const { return -(*this); }
Point2 compose(const Point2& q) const { return (*this)+q;}
Point2 between(const Point2& q) const { return q-(*this);}
Vector2 localCoordinates(const Point2& q) const { return between(q);}
Point2 retract(const Vector2& v) const { return compose(Point2(v));}
static Vector2 Logmap(const Point2& p) { return p;}
static Point2 Expmap(const Vector2& v) { return Point2(v);}
inline double dist(const Point2& p2) const {return distance(p2);}
static boost::optional<Point2> CircleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
static std::list<Point2> CircleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
static std::list<Point2> CircleCircleIntersection(Point2 c1, double r1, Point2 c2, double r2, double tol = 1e-9);
/// @}
#endif
private:
/// @name Advanced Interface
@ -192,13 +140,25 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/)
{
ar & BOOST_SERIALIZATION_NVP(x_);
ar & BOOST_SERIALIZATION_NVP(y_);
}
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Vector2);}
/// @}
/// @}
};
template<>
struct traits<Point2> : public internal::VectorSpace<Point2> {
};
#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
/// Distance of the point from the origin, with Jacobian
double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none);
/// distance between two points
double distance2(const Point2& p1, const Point2& q,
OptionalJacobian<1, 2> H1 = boost::none,
OptionalJacobian<1, 2> H2 = boost::none);
// Convenience typedef
typedef std::pair<Point2, Point2> Point2Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
@ -207,10 +167,45 @@ std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
typedef std::vector<Point2> Point2Vector;
/// multiply with scalar
inline Point2 operator*(double s, const Point2& p) {return p*s;}
inline Point2 operator*(double s, const Point2& p) {
return p * s;
}
template<>
struct traits<Point2> : public internal::VectorSpace<Point2> {};
/*
* @brief Circle-circle intersection, given normalized radii.
* Calculate f and h, respectively the parallel and perpendicular distance of
* the intersections of two circles along and from the line connecting the centers.
* Both are dimensionless fractions of the distance d between the circle centers.
* If the circles do not intersect or they are identical, returns boost::none.
* If one solution (touching circles, as determined by tol), h will be exactly zero.
* h is a good measure for how accurate the intersection will be, as when circles touch
* or nearly touch, the intersection is ill-defined with noisy radius measurements.
* @param R_d : R/d, ratio of radius of first circle to distance between centers
* @param r_d : r/d, ratio of radius of second circle to distance between centers
* @param tol: absolute tolerance below which we consider touching circles
* @return optional Point2 with f and h, boost::none if no solution.
*/
boost::optional<Point2> circleCircleIntersection(double R_d, double r_d, double tol = 1e-9);
/*
* @brief Circle-circle intersection, from the normalized radii solution.
* @param c1 center of first circle
* @param c2 center of second circle
* @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
*/
std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
/**
* @brief Intersect 2 circles
* @param c1 center of first circle
* @param r1 radius of first circle
* @param c2 center of second circle
* @param r2 radius of second circle
* @param tol: absolute tolerance below which we consider touching circles
* @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
*/
std::list<Point2> circleCircleIntersection(Point2 c1, double r1,
Point2 c2, double r2, double tol = 1e-9);
} // \ namespace gtsam

14
gtsam/geometry/Point3.cpp
View File

@ -21,7 +21,7 @@ using namespace std;
namespace gtsam {
#ifndef GTSAM_USE_VECTOR3_POINTS
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
bool Point3::equals(const Point3 &q, double tol) const {
return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol &&
fabs(z() - q.z()) < tol);
@ -34,11 +34,11 @@ void Point3::print(const string& s) const {
/* ************************************************************************* */
double Point3::distance(const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) const {
return gtsam::distance(*this,q,H1,H2);
return gtsam::distance3(*this,q,H1,H2);
}
double Point3::norm(OptionalJacobian<1,3> H) const {
return gtsam::norm(*this, H);
return gtsam::norm3(*this, H);
}
Point3 Point3::normalized(OptionalJacobian<3,3> H) const {
@ -80,8 +80,8 @@ Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
#endif
/* ************************************************************************* */
double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) {
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
OptionalJacobian<1, 3> H2) {
double d = (q - p1).norm();
if (H1) {
*H1 << p1.x() - q.x(), p1.y() - q.y(), p1.z() - q.z();
@ -94,7 +94,7 @@ double distance(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
return d;
}
double norm(const Point3 &p, OptionalJacobian<1, 3> H) {
double norm3(const Point3 &p, OptionalJacobian<1, 3> H) {
double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z());
if (H) {
if (fabs(r) > 1e-10)
@ -106,7 +106,7 @@ double norm(const Point3 &p, OptionalJacobian<1, 3> H) {
}
Point3 normalize(const Point3 &p, OptionalJacobian<3, 3> H) {
Point3 normalized = p / norm(p);
Point3 normalized = p / p.norm();
if (H) {
// 3*3 Derivative
double x2 = p.x() * p.x(), y2 = p.y() * p.y(), z2 = p.z() * p.z();

18
gtsam/geometry/Point3.h
View File

@ -29,7 +29,7 @@
namespace gtsam {
#ifdef GTSAM_USE_VECTOR3_POINTS
#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS
/// As of GTSAM 4, in order to make GTSAM more lean,
/// it is now possible to just typedef Point3 to Vector3
@ -124,9 +124,9 @@ class GTSAM_EXPORT Point3 : public Vector3 {
Point3 inverse() const { return -(*this);}
Point3 compose(const Point3& q) const { return (*this)+q;}
Point3 between(const Point3& q) const { return q-(*this);}
Vector3 localCoordinates(const Point3& q) const { return between(q).vector();}
Vector3 localCoordinates(const Point3& q) const { return between(q);}
Point3 retract(const Vector3& v) const { return compose(Point3(v));}
static Vector3 Logmap(const Point3& p) { return p.vector();}
static Vector3 Logmap(const Point3& p) { return p;}
static Point3 Expmap(const Vector3& v) { return Point3(v);}
inline double dist(const Point3& q) const { return (q - *this).norm(); }
Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const { return normalized(H);}
@ -153,19 +153,19 @@ struct traits<Point3> : public internal::VectorSpace<Point3> {};
template<>
struct traits<const Point3> : public internal::VectorSpace<Point3> {};
#endif
#endif // GTSAM_TYPEDEF_POINTS_TO_VECTORS
// Convenience typedef
typedef std::pair<Point3, Point3> Point3Pair;
std::ostream &operator<<(std::ostream &os, const gtsam::Point3Pair &p);
/// distance between two points
double distance(const Point3& p1, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none);
double distance3(const Point3& p1, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none);
/// Distance of the point from the origin, with Jacobian
double norm(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
double norm3(const Point3& p, OptionalJacobian<1, 3> H = boost::none);
/// normalize, with optional Jacobian
Point3 normalize(const Point3& p, OptionalJacobian<3, 3> H = boost::none);
@ -193,7 +193,7 @@ struct Range<Point3, Point3> {
double operator()(const Point3& p, const Point3& q,
OptionalJacobian<1, 3> H1 = boost::none,
OptionalJacobian<1, 3> H2 = boost::none) {
return distance(p, q, H1, H2);
return distance3(p, q, H1, H2);
}
};

20
gtsam/geometry/Pose2.cpp
View File

@ -53,21 +53,21 @@ void Pose2::print(const string& s) const {
/* ************************************************************************* */
bool Pose2::equals(const Pose2& q, double tol) const {
return t_.equals(q.t_, tol) && r_.equals(q.r_, tol);
return equal_with_abs_tol(t_, q.t_, tol) && r_.equals(q.r_, tol);
}
/* ************************************************************************* */
Pose2 Pose2::Expmap(const Vector3& xi, OptionalJacobian<3, 3> H) {
if (H) *H = Pose2::ExpmapDerivative(xi);
assert(xi.size() == 3);
Point2 v(xi(0),xi(1));
double w = xi(2);
if (H) *H = Pose2::ExpmapDerivative(xi);
const Point2 v(xi(0),xi(1));
const double w = xi(2);
if (std::abs(w) < 1e-10)
return Pose2(xi[0], xi[1], xi[2]);
else {
Rot2 R(Rot2::fromAngle(w));
Point2 v_ortho = R_PI_2 * v; // points towards rot center
Point2 t = (v_ortho - R.rotate(v_ortho)) / w;
const Rot2 R(Rot2::fromAngle(w));
const Point2 v_ortho = R_PI_2 * v; // points towards rot center
const Point2 t = (v_ortho - R.rotate(v_ortho)) / w;
return Pose2(R, t);
}
}
@ -249,7 +249,7 @@ double Pose2::range(const Point2& point,
Point2 d = point - t_;
if (!Hpose && !Hpoint) return d.norm();
Matrix12 D_r_d;
double r = d.norm(D_r_d);
double r = norm2(d, D_r_d);
if (Hpose) {
Matrix23 D_d_pose;
D_d_pose << -r_.c(), r_.s(), 0.0,
@ -267,7 +267,7 @@ double Pose2::range(const Pose2& pose,
Point2 d = pose.t() - t_;
if (!Hpose && !Hother) return d.norm();
Matrix12 D_r_d;
double r = d.norm(D_r_d);
double r = norm2(d, D_r_d);
if (Hpose) {
Matrix23 D_d_pose;
D_d_pose <<
@ -311,7 +311,7 @@ boost::optional<Pose2> align(const vector<Point2Pair>& pairs) {
if (n<2) return boost::none; // we need at least two pairs
// calculate centroids
Point2 cp,cq;
Point2 cp(0,0), cq(0,0);
for(const Point2Pair& pair: pairs) {
cp += pair.first;
cq += pair.second;

8
gtsam/geometry/Pose2.h
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -52,7 +52,9 @@ public:
/// @{
/** default constructor = origin */
Pose2() {} // default is origin
Pose2() :
r_(traits<Rot2>::Identity()), t_(traits<Point2>::Identity()) {
}
/** copy constructor */
Pose2(const Pose2& pose) : r_(pose.r_), t_(pose.t_) {}
@ -86,7 +88,7 @@ public:
/// @{
/** Construct from canonical coordinates \f$ [T_x,T_y,\theta] \f$ (Lie algebra) */
Pose2(const Vector& v) {
Pose2(const Vector& v) : Pose2() {
*this = Expmap(v);
}

2
gtsam/geometry/Pose3.cpp
View File

@ -345,7 +345,7 @@ double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
return local.norm();
} else {
Matrix13 D_r_local;
const double r = norm(local, D_r_local);
const double r = norm3(local, D_r_local);
if (H1) *H1 = D_r_local * D_local_pose;
if (H2) *H2 = D_r_local * D_local_point;
return r;

2
gtsam/geometry/Rot3.h
View File

@ -179,7 +179,7 @@ namespace gtsam {
*/
static Rot3 AxisAngle(const Point3& axis, double angle) {
#ifdef GTSAM_USE_QUATERNIONS
return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis.vector()));
return gtsam::Quaternion(Eigen::AngleAxis<double>(angle, axis));
#else
return Rot3(SO3::AxisAngle(axis,angle));
#endif

2
gtsam/geometry/Unit3.h
View File

@ -78,7 +78,7 @@ public:
/// Construct from 2D point in plane at focal length f
/// Unit3(p,1) can be viewed as normalized homogeneous coordinates of 2D point
explicit Unit3(const Point2& p, double f = 1.0) : p_(p.x(), p.y(), f) {
explicit Unit3(const Point2& p, double f) : p_(p.x(), p.y(), f) {
p_.normalize();
}

2
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -52,7 +52,7 @@ TEST( Cal3Bundler, calibrate )
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 pn_hat = K.calibrate(pi);
CHECK( pn.equals(pn_hat, 1e-5));
CHECK( traits<Point2>::Equals(pn, pn_hat, 1e-5));
}
/* ************************************************************************* */

2
gtsam/geometry/tests/testCal3DS2.cpp
View File

@ -48,7 +48,7 @@ TEST( Cal3DS2, calibrate )
Point2 pn(0.5, 0.5);
Point2 pi = K.uncalibrate(pn);
Point2 pn_hat = K.calibrate(pi);
CHECK( pn.equals(pn_hat, 1e-5));
CHECK( traits<Point2>::Equals(pn, pn_hat, 1e-5));
}
Point2 uncalibrate_(const Cal3DS2& k, const Point2& pt) { return k.uncalibrate(pt); }

2
gtsam/geometry/tests/testCal3Unified.cpp
View File

@ -59,7 +59,7 @@ TEST( Cal3Unified, calibrate)
{
Point2 pi = K.uncalibrate(p);
Point2 pn_hat = K.calibrate(pi);
CHECK( p.equals(pn_hat, 1e-8));
CHECK( traits<Point2>::Equals(p, pn_hat, 1e-8));
}
Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); }

2
gtsam/geometry/tests/testCalibratedCamera.cpp
View File

@ -152,7 +152,7 @@ TEST( CalibratedCamera, Dproject_point_pose_infinity)
Point2 result = camera.project2(pointAtInfinity, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(projectAtInfinity, camera, pointAtInfinity);
Matrix numerical_point = numericalDerivative22(projectAtInfinity, camera, pointAtInfinity);
CHECK(assert_equal(Point2(), result));
CHECK(assert_equal(Point2(0,0), result));
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}

4
gtsam/geometry/tests/testCameraSet.cpp
View File

@ -44,7 +44,7 @@ TEST(CameraSet, Pinhole) {
EXPECT(!set.equals(set2));
// Check measurements
Point2 expected;
Point2 expected(0,0);
ZZ z = set.project2(p);
EXPECT(assert_equal(expected, z[0]));
EXPECT(assert_equal(expected, z[1]));
@ -117,7 +117,7 @@ TEST(CameraSet, Pinhole) {
Unit3 pointAtInfinity(0, 0, 1000);
EXPECT(
assert_equal(pointAtInfinity,
camera.backprojectPointAtInfinity(Point2())));
camera.backprojectPointAtInfinity(Point2(0,0))));
actualV = set.reprojectionError(pointAtInfinity, measured, Fs, E);
EXPECT(assert_equal(expectedV, actualV));
LONGS_EQUAL(2, Fs.size());

14
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -153,12 +153,12 @@ TEST( PinholeCamera, backproject2)
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
Point3 actual = camera.backproject(Point2(), 1.);
Point3 actual = camera.backproject(Point2(0,0), 1.);
Point3 expected(0., 1., 0.);
pair<Point2, bool> x = camera.projectSafe(expected);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x.first));
EXPECT(assert_equal(Point2(0,0), x.first));
EXPECT(x.second);
}
@ -169,12 +169,12 @@ TEST( PinholeCamera, backprojectInfinity2)
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
Unit3 actual = camera.backprojectPointAtInfinity(Point2());
Unit3 actual = camera.backprojectPointAtInfinity(Point2(0,0));
Unit3 expected(0., 1., 0.);
Point2 x = camera.project(expected);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x));
EXPECT(assert_equal(Point2(0,0), x));
}
/* ************************************************************************* */
@ -184,12 +184,12 @@ TEST( PinholeCamera, backprojectInfinity3)
Rot3 rot(1., 0., 0., 0., 1., 0., 0., 0., 1.); // identity
Camera camera(Pose3(rot, origin), K);
Unit3 actual = camera.backprojectPointAtInfinity(Point2());
Unit3 actual = camera.backprojectPointAtInfinity(Point2(0,0));
Unit3 expected(0., 0., 1.);
Point2 x = camera.project(expected);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x));
EXPECT(assert_equal(Point2(0,0), x));
}
/* ************************************************************************* */
@ -278,7 +278,7 @@ TEST( PinholeCamera, range0) {
double result = camera.range(point1, D1, D2);
Matrix Hexpected1 = numericalDerivative21(range0, camera, point1);
Matrix Hexpected2 = numericalDerivative22(range0, camera, point1);
EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result,
EXPECT_DOUBLES_EQUAL(distance3(point1, camera.pose().translation()), result,
1e-9);
EXPECT(assert_equal(Hexpected1, D1, 1e-7));
EXPECT(assert_equal(Hexpected2, D2, 1e-7));

6
gtsam/geometry/tests/testPinholePose.cpp
View File

@ -124,12 +124,12 @@ TEST( PinholePose, backproject2)
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin), K);
Point3 actual = camera.backproject(Point2(), 1.);
Point3 actual = camera.backproject(Point2(0,0), 1.);
Point3 expected(0., 1., 0.);
pair<Point2, bool> x = camera.projectSafe(expected);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Point2(), x.first));
EXPECT(assert_equal(Point2(0,0), x.first));
EXPECT(x.second);
}
@ -212,7 +212,7 @@ TEST( PinholePose, range0) {
double result = camera.range(point1, D1, D2);
Matrix expectedDcamera = numericalDerivative21(range0, camera, point1);
Matrix expectedDpoint = numericalDerivative22(range0, camera, point1);
EXPECT_DOUBLES_EQUAL(distance(point1, camera.pose().translation()), result, 1e-9);
EXPECT_DOUBLES_EQUAL(distance3(point1, camera.pose().translation()), result, 1e-9);
EXPECT(assert_equal(expectedDcamera, D1, 1e-7));
EXPECT(assert_equal(expectedDpoint, D2, 1e-7));
}

4
gtsam/geometry/tests/testPinholeSet.cpp
View File

@ -84,7 +84,7 @@ TEST(PinholeSet, Pinhole) {
EXPECT(!set.equals(set2));
// Check measurements
Point2 expected;
Point2 expected(0,0);
ZZ z = set.project2(p);
EXPECT(assert_equal(expected, z[0]));
EXPECT(assert_equal(expected, z[1]));
@ -131,7 +131,7 @@ TEST(PinholeSet, Pinhole) {
}
EXPECT(
assert_equal(pointAtInfinity,
camera.backprojectPointAtInfinity(Point2())));
camera.backprojectPointAtInfinity(Point2(0,0))));
{
PinholeSet<Camera>::FBlocks Fs;
Matrix E;

61
gtsam/geometry/tests/testPoint2.cpp
View File

@ -1,6 +1,6 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
@ -27,6 +27,11 @@ using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Point2)
GTSAM_CONCEPT_LIE_INST(Point2)
//******************************************************************************
TEST(Point2 , Constructor) {
Point2 p;
}
//******************************************************************************
TEST(Double , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<double>));
@ -95,26 +100,26 @@ TEST( Point2, expmap) {
/* ************************************************************************* */
TEST( Point2, arithmetic) {
EXPECT(assert_equal( Point2(-5,-6), -Point2(5,6) ));
EXPECT(assert_equal( Point2(5,6), Point2(4,5)+Point2(1,1)));
EXPECT(assert_equal( Point2(3,4), Point2(4,5)-Point2(1,1) ));
EXPECT(assert_equal( Point2(8,6), Point2(4,3)*2));
EXPECT(assert_equal( Point2(4,6), 2*Point2(2,3)));
EXPECT(assert_equal( Point2(2,3), Point2(4,6)/2));
EXPECT(assert_equal<Point2>( Point2(-5,-6), -Point2(5,6) ));
EXPECT(assert_equal<Point2>( Point2(5,6), Point2(4,5)+Point2(1,1)));
EXPECT(assert_equal<Point2>( Point2(3,4), Point2(4,5)-Point2(1,1) ));
EXPECT(assert_equal<Point2>( Point2(8,6), Point2(4,3)*2));
EXPECT(assert_equal<Point2>( Point2(4,6), 2*Point2(2,3)));
EXPECT(assert_equal<Point2>( Point2(2,3), Point2(4,6)/2));
}
/* ************************************************************************* */
TEST( Point2, unit) {
Point2 p0(10, 0), p1(0, -10), p2(10, 10);
EXPECT(assert_equal(Point2(1, 0), p0.unit(), 1e-6));
EXPECT(assert_equal(Point2(0,-1), p1.unit(), 1e-6));
EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.unit(), 1e-6));
EXPECT(assert_equal(Point2(1, 0), Point2(p0.normalized()), 1e-6));
EXPECT(assert_equal(Point2(0,-1), Point2(p1.normalized()), 1e-6));
EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), Point2(p2.normalized()), 1e-6));
}
namespace {
/* ************************************************************************* */
// some shared test values
Point2 x1, x2(1, 1), x3(1, 1);
Point2 x1(0,0), x2(1, 1), x3(1, 1);
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
/* ************************************************************************* */
@ -126,19 +131,19 @@ TEST( Point2, norm ) {
Point2 p0(cos(5.0), sin(5.0));
DOUBLES_EQUAL(1, p0.norm(), 1e-6);
Point2 p1(4, 5), p2(1, 1);
DOUBLES_EQUAL( 5, p1.distance(p2), 1e-6);
DOUBLES_EQUAL( 5, distance2(p1, p2), 1e-6);
DOUBLES_EQUAL( 5, (p2-p1).norm(), 1e-6);
Matrix expectedH, actualH;
double actual;
// exception, for (0,0) derivative is [Inf,Inf] but we return [1,1]
actual = x1.norm(actualH);
actual = norm2(x1, actualH);
EXPECT_DOUBLES_EQUAL(0, actual, 1e-9);
expectedH = (Matrix(1, 2) << 1.0, 1.0).finished();
EXPECT(assert_equal(expectedH,actualH));
actual = x2.norm(actualH);
actual = norm2(x2, actualH);
EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual, 1e-9);
expectedH = numericalDerivative11(norm_proxy, x2);
EXPECT(assert_equal(expectedH,actualH));
@ -151,20 +156,20 @@ TEST( Point2, norm ) {
/* ************************************************************************* */
namespace {
double distance_proxy(const Point2& location, const Point2& point) {
return location.distance(point);
return distance2(location, point);
}
}
TEST( Point2, distance ) {
Matrix expectedH1, actualH1, expectedH2, actualH2;
// establish distance is indeed zero
EXPECT_DOUBLES_EQUAL(1, x1.distance(l1), 1e-9);
EXPECT_DOUBLES_EQUAL(1, distance2(x1, l1), 1e-9);
// establish distance is indeed 45 degrees
EXPECT_DOUBLES_EQUAL(sqrt(2.0), x1.distance(l2), 1e-9);
EXPECT_DOUBLES_EQUAL(sqrt(2.0), distance2(x1, l2), 1e-9);
// Another pair
double actual23 = x2.distance(l3, actualH1, actualH2);
double actual23 = distance2(x2, l3, actualH1, actualH2);
EXPECT_DOUBLES_EQUAL(sqrt(2.0), actual23, 1e-9);
// Check numerical derivatives
@ -174,7 +179,7 @@ TEST( Point2, distance ) {
EXPECT(assert_equal(expectedH2,actualH2));
// Another test
double actual34 = x3.distance(l4, actualH1, actualH2);
double actual34 = distance2(x3, l4, actualH1, actualH2);
EXPECT_DOUBLES_EQUAL(2, actual34, 1e-9);
// Check numerical derivatives
@ -190,42 +195,42 @@ TEST( Point2, circleCircleIntersection) {
double offset = 0.994987;
// Test intersections of circle moving from inside to outside
list<Point2> inside = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,0),1);
list<Point2> inside = circleCircleIntersection(Point2(0,0),5,Point2(0,0),1);
EXPECT_LONGS_EQUAL(0,inside.size());
list<Point2> touching1 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(4,0),1);
list<Point2> touching1 = circleCircleIntersection(Point2(0,0),5,Point2(4,0),1);
EXPECT_LONGS_EQUAL(1,touching1.size());
EXPECT(assert_equal(Point2(5,0), touching1.front()));
list<Point2> common = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(5,0),1);
list<Point2> common = circleCircleIntersection(Point2(0,0),5,Point2(5,0),1);
EXPECT_LONGS_EQUAL(2,common.size());
EXPECT(assert_equal(Point2(4.9, offset), common.front(), 1e-6));
EXPECT(assert_equal(Point2(4.9, -offset), common.back(), 1e-6));
list<Point2> touching2 = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(6,0),1);
list<Point2> touching2 = circleCircleIntersection(Point2(0,0),5,Point2(6,0),1);
EXPECT_LONGS_EQUAL(1,touching2.size());
EXPECT(assert_equal(Point2(5,0), touching2.front()));
// test rotated case
list<Point2> rotated = Point2::CircleCircleIntersection(Point2(0,0),5,Point2(0,5),1);
list<Point2> rotated = circleCircleIntersection(Point2(0,0),5,Point2(0,5),1);
EXPECT_LONGS_EQUAL(2,rotated.size());
EXPECT(assert_equal(Point2(-offset, 4.9), rotated.front(), 1e-6));
EXPECT(assert_equal(Point2( offset, 4.9), rotated.back(), 1e-6));
// test r1<r2
list<Point2> smaller = Point2::CircleCircleIntersection(Point2(0,0),1,Point2(5,0),5);
list<Point2> smaller = circleCircleIntersection(Point2(0,0),1,Point2(5,0),5);
EXPECT_LONGS_EQUAL(2,smaller.size());
EXPECT(assert_equal(Point2(0.1, offset), smaller.front(), 1e-6));
EXPECT(assert_equal(Point2(0.1, -offset), smaller.back(), 1e-6));
// test offset case, r1>r2
list<Point2> offset1 = Point2::CircleCircleIntersection(Point2(1,1),5,Point2(6,1),1);
list<Point2> offset1 = circleCircleIntersection(Point2(1,1),5,Point2(6,1),1);
EXPECT_LONGS_EQUAL(2,offset1.size());
EXPECT(assert_equal(Point2(5.9, 1+offset), offset1.front(), 1e-6));
EXPECT(assert_equal(Point2(5.9, 1-offset), offset1.back(), 1e-6));
// test offset case, r1<r2
list<Point2> offset2 = Point2::CircleCircleIntersection(Point2(6,1),1,Point2(1,1),5);
list<Point2> offset2 = circleCircleIntersection(Point2(6,1),1,Point2(1,1),5);
EXPECT_LONGS_EQUAL(2,offset2.size());
EXPECT(assert_equal(Point2(5.9, 1-offset), offset2.front(), 1e-6));
EXPECT(assert_equal(Point2(5.9, 1+offset), offset2.back(), 1e-6));
@ -233,12 +238,14 @@ TEST( Point2, circleCircleIntersection) {
}
/* ************************************************************************* */
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
TEST( Point2, stream) {
Point2 p(1, 2);
std::ostringstream os;
os << p;
EXPECT(os.str() == "(1, 2)");
}
#endif
/* ************************************************************************* */
int main () {

13
gtsam/geometry/tests/testPoint3.cpp
View File

@ -26,6 +26,11 @@ GTSAM_CONCEPT_LIE_INST(Point3)
static Point3 P(0.2, 0.7, -2);
//******************************************************************************
TEST(Point3 , Constructor) {
Point3 p;
}
//******************************************************************************
TEST(Point3 , Concept) {
BOOST_CONCEPT_ASSERT((IsGroup<Point3>));
@ -149,7 +154,7 @@ TEST( Point3, cross2) {
}
/* ************************************************************************* */
#ifndef GTSAM_USE_VECTOR3_POINTS
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
TEST( Point3, stream) {
Point3 p(1, 2, -3);
std::ostringstream os;
@ -178,20 +183,20 @@ TEST (Point3, norm) {
Matrix actualH;
Point3 point(3,4,5); // arbitrary point
double expected = sqrt(50);
EXPECT_DOUBLES_EQUAL(expected, norm(point, actualH), 1e-8);
EXPECT_DOUBLES_EQUAL(expected, norm3(point, actualH), 1e-8);
Matrix expectedH = numericalDerivative11<double, Point3>(norm_proxy, point);
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
/* ************************************************************************* */
double testFunc(const Point3& P, const Point3& Q) {
return distance(P,Q);
return distance3(P, Q);
}
TEST (Point3, distance) {
Point3 P(1., 12.8, -32.), Q(52.7, 4.9, -13.3);
Matrix H1, H2;
double d = distance(P, Q, H1, H2);
double d = distance3(P, Q, H1, H2);
double expectedDistance = 55.542686;
Matrix numH1 = numericalDerivative21(testFunc, P, Q);
Matrix numH2 = numericalDerivative22(testFunc, P, Q);

10
gtsam/geometry/tests/testPose2.cpp
View File

@ -43,7 +43,7 @@ TEST(Pose2 , Concept) {
/* ************************************************************************* */
TEST(Pose2, constructors) {
Point2 p;
Point2 p(0,0);
Pose2 pose(0,p);
Pose2 origin;
assert_equal(pose,origin);
@ -371,7 +371,7 @@ TEST(Pose2, compose_c)
/* ************************************************************************* */
TEST(Pose2, inverse )
{
Point2 origin, t(1,2);
Point2 origin(0,0), t(1,2);
Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y
Pose2 identity, lTg = gTl.inverse();
@ -409,7 +409,7 @@ namespace {
/* ************************************************************************* */
TEST( Pose2, matrix )
{
Point2 origin, t(1,2);
Point2 origin(0,0), t(1,2);
Pose2 gTl(M_PI/2.0, t); // robot at (1,2) looking towards y
Matrix gMl = matrix(gTl);
EXPECT(assert_equal((Matrix(3,3) <<
@ -743,7 +743,7 @@ namespace {
/* ************************************************************************* */
struct Triangle { size_t i_,j_,k_;};
boost::optional<Pose2> align(const vector<Point2>& ps, const vector<Point2>& qs,
boost::optional<Pose2> align2(const vector<Point2>& ps, const vector<Point2>& qs,
const pair<Triangle, Triangle>& trianglePair) {
const Triangle& t1 = trianglePair.first, t2 = trianglePair.second;
vector<Point2Pair> correspondences;
@ -762,7 +762,7 @@ TEST(Pose2, align_4) {
Triangle t1; t1.i_=0; t1.j_=1; t1.k_=2;
Triangle t2; t2.i_=1; t2.j_=2; t2.k_=0;
boost::optional<Pose2> actual = align(ps, qs, make_pair(t1,t2));
boost::optional<Pose2> actual = align2(ps, qs, make_pair(t1,t2));
EXPECT(assert_equal(expected, *actual));
}

4
gtsam/geometry/tests/testSimpleCamera.cpp
View File

@ -104,12 +104,12 @@ TEST( SimpleCamera, backproject2)
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down
SimpleCamera camera(Pose3(rot, origin), K);
Point3 actual = camera.backproject(Point2(), 1.);
Point3 actual = camera.backproject(Point2(0,0), 1.);
Point3 expected(0., 1., 0.);
pair<Point2, bool> x = camera.projectSafe(expected);
CHECK(assert_equal(expected, actual));
CHECK(assert_equal(Point2(), x.first));
CHECK(assert_equal(Point2(0,0), x.first));
CHECK(x.second);
}

4
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -273,7 +273,7 @@ TEST( triangulation, onePose) {
vector<Point2> measurements;
poses += Pose3();
measurements += Point2();
measurements += Point2(0,0);
CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements),
TriangulationUnderconstrainedException);
@ -282,7 +282,7 @@ TEST( triangulation, onePose) {
//******************************************************************************
TEST( triangulation, StereotriangulateNonlinear ) {
Cal3_S2Stereo::shared_ptr stereoK(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612));
auto stereoK = boost::make_shared<Cal3_S2Stereo>(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612);
// two camera poses m1, m2
Matrix4 m1, m2;

7
gtsam/geometry/triangulation.h
View File

@ -112,7 +112,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(CAMERA::Measurement::dimension));
static SharedNoiseModel unit(noiseModel::Unit::Create(
traits<typename CAMERA::Measurement>::dimension));
for (size_t i = 0; i < measurements.size(); i++) {
const CAMERA& camera_i = cameras[i];
graph.push_back(TriangulationFactor<CAMERA> //
@ -457,7 +458,7 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
for(const CAMERA& camera: cameras) {
const Pose3& pose = camera.pose();
if (params.landmarkDistanceThreshold > 0
&& distance(pose.translation(), point)
&& distance3(pose.translation(), point)
> params.landmarkDistanceThreshold)
return TriangulationResult::Degenerate();
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
@ -471,7 +472,7 @@ TriangulationResult triangulateSafe(const std::vector<CAMERA>& cameras,
if (params.dynamicOutlierRejectionThreshold > 0) {
const Point2& zi = measured.at(i);
Point2 reprojectionError(camera.project(point) - zi);
totalReprojError += reprojectionError.vector().norm();
totalReprojError += reprojectionError.norm();
}
i += 1;
}

4
gtsam/inference/Symbol.h
View File

@ -33,8 +33,8 @@ namespace gtsam {
*/
class GTSAM_EXPORT Symbol {
protected:
const unsigned char c_;
const std::uint64_t j_;
unsigned char c_;
std::uint64_t j_;
public:

76
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -31,22 +31,21 @@ using namespace std;
//------------------------------------------------------------------------------
// Inner class PreintegratedCombinedMeasurements
//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::print(
const string& s) const {
PreintegrationBase::print(s);
void PreintegratedCombinedMeasurements::print(const string& s) const {
PreintegrationType::print(s);
cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl;
}
//------------------------------------------------------------------------------
bool PreintegratedCombinedMeasurements::equals(
const PreintegratedCombinedMeasurements& other, double tol) const {
return PreintegrationBase::equals(other, tol) &&
equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
return PreintegrationType::equals(other, tol)
&& equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
}
//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::resetIntegration() {
PreintegrationBase::resetIntegration();
PreintegrationType::resetIntegration();
preintMeasCov_.setZero();
}
@ -68,9 +67,9 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
void PreintegratedCombinedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
// Update preintegrated measurements.
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 B, C;
PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
// Update preintegrated measurements covariance: as in [2] we consider a first
// order propagation that can be seen as a prediction phase in an EKF
@ -79,8 +78,8 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
// and preintegrated measurements
// Single Jacobians to propagate covariance
// TODO(frank): should we not also accout for bias on position?
Matrix3 theta_H_biasOmega = - C.topRows<3>();
// TODO(frank): should we not also account for bias on position?
Matrix3 theta_H_biasOmega = -C.topRows<3>();
Matrix3 vel_H_biasAcc = -B.bottomRows<3>();
// overall Jacobian wrt preintegrated measurements (df/dx)
@ -105,18 +104,18 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
// BLOCK DIAGONAL TERMS
D_t_t(&G_measCov_Gt) = dt * iCov;
D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc *
(aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) *
(vel_H_biasAcc.transpose());
D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega *
(wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) *
(theta_H_biasOmega.transpose());
D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc
* (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0))
* (vel_H_biasAcc.transpose());
D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega
* (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3))
* (theta_H_biasOmega.transpose());
D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
// OFF BLOCK DIAGONAL TERMS
Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0) *
theta_H_biasOmega.transpose();
Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0)
* theta_H_biasOmega.transpose();
D_v_R(&G_measCov_Gt) = temp;
D_R_v(&G_measCov_Gt) = temp.transpose();
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
@ -131,7 +130,7 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInt,
const bool use2ndOrderIntegration) {
if (!use2ndOrderIntegration)
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
biasHat_ = biasHat;
boost::shared_ptr<Params> p = Params::MakeSharedD();
p->gyroscopeCovariance = measuredOmegaCovariance;
@ -148,12 +147,12 @@ PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
//------------------------------------------------------------------------------
// CombinedImuFactor methods
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor(
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
const PreintegratedCombinedMeasurements& pim)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias_i, bias_j),
_PIM_(pim) {}
CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j,
Key vel_j, Key bias_i, Key bias_j,
const PreintegratedCombinedMeasurements& pim) :
Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias_i, bias_j), _PIM_(pim) {
}
//------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
@ -195,8 +194,8 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
Matrix93 D_r_vel_i, D_r_vel_j;
// error wrt preintegrated measurements
Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0,
Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j,
bias_i, H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0,
H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0);
// if we need the jacobians
@ -250,11 +249,11 @@ CombinedImuFactor::CombinedImuFactor(
const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
const bool use2ndOrderCoriolis)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias_i, bias_j),
_PIM_(pim) {
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias_i, bias_j),
_PIM_(pim) {
boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p =
boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
p->n_gravity = n_gravity;
p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor;
@ -263,12 +262,12 @@ CombinedImuFactor::CombinedImuFactor(
}
void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias_i,
CombinedPreintegratedMeasurements& pim,
const Vector3& n_gravity,
const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) {
Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias_i,
CombinedPreintegratedMeasurements& pim,
const Vector3& n_gravity,
const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) {
// use deprecated predict
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
omegaCoriolis, use2ndOrderCoriolis);
@ -277,5 +276,6 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
}
#endif
} /// namespace gtsam
}
/// namespace gtsam

23
gtsam/navigation/CombinedImuFactor.h
View File

@ -22,12 +22,19 @@
#pragma once
/* GTSAM includes */
#include <gtsam/navigation/ManifoldPreintegration.h>
#include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/PreintegrationBase.h>
#include <gtsam/base/Matrix.h>
namespace gtsam {
#ifdef GTSAM_TANGENT_PREINTEGRATION
typedef TangentPreintegration PreintegrationType;
#else
typedef ManifoldPreintegration PreintegrationType;
#endif
/*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
@ -57,7 +64,7 @@ namespace gtsam {
*
* @addtogroup SLAM
*/
class PreintegratedCombinedMeasurements : public PreintegrationBase {
class PreintegratedCombinedMeasurements : public PreintegrationType {
public:
@ -123,7 +130,7 @@ public:
PreintegratedCombinedMeasurements(
const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias())
: PreintegrationBase(p, biasHat) {
: PreintegrationType(p, biasHat) {
preintMeasCov_.setZero();
}
@ -133,10 +140,10 @@ public:
/// @{
/// Re-initialize PreintegratedCombinedMeasurements
void resetIntegration();
void resetIntegration() override;
/// const reference to params, shadows definition in base class
Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
Params& p() const { return *boost::static_pointer_cast<Params>(this->p_);}
/// @}
/// @name Access instance variables
@ -146,7 +153,7 @@ public:
/// @name Testable
/// @{
void print(const std::string& s = "Preintegrated Measurements:") const;
void print(const std::string& s = "Preintegrated Measurements:") const override;
bool equals(const PreintegratedCombinedMeasurements& expected, double tol = 1e-9) const;
/// @}
@ -163,7 +170,7 @@ public:
* frame)
*/
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double deltaT);
const Vector3& measuredOmega, const double dt) override;
/// @}
@ -183,7 +190,7 @@ public:
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
}
};

94
gtsam/navigation/ImuFactor.cpp
View File

@ -32,20 +32,20 @@ using namespace std;
// Inner class PreintegratedImuMeasurements
//------------------------------------------------------------------------------
void PreintegratedImuMeasurements::print(const string& s) const {
PreintegrationBase::print(s);
PreintegrationType::print(s);
cout << " preintMeasCov \n[" << preintMeasCov_ << "]" << endl;
}
//------------------------------------------------------------------------------
bool PreintegratedImuMeasurements::equals(
const PreintegratedImuMeasurements& other, double tol) const {
return PreintegrationBase::equals(other, tol)
return PreintegrationType::equals(other, tol)
&& equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
}
//------------------------------------------------------------------------------
void PreintegratedImuMeasurements::resetIntegration() {
PreintegrationBase::resetIntegration();
PreintegrationType::resetIntegration();
preintMeasCov_.setZero();
}
@ -53,9 +53,9 @@ void PreintegratedImuMeasurements::resetIntegration() {
void PreintegratedImuMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
// Update preintegrated measurements (also get Jacobian)
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix9 A; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 B, C;
PreintegrationBase::integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
PreintegrationType::update(measuredAcc, measuredOmega, dt, &A, &B, &C);
// first order covariance propagation:
// as in [2] we consider a first order propagation that can be seen as a
@ -73,31 +73,33 @@ void PreintegratedImuMeasurements::integrateMeasurement(
preintMeasCov_.noalias() += C * (wCov / dt) * C.transpose();
// NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt), with Gi << Z_3x3, I_3x3, Z_3x3
preintMeasCov_.block<3,3>(3,3).noalias() += iCov * dt;
preintMeasCov_.block<3, 3>(3, 3).noalias() += iCov * dt;
}
//------------------------------------------------------------------------------
void PreintegratedImuMeasurements::integrateMeasurements(const Matrix& measuredAccs,
const Matrix& measuredOmegas,
const Matrix& dts) {
assert(measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1);
void PreintegratedImuMeasurements::integrateMeasurements(
const Matrix& measuredAccs, const Matrix& measuredOmegas,
const Matrix& dts) {
assert(
measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1);
assert(dts.cols() >= 1);
assert(measuredAccs.cols() == dts.cols());
assert(measuredOmegas.cols() == dts.cols());
size_t n = static_cast<size_t>(dts.cols());
for (size_t j = 0; j < n; j++) {
integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0,j));
integrateMeasurement(measuredAccs.col(j), measuredOmegas.col(j), dts(0, j));
}
}
//------------------------------------------------------------------------------
void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, //
Matrix9* H1, Matrix9* H2) {
PreintegrationBase::mergeWith(pim12, H1, H2);
#ifdef GTSAM_TANGENT_PREINTEGRATION
void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, //
Matrix9* H1, Matrix9* H2) {
PreintegrationType::mergeWith(pim12, H1, H2);
preintMeasCov_ =
*H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose();
*H1 * preintMeasCov_ * H1->transpose() + *H2 * pim12.preintMeasCov_ * H2->transpose();
}
#endif
//------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
PreintegratedImuMeasurements::PreintegratedImuMeasurements(
@ -174,16 +176,17 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
}
//------------------------------------------------------------------------------
#ifdef GTSAM_TANGENT_PREINTEGRATION
PreintegratedImuMeasurements ImuFactor::Merge(
const PreintegratedImuMeasurements& pim01,
const PreintegratedImuMeasurements& pim12) {
if (!pim01.matchesParamsWith(pim12))
throw std::domain_error(
"Cannot merge PreintegratedImuMeasurements with different params");
throw std::domain_error(
"Cannot merge PreintegratedImuMeasurements with different params");
if (pim01.params()->body_P_sensor)
throw std::domain_error(
"Cannot merge PreintegratedImuMeasurements with sensor pose yet");
throw std::domain_error(
"Cannot merge PreintegratedImuMeasurements with sensor pose yet");
// the bias for the merged factor will be the bias from 01
PreintegratedImuMeasurements pim02 = pim01;
@ -196,26 +199,27 @@ PreintegratedImuMeasurements ImuFactor::Merge(
//------------------------------------------------------------------------------
ImuFactor::shared_ptr ImuFactor::Merge(const shared_ptr& f01,
const shared_ptr& f12) {
const shared_ptr& f12) {
// IMU bias keys must be the same.
if (f01->key5() != f12->key5())
throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same");
throw std::domain_error("ImuFactor::Merge: IMU bias keys must be the same");
// expect intermediate pose, velocity keys to matchup.
if (f01->key3() != f12->key1() || f01->key4() != f12->key2())
throw std::domain_error(
"ImuFactor::Merge: intermediate pose, velocity keys need to match up");
throw std::domain_error(
"ImuFactor::Merge: intermediate pose, velocity keys need to match up");
// return new factor
auto pim02 =
Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
return boost::make_shared<ImuFactor>(f01->key1(), // P0
f01->key2(), // V0
f12->key3(), // P2
f12->key4(), // V2
f01->key5(), // B
pim02);
Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
return boost::make_shared<ImuFactor>(f01->key1(),// P0
f01->key2(),// V0
f12->key3(),// P2
f12->key4(),// V2
f01->key5(),// B
pim02);
}
#endif
//------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
@ -248,9 +252,11 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
//------------------------------------------------------------------------------
// ImuFactor2 methods
//------------------------------------------------------------------------------
ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias, const PreintegratedImuMeasurements& pim)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j, bias),
_PIM_(pim) {}
ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias,
const PreintegratedImuMeasurements& pim) :
Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j,
bias), _PIM_(pim) {
}
//------------------------------------------------------------------------------
NonlinearFactor::shared_ptr ImuFactor2::clone() const {
@ -266,9 +272,11 @@ std::ostream& operator<<(std::ostream& os, const ImuFactor2& f) {
}
//------------------------------------------------------------------------------
void ImuFactor2::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2())
<< "," << keyFormatter(this->key3()) << ")\n";
void ImuFactor2::print(const string& s,
const KeyFormatter& keyFormatter) const {
cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3())
<< ")\n";
cout << *this << endl;
}
@ -281,15 +289,15 @@ bool ImuFactor2::equals(const NonlinearFactor& other, double tol) const {
}
//------------------------------------------------------------------------------
Vector ImuFactor2::evaluateError(const NavState& state_i, const NavState& state_j,
const imuBias::ConstantBias& bias_i, //
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const {
Vector ImuFactor2::evaluateError(const NavState& state_i,
const NavState& state_j,
const imuBias::ConstantBias& bias_i, //
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const {
return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3);
}
//------------------------------------------------------------------------------
}
// namespace gtsam
// namespace gtsam

35
gtsam/navigation/ImuFactor.h
View File

@ -23,11 +23,18 @@
/* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/PreintegrationBase.h>
#include <gtsam/navigation/ManifoldPreintegration.h>
#include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/base/debug.h>
namespace gtsam {
#ifdef GTSAM_TANGENT_PREINTEGRATION
typedef TangentPreintegration PreintegrationType;
#else
typedef ManifoldPreintegration PreintegrationType;
#endif
/*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating
@ -61,7 +68,7 @@ namespace gtsam {
*
* @addtogroup SLAM
*/
class PreintegratedImuMeasurements: public PreintegrationBase {
class PreintegratedImuMeasurements: public PreintegrationType {
friend class ImuFactor;
friend class ImuFactor2;
@ -85,29 +92,28 @@ public:
*/
PreintegratedImuMeasurements(const boost::shared_ptr<PreintegrationParams>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()) :
PreintegrationBase(p, biasHat) {
PreintegrationType(p, biasHat) {
preintMeasCov_.setZero();
}
/**
* Construct preintegrated directly from members: base class and preintMeasCov
* @param base PreintegrationBase instance
* @param base PreintegrationType instance
* @param preintMeasCov Covariance matrix used in noise model.
*/
PreintegratedImuMeasurements(const PreintegrationBase& base, const Matrix9& preintMeasCov)
: PreintegrationBase(base),
PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov)
: PreintegrationType(base),
preintMeasCov_(preintMeasCov) {
}
/// print
void print(const std::string& s = "Preintegrated Measurements:") const;
void print(const std::string& s = "Preintegrated Measurements:") const override;
/// equals
bool equals(const PreintegratedImuMeasurements& expected,
double tol = 1e-9) const;
bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const;
/// Re-initialize PreintegratedIMUMeasurements
void resetIntegration();
void resetIntegration() override;
/**
* Add a single IMU measurement to the preintegration.
@ -115,7 +121,8 @@ public:
* @param measuredOmega Measured angular velocity (as given by the sensor)
* @param dt Time interval between this and the last IMU measurement
*/
void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double dt);
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double dt) override;
/// Add multiple measurements, in matrix columns
void integrateMeasurements(const Matrix& measuredAccs, const Matrix& measuredOmegas,
@ -124,8 +131,10 @@ public:
/// Return pre-integrated measurement covariance
Matrix preintMeasCov() const { return preintMeasCov_; }
#ifdef GTSAM_TANGENT_PREINTEGRATION
/// Merge in a different set of measurements and update bias derivatives accordingly
void mergeWith(const PreintegratedImuMeasurements& pim, Matrix9* H1, Matrix9* H2);
#endif
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @deprecated constructor
@ -150,7 +159,7 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationType);
ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size()));
}
};
@ -230,6 +239,7 @@ public:
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
boost::none, boost::optional<Matrix&> H5 = boost::none) const;
#ifdef GTSAM_TANGENT_PREINTEGRATION
/// Merge two pre-integrated measurement classes
static PreintegratedImuMeasurements Merge(
const PreintegratedImuMeasurements& pim01,
@ -237,6 +247,7 @@ public:
/// Merge two factors
static shared_ptr Merge(const shared_ptr& f01, const shared_ptr& f12);
#endif
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @deprecated typename

143
gtsam/navigation/ManifoldPreintegration.cpp Normal file
View File

@ -0,0 +1,143 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ManifoldPreintegration.cpp
* @author Luca Carlone
* @author Stephen Williams
* @author Richard Roberts
* @author Vadim Indelman
* @author David Jensen
* @author Frank Dellaert
**/
#include "ManifoldPreintegration.h"
using namespace std;
namespace gtsam {
//------------------------------------------------------------------------------
ManifoldPreintegration::ManifoldPreintegration(
const boost::shared_ptr<Params>& p, const Bias& biasHat) :
PreintegrationBase(p, biasHat) {
resetIntegration();
}
//------------------------------------------------------------------------------
void ManifoldPreintegration::resetIntegration() {
deltaTij_ = 0.0;
deltaXij_ = NavState();
delRdelBiasOmega_.setZero();
delPdelBiasAcc_.setZero();
delPdelBiasOmega_.setZero();
delVdelBiasAcc_.setZero();
delVdelBiasOmega_.setZero();
}
//------------------------------------------------------------------------------
bool ManifoldPreintegration::equals(const ManifoldPreintegration& other,
double tol) const {
return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol
&& biasHat_.equals(other.biasHat_, tol)
&& deltaXij_.equals(other.deltaXij_, tol)
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
&& equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
}
//------------------------------------------------------------------------------
void ManifoldPreintegration::update(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B,
Matrix93* C) {
// Correct for bias in the sensor frame
Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
// Possibly correct for sensor pose
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
if (p().body_P_sensor)
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
// Save current rotation for updating Jacobians
const Rot3 oldRij = deltaXij_.attitude();
// Do update
deltaTij_ += dt;
deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional
if (p().body_P_sensor) {
// More complicated derivatives in case of non-trivial sensor pose
*C *= D_correctedOmega_omega;
if (!p().body_P_sensor->translation().isZero())
*C += *B * D_correctedAcc_omega;
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
}
// Update Jacobians
// TODO(frank): Try same simplification as in new approach
Matrix3 D_acc_R;
oldRij.rotate(acc, D_acc_R);
const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
const Vector3 integratedOmega = omega * dt;
Matrix3 D_incrR_integratedOmega;
const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt;
double dt22 = 0.5 * dt * dt;
const Matrix3 dRij = oldRij.matrix(); // expensive
delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
delVdelBiasAcc_ += -dRij * dt;
delVdelBiasOmega_ += D_acc_biasOmega * dt;
}
//------------------------------------------------------------------------------
Vector9 ManifoldPreintegration::biasCorrectedDelta(
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
// Correct deltaRij, derivative is delRdelBiasOmega_
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
Matrix3 D_correctedRij_bias;
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none,
H ? &D_correctedRij_bias : 0);
if (H)
D_correctedRij_bias *= delRdelBiasOmega_;
Vector9 xi;
Matrix3 D_dR_correctedRij;
// TODO(frank): could line below be simplified? It is equivalent to
// LogMap(deltaRij_.compose(Expmap(biasInducedOmega)))
NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0);
NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer()
+ delPdelBiasOmega_ * biasIncr.gyroscope();
NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer()
+ delVdelBiasOmega_ * biasIncr.gyroscope();
if (H) {
Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias;
D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_;
D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
(*H) << D_dR_bias, D_dP_bias, D_dV_bias;
}
return xi;
}
//------------------------------------------------------------------------------
}// namespace gtsam

133
gtsam/navigation/ManifoldPreintegration.h Normal file
View File

@ -0,0 +1,133 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file ManifoldPreintegration.h
* @author Luca Carlone
* @author Stephen Williams
* @author Richard Roberts
* @author Vadim Indelman
* @author David Jensen
* @author Frank Dellaert
**/
#pragma once
#include <gtsam/navigation/NavState.h>
#include <gtsam/navigation/PreintegrationBase.h>
namespace gtsam {
/**
* IMU pre-integration on NavSatet manifold.
* This corresponds to the original RSS paper (with one difference: V is rotated)
*/
class ManifoldPreintegration : public PreintegrationBase {
protected:
/**
* Pre-integrated navigation state, from frame i to frame j
* Note: relative position does not take into account velocity at time i, see deltap+, in [2]
* Note: velocity is now also in frame i, as opposed to deltaVij in [2]
*/
NavState deltaXij_;
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
/// Default constructor for serialization
ManifoldPreintegration() {
resetIntegration();
}
public:
/// @name Constructors
/// @{
/**
* Constructor, initializes the variables in the base class
* @param p Parameters, typically fixed in a single application
* @param bias Current estimate of acceleration and rotation rate biases
*/
ManifoldPreintegration(const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
/// @}
/// @name Basic utilities
/// @{
/// Re-initialize PreintegratedMeasurements
void resetIntegration() override;
/// @}
/// @name Instance variables access
/// @{
NavState deltaXij() const override { return deltaXij_; }
Rot3 deltaRij() const override { return deltaXij_.attitude(); }
Vector3 deltaPij() const override { return deltaXij_.position(); }
Vector3 deltaVij() const override { return deltaXij_.velocity(); }
Matrix3 delRdelBiasOmega() const { return delRdelBiasOmega_; }
Matrix3 delPdelBiasAcc() const { return delPdelBiasAcc_; }
Matrix3 delPdelBiasOmega() const { return delPdelBiasOmega_; }
Matrix3 delVdelBiasAcc() const { return delVdelBiasAcc_; }
Matrix3 delVdelBiasOmega() const { return delVdelBiasOmega_; }
/// @name Testable
/// @{
bool equals(const ManifoldPreintegration& other, double tol) const;
/// @}
/// @name Main functionality
/// @{
/// Update preintegrated measurements and get derivatives
/// It takes measured quantities in the j frame
/// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
/// NOTE(frank): implementation is different in two versions
void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt,
Matrix9* A, Matrix93* B, Matrix93* C) override;
/// Given the estimate of the bias, return a NavState tangent vector
/// summarizing the preintegrated IMU measurements so far
/// NOTE(frank): implementation is different in two versions
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> H = boost::none) const override;
/** Dummy clone for MATLAB */
virtual boost::shared_ptr<ManifoldPreintegration> clone() const {
return boost::shared_ptr<ManifoldPreintegration>();
}
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(deltaXij_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size()));
ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size()));
ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size()));
ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size()));
ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size()));
}
};
} /// namespace gtsam

173
gtsam/navigation/NavState.cpp
View File

@ -24,6 +24,18 @@ namespace gtsam {
#define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_;
//------------------------------------------------------------------------------
NavState NavState::Create(const Rot3& R, const Point3& t, const Velocity3& v,
OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2,
OptionalJacobian<9, 3> H3) {
if (H1)
*H1 << I_3x3, Z_3x3, Z_3x3;
if (H2)
*H2 << Z_3x3, R.transpose(), Z_3x3;
if (H3)
*H3 << Z_3x3, Z_3x3, R.transpose();
return NavState(R, t, v);
}
//------------------------------------------------------------------------------
NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel,
OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2) {
@ -94,138 +106,55 @@ bool NavState::equals(const NavState& other, double tol) const {
}
//------------------------------------------------------------------------------
NavState NavState::inverse() const {
NavState NavState::retract(const Vector9& xi, //
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
TIE(nRb, n_t, n_v, *this);
const Rot3 bRn = nRb.inverse();
return NavState(bRn, -(bRn * n_t), -(bRn * n_v));
}
//------------------------------------------------------------------------------
NavState NavState::operator*(const NavState& bTc) const {
TIE(nRb, n_t, n_v, *this);
TIE(bRc, b_t, b_v, bTc);
return NavState(nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v);
}
//------------------------------------------------------------------------------
NavState::PositionAndVelocity //
NavState::operator*(const PositionAndVelocity& b_tv) const {
TIE(nRb, n_t, n_v, *this);
const Point3& b_t = b_tv.first;
const Velocity3& b_v = b_tv.second;
return PositionAndVelocity(nRb * b_t + n_t, nRb * b_v + n_v);
}
//------------------------------------------------------------------------------
Point3 NavState::operator*(const Point3& b_t) const {
return Point3(R_ * b_t + t_);
}
//------------------------------------------------------------------------------
NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
OptionalJacobian<9, 9> H) {
Matrix3 D_R_xi;
const Rot3 R = Rot3::Expmap(dR(xi), H ? &D_R_xi : 0);
const Point3 p = Point3(dP(xi));
const Vector v = dV(xi);
const NavState result(R, p, v);
if (H) {
*H << D_R_xi, Z_3x3, Z_3x3, //
Z_3x3, R.transpose(), Z_3x3, //
Z_3x3, Z_3x3, R.transpose();
Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb;
const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0);
const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0);
const Point3 t = n_t + nRb.rotate(dP(xi), H1 ? &D_t_nRb : 0);
const Point3 v = n_v + nRb.rotate(dV(xi), H1 ? &D_v_nRb : 0);
if (H1) {
*H1 << D_R_nRb, Z_3x3, Z_3x3, //
// Note(frank): the derivative of n_t with respect to xi is nRb
// We pre-multiply with nRc' to account for NavState::Create
// Then we make use of the identity nRc' * nRb = bRc'
nRc.transpose() * D_t_nRb, bRc.transpose(), Z_3x3,
// Similar reasoning for v:
nRc.transpose() * D_v_nRb, Z_3x3, bRc.transpose();
}
return result;
if (H2) {
*H2 << D_bRc_xi, Z_3x3, Z_3x3, //
Z_3x3, bRc.transpose(), Z_3x3, //
Z_3x3, Z_3x3, bRc.transpose();
}
return NavState(nRc, t, v);
}
//------------------------------------------------------------------------------
Vector9 NavState::ChartAtOrigin::Local(const NavState& x,
OptionalJacobian<9, 9> H) {
Vector9 NavState::localCoordinates(const NavState& g, //
OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
Matrix3 D_dR_R, D_dt_R, D_dv_R;
const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0);
const Point3 dt = R_.unrotate(g.t_ - t_, H1 ? &D_dt_R : 0);
const Vector dv = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0);
Vector9 xi;
Matrix3 D_xi_R;
xi << Rot3::Logmap(x.R_, H ? &D_xi_R : 0), x.t(), x.v();
if (H) {
*H << D_xi_R, Z_3x3, Z_3x3, //
Z_3x3, x.R(), Z_3x3, //
Z_3x3, Z_3x3, x.R();
xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dt, dv;
if (H1) {
*H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
D_dt_R, -I_3x3, Z_3x3, //
D_dv_R, Z_3x3, -I_3x3;
}
if (H2) {
*H2 << D_xi_R, Z_3x3, Z_3x3, //
Z_3x3, dR.matrix(), Z_3x3, //
Z_3x3, Z_3x3, dR.matrix();
}
return xi;
}
//------------------------------------------------------------------------------
NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) {
if (H)
throw runtime_error("NavState::Expmap derivative not implemented yet");
Eigen::Block<const Vector9, 3, 1> n_omega_nb = dR(xi);
Eigen::Block<const Vector9, 3, 1> v = dP(xi);
Eigen::Block<const Vector9, 3, 1> a = dV(xi);
// NOTE(frank): See Pose3::Expmap
Rot3 nRb = Rot3::Expmap(n_omega_nb);
double theta2 = n_omega_nb.dot(n_omega_nb);
if (theta2 > numeric_limits<double>::epsilon()) {
// Expmap implements a "screw" motion in the direction of n_omega_nb
Vector3 n_t_parallel = n_omega_nb * n_omega_nb.dot(v); // component along rotation axis
Vector3 omega_cross_v = n_omega_nb.cross(v); // points towards axis
Point3 n_t = Point3(omega_cross_v - nRb * omega_cross_v + n_t_parallel)
/ theta2;
Vector3 n_v_parallel = n_omega_nb * n_omega_nb.dot(a); // component along rotation axis
Vector3 omega_cross_a = n_omega_nb.cross(a); // points towards axis
Vector3 n_v = (omega_cross_a - nRb * omega_cross_a + n_v_parallel) / theta2;
return NavState(nRb, n_t, n_v);
} else {
return NavState(nRb, Point3(v), a);
}
}
//------------------------------------------------------------------------------
Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) {
if (H)
throw runtime_error("NavState::Logmap derivative not implemented yet");
TIE(nRb, n_p, n_v, nTb);
Vector3 n_t = n_p;
// NOTE(frank): See Pose3::Logmap
Vector9 xi;
Vector3 n_omega_nb = Rot3::Logmap(nRb);
double theta = n_omega_nb.norm();
if (theta * theta <= numeric_limits<double>::epsilon()) {
xi << n_omega_nb, n_t, n_v;
} else {
Matrix3 W = skewSymmetric(n_omega_nb / theta);
// Formula from Agrawal06iros, equation (14)
// simplified with Mathematica, and multiplying in n_t to avoid matrix math
double factor = (1 - theta / (2. * tan(0.5 * theta)));
Vector3 n_x = W * n_t;
Vector3 v = n_t - (0.5 * theta) * n_x + factor * (W * n_x);
Vector3 n_y = W * n_v;
Vector3 a = n_v - (0.5 * theta) * n_y + factor * (W * n_y);
xi << n_omega_nb, v, a;
}
return xi;
}
//------------------------------------------------------------------------------
Matrix9 NavState::AdjointMap() const {
// NOTE(frank): See Pose3::AdjointMap
const Matrix3 nRb = R();
Matrix3 pAr = skewSymmetric(t()) * nRb;
Matrix3 vAr = skewSymmetric(v()) * nRb;
Matrix9 adj;
// nR/bR nR/bP nR/bV nP/bR nP/bP nP/bV nV/bR nV/bP nV/bV
adj << nRb, Z_3x3, Z_3x3, pAr, nRb, Z_3x3, vAr, Z_3x3, nRb;
return adj;
}
//------------------------------------------------------------------------------
Matrix7 NavState::wedge(const Vector9& xi) {
const Matrix3 Omega = skewSymmetric(dR(xi));
Matrix7 T;
T << Omega, Z_3x3, dP(xi), Z_3x3, Omega, dV(xi), Vector6::Zero().transpose(), 1.0;
return T;
}
//------------------------------------------------------------------------------
// sugar for derivative blocks
#define D_R_R(H) (H)->block<3,3>(0,0)
@ -239,7 +168,6 @@ Matrix7 NavState::wedge(const Vector9& xi) {
#define D_v_v(H) (H)->block<3,3>(6,6)
//------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
OptionalJacobian<9, 3> G2) const {
@ -282,7 +210,6 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
}
return newState;
}
#endif
//------------------------------------------------------------------------------
Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,

90
gtsam/navigation/NavState.h
View File

@ -20,7 +20,7 @@
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/ProductLieGroup.h>
#include <gtsam/base/Manifold.h>
namespace gtsam {
@ -29,9 +29,9 @@ typedef Vector3 Velocity3;
/**
* Navigation state: Pose (rotation, translation) + velocity
* Implements semi-direct Lie group product of SO(3) and R^6, where R^6 is position/velocity
* NOTE(frank): it does not make sense to make this a Lie group, but it is a 9D manifold
*/
class NavState: public LieGroup<NavState, 9> {
class NavState {
private:
// TODO(frank):
@ -42,6 +42,10 @@ private:
public:
enum {
dimension = 9
};
typedef std::pair<Point3, Velocity3> PositionAndVelocity;
/// @name Constructors
@ -49,7 +53,7 @@ public:
/// Default constructor
NavState() :
t_(0,0,0), v_(Vector3::Zero()) {
t_(0, 0, 0), v_(Vector3::Zero()) {
}
/// Construct from attitude, position, velocity
NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
@ -59,15 +63,15 @@ public:
NavState(const Pose3& pose, const Velocity3& v) :
R_(pose.rotation()), t_(pose.translation()), v_(v) {
}
/// Construct from Matrix group representation (no checking)
NavState(const Matrix7& T) :
R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 6)), v_(T.block<3, 1>(3, 6)) {
}
/// Construct from SO(3) and R^6
NavState(const Matrix3& R, const Vector9 tv) :
R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
}
/// Named constructor with derivatives
static NavState Create(const Rot3& R, const Point3& t, const Velocity3& v,
OptionalJacobian<9, 3> H1, OptionalJacobian<9, 3> H2,
OptionalJacobian<9, 3> H3);
/// Named constructor with derivatives
static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2);
@ -116,7 +120,8 @@ public:
/// @{
/// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const NavState& state);
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os, const NavState& state);
/// print
void print(const std::string& s = "") const;
@ -124,29 +129,6 @@ public:
/// equals
bool equals(const NavState& other, double tol = 1e-8) const;
/// @}
/// @name Group
/// @{
/// identity for group operation
static NavState identity() {
return NavState();
}
/// inverse transformation with derivatives
NavState inverse() const;
/// Group compose is the semi-direct product as specified below:
/// nTc = nTb * bTc = (nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v)
NavState operator*(const NavState& bTc) const;
/// Native group action is on position/velocity pairs *in the body frame* as follows:
/// nTb * (b_t,b_v) = (nRb * b_t + n_t, nRb * b_v + n_v)
PositionAndVelocity operator*(const PositionAndVelocity& b_tv) const;
/// Act on position alone, n_t = nRb * b_t + n_t
Point3 operator*(const Point3& b_t) const;
/// @}
/// @name Manifold
/// @{
@ -172,44 +154,25 @@ public:
return v.segment<3>(6);
}
// Chart at origin, constructs components separately (as opposed to Expmap)
struct ChartAtOrigin {
static NavState Retract(const Vector9& xi, //
OptionalJacobian<9, 9> H = boost::none);
static Vector9 Local(const NavState& x, //
OptionalJacobian<9, 9> H = boost::none);
};
/// retract with optional derivatives
NavState retract(const Vector9& v, //
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
boost::none) const;
/// @}
/// @name Lie Group
/// @{
/// Exponential map at identity - create a NavState from canonical coordinates
static NavState Expmap(const Vector9& xi, //
OptionalJacobian<9, 9> H = boost::none);
/// Log map at identity - return the canonical coordinates for this NavState
static Vector9 Logmap(const NavState& p, //
OptionalJacobian<9, 9> H = boost::none);
/// Calculate Adjoint map, a 9x9 matrix that takes a tangent vector in the body frame, and transforms
/// it to a tangent vector at identity, such that Exmap(AdjointMap()*xi) = (*this) * Exmpap(xi);
Matrix9 AdjointMap() const;
/// wedge creates Lie algebra element from tangent vector
static Matrix7 wedge(const Vector9& xi);
/// localCoordinates with optional derivatives
Vector9 localCoordinates(const NavState& g, //
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
boost::none) const;
/// @}
/// @name Dynamics
/// @{
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// Integrate forward in time given angular velocity and acceleration in body frame
/// Uses second order integration for position, returns derivatives except dt.
NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
OptionalJacobian<9, 3> G2) const;
#endif
/// Compute tangent space contribution due to Coriolis forces
Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
@ -239,14 +202,7 @@ private:
// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
template<>
struct traits<NavState> : Testable<NavState>, internal::LieGroupTraits<NavState> {
struct traits<NavState> : internal::Manifold<NavState> {
};
// Partial specialization of wedge
// TODO: deprecate, make part of traits
template<>
inline Matrix wedge<NavState>(const Vector& xi) {
return NavState::wedge(xi);
}
} // namespace gtsam

241
gtsam/navigation/PreintegrationBase.cpp
View File

@ -31,21 +31,12 @@ namespace gtsam {
PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p,
const Bias& biasHat)
: p_(p), biasHat_(biasHat), deltaTij_(0.0) {
resetIntegration();
}
//------------------------------------------------------------------------------
void PreintegrationBase::resetIntegration() {
deltaTij_ = 0.0;
preintegrated_.setZero();
preintegrated_H_biasAcc_.setZero();
preintegrated_H_biasOmega_.setZero();
}
//------------------------------------------------------------------------------
ostream& operator<<(ostream& os, const PreintegrationBase& pim) {
os << " deltaTij " << pim.deltaTij_ << endl;
os << " deltaRij " << Point3(pim.theta()) << endl;
os << " deltaRij.ypr = (" << pim.deltaRij().ypr().transpose() << ")" << endl;
os << " deltaPij " << Point3(pim.deltaPij()) << endl;
os << " deltaVij " << Point3(pim.deltaVij()) << endl;
os << " gyrobias " << Point3(pim.biasHat_.gyroscope()) << endl;
@ -59,14 +50,9 @@ void PreintegrationBase::print(const string& s) const {
}
//------------------------------------------------------------------------------
bool PreintegrationBase::equals(const PreintegrationBase& other,
double tol) const {
return p_->equals(*other.p_, tol)
&& fabs(deltaTij_ - other.deltaTij_) < tol
&& biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(preintegrated_, other.preintegrated_, tol)
&& equal_with_abs_tol(preintegrated_H_biasAcc_, other.preintegrated_H_biasAcc_, tol)
&& equal_with_abs_tol(preintegrated_H_biasOmega_, other.preintegrated_H_biasOmega_, tol);
void PreintegrationBase::resetIntegrationAndSetBias(const Bias& biasHat) {
biasHat_ = biasHat;
resetIntegration();
}
//------------------------------------------------------------------------------
@ -105,7 +91,8 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
// Update derivative: centrifugal causes the correlation between acc and omega!!!
if (correctedAcc_H_unbiasedOmega) {
double wdp = correctedOmega.dot(b_arm);
*correctedAcc_H_unbiasedOmega = -( (Matrix) Vector3::Constant(wdp).asDiagonal()
const Matrix3 diag_wdp = Vector3::Constant(wdp).asDiagonal();
*correctedAcc_H_unbiasedOmega = -( diag_wdp
+ correctedOmega * b_arm.transpose()) * bRs.matrix()
+ 2 * b_arm * unbiasedOmega.transpose();
}
@ -114,139 +101,38 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsBySensorPose(
return make_pair(correctedAcc, correctedOmega);
}
//------------------------------------------------------------------------------
// See extensive discussion in ImuFactor.lyx
Vector9 PreintegrationBase::UpdatePreintegrated(
const Vector3& a_body, const Vector3& w_body, double dt,
const Vector9& preintegrated, OptionalJacobian<9, 9> A,
OptionalJacobian<9, 3> B, OptionalJacobian<9, 3> C) {
const auto theta = preintegrated.segment<3>(0);
const auto position = preintegrated.segment<3>(3);
const auto velocity = preintegrated.segment<3>(6);
// This functor allows for saving computation when exponential map and its
// derivatives are needed at the same location in so<3>
so3::DexpFunctor local(theta);
// Calculate exact mean propagation
Matrix3 w_tangent_H_theta, invH;
const Vector3 w_tangent = // angular velocity mapped back to tangent space
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
const SO3 R = local.expmap();
const Vector3 a_nav = R * a_body;
const double dt22 = 0.5 * dt * dt;
Vector9 preintegratedPlus;
preintegratedPlus << // new preintegrated vector:
theta + w_tangent* dt, // theta
position + velocity* dt + a_nav* dt22, // position
velocity + a_nav* dt; // velocity
if (A) {
// Exact derivative of R*a with respect to theta:
const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp();
A->setIdentity();
A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
}
if (B) {
B->block<3, 3>(0, 0) = Z_3x3;
B->block<3, 3>(3, 0) = R * dt22;
B->block<3, 3>(6, 0) = R * dt;
}
if (C) {
C->block<3, 3>(0, 0) = invH * dt;
C->block<3, 3>(3, 0) = Z_3x3;
C->block<3, 3>(6, 0) = Z_3x3;
}
return preintegratedPlus;
}
//------------------------------------------------------------------------------
void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega,
double dt, Matrix9* A,
Matrix93* B, Matrix93* C) {
// Correct for bias in the sensor frame
Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
// Possibly correct for sensor pose
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
if (p().body_P_sensor)
boost::tie(acc, omega) = correctMeasurementsBySensorPose( acc, omega,
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
// Do update
deltaTij_ += dt;
preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C);
if (p().body_P_sensor) {
// More complicated derivatives in case of non-trivial sensor pose
*C *= D_correctedOmega_omega;
if (!p().body_P_sensor->translation().isZero())
*C += *B* D_correctedAcc_omega;
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
}
// D_plus_abias = D_plus_preintegrated * D_preintegrated_abias + D_plus_a * D_a_abias
preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B);
// D_plus_wbias = D_plus_preintegrated * D_preintegrated_wbias + D_plus_w * D_w_wbias
preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C);
}
void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega,
double dt) {
// NOTE(frank): integrateMeasuremtn always needs to compute the derivatives,
const Vector3& measuredOmega, double dt) {
// NOTE(frank): integrateMeasurement always needs to compute the derivatives,
// even when not of interest to the caller. Provide scratch space here.
Matrix9 A;
Matrix93 B, C;
integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
}
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::biasCorrectedDelta(
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
// We correct for a change between bias_i and the biasHat_ used to integrate
// This is a simple linear correction with obvious derivatives
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
const Vector9 biasCorrected =
preintegrated() + preintegrated_H_biasAcc_ * biasIncr.accelerometer() +
preintegrated_H_biasOmega_ * biasIncr.gyroscope();
if (H) {
(*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_;
}
return biasCorrected;
update(measuredAcc, measuredOmega, dt, &A, &B, &C);
}
//------------------------------------------------------------------------------
NavState PreintegrationBase::predict(const NavState& state_i,
const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 6> H2) const {
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 6> H2) const {
// TODO(frank): make sure this stuff is still correct
Matrix96 D_biasCorrected_bias;
Vector9 biasCorrected =
biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : 0);
Vector9 biasCorrected = biasCorrectedDelta(bias_i,
H2 ? &D_biasCorrected_bias : 0);
// Correct for initial velocity and gravity
Matrix9 D_delta_state, D_delta_biasCorrected;
Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
p().omegaCoriolis, p().use2ndOrderCoriolis,
H1 ? &D_delta_state : 0,
H2 ? &D_delta_biasCorrected : 0);
p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
H2 ? &D_delta_biasCorrected : 0);
// Use retract to get back to NavState manifold
Matrix9 D_predict_state, D_predict_delta;
NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
if (H1) *H1 = D_predict_state + D_predict_delta* D_delta_state;
if (H2) *H2 = D_predict_delta* D_delta_biasCorrected* D_biasCorrected_bias;
if (H1)
*H1 = D_predict_state + D_predict_delta * D_delta_state;
if (H2)
*H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias;
return state_j;
}
@ -306,94 +192,6 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
return error;
}
//------------------------------------------------------------------------------
// sugar for derivative blocks
#define D_R_R(H) (H)->block<3,3>(0,0)
#define D_R_t(H) (H)->block<3,3>(0,3)
#define D_R_v(H) (H)->block<3,3>(0,6)
#define D_t_R(H) (H)->block<3,3>(3,0)
#define D_t_t(H) (H)->block<3,3>(3,3)
#define D_t_v(H) (H)->block<3,3>(3,6)
#define D_v_R(H) (H)->block<3,3>(6,0)
#define D_v_t(H) (H)->block<3,3>(6,3)
#define D_v_v(H) (H)->block<3,3>(6,6)
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::Compose(const Vector9& zeta01,
const Vector9& zeta12, double deltaT12,
OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 9> H2) {
const auto t01 = zeta01.segment<3>(0);
const auto p01 = zeta01.segment<3>(3);
const auto v01 = zeta01.segment<3>(6);
const auto t12 = zeta12.segment<3>(0);
const auto p12 = zeta12.segment<3>(3);
const auto v12 = zeta12.segment<3>(6);
Matrix3 R01_H_t01, R12_H_t12;
const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01);
const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12);
Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity
const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12);
Matrix3 t02_H_R02;
Vector9 zeta02;
const Matrix3 R = R01.matrix();
zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta
p01 + v01 * deltaT12 + R * p12, // position
v01 + R * v12; // velocity
if (H1) {
H1->setIdentity();
D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01;
D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01;
D_t_v(H1) = I_3x3 * deltaT12;
D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01;
}
if (H2) {
H2->setZero();
D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12;
D_t_t(H2) = R;
D_v_v(H2) = R;
}
return zeta02;
}
//------------------------------------------------------------------------------
void PreintegrationBase::mergeWith(const PreintegrationBase& pim12, Matrix9* H1,
Matrix9* H2) {
if (!matchesParamsWith(pim12)) {
throw std::domain_error("Cannot merge pre-integrated measurements with different params");
}
if (params()->body_P_sensor) {
throw std::domain_error("Cannot merge pre-integrated measurements with sensor pose yet");
}
const double t01 = deltaTij();
const double t12 = pim12.deltaTij();
deltaTij_ = t01 + t12;
const Vector9 zeta01 = preintegrated();
Vector9 zeta12 = pim12.preintegrated(); // will be modified.
const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat();
zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope()
+ pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer();
preintegrated_ = PreintegrationBase::Compose(zeta01, zeta12, t12, H1, H2);
preintegrated_H_biasAcc_ =
(*H1) * preintegrated_H_biasAcc_ + (*H2) * pim12.preintegrated_H_biasAcc_;
preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_ +
(*H2) * pim12.preintegrated_H_biasOmega_;
}
//------------------------------------------------------------------------------
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
@ -408,6 +206,7 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
p_ = q;
return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
}
#endif
//------------------------------------------------------------------------------

100
gtsam/navigation/PreintegrationBase.h
View File

@ -75,29 +75,13 @@ class PreintegrationBase {
/// Time interval from i to j
double deltaTij_;
/**
* Preintegrated navigation state, from frame i to frame j
* Order is: theta, position, velocity
* Note: relative position does not take into account velocity at time i, see deltap+, in [2]
* Note: velocity is now also in frame i, as opposed to deltaVij in [2]
*/
Vector9 preintegrated_;
Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated preintegrated w.r.t. acceleration bias
Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated preintegrated w.r.t. angular rate bias
/// Default constructor for serialization
PreintegrationBase() {
resetIntegration();
}
PreintegrationBase() {}
public:
/// @name Constructors
/// @{
/// @name Constructors
/// @{
/**
* Constructor, initializes the variables in the base class
* @param p Parameters, typically fixed in a single application
@ -111,7 +95,12 @@ public:
/// @name Basic utilities
/// @{
/// Re-initialize PreintegratedMeasurements
void resetIntegration();
virtual void resetIntegration()=0;
/// @name Basic utilities
/// @{
/// Re-initialize PreintegratedMeasurements and set new bias
void resetIntegrationAndSetBias(const Bias& biasHat);
/// check parameters equality: checks whether shared pointer points to same Params object.
bool matchesParamsWith(const PreintegrationBase& other) const {
@ -140,17 +129,10 @@ public:
const imuBias::ConstantBias& biasHat() const { return biasHat_; }
double deltaTij() const { return deltaTij_; }
const Vector9& preintegrated() const { return preintegrated_; }
Vector3 theta() const { return preintegrated_.head<3>(); }
Vector3 deltaPij() const { return preintegrated_.segment<3>(3); }
Vector3 deltaVij() const { return preintegrated_.tail<3>(); }
Rot3 deltaRij() const { return Rot3::Expmap(theta()); }
NavState deltaXij() const { return NavState::Retract(preintegrated_); }
const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; }
const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; }
virtual Vector3 deltaPij() const=0;
virtual Vector3 deltaVij() const=0;
virtual Rot3 deltaRij() const=0;
virtual NavState deltaXij() const=0;
// Exposed for MATLAB
Vector6 biasHatVector() const { return biasHat_.vector(); }
@ -159,8 +141,7 @@ public:
/// @name Testable
/// @{
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim);
void print(const std::string& s) const;
bool equals(const PreintegrationBase& other, double tol) const;
virtual void print(const std::string& s) const;
/// @}
/// @name Main functionality
@ -175,30 +156,20 @@ public:
OptionalJacobian<3, 3> correctedAcc_H_unbiasedOmega = boost::none,
OptionalJacobian<3, 3> correctedOmega_H_unbiasedOmega = boost::none) const;
// Update integrated vector on tangent manifold preintegrated with acceleration
// Static, functional version.
static Vector9 UpdatePreintegrated(const Vector3& a_body,
const Vector3& w_body, double dt,
const Vector9& preintegrated,
OptionalJacobian<9, 9> A = boost::none,
OptionalJacobian<9, 3> B = boost::none,
OptionalJacobian<9, 3> C = boost::none);
/// Update preintegrated measurements and get derivatives
/// It takes measured quantities in the j frame
/// Modifies preintegrated_ in place after correcting for bias and possibly sensor pose
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double deltaT,
Matrix9* A, Matrix93* B, Matrix93* C);
/// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
virtual void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
const double dt, Matrix9* A, Matrix93* B, Matrix93* C)=0;
// Version without derivatives
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double deltaT);
/// Version without derivatives
virtual void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double dt);
/// Given the estimate of the bias, return a NavState tangent vector
/// summarizing the preintegrated IMU measurements so far
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> H = boost::none) const;
virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> H = boost::none) const=0;
/// Predict state at time j
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
@ -219,23 +190,6 @@ public:
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
// Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives
static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12,
double deltaT12,
OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 9> H2 = boost::none);
/// Merge in a different set of measurements and update bias derivatives accordingly
/// The derivatives apply to the preintegrated Vector9
void mergeWith(const PreintegrationBase& pim, Matrix9* H1, Matrix9* H2);
/// @}
/** Dummy clone for MATLAB */
virtual boost::shared_ptr<PreintegrationBase> clone() const {
return boost::shared_ptr<PreintegrationBase>();
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
@ -249,20 +203,6 @@ public:
#endif
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size()));
ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size()));
ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size()));
}
};
} /// namespace gtsam

249
gtsam/navigation/TangentPreintegration.cpp Normal file
View File

@ -0,0 +1,249 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file TangentPreintegration.cpp
* @author Frank Dellaert
* @author Adam Bry
**/
#include "TangentPreintegration.h"
#include <gtsam/base/numericalDerivative.h>
#include <boost/make_shared.hpp>
using namespace std;
namespace gtsam {
//------------------------------------------------------------------------------
TangentPreintegration::TangentPreintegration(const boost::shared_ptr<Params>& p,
const Bias& biasHat) :
PreintegrationBase(p, biasHat) {
resetIntegration();
}
//------------------------------------------------------------------------------
void TangentPreintegration::resetIntegration() {
deltaTij_ = 0.0;
preintegrated_.setZero();
preintegrated_H_biasAcc_.setZero();
preintegrated_H_biasOmega_.setZero();
}
//------------------------------------------------------------------------------
bool TangentPreintegration::equals(const TangentPreintegration& other,
double tol) const {
return p_->equals(*other.p_, tol) && fabs(deltaTij_ - other.deltaTij_) < tol
&& biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(preintegrated_, other.preintegrated_, tol)
&& equal_with_abs_tol(preintegrated_H_biasAcc_,
other.preintegrated_H_biasAcc_, tol)
&& equal_with_abs_tol(preintegrated_H_biasOmega_,
other.preintegrated_H_biasOmega_, tol);
}
//------------------------------------------------------------------------------
// See extensive discussion in ImuFactor.lyx
Vector9 TangentPreintegration::UpdatePreintegrated(const Vector3& a_body,
const Vector3& w_body, double dt, const Vector9& preintegrated,
OptionalJacobian<9, 9> A, OptionalJacobian<9, 3> B,
OptionalJacobian<9, 3> C) {
const auto theta = preintegrated.segment<3>(0);
const auto position = preintegrated.segment<3>(3);
const auto velocity = preintegrated.segment<3>(6);
// This functor allows for saving computation when exponential map and its
// derivatives are needed at the same location in so<3>
so3::DexpFunctor local(theta);
// Calculate exact mean propagation
Matrix3 w_tangent_H_theta, invH;
const Vector3 w_tangent = // angular velocity mapped back to tangent space
local.applyInvDexp(w_body, A ? &w_tangent_H_theta : 0, C ? &invH : 0);
const SO3 R = local.expmap();
const Vector3 a_nav = R * a_body;
const double dt22 = 0.5 * dt * dt;
Vector9 preintegratedPlus;
preintegratedPlus << // new preintegrated vector:
theta + w_tangent * dt, // theta
position + velocity * dt + a_nav * dt22, // position
velocity + a_nav * dt; // velocity
if (A) {
// Exact derivative of R*a with respect to theta:
const Matrix3 a_nav_H_theta = R * skewSymmetric(-a_body) * local.dexp();
A->setIdentity();
A->block<3, 3>(0, 0).noalias() += w_tangent_H_theta * dt; // theta
A->block<3, 3>(3, 0) = a_nav_H_theta * dt22; // position wrpt theta...
A->block<3, 3>(3, 6) = I_3x3 * dt; // .. and velocity
A->block<3, 3>(6, 0) = a_nav_H_theta * dt; // velocity wrpt theta
}
if (B) {
B->block<3, 3>(0, 0) = Z_3x3;
B->block<3, 3>(3, 0) = R * dt22;
B->block<3, 3>(6, 0) = R * dt;
}
if (C) {
C->block<3, 3>(0, 0) = invH * dt;
C->block<3, 3>(3, 0) = Z_3x3;
C->block<3, 3>(6, 0) = Z_3x3;
}
return preintegratedPlus;
}
//------------------------------------------------------------------------------
void TangentPreintegration::update(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B,
Matrix93* C) {
// Correct for bias in the sensor frame
Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
// Possibly correct for sensor pose
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
if (p().body_P_sensor)
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
// Do update
deltaTij_ += dt;
preintegrated_ = UpdatePreintegrated(acc, omega, dt, preintegrated_, A, B, C);
if (p().body_P_sensor) {
// More complicated derivatives in case of non-trivial sensor pose
*C *= D_correctedOmega_omega;
if (!p().body_P_sensor->translation().isZero())
*C += *B * D_correctedAcc_omega;
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
}
// new_H_biasAcc = new_H_old * old_H_biasAcc + new_H_acc * acc_H_biasAcc
// where acc_H_biasAcc = -I_3x3, hence
// new_H_biasAcc = new_H_old * old_H_biasAcc - new_H_acc
preintegrated_H_biasAcc_ = (*A) * preintegrated_H_biasAcc_ - (*B);
// new_H_biasOmega = new_H_old * old_H_biasOmega + new_H_omega * omega_H_biasOmega
// where omega_H_biasOmega = -I_3x3, hence
// new_H_biasOmega = new_H_old * old_H_biasOmega - new_H_omega
preintegrated_H_biasOmega_ = (*A) * preintegrated_H_biasOmega_ - (*C);
}
//------------------------------------------------------------------------------
Vector9 TangentPreintegration::biasCorrectedDelta(
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
// We correct for a change between bias_i and the biasHat_ used to integrate
// This is a simple linear correction with obvious derivatives
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
const Vector9 biasCorrected = preintegrated()
+ preintegrated_H_biasAcc_ * biasIncr.accelerometer()
+ preintegrated_H_biasOmega_ * biasIncr.gyroscope();
if (H) {
(*H) << preintegrated_H_biasAcc_, preintegrated_H_biasOmega_;
}
return biasCorrected;
}
//------------------------------------------------------------------------------
// sugar for derivative blocks
#define D_R_R(H) (H)->block<3,3>(0,0)
#define D_R_t(H) (H)->block<3,3>(0,3)
#define D_R_v(H) (H)->block<3,3>(0,6)
#define D_t_R(H) (H)->block<3,3>(3,0)
#define D_t_t(H) (H)->block<3,3>(3,3)
#define D_t_v(H) (H)->block<3,3>(3,6)
#define D_v_R(H) (H)->block<3,3>(6,0)
#define D_v_t(H) (H)->block<3,3>(6,3)
#define D_v_v(H) (H)->block<3,3>(6,6)
//------------------------------------------------------------------------------
Vector9 TangentPreintegration::Compose(const Vector9& zeta01,
const Vector9& zeta12, double deltaT12, OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 9> H2) {
const auto t01 = zeta01.segment<3>(0);
const auto p01 = zeta01.segment<3>(3);
const auto v01 = zeta01.segment<3>(6);
const auto t12 = zeta12.segment<3>(0);
const auto p12 = zeta12.segment<3>(3);
const auto v12 = zeta12.segment<3>(6);
Matrix3 R01_H_t01, R12_H_t12;
const Rot3 R01 = Rot3::Expmap(t01, R01_H_t01);
const Rot3 R12 = Rot3::Expmap(t12, R12_H_t12);
Matrix3 R02_H_R01, R02_H_R12; // NOTE(frank): R02_H_R12 == Identity
const Rot3 R02 = R01.compose(R12, R02_H_R01, R02_H_R12);
Matrix3 t02_H_R02;
Vector9 zeta02;
const Matrix3 R = R01.matrix();
zeta02 << Rot3::Logmap(R02, t02_H_R02), // theta
p01 + v01 * deltaT12 + R * p12, // position
v01 + R * v12; // velocity
if (H1) {
H1->setIdentity();
D_R_R(H1) = t02_H_R02 * R02_H_R01 * R01_H_t01;
D_t_R(H1) = R * skewSymmetric(-p12) * R01_H_t01;
D_t_v(H1) = I_3x3 * deltaT12;
D_v_R(H1) = R * skewSymmetric(-v12) * R01_H_t01;
}
if (H2) {
H2->setZero();
D_R_R(H2) = t02_H_R02 * R02_H_R12 * R12_H_t12;
D_t_t(H2) = R;
D_v_v(H2) = R;
}
return zeta02;
}
//------------------------------------------------------------------------------
void TangentPreintegration::mergeWith(const TangentPreintegration& pim12,
Matrix9* H1, Matrix9* H2) {
if (!matchesParamsWith(pim12)) {
throw std::domain_error(
"Cannot merge pre-integrated measurements with different params");
}
if (params()->body_P_sensor) {
throw std::domain_error(
"Cannot merge pre-integrated measurements with sensor pose yet");
}
const double t01 = deltaTij();
const double t12 = pim12.deltaTij();
deltaTij_ = t01 + t12;
const Vector9 zeta01 = preintegrated();
Vector9 zeta12 = pim12.preintegrated(); // will be modified.
const imuBias::ConstantBias bias_incr_for_12 = biasHat() - pim12.biasHat();
zeta12 += pim12.preintegrated_H_biasOmega_ * bias_incr_for_12.gyroscope()
+ pim12.preintegrated_H_biasAcc_ * bias_incr_for_12.accelerometer();
preintegrated_ = TangentPreintegration::Compose(zeta01, zeta12, t12, H1, H2);
preintegrated_H_biasAcc_ = (*H1) * preintegrated_H_biasAcc_
+ (*H2) * pim12.preintegrated_H_biasAcc_;
preintegrated_H_biasOmega_ = (*H1) * preintegrated_H_biasOmega_
+ (*H2) * pim12.preintegrated_H_biasOmega_;
}
//------------------------------------------------------------------------------
}// namespace gtsam

140
gtsam/navigation/TangentPreintegration.h Normal file
View File

@ -0,0 +1,140 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file TangentPreintegration.h
* @author Frank Dellaert
* @author Adam Bry
**/
#pragma once
#include <gtsam/navigation/PreintegrationBase.h>
namespace gtsam {
/**
* Integrate on the 9D tangent space of the NavState manifold.
* See extensive discussion in ImuFactor.lyx
*/
class TangentPreintegration : public PreintegrationBase {
protected:
/**
* Preintegrated navigation state, as a 9D vector on tangent space at frame i
* Order is: theta, position, velocity
*/
Vector9 preintegrated_;
Matrix93 preintegrated_H_biasAcc_; ///< Jacobian of preintegrated_ w.r.t. acceleration bias
Matrix93 preintegrated_H_biasOmega_; ///< Jacobian of preintegrated_ w.r.t. angular rate bias
/// Default constructor for serialization
TangentPreintegration() {
resetIntegration();
}
public:
/// @name Constructors
/// @{
/**
* Constructor, initializes the variables in the base class
* @param p Parameters, typically fixed in a single application
* @param bias Current estimate of acceleration and rotation rate biases
*/
TangentPreintegration(const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat = imuBias::ConstantBias());
/// @}
/// @name Basic utilities
/// @{
/// Re-initialize PreintegratedMeasurements
void resetIntegration() override;
/// @}
/// @name Instance variables access
/// @{
Vector3 deltaPij() const override { return preintegrated_.segment<3>(3); }
Vector3 deltaVij() const override { return preintegrated_.tail<3>(); }
Rot3 deltaRij() const override { return Rot3::Expmap(theta()); }
NavState deltaXij() const override { return NavState().retract(preintegrated_); }
const Vector9& preintegrated() const { return preintegrated_; }
Vector3 theta() const { return preintegrated_.head<3>(); }
const Matrix93& preintegrated_H_biasAcc() const { return preintegrated_H_biasAcc_; }
const Matrix93& preintegrated_H_biasOmega() const { return preintegrated_H_biasOmega_; }
/// @name Testable
/// @{
bool equals(const TangentPreintegration& other, double tol) const;
/// @}
/// @name Main functionality
/// @{
// Update integrated vector on tangent manifold preintegrated with acceleration
// Static, functional version.
static Vector9 UpdatePreintegrated(const Vector3& a_body,
const Vector3& w_body, const double dt,
const Vector9& preintegrated,
OptionalJacobian<9, 9> A = boost::none,
OptionalJacobian<9, 3> B = boost::none,
OptionalJacobian<9, 3> C = boost::none);
/// Update preintegrated measurements and get derivatives
/// It takes measured quantities in the j frame
/// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
/// NOTE(frank): implementation is different in two versions
void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
const double dt, Matrix9* A, Matrix93* B, Matrix93* C) override;
/// Given the estimate of the bias, return a NavState tangent vector
/// summarizing the preintegrated IMU measurements so far
/// NOTE(frank): implementation is different in two versions
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> H = boost::none) const override;
// Compose the two pre-integrated 9D-vectors zeta01 and zeta02, with derivatives
static Vector9 Compose(const Vector9& zeta01, const Vector9& zeta12,
double deltaT12,
OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 9> H2 = boost::none);
/// Merge in a different set of measurements and update bias derivatives accordingly
/// The derivatives apply to the preintegrated Vector9
void mergeWith(const TangentPreintegration& pim, Matrix9* H1, Matrix9* H2);
/// @}
/** Dummy clone for MATLAB */
virtual boost::shared_ptr<TangentPreintegration> clone() const {
return boost::shared_ptr<TangentPreintegration>();
}
/// @}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & bs::make_nvp("preintegrated_", bs::make_array(preintegrated_.data(), preintegrated_.size()));
ar & bs::make_nvp("preintegrated_H_biasAcc_", bs::make_array(preintegrated_H_biasAcc_.data(), preintegrated_H_biasAcc_.size()));
ar & bs::make_nvp("preintegrated_H_biasOmega_", bs::make_array(preintegrated_H_biasOmega_.data(), preintegrated_H_biasOmega_.size()));
}
};
} /// namespace gtsam

2
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -132,6 +132,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) {
}
/* ************************************************************************* */
#ifdef GTSAM_TANGENT_PREINTEGRATION
TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
auto p = testing::Params();
testing::SomeMeasurements measurements;
@ -151,6 +152,7 @@ TEST(CombinedImuFactor, FirstOrderPreIntegratedMeasurements) {
EXPECT(assert_equal(numericalDerivative22<Vector9, Vector3, Vector3>(preintegrated, Z_3x1, Z_3x1),
pim.preintegrated_H_biasOmega(), 1e-3));
}
#endif
/* ************************************************************************* */
TEST(CombinedImuFactor, PredictPositionAndVelocity) {

76
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -57,6 +57,41 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i,
} // namespace
/* ************************************************************************* */
TEST(ImuFactor, PreintegratedMeasurementsConstruction) {
// Actual pre-integrated values
PreintegratedImuMeasurements actual(testing::Params());
EXPECT(assert_equal(Rot3(), actual.deltaRij()));
EXPECT(assert_equal(kZero, actual.deltaPij()));
EXPECT(assert_equal(kZero, actual.deltaVij()));
DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9);
}
/* ************************************************************************* */
TEST(ImuFactor, PreintegratedMeasurementsReset) {
auto p = testing::Params();
// Create a preintegrated measurement struct and integrate
PreintegratedImuMeasurements pimActual(p);
Vector3 measuredAcc(0.5, 1.0, 0.5);
Vector3 measuredOmega(0.1, 0.3, 0.1);
double deltaT = 1.0;
pimActual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// reset and make sure that it is the same as a fresh one
pimActual.resetIntegration();
CHECK(assert_equal(pimActual, PreintegratedImuMeasurements(p)));
// Now create one with a different bias ..
Bias nonZeroBias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3));
PreintegratedImuMeasurements pimExpected(p, nonZeroBias);
// integrate again, then reset to a new bias
pimActual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
pimActual.resetIntegrationAndSetBias(nonZeroBias);
CHECK(assert_equal(pimActual, pimExpected));
}
/* ************************************************************************* */
TEST(ImuFactor, Accelerating) {
const double a = 0.2, v = 50;
@ -83,24 +118,20 @@ TEST(ImuFactor, Accelerating) {
/* ************************************************************************* */
TEST(ImuFactor, PreintegratedMeasurements) {
// Measurements
Vector3 measuredAcc(0.1, 0.0, 0.0);
Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
const double a = 0.1, w = M_PI / 100.0;
Vector3 measuredAcc(a, 0.0, 0.0);
Vector3 measuredOmega(w, 0.0, 0.0);
double deltaT = 0.5;
// Expected pre-integrated values
Vector3 expectedDeltaR1(0.5 * M_PI / 100.0, 0.0, 0.0);
Vector3 expectedDeltaP1(0.5 * 0.1 * 0.5 * 0.5, 0, 0);
Vector3 expectedDeltaR1(w * deltaT, 0.0, 0.0);
Vector3 expectedDeltaP1(0.5 * a * deltaT*deltaT, 0, 0);
Vector3 expectedDeltaV1(0.05, 0.0, 0.0);
// Actual pre-integrated values
PreintegratedImuMeasurements actual(testing::Params());
EXPECT(assert_equal(kZero, actual.theta()));
EXPECT(assert_equal(kZero, actual.deltaPij()));
EXPECT(assert_equal(kZero, actual.deltaVij()));
DOUBLES_EQUAL(0.0, actual.deltaTij(), 1e-9);
actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR1, actual.theta()));
EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR1), actual.deltaRij()));
EXPECT(assert_equal(expectedDeltaP1, actual.deltaPij()));
EXPECT(assert_equal(expectedDeltaV1, actual.deltaVij()));
DOUBLES_EQUAL(0.5, actual.deltaTij(), 1e-9);
@ -129,7 +160,7 @@ TEST(ImuFactor, PreintegratedMeasurements) {
// Actual pre-integrated values
actual.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR2, actual.theta()));
EXPECT(assert_equal(Rot3::Expmap(expectedDeltaR2), actual.deltaRij()));
EXPECT(assert_equal(expectedDeltaP2, actual.deltaPij()));
EXPECT(assert_equal(expectedDeltaV2, actual.deltaVij()));
DOUBLES_EQUAL(1.0, actual.deltaTij(), 1e-9);
@ -438,27 +469,6 @@ TEST(ImuFactor, fistOrderExponential) {
EXPECT(assert_equal(expectedRot, actualRot));
}
/* ************************************************************************* */
TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
testing::SomeMeasurements measurements;
boost::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
[=](const Vector3& a, const Vector3& w) {
PreintegratedImuMeasurements pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.preintegrated();
};
// Actual pre-integrated values
PreintegratedImuMeasurements pim(testing::Params());
testing::integrateMeasurements(measurements, &pim);
EXPECT(assert_equal(numericalDerivative21(preintegrated, kZero, kZero),
pim.preintegrated_H_biasAcc()));
EXPECT(assert_equal(numericalDerivative22(preintegrated, kZero, kZero),
pim.preintegrated_H_biasOmega(), 1e-3));
}
/* ************************************************************************* */
Vector3 correctedAcc(const PreintegratedImuMeasurements& pim,
const Vector3& measuredAcc, const Vector3& measuredOmega) {
@ -789,6 +799,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
}
/* ************************************************************************* */
#ifdef GTSAM_TANGENT_PREINTEGRATION
static const double kVelocity = 2.0, kAngularVelocity = M_PI / 6;
struct ImuFactorMergeTest {
@ -883,6 +894,7 @@ TEST(ImuFactor, MergeWithCoriolis) {
mergeTest.p_->omegaCoriolis = Vector3(0.1, 0.2, -0.1);
mergeTest.TestScenarios(result_, name_, kZeroBias, kZeroBias, 1e-4);
}
#endif
/* ************************************************************************* */
// Same values as pre-integration test but now testing covariance

114
gtsam/navigation/tests/testManifoldPreintegration.cpp Normal file
View File

@ -0,0 +1,114 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testManifoldPreintegration.cpp
* @brief Unit test for the ManifoldPreintegration
* @author Luca Carlone
*/
#include <gtsam/navigation/ManifoldPreintegration.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
#include "imuFactorTesting.h"
namespace testing {
// Create default parameters with Z-down and above noise parameters
static boost::shared_ptr<PreintegrationParams> Params() {
auto p = PreintegrationParams::MakeSharedD(kGravity);
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
p->integrationCovariance = 0.0001 * I_3x3;
return p;
}
}
/* ************************************************************************* */
TEST(ManifoldPreintegration, BiasCorrectionJacobians) {
testing::SomeMeasurements measurements;
boost::function<Rot3(const Vector3&, const Vector3&)> deltaRij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaRij();
};
boost::function<Point3(const Vector3&, const Vector3&)> deltaPij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaPij();
};
boost::function<Vector3(const Vector3&, const Vector3&)> deltaVij =
[=](const Vector3& a, const Vector3& w) {
ManifoldPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.deltaVij();
};
// Actual pre-integrated values
ManifoldPreintegration pim(testing::Params());
testing::integrateMeasurements(measurements, &pim);
EXPECT(
assert_equal(numericalDerivative21(deltaRij, kZero, kZero),
Matrix3(Z_3x3)));
EXPECT(
assert_equal(numericalDerivative22(deltaRij, kZero, kZero),
pim.delRdelBiasOmega(), 1e-3));
EXPECT(
assert_equal(numericalDerivative21(deltaPij, kZero, kZero),
pim.delPdelBiasAcc()));
EXPECT(
assert_equal(numericalDerivative22(deltaPij, kZero, kZero),
pim.delPdelBiasOmega(), 1e-3));
EXPECT(
assert_equal(numericalDerivative21(deltaVij, kZero, kZero),
pim.delVdelBiasAcc()));
EXPECT(
assert_equal(numericalDerivative22(deltaVij, kZero, kZero),
pim.delVdelBiasOmega(), 1e-3));
}
/* ************************************************************************* */
TEST(ManifoldPreintegration, computeError) {
ManifoldPreintegration pim(testing::Params());
NavState x1, x2;
imuBias::ConstantBias bias;
Matrix9 aH1, aH2;
Matrix96 aH3;
pim.computeError(x1, x2, bias, aH1, aH2, aH3);
boost::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)> f =
boost::bind(&ManifoldPreintegration::computeError, pim, _1, _2, _3,
boost::none, boost::none, boost::none);
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9));
EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

91
gtsam/navigation/tests/testNavState.cpp
View File

@ -36,6 +36,26 @@ static const Vector9 kZeroXi = Vector9::Zero();
/* ************************************************************************* */
TEST(NavState, Constructor) {
boost::function<NavState(const Rot3&, const Point3&, const Vector3&)> create =
boost::bind(&NavState::Create, _1, _2, _3, boost::none, boost::none,
boost::none);
Matrix aH1, aH2, aH3;
EXPECT(
assert_equal(kState1,
NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3)));
EXPECT(
assert_equal(
numericalDerivative31(create, kAttitude, kPosition, kVelocity), aH1));
EXPECT(
assert_equal(
numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
EXPECT(
assert_equal(
numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
}
/* ************************************************************************* */
TEST(NavState, Constructor2) {
boost::function<NavState(const Pose3&, const Vector3&)> construct =
boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none,
boost::none);
@ -87,19 +107,6 @@ TEST( NavState, BodyVelocity) {
EXPECT(assert_equal((Matrix )eH, aH));
}
/* ************************************************************************* */
TEST( NavState, MatrixGroup ) {
// check roundtrip conversion to 7*7 matrix representation
Matrix7 T = kState1.matrix();
EXPECT(assert_equal(kState1, NavState(T)));
// check group product agrees with matrix product
NavState state2 = kState1 * kState1;
Matrix T2 = T * T;
EXPECT(assert_equal(state2, NavState(T2)));
EXPECT(assert_equal(T2, state2.matrix()));
}
/* ************************************************************************* */
TEST( NavState, Manifold ) {
// Check zero xi
@ -114,7 +121,9 @@ TEST( NavState, Manifold ) {
Rot3 drot = Rot3::Expmap(xi.head<3>());
Point3 dt = Point3(xi.segment<3>(3));
Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3);
NavState state2 = kState1 * NavState(drot, dt, dvel);
NavState state2 = NavState(kState1.attitude() * drot,
kState1.position() + kState1.attitude() * dt,
kState1.velocity() + kState1.attitude() * dvel);
EXPECT(assert_equal(state2, kState1.retract(xi)));
EXPECT(assert_equal(xi, kState1.localCoordinates(state2)));
@ -122,27 +131,6 @@ TEST( NavState, Manifold ) {
NavState state3 = state2.retract(xi);
EXPECT(assert_equal(xi, state2.localCoordinates(state3)));
// Check derivatives for ChartAtOrigin::Retract
Matrix9 aH;
// For zero xi
boost::function<NavState(const Vector9&)> Retract = boost::bind(
NavState::ChartAtOrigin::Retract, _1, boost::none);
NavState::ChartAtOrigin::Retract(kZeroXi, aH);
EXPECT(assert_equal(numericalDerivative11(Retract, kZeroXi), aH));
// For non-zero xi
NavState::ChartAtOrigin::Retract(xi, aH);
EXPECT(assert_equal(numericalDerivative11(Retract, xi), aH));
// Check derivatives for ChartAtOrigin::Local
// For zero xi
boost::function<Vector9(const NavState&)> Local = boost::bind(
NavState::ChartAtOrigin::Local, _1, boost::none);
NavState::ChartAtOrigin::Local(kIdentity, aH);
EXPECT(assert_equal(numericalDerivative11(Local, kIdentity), aH));
// For non-zero xi
NavState::ChartAtOrigin::Local(kState1, aH);
EXPECT(assert_equal(numericalDerivative11(Local, kState1), aH));
// Check retract derivatives
Matrix9 aH1, aH2;
kState1.retract(xi, aH1, aH2);
@ -151,6 +139,12 @@ TEST( NavState, Manifold ) {
EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1));
EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2));
// Check retract derivatives on state 2
const Vector9 xi2 = -3.0*xi;
state2.retract(xi2, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(retract, state2, xi2), aH1));
EXPECT(assert_equal(numericalDerivative22(retract, state2, xi2), aH2));
// Check localCoordinates derivatives
boost::function<Vector9(const NavState&, const NavState&)> local =
boost::bind(&NavState::localCoordinates, _1, _2, boost::none,
@ -169,29 +163,6 @@ TEST( NavState, Manifold ) {
EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2));
}
/* ************************************************************************* */
TEST( NavState, Lie ) {
// Check zero xi
EXPECT(assert_equal(kIdentity, kIdentity.expmap(kZeroXi)));
EXPECT(assert_equal(kZeroXi, kIdentity.logmap(kIdentity)));
EXPECT(assert_equal(kState1, kState1.expmap(kZeroXi)));
EXPECT(assert_equal(kZeroXi, kState1.logmap(kState1)));
// Expmap/Logmap roundtrip
Vector xi(9);
xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
NavState state2 = NavState::Expmap(xi);
EXPECT(assert_equal(xi, NavState::Logmap(state2)));
// roundtrip from state2 to state3 and back
NavState state3 = state2.expmap(xi);
EXPECT(assert_equal(xi, state2.logmap(state3)));
// For the expmap/logmap (not necessarily expmap/local) -xi goes other way
EXPECT(assert_equal(state2, state3.expmap(-xi)));
EXPECT(assert_equal(xi, -state3.logmap(state2)));
}
/* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
TEST(NavState, Update) {
@ -201,8 +172,8 @@ TEST(NavState, Update) {
Matrix9 aF;
Matrix93 aG1, aG2;
boost::function<NavState(const NavState&, const Vector3&, const Vector3&)> update =
boost::bind(&NavState::update, _1, _2, _3, dt, boost::none,
boost::none, boost::none);
boost::bind(&NavState::update, _1, _2, _3, dt, boost::none,
boost::none, boost::none);
Vector3 b_acc = kAttitude * acc;
NavState expected(kAttitude.expmap(dt * omega),
kPosition + Point3((kVelocity + b_acc * dt / 2) * dt),

61
gtsam/navigation/tests/testPreintegrationBase.cpp → gtsam/navigation/tests/testTangentPreintegration.cpp
View File

@ -10,12 +10,12 @@
* -------------------------------------------------------------------------- */
/**
* @file testPreintegrationBase.cpp
* @file testTangentPreintegration.cpp
* @brief Unit test for the InertialNavFactor
* @author Frank Dellaert
*/
#include <gtsam/navigation/PreintegrationBase.h>
#include <gtsam/navigation/TangentPreintegration.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
@ -29,7 +29,7 @@
static const double kDt = 0.1;
Vector9 f(const Vector9& zeta, const Vector3& a, const Vector3& w) {
return PreintegrationBase::UpdatePreintegrated(a, w, kDt, zeta);
return TangentPreintegration::UpdatePreintegrated(a, w, kDt, zeta);
}
namespace testing {
@ -44,8 +44,8 @@ static boost::shared_ptr<PreintegrationParams> Params() {
}
/* ************************************************************************* */
TEST(PreintegrationBase, UpdateEstimate1) {
PreintegrationBase pim(testing::Params());
TEST(TangentPreintegration, UpdateEstimate1) {
TangentPreintegration pim(testing::Params());
const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
Vector9 zeta;
zeta.setZero();
@ -58,8 +58,8 @@ TEST(PreintegrationBase, UpdateEstimate1) {
}
/* ************************************************************************* */
TEST(PreintegrationBase, UpdateEstimate2) {
PreintegrationBase pim(testing::Params());
TEST(TangentPreintegration, UpdateEstimate2) {
TangentPreintegration pim(testing::Params());
const Vector3 acc(0.1, 0.2, 10), omega(0.1, 0.2, 0.3);
Vector9 zeta;
zeta << 0.01, 0.02, 0.03, 100, 200, 300, 10, 5, 3;
@ -73,8 +73,31 @@ TEST(PreintegrationBase, UpdateEstimate2) {
}
/* ************************************************************************* */
TEST(PreintegrationBase, computeError) {
PreintegrationBase pim(testing::Params());
TEST(ImuFactor, BiasCorrectionJacobians) {
testing::SomeMeasurements measurements;
boost::function<Vector9(const Vector3&, const Vector3&)> preintegrated =
[=](const Vector3& a, const Vector3& w) {
TangentPreintegration pim(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim);
return pim.preintegrated();
};
// Actual pre-integrated values
TangentPreintegration pim(testing::Params());
testing::integrateMeasurements(measurements, &pim);
EXPECT(
assert_equal(numericalDerivative21(preintegrated, kZero, kZero),
pim.preintegrated_H_biasAcc()));
EXPECT(
assert_equal(numericalDerivative22(preintegrated, kZero, kZero),
pim.preintegrated_H_biasOmega(), 1e-3));
}
/* ************************************************************************* */
TEST(TangentPreintegration, computeError) {
TangentPreintegration pim(testing::Params());
NavState x1, x2;
imuBias::ConstantBias bias;
Matrix9 aH1, aH2;
@ -82,7 +105,7 @@ TEST(PreintegrationBase, computeError) {
pim.computeError(x1, x2, bias, aH1, aH2, aH3);
boost::function<Vector9(const NavState&, const NavState&,
const imuBias::ConstantBias&)> f =
boost::bind(&PreintegrationBase::computeError, pim, _1, _2, _3,
boost::bind(&TangentPreintegration::computeError, pim, _1, _2, _3,
boost::none, boost::none, boost::none);
// NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0
EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9));
@ -91,47 +114,47 @@ TEST(PreintegrationBase, computeError) {
}
/* ************************************************************************* */
TEST(PreintegrationBase, Compose) {
TEST(TangentPreintegration, Compose) {
testing::SomeMeasurements measurements;
PreintegrationBase pim(testing::Params());
TangentPreintegration pim(testing::Params());
testing::integrateMeasurements(measurements, &pim);
boost::function<Vector9(const Vector9&, const Vector9&)> f =
[pim](const Vector9& zeta01, const Vector9& zeta12) {
return PreintegrationBase::Compose(zeta01, zeta12, pim.deltaTij());
return TangentPreintegration::Compose(zeta01, zeta12, pim.deltaTij());
};
// Expected merge result
PreintegrationBase expected_pim02(testing::Params());
TangentPreintegration expected_pim02(testing::Params());
testing::integrateMeasurements(measurements, &expected_pim02);
testing::integrateMeasurements(measurements, &expected_pim02);
// Actual result
Matrix9 H1, H2;
PreintegrationBase actual_pim02 = pim;
TangentPreintegration actual_pim02 = pim;
actual_pim02.mergeWith(pim, &H1, &H2);
const Vector9 zeta = pim.preintegrated();
const Vector9 actual_zeta =
PreintegrationBase::Compose(zeta, zeta, pim.deltaTij());
TangentPreintegration::Compose(zeta, zeta, pim.deltaTij());
EXPECT(assert_equal(expected_pim02.preintegrated(), actual_zeta, 1e-7));
EXPECT(assert_equal(numericalDerivative21(f, zeta, zeta), H1, 1e-7));
EXPECT(assert_equal(numericalDerivative22(f, zeta, zeta), H2, 1e-7));
}
/* ************************************************************************* */
TEST(PreintegrationBase, MergedBiasDerivatives) {
TEST(TangentPreintegration, MergedBiasDerivatives) {
testing::SomeMeasurements measurements;
auto f = [=](const Vector3& a, const Vector3& w) {
PreintegrationBase pim02(testing::Params(), Bias(a, w));
TangentPreintegration pim02(testing::Params(), Bias(a, w));
testing::integrateMeasurements(measurements, &pim02);
testing::integrateMeasurements(measurements, &pim02);
return pim02.preintegrated();
};
// Expected merge result
PreintegrationBase expected_pim02(testing::Params());
TangentPreintegration expected_pim02(testing::Params());
testing::integrateMeasurements(measurements, &expected_pim02);
testing::integrateMeasurements(measurements, &expected_pim02);

14
gtsam/nonlinear/ExtendedKalmanFilter-inl.h
View File

@ -61,16 +61,14 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class VALUE>
ExtendedKalmanFilter<VALUE>::ExtendedKalmanFilter(Key key_initial, T x_initial,
noiseModel::Gaussian::shared_ptr P_initial) {
// Set the initial linearization point to the provided mean
x_ = x_initial;
template <class VALUE>
ExtendedKalmanFilter<VALUE>::ExtendedKalmanFilter(
Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial)
: x_(x_initial) // Set the initial linearization point
{
// Create a Jacobian Prior Factor directly P_initial.
// Since x0 is set to the provided mean, the b vector in the prior will be zero
// TODO Frank asks: is there a reason why noiseModel is not simply P_initial ?
// TODO(Frank): is there a reason why noiseModel is not simply P_initial?
int n = traits<T>::GetDimension(x_initial);
priorFactor_ = JacobianFactor::shared_ptr(
new JacobianFactor(key_initial, P_initial->R(), Vector::Zero(n),

2
gtsam/nonlinear/NonlinearEquality.h
View File

@ -260,7 +260,7 @@ public:
std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key())
<< ")," << "\n";
this->noiseModel_->print();
value_.print("Value");
traits<X>::Print(value_, "Value");
}
private:

35
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -80,7 +80,7 @@ TEST(Expression, Leaves) {
// Unary(Leaf)
namespace unary {
Point2 f1(const Point3& p, OptionalJacobian<2, 3> H) {
return Point2();
return Point2(0,0);
}
double f2(const Point3& p, OptionalJacobian<1, 3> H) {
return 0.0;
@ -111,24 +111,43 @@ TEST(Expression, Unary3) {
}
/* ************************************************************************* */
class Class : public Point3 {
public:
enum {dimension = 3};
using Point3::Point3;
const Vector3& vector() const { return *this; }
inline static Class identity() { return Class(0,0,0); }
double norm(OptionalJacobian<1,3> H = boost::none) const {
return norm3(*this, H);
}
bool equals(const Class &q, double tol) const {
return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol && fabs(z() - q.z()) < tol);
}
void print(const string& s) const { cout << s << *this << endl;}
};
namespace gtsam {
template<> struct traits<Class> : public internal::VectorSpace<Class> {};
}
// Nullary Method
TEST(Expression, NullaryMethod) {
// Create expression
Expression<Point3> p(67);
Expression<double> norm(&gtsam::norm, p);
Expression<Class> p(67);
Expression<double> norm_(p, &Class::norm);
// Create Values
Values values;
values.insert(67, Point3(3, 4, 5));
values.insert(67, Class(3, 4, 5));
// Check dims as map
std::map<Key, int> map;
norm.dims(map);
norm_.dims(map);
LONGS_EQUAL(1, map.size());
// Get value and Jacobians
std::vector<Matrix> H(1);
double actual = norm.value(values, H);
double actual = norm_.value(values, H);
// Check all
EXPECT(actual == sqrt(50));
@ -370,7 +389,7 @@ TEST(Expression, TripleSum) {
/* ************************************************************************* */
TEST(Expression, SumOfUnaries) {
const Key key(67);
const Double_ norm_(&gtsam::norm, Point3_(key));
const Double_ norm_(&gtsam::norm3, Point3_(key));
const Double_ sum_ = norm_ + norm_;
Values values;
@ -389,7 +408,7 @@ TEST(Expression, SumOfUnaries) {
TEST(Expression, UnaryOfSum) {
const Key key1(42), key2(67);
const Point3_ sum_ = Point3_(key1) + Point3_(key2);
const Double_ norm_(&gtsam::norm, sum_);
const Double_ norm_(&gtsam::norm3, sum_);
map<Key, int> actual_dims, expected_dims = map_list_of<Key, int>(key1, 3)(key2, 3);
norm_.dims(actual_dims);

6
gtsam/slam/EssentialMatrixFactor.h
View File

@ -139,7 +139,7 @@ public:
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s);
std::cout << " EssentialMatrixFactor2 with measurements\n ("
<< dP1_.transpose() << ")' and (" << pn_.vector().transpose()
<< dP1_.transpose() << ")' and (" << pn_.transpose()
<< ")'" << std::endl;
}
@ -162,7 +162,7 @@ public:
// The point d*P1 = (x,y,1) is computed in constructor as dP1_
// Project to normalized image coordinates, then uncalibrate
Point2 pn;
Point2 pn(0,0);
if (!DE && !Dd) {
Point3 _1T2 = E.direction().point3();
@ -195,7 +195,7 @@ public:
}
Point2 reprojectionError = pn - pn_;
return f_ * reprojectionError.vector();
return f_ * reprojectionError;
}
};

17
gtsam/slam/GeneralSFMFactor.h
View File

@ -107,7 +107,7 @@ public:
*/
void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/**
@ -115,15 +115,14 @@ public:
*/
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol);
return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
}
/** h(x)-z */
Vector evaluateError(const CAMERA& camera, const LANDMARK& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const {
try {
Point2 reprojError(camera.project2(point,H1,H2) - measured_);
return reprojError.vector();
return camera.project2(point,H1,H2) - measured_;
}
catch( CheiralityException& e) {
if (H1) *H1 = JacobianC::Zero();
@ -145,8 +144,7 @@ public:
try {
const CAMERA& camera = values.at<CAMERA>(key1);
const LANDMARK& point = values.at<LANDMARK>(key2);
Point2 reprojError(camera.project2(point, H1, H2) - measured());
b = -reprojError.vector();
b = measured() - camera.project2(point, H1, H2);
} catch (CheiralityException& e) {
H1.setZero();
H2.setZero();
@ -243,7 +241,7 @@ public:
*/
void print(const std::string& s = "SFMFactor2", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/**
@ -251,7 +249,7 @@ public:
*/
bool equals(const NonlinearFactor &p, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol);
return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol);
}
/** h(x)-z */
@ -262,8 +260,7 @@ public:
{
try {
Camera camera(pose3,calib);
Point2 reprojError(camera.project(point, H1, H2, H3) - measured_);
return reprojError.vector();
return camera.project(point, H1, H2, H3) - measured_;
}
catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2, 6);

16
gtsam/slam/ProjectionFactor.h
View File

@ -56,7 +56,9 @@ namespace gtsam {
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
GenericProjectionFactor() : throwCheirality_(false), verboseCheirality_(false) {}
GenericProjectionFactor() :
measured_(0, 0), throwCheirality_(false), verboseCheirality_(false) {
}
/**
* Constructor
@ -108,7 +110,7 @@ namespace gtsam {
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "GenericProjectionFactor, z = ";
measured_.print();
traits<Point2>::Print(measured_);
if(this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
Base::print("", keyFormatter);
@ -119,7 +121,7 @@ namespace gtsam {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol)
&& traits<Point2>::Equals(this->measured_, e->measured_, tol)
&& this->K_->equals(*e->K_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
}
@ -134,16 +136,14 @@ namespace gtsam {
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
*H1 = *H1 * H0;
return reprojectionError.vector();
return reprojectionError;
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
return reprojectionError.vector();
return camera.project(point, H1, H2, boost::none) - measured_;
}
} else {
PinholeCamera<CALIBRATION> camera(pose, *K_);
Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
return reprojectionError.vector();
return camera.project(point, H1, H2, boost::none) - measured_;
}
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);

4
gtsam/slam/ReferenceFrameFactor.h
View File

@ -84,7 +84,7 @@ public:
* each degree of freedom.
*/
ReferenceFrameFactor(Key globalKey, Key transKey, Key localKey, double sigma = 1e-2)
: Base(noiseModel::Isotropic::Sigma(POINT::dimension, sigma),
: Base(noiseModel::Isotropic::Sigma(traits<POINT>::dimension, sigma),
globalKey, transKey, localKey) {}
virtual ~ReferenceFrameFactor(){}
@ -100,7 +100,7 @@ public:
boost::optional<Matrix&> Dlocal = boost::none) const {
Point newlocal = transform_point<Transform,Point>(trans, global, Dtrans, Dforeign);
if (Dlocal)
*Dlocal = -1* Matrix::Identity(Point::dimension,Point::dimension);
*Dlocal = -1* Matrix::Identity(traits<Point>::dimension, traits<Point>::dimension);
return traits<Point>::Local(local,newlocal);
}

2
gtsam/slam/SmartFactorBase.h
View File

@ -189,7 +189,7 @@ public:
bool areMeasurementsEqual = true;
for (size_t i = 0; i < measured_.size(); i++) {
if (this->measured_.at(i).equals(e->measured_.at(i), tol) == false)
if (traits<Z>::Equals(this->measured_.at(i), e->measured_.at(i), tol) == false)
areMeasurementsEqual = false;
break;
}

4
gtsam/slam/SmartProjectionFactor.h
View File

@ -499,8 +499,8 @@ public:
return Base::totalReprojectionError(cameras, *result_);
else if (params_.degeneracyMode == HANDLE_INFINITY) {
// Otherwise, manage the exceptions with rotation-only factors
const Point2& z0 = this->measured_.at(0);
Unit3 backprojected = cameras.front().backprojectPointAtInfinity(z0);
Unit3 backprojected = cameras.front().backprojectPointAtInfinity(
this->measured_.at(0));
return Base::totalReprojectionError(cameras, backprojected);
} else
// if we don't want to manage the exceptions we discard the factor

24
gtsam/slam/TriangulationFactor.h
View File

@ -78,12 +78,11 @@ public:
bool verboseCheirality = false) :
Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_(
throwCheirality), verboseCheirality_(verboseCheirality) {
if (model && model->dim() != Measurement::dimension)
if (model && model->dim() != traits<Measurement>::dimension)
throw std::invalid_argument(
"TriangulationFactor must be created with "
+ boost::lexical_cast<std::string>((int) Measurement::dimension)
+ boost::lexical_cast<std::string>((int) traits<Measurement>::dimension)
+ "-dimensional noise model.");
}
/** Virtual destructor */
@ -105,7 +104,7 @@ public:
DefaultKeyFormatter) const {
std::cout << s << "TriangulationFactor,";
camera_.print("camera");
measured_.print("z");
traits<Measurement>::Print(measured_, "z");
Base::print("", keyFormatter);
}
@ -113,25 +112,24 @@ public:
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->camera_.equals(e->camera_, tol)
&& this->measured_.equals(e->measured_, tol);
&& traits<Measurement>::Equals(this->measured_, e->measured_, tol);
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
boost::none) const {
try {
Measurement error(camera_.project2(point, boost::none, H2) - measured_);
return error.vector();
return traits<Measurement>::Local(measured_, camera_.project2(point, boost::none, H2));
} catch (CheiralityException& e) {
if (H2)
*H2 = Matrix::Zero(Measurement::dimension, 3);
*H2 = Matrix::Zero(traits<Measurement>::dimension, 3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "
<< DefaultKeyFormatter(this->key()) << " moved behind camera"
<< std::endl;
if (throwCheirality_)
throw e;
return Eigen::Matrix<double,Measurement::dimension,1>::Constant(2.0 * camera_.calibration().fx());
return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * camera_.calibration().fx());
}
}
@ -153,14 +151,14 @@ public:
// Allocate memory for Jacobian factor, do only once
if (Ab.rows() == 0) {
std::vector<size_t> dimensions(1, 3);
Ab = VerticalBlockMatrix(dimensions, Measurement::dimension, true);
A.resize(Measurement::dimension,3);
b.resize(Measurement::dimension);
Ab = VerticalBlockMatrix(dimensions, traits<Measurement>::dimension, true);
A.resize(traits<Measurement>::dimension,3);
b.resize(traits<Measurement>::dimension);
}
// Would be even better if we could pass blocks to project
const Point3& point = x.at<Point3>(key());
b = -(camera_.project2(point, boost::none, A) - measured_).vector();
b = traits<Measurement>::Local(camera_.project2(point, boost::none, A), measured_);
if (noiseModel_)
this->noiseModel_->WhitenSystem(A, b);

12
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -222,8 +222,7 @@ TEST (EssentialMatrixFactor2, factor) {
// Check evaluation
Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1);
const Point2 pi = PinholeBase::Project(P2);
Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
Point2 expected(pi - pB(i));
Matrix Hactual1, Hactual2;
double d(baseline / P1.z());
@ -296,8 +295,7 @@ TEST (EssentialMatrixFactor3, factor) {
// Check evaluation
Point3 P1 = data.tracks[i].p;
const Point2 pi = camera2.project(P1);
Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
Point2 expected(pi - pB(i));
Matrix Hactual1, Hactual2;
double d(baseline / P1.z());
@ -438,8 +436,7 @@ TEST (EssentialMatrixFactor2, extraTest) {
// Check evaluation
Point3 P1 = data.tracks[i].p;
const Point2 pi = camera2.project(P1);
Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
Point2 expected(pi - pB(i));
Matrix Hactual1, Hactual2;
double d(baseline / P1.z());
@ -507,8 +504,7 @@ TEST (EssentialMatrixFactor3, extraTest) {
// Check evaluation
Point3 P1 = data.tracks[i].p;
const Point2 pi = camera2.project(P1);
Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
Point2 expected(pi - pB(i));
Matrix Hactual1, Hactual2;
double d(baseline / P1.z());

4
gtsam/slam/tests/testSmartFactorBase.cpp
View File

@ -55,8 +55,8 @@ struct traits<PinholeFactor> : public Testable<PinholeFactor> {};
TEST(SmartFactorBase, Pinhole) {
PinholeFactor f= PinholeFactor(unit2);
f.add(Point2(), 1);
f.add(Point2(), 2);
f.add(Point2(0,0), 1);
f.add(Point2(0,0), 2);
EXPECT_LONGS_EQUAL(2 * 2, f.dim());
}

10
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

@ -300,20 +300,14 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
SfM_Track track1;
for (size_t i = 0; i < 3; ++i) {
SfM_Measurement measures;
measures.first = i + 1; // cameras are from 1 to 3
measures.second = measurements_cam1.at(i);
track1.measurements.push_back(measures);
track1.measurements.emplace_back(i + 1, measurements_cam1.at(i));
}
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
smartFactor1->add(track1);
SfM_Track track2;
for (size_t i = 0; i < 3; ++i) {
SfM_Measurement measures;
measures.first = i + 1; // cameras are from 1 to 3
measures.second = measurements_cam2.at(i);
track2.measurements.push_back(measures);
track2.measurements.emplace_back(i + 1, measurements_cam2.at(i));
}
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
smartFactor2->add(track2);

2
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -172,7 +172,7 @@ double PoseRTV::range(const PoseRTV& other,
const Point3 t1 = pose().translation(H1 ? &D_t1_pose : 0);
const Point3 t2 = other.pose().translation(H2 ? &D_t2_other : 0);
Matrix13 D_d_t1, D_d_t2;
double d = distance(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0);
double d = distance3(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0);
if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0;
if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0;
return d;

2
gtsam_unstable/geometry/Event.h
View File

@ -85,7 +85,7 @@ public:
OptionalJacobian<1, 3> H2 = boost::none) const {
static const double Speed = 330;
Matrix13 D1, D2;
double distance = gtsam::distance(location_, microphone, D1, D2);
double distance = gtsam::distance3(location_, microphone, D1, D2);
if (H1)
// derivative of toa with respect to event
*H1 << 1.0, D1 / Speed;

18
gtsam_unstable/geometry/SimPolygon2D.cpp
View File

@ -46,7 +46,7 @@ SimPolygon2D SimPolygon2D::createRectangle(const Point2& p, double height, doubl
bool SimPolygon2D::equals(const SimPolygon2D& p, double tol) const {
if (p.size() != size()) return false;
for (size_t i=0; i<size(); ++i)
if (!landmarks_[i].equals(p.landmarks_[i], tol))
if (!traits<Point2>::Equals(landmarks_[i], p.landmarks_[i], tol))
return false;
return true;
}
@ -55,7 +55,7 @@ bool SimPolygon2D::equals(const SimPolygon2D& p, double tol) const {
void SimPolygon2D::print(const string& s) const {
cout << "SimPolygon " << s << ": " << endl;
for(const Point2& p: landmarks_)
p.print(" ");
traits<Point2>::Print(p, " ");
}
/* ************************************************************************* */
@ -151,7 +151,7 @@ SimPolygon2D SimPolygon2D::randomTriangle(
Pose2 xC = xB.retract(Vector::Unit(3,0)*dBC);
// use triangle equality to verify non-degenerate triangle
double dAC = xA.t().distance(xC.t());
double dAC = distance2(xA.t(), xC.t());
// form a triangle and test if it meets requirements
SimPolygon2D test_tri = SimPolygon2D::createTriangle(xA.t(), xB.t(), xC.t());
@ -164,7 +164,7 @@ SimPolygon2D SimPolygon2D::randomTriangle(
insideBox(side_len, test_tri.landmark(0)) &&
insideBox(side_len, test_tri.landmark(1)) &&
insideBox(side_len, test_tri.landmark(2)) &&
test_tri.landmark(1).distance(test_tri.landmark(2)) > min_side_len &&
distance2(test_tri.landmark(1), test_tri.landmark(2)) > min_side_len &&
!nearExisting(lms, test_tri.landmark(0), min_vertex_dist) &&
!nearExisting(lms, test_tri.landmark(1), min_vertex_dist) &&
!nearExisting(lms, test_tri.landmark(2), min_vertex_dist) &&
@ -260,7 +260,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
return p;
}
throw runtime_error("Failed to find a place for a landmark!");
return Point2();
return Point2(0,0);
}
/* ***************************************************************** */
@ -272,7 +272,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
return p;
}
throw runtime_error("Failed to find a place for a landmark!");
return Point2();
return Point2(0,0);
}
/* ***************************************************************** */
@ -285,7 +285,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(double boundary_size,
return p;
}
throw runtime_error("Failed to find a place for a landmark!");
return Point2();
return Point2(0,0);
}
/* ***************************************************************** */
@ -303,7 +303,7 @@ Point2 SimPolygon2D::randomBoundedPoint2(
return p;
}
throw runtime_error("Failed to find a place for a landmark!");
return Point2();
return Point2(0,0);
}
/* ***************************************************************** */
@ -320,7 +320,7 @@ bool SimPolygon2D::insideBox(double s, const Point2& p) {
bool SimPolygon2D::nearExisting(const std::vector<Point2>& S,
const Point2& p, double threshold) {
for(const Point2& Sp: S)
if (Sp.distance(p) < threshold)
if (distance2(Sp, p) < threshold)
return true;
return false;
}

21
gtsam_unstable/geometry/SimWall2D.cpp
View File

@ -14,13 +14,14 @@ using namespace std;
/* ************************************************************************* */
void SimWall2D::print(const std::string& s) const {
std::cout << "SimWall2D " << s << ":" << std::endl;
a_.print(" a");
b_.print(" b");
traits<Point2>::Print(a_, " a");
traits<Point2>::Print(b_, " b");
}
/* ************************************************************************* */
bool SimWall2D::equals(const SimWall2D& other, double tol) const {
return a_.equals(other.a_, tol) && b_.equals(other.b_, tol);
return traits<Point2>::Equals(a_, other.a_, tol) &&
traits<Point2>::Equals(b_, other.b_, tol);
}
/* ************************************************************************* */
@ -37,8 +38,8 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional<Point2&> pt) cons
if (debug) cout << "len: " << len << endl;
Point2 Ba = transform.transform_to(B.a()),
Bb = transform.transform_to(B.b());
if (debug) Ba.print("Ba");
if (debug) Bb.print("Bb");
if (debug) traits<Point2>::Print(Ba, "Ba");
if (debug) traits<Point2>::Print(Bb, "Bb");
// check sides of line
if (Ba.y() * Bb.y() > 0.0 ||
@ -73,7 +74,7 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional<Point2&> pt) cons
}
// find lower point by y
Point2 low, high;
Point2 low(0,0), high(0,0);
if (Ba.y() > Bb.y()) {
high = Ba;
low = Bb;
@ -81,8 +82,8 @@ bool SimWall2D::intersects(const SimWall2D& B, boost::optional<Point2&> pt) cons
high = Bb;
low = Ba;
}
if (debug) high.print("high");
if (debug) low.print("low");
if (debug) traits<Point2>::Print(high, "high");
if (debug) traits<Point2>::Print(low, "low");
// find x-intercept
double slope = (high.y() - low.y())/(high.x() - low.x());
@ -138,7 +139,7 @@ std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
Point2 cur_intersection;
if (wall.intersects(traj,cur_intersection)) {
collision = true;
if (cur_pose.t().distance(cur_intersection) < cur_pose.t().distance(intersection)) {
if (distance2(cur_pose.t(), cur_intersection) < distance2(cur_pose.t(), intersection)) {
intersection = cur_intersection;
closest_wall = wall;
}
@ -154,7 +155,7 @@ std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
norm = norm / norm.norm();
// Simple check to make sure norm is on side closest robot
if (cur_pose.t().distance(intersection + norm) > cur_pose.t().distance(intersection - norm))
if (distance2(cur_pose.t(), intersection + norm) > distance2(cur_pose.t(), intersection - norm))
norm = - norm;
// using the reflection

2
gtsam_unstable/geometry/SimWall2D.h
View File

@ -43,7 +43,7 @@ namespace gtsam {
SimWall2D scale(double s) const { return SimWall2D(s*a_, s*b_); }
/** geometry */
double length() const { return a_.distance(b_); }
double length() const { return distance2(a_, b_); }
Point2 midpoint() const;
/**

2
gtsam_unstable/geometry/tests/testSimPolygon2D.cpp
View File

@ -16,7 +16,7 @@ const double tol=1e-5;
TEST(testPolygon, triangle_basic) {
// create a triangle from points, extract landmarks/walls, check occupancy
Point2 pA, pB(2.0, 0.0), pC(0.0, 1.0);
Point2 pA(0,0), pB(2.0, 0.0), pC(0.0, 1.0);
// construct and extract data
SimPolygon2D actTriangle = SimPolygon2D::createTriangle(pA, pB, pC);

4
gtsam_unstable/geometry/tests/testSimWall2D.cpp
View File

@ -24,7 +24,7 @@ TEST(testSimWall2D2D, construction ) {
/* ************************************************************************* */
TEST(testSimWall2D2D, equals ) {
Point2 p1(1.0, 0.0), p2(1.0, 2.0), p3;
Point2 p1(1.0, 0.0), p2(1.0, 2.0), p3(0,0);
SimWall2D w1(p1, p2), w2(p1, p3);
EXPECT(assert_equal(w1, w1));
EXPECT(assert_inequal(w1, w2));
@ -34,7 +34,7 @@ TEST(testSimWall2D2D, equals ) {
/* ************************************************************************* */
TEST(testSimWall2D2D, intersection1 ) {
SimWall2D w1(2.0, 2.0, 6.0, 2.0), w2(4.0, 4.0, 4.0, 0.0);
Point2 pt;
Point2 pt(0,0);
EXPECT(w1.intersects(w2));
EXPECT(w2.intersects(w1));
w1.intersects(w2, pt);

45
gtsam_unstable/slam/InvDepthFactor3.h
View File

@ -24,17 +24,17 @@ namespace gtsam {
* Ternary factor representing a visual measurement that includes inverse depth
*/
template<class POSE, class LANDMARK, class INVDEPTH>
class InvDepthFactor3: public gtsam::NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> {
class InvDepthFactor3: public NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> {
protected:
// Keep a copy of measurement and calibration for I/O
gtsam::Point2 measured_; ///< 2D measurement
boost::shared_ptr<gtsam::Cal3_S2> K_; ///< shared pointer to calibration object
Point2 measured_; ///< 2D measurement
boost::shared_ptr<Cal3_S2> K_; ///< shared pointer to calibration object
public:
/// shorthand for base class type
typedef gtsam::NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
typedef NoiseModelFactor3<POSE, LANDMARK, INVDEPTH> Base;
/// shorthand for this class
typedef InvDepthFactor3<POSE, LANDMARK, INVDEPTH> This;
@ -43,7 +43,9 @@ public:
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
InvDepthFactor3() : K_(new gtsam::Cal3_S2(444, 555, 666, 777, 888)) {}
InvDepthFactor3() :
measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
}
/**
* Constructor
@ -55,8 +57,8 @@ public:
* @param invDepthKey is the index of inverse depth
* @param K shared pointer to the constant calibration
*/
InvDepthFactor3(const gtsam::Point2& measured, const gtsam::SharedNoiseModel& model,
const gtsam::Key poseKey, gtsam::Key pointKey, gtsam::Key invDepthKey, const Cal3_S2::shared_ptr& K) :
InvDepthFactor3(const Point2& measured, const SharedNoiseModel& model,
const Key poseKey, Key pointKey, Key invDepthKey, const Cal3_S2::shared_ptr& K) :
Base(model, poseKey, pointKey, invDepthKey), measured_(measured), K_(K) {}
/** Virtual destructor */
@ -68,44 +70,43 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "InvDepthFactor3",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/// equals
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && this->measured_.equals(e->measured_, tol) && this->K_->equals(*e->K_, tol);
return e && Base::equals(p, tol) && traits<Point2>::Equals(this->measured_, e->measured_, tol) && this->K_->equals(*e->K_, tol);
}
/// Evaluate error h(x)-z and optionally derivatives
gtsam::Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none,
boost::optional<gtsam::Matrix&> H3=boost::none) const {
Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const {
try {
InvDepthCamera3<gtsam::Cal3_S2> camera(pose, K_);
gtsam::Point2 reprojectionError(camera.project(point, invDepth, H1, H2, H3) - measured_);
return reprojectionError.vector();
InvDepthCamera3<Cal3_S2> camera(pose, K_);
return camera.project(point, invDepth, H1, H2, H3) - measured_;
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);
if (H2) *H2 = Matrix::Zero(2,5);
if (H3) *H2 = Matrix::Zero(2,1);
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
return (gtsam::Vector(1) << 0.0).finished();
return (Vector(1) << 0.0).finished();
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
const Point2& imagePoint() const {
return measured_;
}
/** return the calibration object */
inline const gtsam::Cal3_S2::shared_ptr calibration() const {
inline const Cal3_S2::shared_ptr calibration() const {
return K_;
}

25
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -41,7 +41,9 @@ public:
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
InvDepthFactorVariant1() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
InvDepthFactorVariant1() :
measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
}
/**
* Constructor
@ -64,17 +66,17 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "InvDepthFactorVariant1",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/// equals
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol)
&& traits<Point2>::Equals(this->measured_, e->measured_, tol)
&& this->K_->equals(*e->K_, tol);
}
@ -86,22 +88,21 @@ public:
Point3 world_P_landmark = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
// Project landmark into Pose2
PinholeCamera<Cal3_S2> camera(pose, *K_);
gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
return reprojectionError.vector();
return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) {
std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
<< std::endl;
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
return (gtsam::Vector(1) << 0.0).finished();
return (Vector(1) << 0.0).finished();
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Vector6& landmark,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if (H1) {
(*H1) = numericalDerivative11<Vector, Pose3>(
@ -118,7 +119,7 @@ public:
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
const Point2& imagePoint() const {
return measured_;
}

25
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -43,7 +43,9 @@ public:
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
InvDepthFactorVariant2() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
InvDepthFactorVariant2() :
measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
}
/**
* Constructor
@ -67,17 +69,17 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "InvDepthFactorVariant2",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/// equals
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol)
&& traits<Point2>::Equals(this->measured_, e->measured_, tol)
&& this->K_->equals(*e->K_, tol)
&& traits<Point3>::Equals(this->referencePoint_, e->referencePoint_, tol);
}
@ -89,22 +91,21 @@ public:
Point3 world_P_landmark = referencePoint_ + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
// Project landmark into Pose2
PinholeCamera<Cal3_S2> camera(pose, *K_);
gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
return reprojectionError.vector();
return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) {
std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key2()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) <<"]"
<< std::endl;
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
return (gtsam::Vector(1) << 0.0).finished();
return (Vector(1) << 0.0).finished();
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Vector3& landmark,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if (H1) {
(*H1) = numericalDerivative11<Vector, Pose3>(
@ -121,7 +122,7 @@ public:
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
const Point2& imagePoint() const {
return measured_;
}

48
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -41,7 +41,9 @@ public:
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
InvDepthFactorVariant3a() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
InvDepthFactorVariant3a() :
measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
}
/**
* Constructor
@ -66,17 +68,17 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "InvDepthFactorVariant3a",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/// equals
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol)
&& traits<Point2>::Equals(this->measured_, e->measured_, tol)
&& this->K_->equals(*e->K_, tol);
}
@ -89,22 +91,21 @@ public:
Point3 world_P_landmark = pose.transform_from(pose_P_landmark);
// Project landmark into Pose2
PinholeCamera<Cal3_S2> camera(pose, *K_);
gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
return reprojectionError.vector();
return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) {
std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key2()) << "]"
<< " moved behind camera [" << DefaultKeyFormatter(this->key1()) << "]"
<< std::endl;
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
return (Vector(1) << 0.0).finished();
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Vector3& landmark,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) {
(*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose);
@ -117,7 +118,7 @@ public:
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
const Point2& imagePoint() const {
return measured_;
}
@ -160,7 +161,9 @@ public:
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
InvDepthFactorVariant3b() : K_(new Cal3_S2(444, 555, 666, 777, 888)) {}
InvDepthFactorVariant3b() :
measured_(0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) {
}
/**
* Constructor
@ -185,17 +188,17 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "InvDepthFactorVariant3",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const {
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
measured_.print(s + ".z");
traits<Point2>::Print(measured_, s + ".z");
}
/// equals
virtual bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const {
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol)
&& traits<Point2>::Equals(this->measured_, e->measured_, tol)
&& this->K_->equals(*e->K_, tol);
}
@ -208,23 +211,22 @@ public:
Point3 world_P_landmark = pose1.transform_from(pose1_P_landmark);
// Project landmark into Pose2
PinholeCamera<Cal3_S2> camera(pose2, *K_);
gtsam::Point2 reprojectionError(camera.project(world_P_landmark) - measured_);
return reprojectionError.vector();
return camera.project(world_P_landmark) - measured_;
} catch( CheiralityException& e) {
std::cout << e.what()
<< ": Inverse Depth Landmark [" << DefaultKeyFormatter(this->key1()) << "," << DefaultKeyFormatter(this->key3()) << "]"
<< " moved behind camera " << DefaultKeyFormatter(this->key2())
<< std::endl;
return gtsam::Vector::Ones(2) * 2.0 * K_->fx();
return Vector::Ones(2) * 2.0 * K_->fx();
}
return (Vector(1) << 0.0).finished();
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark,
boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none,
boost::optional<gtsam::Matrix&> H3=boost::none) const {
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none,
boost::optional<Matrix&> H3=boost::none) const {
if(H1)
(*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1);
@ -239,7 +241,7 @@ public:
}
/** return the measurement */
const gtsam::Point2& imagePoint() const {
const Point2& imagePoint() const {
return measured_;
}

22
gtsam_unstable/slam/MultiProjectionFactor.h
View File

@ -101,9 +101,9 @@ namespace gtsam {
virtual ~MultiProjectionFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
virtual NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
@ -143,20 +143,20 @@ namespace gtsam {
//
// if(body_P_sensor_) {
// if(H1) {
// gtsam::Matrix H0;
// Matrix H0;
// PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
// *H1 = *H1 * H0;
// return reprojectionError.vector();
// return reprojectionError;
// } else {
// PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
// return reprojectionError.vector();
// return reprojectionError;
// }
// } else {
// PinholeCamera<CALIBRATION> camera(pose, *K_);
// Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
// return reprojectionError.vector();
// return reprojectionError;
// }
// }
@ -168,20 +168,20 @@ namespace gtsam {
try {
if(body_P_sensor_) {
if(H1) {
gtsam::Matrix H0;
Matrix H0;
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
*H1 = *H1 * H0;
return reprojectionError.vector();
return reprojectionError;
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
return reprojectionError.vector();
return reprojectionError;
}
} else {
PinholeCamera<CALIBRATION> camera(pose, *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_);
return reprojectionError.vector();
return reprojectionError;
}
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);

21
gtsam_unstable/slam/ProjectionFactorPPP.h
View File

@ -54,7 +54,9 @@ namespace gtsam {
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
ProjectionFactorPPP() : throwCheirality_(false), verboseCheirality_(false) {}
ProjectionFactorPPP() :
measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
}
/**
* Constructor
@ -94,9 +96,9 @@ namespace gtsam {
virtual ~ProjectionFactorPPP() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
virtual NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
@ -105,7 +107,7 @@ namespace gtsam {
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "ProjectionFactorPPP, z = ";
measured_.print();
traits<Point2>::Print(measured_);
Base::print("", keyFormatter);
}
@ -114,7 +116,7 @@ namespace gtsam {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol)
&& traits<Point2>::Equals(this->measured_, e->measured_, tol)
&& this->K_->equals(*e->K_, tol);
}
@ -125,16 +127,15 @@ namespace gtsam {
boost::optional<Matrix&> H3 = boost::none) const {
try {
if(H1 || H2 || H3) {
gtsam::Matrix H0, H02;
Matrix H0, H02;
PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), *K_);
Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_);
*H2 = *H1 * H02;
*H1 = *H1 * H0;
return reprojectionError.vector();
return reprojectionError;
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(transform), *K_);
Point2 reprojectionError(camera.project(point, H1, H3, boost::none) - measured_);
return reprojectionError.vector();
return camera.project(point, H1, H3, boost::none) - measured_;
}
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);

21
gtsam_unstable/slam/ProjectionFactorPPPC.h
View File

@ -52,7 +52,9 @@ namespace gtsam {
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
ProjectionFactorPPPC() : throwCheirality_(false), verboseCheirality_(false) {}
ProjectionFactorPPPC() :
measured_(0.0, 0.0), throwCheirality_(false), verboseCheirality_(false) {
}
/**
* Constructor
@ -89,9 +91,9 @@ namespace gtsam {
virtual ~ProjectionFactorPPPC() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
virtual NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
@ -100,7 +102,7 @@ namespace gtsam {
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "ProjectionFactorPPPC, z = ";
measured_.print();
traits<Point2>::Print(measured_);
Base::print("", keyFormatter);
}
@ -109,7 +111,7 @@ namespace gtsam {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol);
&& traits<Point2>::Equals(this->measured_, e->measured_, tol);
}
/// Evaluate error h(x)-z and optionally derivatives
@ -120,16 +122,15 @@ namespace gtsam {
boost::optional<Matrix&> H4 = boost::none) const {
try {
if(H1 || H2 || H3 || H4) {
gtsam::Matrix H0, H02;
Matrix H0, H02;
PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), K);
Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
*H2 = *H1 * H02;
*H1 = *H1 * H0;
return reprojectionError.vector();
return reprojectionError;
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(transform), K);
Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
return reprojectionError.vector();
return camera.project(point, H1, H3, H4) - measured_;
}
} catch( CheiralityException& e) {
if (H1) *H1 = Matrix::Zero(2,6);

10
gtsam_unstable/slam/SmartRangeFactor.h
View File

@ -101,11 +101,11 @@ public:
// loop over all circles
for(const Circle2& it: circles) {
// distance between circle centers.
double d = circle1.center.dist(it.center);
double d = distance2(circle1.center, it.center);
if (d < 1e-9)
continue; // skip circles that are in the same location
// Find f and h, the intersection points in normalized circles
boost::optional<Point2> fh = Point2::CircleCircleIntersection(
boost::optional<Point2> fh = circleCircleIntersection(
circle1.radius / d, it.radius / d);
// Check if this pair is better by checking h = fh->y()
// if h is large, the intersections are well defined.
@ -116,15 +116,15 @@ public:
}
// use best fh to find actual intersection points
std::list<Point2> intersections = Point2::CircleCircleIntersection(
std::list<Point2> intersections = circleCircleIntersection(
circle1.center, best_circle->center, best_fh);
// pick winner based on other measurements
double error1 = 0, error2 = 0;
Point2 p1 = intersections.front(), p2 = intersections.back();
for(const Circle2& it: circles) {
error1 += it.center.dist(p1);
error2 += it.center.dist(p2);
error1 += distance2(it.center, p1);
error2 += distance2(it.center, p2);
}
return (error1 < error2) ? p1 : p2;
//gttoc_(triangulate);

8
gtsam_unstable/slam/TSAMFactors.h
View File

@ -48,9 +48,7 @@ public:
Vector evaluateError(const Pose2& pose, const Point2& point,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
Point2 d = pose.transform_to(point, H1, H2);
Point2 e = d - measured_;
return e.vector();
return pose.transform_to(point, H1, H2) - measured_;
}
};
@ -99,12 +97,12 @@ public:
*H3 = D_e_point_g * D_point_g_base2;
if (H4)
*H4 = D_e_point_g * D_point_g_point;
return (d - measured_).vector();
return d - measured_;
} else {
Pose2 pose_g = base1.compose(pose);
Point2 point_g = base2.transform_from(point);
Point2 d = pose_g.transform_to(point_g);
return (d - measured_).vector();
return d - measured_;
}
}
};

2
matlab.h
View File

@ -91,7 +91,7 @@ Matrix extractPoint2(const Values& values) {
Values::ConstFiltered<Point2> points = values.filter<Point2>();
Matrix result(points.size(), 2);
for(const auto& key_value: points)
result.row(j++) = key_value.value.vector();
result.row(j++) = key_value.value;
return result;
}

2
python/gtsam_examples/ImuFactorExample.py
View File

@ -69,7 +69,7 @@ class ImuFactorExample(PreintegrationExample):
# get measurements and add them to PIM
measuredOmega = self.runner.measuredAngularVelocity(t)
measuredAcc = self.runner.measuredSpecificForce(t)
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt, H9, H9)
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
# Plot IMU many times
if k % 10 == 0:

2
python/gtsam_examples/PreintegrationExample.py
View File

@ -101,7 +101,7 @@ class PreintegrationExample(object):
actualPose = self.scenario.pose(t)
plotPose3(POSES_FIG, actualPose, 0.3)
t = actualPose.translation()
self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim])
self.maxDim = max([abs(t[0]), abs(t[1]), abs(t[2]), self.maxDim])
ax = plt.gca()
ax.set_xlim3d(-self.maxDim, self.maxDim)
ax.set_ylim3d(-self.maxDim, self.maxDim)

2
python/gtsam_utils/plot.py
View File

@ -33,7 +33,7 @@ def plot3DPoints(fignum, values, linespec, marginals=None):
def plotPose3OnAxes(ax, pose, axisLength=0.1):
# get rotation and translation (center)
gRp = pose.rotation().matrix() # rotation from pose to global
C = pose.translation().vector()
C = pose.translation()
# draw the camera axes
xAxis = C + gRp[:, 0] * axisLength

13
python/handwritten/geometry/Point2.cpp
View File

@ -25,23 +25,26 @@
using namespace boost::python;
using namespace gtsam;
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point2::print, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point2::equals, 1, 2)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compose_overloads, Point2::compose, 1, 3)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(distance_overloads, Point2::distance, 1, 3)
#endif
void exportPoint2(){
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
class_<Point2>("Point2", init<>())
.def(init<double, double>())
.def(init<const Vector2 &>())
.def("identity", &Point2::identity)
.def("dist", &Point2::dist)
.def("distance", &Point2::distance)
.def("distance", &Point2::distance, distance_overloads(args("q","H1","H2")))
.def("equals", &Point2::equals, equals_overloads(args("q","tol")))
.def("norm", &Point2::norm)
.def("print", &Point2::print, print_overloads(args("s")))
.def("unit", &Point2::unit)
.def("vector", &Point2::vector)
.def("vector", &Point2::vector, return_value_policy<copy_const_reference>())
.def("x", &Point2::x)
.def("y", &Point2::y)
.def(self * other<double>()) // __mult__
@ -54,5 +57,5 @@ void exportPoint2(){
.def(repr(self))
.def(self == self)
;
}
#endif
}

30
python/handwritten/geometry/Point3.cpp
View File

@ -25,31 +25,32 @@
using namespace boost::python;
using namespace gtsam;
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(print_overloads, Point3::print, 0, 1)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(equals_overloads, Point3::equals, 1, 2)
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(norm_overloads, Point3::norm, 0, 1)
#endif
void exportPoint3(){
#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
class_<Point3>("Point3")
.def(init<>())
.def(init<double,double,double>())
.def(init<const Vector3 &>())
.def("identity", &Point3::identity)
.staticmethod("identity")
.def("cross", &Point3::cross)
.def("distance", &Point3::distance)
.def("dot", &Point3::dot)
.def("equals", &Point3::equals, equals_overloads(args("q","tol")))
.def("norm", &Point3::norm, norm_overloads(args("OptionalJacobian<1,3>")))
.def("normalized", &Point3::normalized)
.def("print", &Point3::print, print_overloads(args("s")))
#ifndef GTSAM_USE_VECTOR3_POINTS
.def("vector", &Point3::vector, return_value_policy<copy_const_reference>())
.def("x", &Point3::x)
.def("y", &Point3::y)
.def("z", &Point3::z)
#endif
.def("print", &Point3::print, print_overloads(args("s")))
.def("equals", &Point3::equals, equals_overloads(args("q","tol")))
.def("distance", &Point3::distance)
.def("cross", &Point3::cross)
.def("dot", &Point3::dot)
.def("norm", &Point3::norm, norm_overloads(args("OptionalJacobian<1,3>")))
.def("normalized", &Point3::normalized)
.def("identity", &Point3::identity)
.staticmethod("identity")
.def(self * other<double>())
.def(other<double>() * self)
.def(self + self)
@ -58,7 +59,10 @@ class_<Point3>("Point3")
.def(self / other<double>())
.def(self_ns::str(self))
.def(repr(self))
.def(self == self)
;
.def(self == self);
#endif
class_<Point3Pair>("Point3Pair", init<Point3, Point3>())
.def_readwrite("first", &Point3Pair::first)
.def_readwrite("second", &Point3Pair::second);
}

18
python/handwritten/navigation/ImuFactor.cpp
View File

@ -80,15 +80,19 @@ void exportImuFactor() {
// NOTE(frank): https://mail.python.org/pipermail/cplusplus-sig/2016-January/017362.html
register_ptr_to_python< boost::shared_ptr<PreintegrationParams> >();
class_<PreintegrationBase>(
"PreintegrationBase",
class_<PreintegrationType>(
#ifdef GTSAM_TANGENT_PREINTEGRATION
"TangentPreintegration",
#else
"ManifoldPreintegration",
#endif
init<const boost::shared_ptr<PreintegrationParams>&, const imuBias::ConstantBias&>())
.def("predict", &PreintegrationBase::predict, predict_overloads())
.def("computeError", &PreintegrationBase::computeError)
.def("resetIntegration", &PreintegrationBase::resetIntegration)
.def("deltaTij", &PreintegrationBase::deltaTij);
.def("predict", &PreintegrationType::predict, predict_overloads())
.def("computeError", &PreintegrationType::computeError)
.def("resetIntegration", &PreintegrationType::resetIntegration)
.def("deltaTij", &PreintegrationType::deltaTij);
class_<PreintegratedImuMeasurements, bases<PreintegrationBase>>(
class_<PreintegratedImuMeasurements, bases<PreintegrationType>>(
"PreintegratedImuMeasurements",
init<const boost::shared_ptr<PreintegrationParams>&, const imuBias::ConstantBias&>())
.def(repr(self))

6
tests/simulated2D.h
View File

@ -140,7 +140,7 @@ namespace simulated2D {
/// Return error and optional derivative
Vector evaluateError(const Pose& x, boost::optional<Matrix&> H = boost::none) const {
return (prior(x, H) - measured_).vector();
return (prior(x, H) - measured_);
}
virtual ~GenericPrior() {}
@ -186,7 +186,7 @@ namespace simulated2D {
Vector evaluateError(const Pose& x1, const Pose& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
return (odo(x1, x2, H1, H2) - measured_).vector();
return (odo(x1, x2, H1, H2) - measured_);
}
virtual ~GenericOdometry() {}
@ -233,7 +233,7 @@ namespace simulated2D {
Vector evaluateError(const Pose& x1, const Landmark& x2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
return (mea(x1, x2, H1, H2) - measured_).vector();
return (mea(x1, x2, H1, H2) - measured_);
}
virtual ~GenericMeasurement() {}

2
tests/simulated2DConstraints.h
View File

@ -111,7 +111,7 @@ namespace simulated2D {
* @return a scalar distance between values
*/
template<class T1, class T2>
double range_trait(const T1& a, const T2& b) { return a.dist(b); }
double range_trait(const T1& a, const T2& b) { return distance2(a, b); }
/**
* Binary inequality constraint forcing the range between points

8
tests/smallExample.h
View File

@ -180,7 +180,7 @@ inline boost::shared_ptr<const NonlinearFactorGraph> sharedNonlinearFactorGraph(
new NonlinearFactorGraph);
// prior on x1
Point2 mu;
Point2 mu(0,0);
shared_nlf f1(new simulated2D::Prior(mu, sigma0_1, X(1)));
nlfg->push_back(f1);
@ -337,7 +337,7 @@ struct UnaryFactor: public gtsam::NoiseModelFactor1<Point2> {
Vector evaluateError(const Point2& x, boost::optional<Matrix&> A = boost::none) const {
if (A) *A = H(x);
return (h(x) - z_).vector();
return (h(x) - z_);
}
};
@ -593,11 +593,11 @@ inline boost::tuple<GaussianFactorGraph, VectorValues> planarGraph(size_t N) {
Values zeros;
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y <= N; y++)
zeros.insert(key(x, y), Point2());
zeros.insert(key(x, y), Point2(0,0));
VectorValues xtrue;
for (size_t x = 1; x <= N; x++)
for (size_t y = 1; y <= N; y++)
xtrue.insert(key(x, y), Point2((double)x, (double)y).vector());
xtrue.insert(key(x, y), Point2((double)x, (double)y));
// linearize around zero
boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros);

6
tests/testBoundingConstraint.cpp
View File

@ -181,7 +181,7 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
/* ************************************************************************* */
TEST( testBoundingConstraint, MaxDistance_basics) {
gtsam::Key key1 = 1, key2 = 2;
Point2 pt1, pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0);
Point2 pt1(0,0), pt2(1.0, 0.0), pt3(2.0, 0.0), pt4(3.0, 0.0);
iq2D::PoseMaxDistConstraint rangeBound(key1, key2, 2.0, mu);
EXPECT_DOUBLES_EQUAL(2.0, rangeBound.threshold(), tol);
EXPECT(!rangeBound.isGreaterThan());
@ -220,7 +220,7 @@ TEST( testBoundingConstraint, MaxDistance_basics) {
/* ************************************************************************* */
TEST( testBoundingConstraint, MaxDistance_simple_optimization) {
Point2 pt1, pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
Point2 pt1(0,0), pt2_init(5.0, 0.0), pt2_goal(2.0, 0.0);
Symbol x1('x',1), x2('x',2);
NonlinearFactorGraph graph;
@ -246,7 +246,7 @@ TEST( testBoundingConstraint, avoid_demo) {
Symbol x1('x',1), x2('x',2), x3('x',3), l1('l',1);
double radius = 1.0;
Point2 x1_pt, x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
Point2 x1_pt(0,0), x2_init(2.0, 0.5), x2_goal(2.0, 1.0), x3_pt(4.0, 0.0), l1_pt(2.0, 0.0);
Point2 odo(2.0, 0.0);
NonlinearFactorGraph graph;

4
tests/testExpressionFactor.cpp
View File

@ -193,7 +193,7 @@ TEST(ExpressionFactor, Binary) {
internal::ExecutionTraceStorage traceStorage[size];
internal::ExecutionTrace<Point2> trace;
Point2 value = binary.traceExecution(values, trace, traceStorage);
EXPECT(assert_equal(Point2(),value, 1e-9));
EXPECT(assert_equal(Point2(0,0),value, 1e-9));
// trace.print();
// Expected Jacobians
@ -248,7 +248,7 @@ TEST(ExpressionFactor, Shallow) {
internal::ExecutionTraceStorage traceStorage[size];
internal::ExecutionTrace<Point2> trace;
Point2 value = expression.traceExecution(values, trace, traceStorage);
EXPECT(assert_equal(Point2(),value, 1e-9));
EXPECT(assert_equal(Point2(0,0),value, 1e-9));
// trace.print();
// Expected Jacobians

4
tests/testExtendedKalmanFilter.cpp
View File

@ -226,7 +226,7 @@ public:
*H2 = Matrix::Identity(dim(),dim());
// Return the error between the prediction and the supplied value of p2
return (p2 - prediction).vector();
return (p2 - prediction);
}
};
@ -400,7 +400,7 @@ TEST( ExtendedKalmanFilter, nonlinear ) {
ExtendedKalmanFilter<Point2> ekf(X(0), x_initial, P_initial);
// Enter Predict-Update Loop
Point2 x_predict, x_update;
Point2 x_predict(0,0), x_update(0,0);
for(unsigned int i = 0; i < 10; ++i){
// Create motion factor
NonlinearMotionModel motionFactor(X(i), X(i+1));

Some files were not shown because too many files have changed in this diff Show More