Merge branch 'develop' into feature/LPSolver

CMakeLists.txt

@@ -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)
+if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS)
-    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.")
+    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_USE_QUATERNIONS}             "Quaternions as default Rot3     ")
-print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS}   "Runtime consistency checking   ")
+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_ROT3_EXPMAP}                 "Rot3 retract is full ExpMap     ")
-print_config_flag(${GTSAM_POSE3_EXPMAP}                "Pose3 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_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_TYPEDEF_POINTS_TO_VECTORS}          "Point3 is typedef to Vector3    ")
-print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION}   "Metis-based Nested Dissection  ")
+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         ")

README.md

@@ -1,59 +1,77 @@
-README - Georgia Tech Smoothing and Mapping library
+README - Georgia Tech Smoothing and Mapping library
-===================================================
+===================================================
-
+
-What is GTSAM?
+What is GTSAM?
---------------
+--------------
-
+
-GTSAM is a library of C++ classes that implement smoothing and
+GTSAM is a library of C++ classes that implement smoothing and
-mapping (SAM) in robotics and vision, using factor graphs and Bayes
+mapping (SAM) in robotics and vision, using factor graphs and Bayes
-networks as the underlying computing paradigm rather than sparse
+networks as the underlying computing paradigm rather than sparse
-matrices.
+matrices.
-
+
-On top of the C++ library, GTSAM includes a MATLAB interface (enable
+On top of the C++ library, GTSAM includes a MATLAB interface (enable
-GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
+GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
-is under development.
+is under development.
-
+
-Quickstart
+Quickstart
-----------
+----------
-
+
-In the root library folder execute:
+In the root library folder execute:
-
+
-```
+```
-#!bash
+#!bash
-$ mkdir build
+$ mkdir build
-$ cd build
+$ cd build
-$ cmake ..
+$ cmake ..
-$ make check (optional, runs unit tests)
+$ make check (optional, runs unit tests)
-$ make install
+$ make install
-```
+```
-
+
-Prerequisites:
+Prerequisites:
-
+
-- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
+- [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`)
+- [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.
+- A modern compiler, i.e., at least gcc 4.7.3 on Linux.
-
+
-Optional prerequisites - used automatically if findable by CMake:
+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 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)
+- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
-
+
-GTSAM 4 Compatibility
+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.
+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.
+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
+The Preintegrated IMU Factor
-----------------------
+----------------------------
-
+
-Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here.
+GTSAM includes a state of the art IMU handling scheme based on
-
+
-See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
+- 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.
-
+
-GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
+Our implementation improves on this using integration on the manifold, as detailed in
-
+
-Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
+- 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).

examples/CameraResectioning.cpp

@@ -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 camera.project(P_, H, boost::none, boost::none) - p_;
-    return reprojectionError.vector();
   }
 };
 

examples/ImuFactorsExample.cpp

@@ -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;
+}

examples/README

@@ -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

examples/README.md

@@ -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

examples/SFMExample_SmartFactor.cpp

@@ -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

examples/SFMExample_SmartFactorPCG.cpp

@@ -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.

examples/easyPoint2KalmanFilter.cpp

@@ -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;
 }

gtsam.h

@@ -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 {

gtsam/config.h.in

@@ -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

gtsam/geometry/Cal3Bundler.cpp

@@ -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;

gtsam/geometry/Cal3DS2_Base.cpp

@@ -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);

gtsam/geometry/CalibratedCamera.cpp

@@ -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)

gtsam/geometry/CalibratedCamera.h

@@ -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:

gtsam/geometry/CameraSet.h

@@ -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) = traits<Z>::Local(measured[i], predicted[i]);
-      b.segment<ZDim>(row) = e.vector();
     }
     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;
     }

gtsam/geometry/Point2.cpp

@@ -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 {
+double norm2(const Point2& p, OptionalJacobian<1,2> H) {
-  cout << s << *this << endl;
+  double r = std::sqrt(p.x() * p.x() + p.y() * p.y());
-}
-
-/* ************************************************************************* */
-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_);
   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,
+double distance2(const Point2& p, const Point2& q, OptionalJacobian<1, 2> H1,
-    OptionalJacobian<1,2> H2) const {
+                 OptionalJacobian<1, 2> H2) {
-  Point2 d = point - *this;
+  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();
+  }
 }
 
-/*
+#ifndef GTSAM_TYPEDEF_POINTS_TO_VECTORS
- * 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.
+void Point2::print(const string& s) const {
- * If the circles do not intersect or they are identical, returns boost::none.
+  cout << s << *this << endl;
- * 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
+bool Point2::equals(const Point2& q, double tol) const {
- * @param r_d : r/d, ratio of radius of second circle to distance between centers
+  return (fabs(x() - q.x()) < tol && fabs(y() - q.y()) < tol);
- * @param tol: absolute tolerance below which we consider touching circles
+}
- */
+
+/* ************************************************************************* */
+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);
+  return circleCircleIntersection(c1, c2, fh);
-}
-
-/* ************************************************************************* */
-ostream &operator<<(ostream &os, const Point2& p) {
-  os << '(' << p.x() << ", " << p.y() << ')';
-  return os;
 }
 
 /* ************************************************************************* */

gtsam/geometry/Point2.h

@@ -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
+  using Vector2::Vector2;
-  Point2(double x, double y): x_(x), y_(y) {}
 
   /// @}
   /// @name Advanced Constructors
   /// @{
 
   /// construct from 2D vector
-  explicit Point2(const Vector2& v) {
+  explicit Point2(const Vector2& v):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);
-
   /// @}
   /// @name Testable
   /// @{
@@ -107,21 +76,7 @@ public:
   /// @{
 
   /// identity
-  inline static Point2 identity() {return Point2();}
+  inline static Point2 identity() {return Point2(0,0);}
-
-  /// 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_);}
 
   /// @}
   /// @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?
+  /// return vectorized form (column-wise).
-  Vector2 vector() const { return Vector2(x_, y_); }
+  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_BASE_OBJECT_NVP(Vector2);}
-    ar & BOOST_SERIALIZATION_NVP(y_);
-  }
 
-  /// @}
+ /// @}
 };
 
+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
 

gtsam/geometry/Point3.cpp

@@ -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,
+double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian<1, 3> H1,
-                OptionalJacobian<1, 3> H2) {
+                 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();

gtsam/geometry/Point3.h

@@ -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,
+double distance3(const Point3& p1, const Point3& q,
-                OptionalJacobian<1, 3> H1 = boost::none,
+                 OptionalJacobian<1, 3> H1 = boost::none,
-                OptionalJacobian<1, 3> H2 = 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);
   }
 };
 

gtsam/geometry/Pose2.cpp

@@ -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));
+  if (H) *H = Pose2::ExpmapDerivative(xi);
-  double w = xi(2);
+  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));
+    const Rot2 R(Rot2::fromAngle(w));
-    Point2 v_ortho = R_PI_2 * v; // points towards rot center
+    const Point2 v_ortho = R_PI_2 * v; // points towards rot center
-    Point2 t = (v_ortho - R.rotate(v_ortho)) / w;
+    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;

gtsam/geometry/Pose2.h

@@ -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);
   }
 

gtsam/geometry/Pose3.cpp

@@ -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;

gtsam/geometry/Rot3.h

@@ -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

gtsam/geometry/Unit3.h

@@ -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();
   }
 

gtsam/geometry/tests/testCal3Bundler.cpp

@@ -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));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testCal3DS2.cpp

@@ -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); }

gtsam/geometry/tests/testCal3Unified.cpp

@@ -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); }

gtsam/geometry/tests/testCalibratedCamera.cpp

@@ -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));
 }

gtsam/geometry/tests/testCameraSet.cpp

@@ -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());

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -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));

gtsam/geometry/tests/testPinholePose.cpp

@@ -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));
 }

gtsam/geometry/tests/testPinholeSet.cpp

@@ -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;

gtsam/geometry/tests/testPoint2.cpp

@@ -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>( Point2(-5,-6), -Point2(5,6) ));
-  EXPECT(assert_equal( Point2(5,6), Point2(4,5)+Point2(1,1)));
+  EXPECT(assert_equal<Point2>( 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>( Point2(3,4), Point2(4,5)-Point2(1,1) ));
-  EXPECT(assert_equal( Point2(8,6), Point2(4,3)*2));
+  EXPECT(assert_equal<Point2>( Point2(8,6), Point2(4,3)*2));
-  EXPECT(assert_equal( Point2(4,6), 2*Point2(2,3)));
+  EXPECT(assert_equal<Point2>( Point2(4,6), 2*Point2(2,3)));
-  EXPECT(assert_equal( Point2(2,3), Point2(4,6)/2));
+  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(1, 0), Point2(p0.normalized()), 1e-6));
-  EXPECT(assert_equal(Point2(0,-1), p1.unit(), 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), p2.unit(), 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 () {

gtsam/geometry/tests/testPoint3.cpp

@@ -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);

gtsam/geometry/tests/testPose2.cpp

@@ -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));
 }
 

gtsam/geometry/tests/testSimpleCamera.cpp

@@ -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);
 }
 

gtsam/geometry/tests/testTriangulation.cpp

@@ -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;

gtsam/geometry/triangulation.h

@@ -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;
       }

gtsam/inference/Symbol.h

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

gtsam/navigation/CombinedImuFactor.cpp

@@ -31,22 +31,21 @@ using namespace std;
 //------------------------------------------------------------------------------
 // Inner class PreintegratedCombinedMeasurements
 //------------------------------------------------------------------------------
-void PreintegratedCombinedMeasurements::print(
+void PreintegratedCombinedMeasurements::print(const string& s) const {
-    const string& s) const {
+  PreintegrationType::print(s);
-  PreintegrationBase::print(s);
   cout << "  preintMeasCov [ " << preintMeasCov_ << " ]" << endl;
 }
 
 //------------------------------------------------------------------------------
 bool PreintegratedCombinedMeasurements::equals(
     const PreintegratedCombinedMeasurements& other, double tol) const {
-  return PreintegrationBase::equals(other, tol) &&
+  return PreintegrationType::equals(other, tol)
-         equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, 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?
+  // TODO(frank): should we not also account for bias on position?
-  Matrix3 theta_H_biasOmega = - C.topRows<3>();
+  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 *
+  D_v_v(&G_measCov_Gt) = (1 / dt) * vel_H_biasAcc
-                         (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0)) *
+      * (aCov + p().biasAccOmegaInt.block<3, 3>(0, 0))
-                         (vel_H_biasAcc.transpose());
+      * (vel_H_biasAcc.transpose());
-  D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega *
+  D_R_R(&G_measCov_Gt) = (1 / dt) * theta_H_biasOmega
-                         (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3)) *
+      * (wCov + p().biasAccOmegaInt.block<3, 3>(3, 3))
-                         (theta_H_biasOmega.transpose());
+      * (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) *
+  Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInt.block<3, 3>(3, 0)
-                 theta_H_biasOmega.transpose();
+      * 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(
+CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j,
-    Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
+    Key vel_j, Key bias_i, Key bias_j,
-    const PreintegratedCombinedMeasurements& pim)
+    const PreintegratedCombinedMeasurements& pim) :
-    : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
+    Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
-           pose_j, vel_j, bias_i, bias_j),
+        pose_j, vel_j, bias_i, bias_j), _PIM_(pim) {
-      _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,
+  Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j,
-      H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0,
+      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,
+: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
-           pose_j, vel_j, bias_i, bias_j),
+    pose_j, vel_j, bias_i, bias_j),
-      _PIM_(pim) {
+_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,
+    Pose3& pose_j, Vector3& vel_j,
-                                const imuBias::ConstantBias& bias_i,
+    const imuBias::ConstantBias& bias_i,
-                                CombinedPreintegratedMeasurements& pim,
+    CombinedPreintegratedMeasurements& pim,
-                                const Vector3& n_gravity,
+    const Vector3& n_gravity,
-                                const Vector3& omegaCoriolis,
+    const Vector3& omegaCoriolis,
-                                const bool use2ndOrderCoriolis) {
+    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
 

gtsam/navigation/CombinedImuFactor.h

@@ -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_);
   }
 };

gtsam/navigation/ImuFactor.cpp

@@ -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,
+void PreintegratedImuMeasurements::integrateMeasurements(
-                                                         const Matrix& measuredOmegas,
+    const Matrix& measuredAccs, const Matrix& measuredOmegas,
-                                                         const Matrix& dts) {
+    const Matrix& dts) {
-  assert(measuredAccs.rows() == 3 && measuredOmegas.rows() == 3 && dts.rows() == 1);
+  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,  //
+#ifdef GTSAM_TANGENT_PREINTEGRATION
-                                             Matrix9* H1, Matrix9* H2) {
+void PreintegratedImuMeasurements::mergeWith(const PreintegratedImuMeasurements& pim12, //
-  PreintegrationBase::mergeWith(pim12, H1, H2);
+    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(
+  throw std::domain_error(
-        "Cannot merge PreintegratedImuMeasurements with different params");
+      "Cannot merge PreintegratedImuMeasurements with different params");
 
   if (pim01.params()->body_P_sensor)
-    throw std::domain_error(
+  throw std::domain_error(
-        "Cannot merge PreintegratedImuMeasurements with sensor pose yet");
+      "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(
+  throw std::domain_error(
-        "ImuFactor::Merge: intermediate pose, velocity keys need to match up");
+      "ImuFactor::Merge: intermediate pose, velocity keys need to match up");
 
   // return new factor
   auto pim02 =
-      Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
+  Merge(f01->preintegratedMeasurements(), f12->preintegratedMeasurements());
-  return boost::make_shared<ImuFactor>(f01->key1(),  // P0
+  return boost::make_shared<ImuFactor>(f01->key1(),// P0
-                                       f01->key2(),  // V0
+      f01->key2(),// V0
-                                       f12->key3(),  // P2
+      f12->key3(),// P2
-                                       f12->key4(),  // V2
+      f12->key4(),// V2
-                                       f01->key5(),  // B
+      f01->key5(),// B
-                                       pim02);
+      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)
+ImuFactor2::ImuFactor2(Key state_i, Key state_j, Key bias,
-    : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), state_i, state_j, bias),
+    const PreintegratedImuMeasurements& pim) :
-      _PIM_(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 {
+void ImuFactor2::print(const string& s,
-  cout << s << "ImuFactor2(" << keyFormatter(this->key1()) << "," << keyFormatter(this->key2())
+    const KeyFormatter& keyFormatter) const {
-       << "," << keyFormatter(this->key3()) << ")\n";
+  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,
+Vector ImuFactor2::evaluateError(const NavState& state_i,
-                                 const imuBias::ConstantBias& bias_i,  //
+    const NavState& state_j,
-                                 boost::optional<Matrix&> H1,
+    const imuBias::ConstantBias& bias_i, //
-                                 boost::optional<Matrix&> H2,
+    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
-                                 boost::optional<Matrix&> H3) const {
+    boost::optional<Matrix&> H3) const {
   return _PIM_.computeError(state_i, state_j, bias_i, H1, H2, H3);
 }
 
 //------------------------------------------------------------------------------
 
 }
- // namespace gtsam
+// namespace gtsam

gtsam/navigation/ImuFactor.h

@@ -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)
+  PreintegratedImuMeasurements(const PreintegrationType& base, const Matrix9& preintMeasCov)
-     : PreintegrationBase(base),
+     : 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,
+  bool equals(const PreintegratedImuMeasurements& expected, double tol = 1e-9) const;
-      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

gtsam/navigation/ManifoldPreintegration.cpp

@@ -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

gtsam/navigation/ManifoldPreintegration.h

@@ -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

gtsam/navigation/NavState.cpp

@@ -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();
+  Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb;
-  return NavState(bRn, -(bRn * n_t), -(bRn * n_v));
+  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);
-NavState NavState::operator*(const NavState& bTc) const {
+  if (H1) {
-  TIE(nRb, n_t, n_v, *this);
+    *H1 << D_R_nRb, Z_3x3, Z_3x3, //
-  TIE(bRc, b_t, b_v, bTc);
+    // Note(frank): the derivative of n_t with respect to xi is nRb
-  return NavState(nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v);
+    // 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:
-NavState::PositionAndVelocity //
+    nRc.transpose() * D_v_nRb, Z_3x3, bRc.transpose();
-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();
   }
-  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,
+Vector9 NavState::localCoordinates(const NavState& g, //
-    OptionalJacobian<9, 9> H) {
+    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();
+  xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dt, dv;
-  if (H) {
+  if (H1) {
-    *H << D_xi_R, Z_3x3, Z_3x3, //
+    *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, //
-    Z_3x3, x.R(), Z_3x3, //
+    D_dt_R, -I_3x3, Z_3x3, //
-    Z_3x3, Z_3x3, x.R();
+    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,

gtsam/navigation/NavState.h

@@ -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)
+  /// retract with optional derivatives
-  struct ChartAtOrigin {
+  NavState retract(const Vector9& v, //
-    static NavState Retract(const Vector9& xi, //
+      OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
-        OptionalJacobian<9, 9> H = boost::none);
+          boost::none) const;
-    static Vector9 Local(const NavState& x, //
-        OptionalJacobian<9, 9> H = boost::none);
-  };
 
-  /// @}
+  /// localCoordinates with optional derivatives
-  /// @name Lie Group
+  Vector9 localCoordinates(const NavState& g, //
-  /// @{
+      OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 9> H2 =
-
+          boost::none) const;
-  /// 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);
 
   /// @}
   /// @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

gtsam/navigation/PreintegrationBase.cpp

@@ -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,
+void PreintegrationBase::resetIntegrationAndSetBias(const Bias& biasHat) {
-    double tol) const {
+	biasHat_ = biasHat;
-  return p_->equals(*other.p_, tol)
+	resetIntegration();
-      && 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);
 }
 
 //------------------------------------------------------------------------------
@@ -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,
+    const Vector3& measuredOmega, double dt) {
-                                              double dt, Matrix9* A,
+  // NOTE(frank): integrateMeasurement always needs to compute the derivatives,
-                                              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,
   // even when not of interest to the caller. Provide scratch space here.
   Matrix9 A;
   Matrix93 B, C;
-  integrateMeasurement(measuredAcc, measuredOmega, dt, &A, &B, &C);
+  update(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;
 }
 
 //------------------------------------------------------------------------------
 NavState PreintegrationBase::predict(const NavState& state_i,
-                                     const imuBias::ConstantBias& bias_i,
+    const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
-                                     OptionalJacobian<9, 9> H1,
+    OptionalJacobian<9, 6> H2) const {
-                                     OptionalJacobian<9, 6> H2) const {
   // TODO(frank): make sure this stuff is still correct
   Matrix96 D_biasCorrected_bias;
-  Vector9 biasCorrected =
+  Vector9 biasCorrected = biasCorrectedDelta(bias_i,
-      biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : 0);
+      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,
+      p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
-                                  H1 ? &D_delta_state : 0,
+      H2 ? &D_delta_biasCorrected : 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 (H1)
-  if (H2) *H2 = D_predict_delta* D_delta_biasCorrected* D_biasCorrected_bias;
+    *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
 //------------------------------------------------------------------------------
 

gtsam/navigation/PreintegrationBase.h

@@ -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() {
+  PreintegrationBase() {}
-    resetIntegration();
-  }
 
 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_; }
+  virtual Vector3  deltaPij() const=0;
-
+  virtual Vector3  deltaVij() const=0;
-  Vector3 theta() const { return preintegrated_.head<3>(); }
+  virtual Rot3     deltaRij() const=0;
-  Vector3 deltaPij() const { return preintegrated_.segment<3>(3); }
+  virtual NavState deltaXij() const=0;
-  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_; }
 
   // 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;
+  virtual void print(const std::string& s) const;
-  bool equals(const PreintegrationBase& other, double tol) 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
+  /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose
-  void integrateMeasurement(const Vector3& measuredAcc,
+  virtual void update(const Vector3& measuredAcc, const Vector3& measuredOmega,
-                            const Vector3& measuredOmega, const double deltaT,
+      const double dt, Matrix9* A, Matrix93* B, Matrix93* C)=0;
-                            Matrix9* A, Matrix93* B, Matrix93* C);
 
-  // Version without derivatives
+  /// Version without derivatives
-  void integrateMeasurement(const Vector3& measuredAcc,
+  virtual void integrateMeasurement(const Vector3& measuredAcc,
-                            const Vector3& measuredOmega, const double deltaT);
+      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,
+  virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
-      OptionalJacobian<9, 6> H = boost::none) const;
+      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

gtsam/navigation/TangentPreintegration.cpp

@@ -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

gtsam/navigation/TangentPreintegration.h

@@ -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

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -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) {

gtsam/navigation/tests/testImuFactor.cpp

@@ -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);
+  const double a = 0.1, w = M_PI / 100.0;
-  Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.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 expectedDeltaR1(w * deltaT, 0.0, 0.0);
-  Vector3 expectedDeltaP1(0.5 * 0.1 * 0.5 * 0.5, 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

gtsam/navigation/tests/testManifoldPreintegration.cpp

@@ -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);
+}
+/* ************************************************************************* */

gtsam/navigation/tests/testNavState.cpp

@@ -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::bind(&NavState::update, _1, _2, _3, dt, boost::none,
-          boost::none, 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),

gtsam/navigation/tests/testTangentPreintegration.cpp

@@ -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) {
+TEST(TangentPreintegration, UpdateEstimate1) {
-  PreintegrationBase pim(testing::Params());
+  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) {
+TEST(TangentPreintegration, UpdateEstimate2) {
-  PreintegrationBase pim(testing::Params());
+  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) {
+TEST(ImuFactor, BiasCorrectionJacobians) {
-  PreintegrationBase pim(testing::Params());
+  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);
 

gtsam/nonlinear/ExtendedKalmanFilter-inl.h

@@ -61,16 +61,14 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  template<class VALUE>
+  template <class VALUE>
-  ExtendedKalmanFilter<VALUE>::ExtendedKalmanFilter(Key key_initial, T x_initial,
+  ExtendedKalmanFilter<VALUE>::ExtendedKalmanFilter(
-      noiseModel::Gaussian::shared_ptr P_initial) {
+      Key key_initial, T x_initial, noiseModel::Gaussian::shared_ptr P_initial)
-
+      : x_(x_initial)  // Set the initial linearization point
-    // Set the initial linearization point to the provided mean
+  {
-    x_ = x_initial;
-
     // 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),

gtsam/nonlinear/NonlinearEquality.h

@@ -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:

gtsam/nonlinear/tests/testExpression.cpp

@@ -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<Class> p(67);
-  Expression<double> norm(&gtsam::norm, p);
+  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);

gtsam/slam/EssentialMatrixFactor.h

@@ -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;
   }
 
 };

gtsam/slam/GeneralSFMFactor.h

@@ -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 camera.project2(point,H1,H2) - measured_;
-      return reprojError.vector();
     }
     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 = measured() - camera.project2(point, H1, H2);
-      b = -reprojError.vector();
     } 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 camera.project(point, H1, H2, H3) - measured_;
-      return reprojError.vector();
     }
     catch( CheiralityException& e) {
       if (H1) *H1 = Matrix::Zero(2, 6);

gtsam/slam/ProjectionFactor.h

@@ -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 camera.project(point, H1, H2, boost::none) - measured_;
-            return reprojectionError.vector();
           }
         } else {
           PinholeCamera<CALIBRATION> camera(pose, *K_);
-          Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
+          return camera.project(point, H1, H2, boost::none) - measured_;
-          return reprojectionError.vector();
         }
       } catch( CheiralityException& e) {
         if (H1) *H1 = Matrix::Zero(2,6);

gtsam/slam/ReferenceFrameFactor.h

@@ -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);
   }
 

gtsam/slam/SmartFactorBase.h

@@ -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;
     }

gtsam/slam/SmartProjectionFactor.h

@@ -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(
-      Unit3 backprojected = cameras.front().backprojectPointAtInfinity(z0);
+          this->measured_.at(0));
       return Base::totalReprojectionError(cameras, backprojected);
     } else
       // if we don't want to manage the exceptions we discard the factor

gtsam/slam/TriangulationFactor.h

@@ -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 traits<Measurement>::Local(measured_, camera_.project2(point, boost::none, H2));
-      return error.vector();
     } 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);
+      Ab = VerticalBlockMatrix(dimensions, traits<Measurement>::dimension, true);
-      A.resize(Measurement::dimension,3);
+      A.resize(traits<Measurement>::dimension,3);
-      b.resize(Measurement::dimension);
+      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);
 

gtsam/slam/tests/testEssentialMatrixFactor.cpp

@@ -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));
+    Point2 expected(pi - pB(i));
-    Vector expected = reprojectionError.vector();
 
     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));
+    Point2 expected(pi - pB(i));
-    Vector expected = reprojectionError.vector();
 
     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));
+    Point2 expected(pi - pB(i));
-    Vector expected = reprojectionError.vector();
 
     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));
+    Point2 expected(pi - pB(i));
-    Vector expected = reprojectionError.vector();
 
     Matrix Hactual1, Hactual2;
     double d(baseline / P1.z());

gtsam/slam/tests/testSmartFactorBase.cpp

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

gtsam/slam/tests/testSmartProjectionCameraFactor.cpp

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

gtsam_unstable/dynamics/PoseRTV.cpp

@@ -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;

gtsam_unstable/geometry/Event.h

@@ -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;

gtsam_unstable/geometry/SimPolygon2D.cpp

@@ -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;
 }

gtsam_unstable/geometry/SimWall2D.cpp

@@ -14,13 +14,14 @@ using namespace std;
 /* ************************************************************************* */
 void SimWall2D::print(const std::string& s) const {
   std::cout << "SimWall2D " << s << ":" << std::endl;
-  a_.print("  a");
+  traits<Point2>::Print(a_, "  a");
-  b_.print("  b");
+  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) traits<Point2>::Print(Ba, "Ba");
-  if (debug) Bb.print("Bb");
+  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) traits<Point2>::Print(high, "high");
-  if (debug) low.print("low");
+  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

gtsam_unstable/geometry/SimWall2D.h

@@ -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;
 
     /**

gtsam_unstable/geometry/tests/testSimPolygon2D.cpp

@@ -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);

gtsam_unstable/geometry/tests/testSimWall2D.cpp

@@ -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);

gtsam_unstable/slam/InvDepthFactor3.h

@@ -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
+  Point2 measured_;                ///< 2D measurement
-  boost::shared_ptr<gtsam::Cal3_S2> K_;  ///< shared pointer to calibration object
+  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,
+  InvDepthFactor3(const Point2& measured, const SharedNoiseModel& model,
-      const gtsam::Key poseKey, gtsam::Key pointKey, gtsam::Key invDepthKey, const Cal3_S2::shared_ptr& K) :
+      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,
+  Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
-      boost::optional<gtsam::Matrix&> H1=boost::none,
+      boost::optional<Matrix&> H1=boost::none,
-      boost::optional<gtsam::Matrix&> H2=boost::none,
+      boost::optional<Matrix&> H2=boost::none,
-      boost::optional<gtsam::Matrix&> H3=boost::none) const {
+      boost::optional<Matrix&> H3=boost::none) const {
     try {
-      InvDepthCamera3<gtsam::Cal3_S2> camera(pose, K_);
+      InvDepthCamera3<Cal3_S2> camera(pose, K_);
-      gtsam::Point2 reprojectionError(camera.project(point, invDepth, H1, H2, H3) - measured_);
+      return camera.project(point, invDepth, H1, H2, H3) - measured_;
-      return reprojectionError.vector();
     } 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_;
   }
 

gtsam_unstable/slam/InvDepthFactorVariant1.h

@@ -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 camera.project(world_P_landmark) - measured_;
-      return reprojectionError.vector();
     } 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<Matrix&> H1=boost::none,
-      boost::optional<gtsam::Matrix&> H2=boost::none) const {
+      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_;
   }
 

gtsam_unstable/slam/InvDepthFactorVariant2.h

@@ -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 camera.project(world_P_landmark) - measured_;
-      return reprojectionError.vector();
     } 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<Matrix&> H1=boost::none,
-      boost::optional<gtsam::Matrix&> H2=boost::none) const {
+      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_;
   }
 

gtsam_unstable/slam/InvDepthFactorVariant3.h

@@ -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 camera.project(world_P_landmark) - measured_;
-      return reprojectionError.vector();
     } 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<Matrix&> H1=boost::none,
-      boost::optional<gtsam::Matrix&> H2=boost::none) const {
+      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 camera.project(world_P_landmark) - measured_;
-      return reprojectionError.vector();
     } 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<Matrix&> H1=boost::none,
-      boost::optional<gtsam::Matrix&> H2=boost::none,
+      boost::optional<Matrix&> H2=boost::none,
-      boost::optional<gtsam::Matrix&> H3=boost::none) const {
+      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_;
   }
 

gtsam_unstable/slam/MultiProjectionFactor.h

@@ -101,9 +101,9 @@ namespace gtsam {
     virtual ~MultiProjectionFactor() {}
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    virtual NonlinearFactor::shared_ptr clone() const {
-      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+      return boost::static_pointer_cast<NonlinearFactor>(
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+          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);

gtsam_unstable/slam/ProjectionFactorPPP.h

@@ -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 {
+    virtual NonlinearFactor::shared_ptr clone() const {
-      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+      return boost::static_pointer_cast<NonlinearFactor>(
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+          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 camera.project(point, H1, H3, boost::none) - measured_;
-            return reprojectionError.vector();
           }
       } catch( CheiralityException& e) {
         if (H1) *H1 = Matrix::Zero(2,6);

gtsam_unstable/slam/ProjectionFactorPPPC.h

@@ -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 {
+    virtual NonlinearFactor::shared_ptr clone() const {
-      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+      return boost::static_pointer_cast<NonlinearFactor>(
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+          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 camera.project(point, H1, H3, H4) - measured_;
-            return reprojectionError.vector();
           }
       } catch( CheiralityException& e) {
         if (H1) *H1 = Matrix::Zero(2,6);

gtsam_unstable/slam/SmartRangeFactor.h

@@ -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);
+      error1 += distance2(it.center, p1);
-      error2 += it.center.dist(p2);
+      error2 += distance2(it.center, p2);
     }
     return (error1 < error2) ? p1 : p2;
     //gttoc_(triangulate);

gtsam_unstable/slam/TSAMFactors.h

@@ -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);
+    return pose.transform_to(point, H1, H2) - measured_;
-    Point2 e = d - measured_;
-    return e.vector();
   }
 };
 
@@ -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_;
     }
   }
 };

matlab.h

@@ -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;
 }
 

python/gtsam_examples/ImuFactorExample.py

@@ -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:

python/gtsam_examples/PreintegrationExample.py

@@ -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)

python/gtsam_utils/plot.py

@@ -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

python/handwritten/geometry/Point2.cpp

@@ -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, distance_overloads(args("q","H1","H2")))
-    .def("distance", &Point2::distance)
     .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
-}
+}

python/handwritten/geometry/Point3.cpp

@@ -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);
 }

python/handwritten/navigation/ImuFactor.cpp

@@ -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>(
+  class_<PreintegrationType>(
-      "PreintegrationBase",
+#ifdef GTSAM_TANGENT_PREINTEGRATION
+      "TangentPreintegration",
+#else
+      "ManifoldPreintegration",
+#endif
       init<const boost::shared_ptr<PreintegrationParams>&, const imuBias::ConstantBias&>())
-      .def("predict", &PreintegrationBase::predict, predict_overloads())
+      .def("predict", &PreintegrationType::predict, predict_overloads())
-      .def("computeError", &PreintegrationBase::computeError)
+      .def("computeError", &PreintegrationType::computeError)
-      .def("resetIntegration", &PreintegrationBase::resetIntegration)
+      .def("resetIntegration", &PreintegrationType::resetIntegration)
-      .def("deltaTij", &PreintegrationBase::deltaTij);
+      .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))

tests/simulated2D.h

@@ -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() {}

tests/simulated2DConstraints.h

@@ -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

tests/smallExample.h

@@ -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);

tests/testBoundingConstraint.cpp

@@ -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;

tests/testExpressionFactor.cpp

@@ -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

tests/testExtendedKalmanFilter.cpp

@@ -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));