Merge remote-tracking branch 'origin/develop' into feature/SoundSlam

Conflicts:
	.cproject

.gitignore

@@ -5,3 +5,5 @@
 /examples/Data/dubrovnik-3-7-pre-rewritten.txt
 /examples/Data/pose2example-rewritten.txt
 /examples/Data/pose3example-rewritten.txt
+*.txt.user
+*.txt.user.6d59f0c

README.md

@@ -1,47 +1,49 @@
-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`)
-
+
-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)
-
+
-Additional Information
+Additional Information
-----------------------
+----------------------
-
+
-See the [`INSTALL`](https://bitbucket.org/gtborg/gtsam/src/develop/INSTALL) file for more detailed installation instructions.
+Read about important [`GTSAM-Concepts`] here.
-
+
-GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files.
+See the [`INSTALL`] file for more detailed installation instructions.
-
+
-Please see the [`examples/`](https://bitbucket.org/gtborg/gtsam/src/develop/examples) directory and the [`USAGE`](https://bitbucket.org/gtborg/gtsam/src/develop/USAGE) file for examples on how to use GTSAM.
+GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files.
+
+Please see the [`examples/`](examples) directory and the [`USAGE`] file for examples on how to use GTSAM.

cmake/GtsamBuildTypes.cmake

@@ -15,8 +15,8 @@ option(GTSAM_BUILD_TYPE_POSTFIXES        "Enable/Disable appending the build typ
 # Add debugging flags but only on the first pass
 if(NOT FIRST_PASS_DONE)
   if(MSVC)
-    set(CMAKE_C_FLAGS_DEBUG            "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
+    set(CMAKE_C_FLAGS_DEBUG            "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
-    set(CMAKE_CXX_FLAGS_DEBUG          "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
+    set(CMAKE_CXX_FLAGS_DEBUG          "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
     set(CMAKE_C_FLAGS_RELWITHDEBINFO   "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
     set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
     set(CMAKE_C_FLAGS_RELEASE          "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE)
@@ -34,8 +34,8 @@ if(NOT FIRST_PASS_DONE)
     set(CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
     mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING CMAKE_MODULE_LINKER_FLAGS_PROFILING)
   else()
-    set(CMAKE_C_FLAGS_DEBUG            "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
+    set(CMAKE_C_FLAGS_DEBUG            "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
-    set(CMAKE_CXX_FLAGS_DEBUG          "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
+    set(CMAKE_CXX_FLAGS_DEBUG          "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
     set(CMAKE_C_FLAGS_RELWITHDEBINFO   "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
     set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
     set(CMAKE_C_FLAGS_RELEASE          "-O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE)

gtsam.h

@@ -156,12 +156,6 @@ virtual class Value {
   size_t dim() const;
 };
 
-class Vector3 {
-  Vector3(Vector v);
-};
-class Vector6 {
-  Vector6(Vector v);
-};
 #include <gtsam/base/LieScalar.h>
 class LieScalar {
   // Standard constructors
@@ -758,10 +752,6 @@ class CalibratedCamera {
   gtsam::CalibratedCamera retract(const Vector& d) const;
   Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
 
-  // Group
-  gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const;
-  gtsam::CalibratedCamera inverse() const;
-
   // Action on Point3
   gtsam::Point2 project(const gtsam::Point3& point) const;
   static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
@@ -1727,6 +1717,7 @@ class Values {
   // void insert(size_t j, const gtsam::Value& value);
   // void update(size_t j, const gtsam::Value& val);
   // gtsam::Value at(size_t j) const;
+
   void insert(size_t j, const gtsam::Point2& t);
   void insert(size_t j, const gtsam::Point3& t);
   void insert(size_t j, const gtsam::Rot2& t);
@@ -1738,6 +1729,11 @@ class Values {
   void insert(size_t j, const gtsam::Cal3Bundler& t);
   void insert(size_t j, const gtsam::EssentialMatrix& t);
   void insert(size_t j, const gtsam::imuBias::ConstantBias& t);
+  void insert(size_t j, Vector t);
+  void insert(size_t j, Matrix t);
+
+  // Fixed size version
+  void insertFixed(size_t j, Vector t, size_t n);
 
   void update(size_t j, const gtsam::Point2& t);
   void update(size_t j, const gtsam::Point3& t);
@@ -1750,9 +1746,10 @@ class Values {
   void update(size_t j, const gtsam::Cal3Bundler& t);
   void update(size_t j, const gtsam::EssentialMatrix& t);
   void update(size_t j, const gtsam::imuBias::ConstantBias& t);
+  void update(size_t j, Vector t);
+  void update(size_t j, Matrix t);
 
-  template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2,
+  template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
-      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::imuBias::ConstantBias}>
   T at(size_t j);
 };
 
@@ -2150,7 +2147,7 @@ class NonlinearISAM {
 #include <gtsam/geometry/StereoPoint2.h>
 
 #include <gtsam/slam/PriorFactor.h>
-template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
+template<T = { gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
 virtual class PriorFactor : gtsam::NoiseModelFactor {
   PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
   T prior() const;
@@ -2195,10 +2192,14 @@ typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
 typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
 typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
-typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
+
-typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
+// Commented out by Frank Dec 2014: not poses!
-typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
+// If needed, we need a RangeFactor that takes a camera, extracts the pose
-typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
+// Should be easy with Expressions
+//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
+//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
+//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
+//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
 
 
 #include <gtsam/slam/BearingFactor.h>
@@ -2391,7 +2392,7 @@ class ConstantBias {
 
 #include <gtsam/navigation/ImuFactor.h>
 class PoseVelocity{
-    PoseVelocity(const gtsam::Pose3& pose, const gtsam::Vector3 velocity);
+    PoseVelocity(const gtsam::Pose3& pose, Vector velocity);
   };
 class ImuFactorPreintegratedMeasurements {
   // Standard Constructor
@@ -2430,7 +2431,7 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
       const gtsam::Pose3& body_P_sensor);
   // Standard Interface
   gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
-  gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias,
+  gtsam::PoseVelocity Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
       const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
       Vector gravity, Vector omegaCoriolis) const;
 };
@@ -2476,7 +2477,7 @@ virtual class AHRSFactor : gtsam::NonlinearFactor {
 
 #include <gtsam/navigation/CombinedImuFactor.h>
 class PoseVelocityBias{
-    PoseVelocityBias(const gtsam::Pose3& pose, const gtsam::Vector3 velocity, const gtsam::imuBias::ConstantBias& bias);
+    PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
   };
 class CombinedImuFactorPreintegratedMeasurements {
   // Standard Constructor
@@ -2527,7 +2528,7 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
 
   // Standard Interface
   gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
-  gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, const gtsam::imuBias::ConstantBias& bias_i,
+  gtsam::PoseVelocityBias Predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias_i,
       const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
       Vector gravity, Vector omegaCoriolis);
 };

gtsam/3rdparty/CMakeLists.txt

@@ -45,6 +45,8 @@ endif()
 option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF)
 add_subdirectory(metis)
 
+add_subdirectory(ceres)
+
 ############ NOTE:  When updating GeographicLib be sure to disable building their examples
 ############        and unit tests by commenting out their lines:
 # add_subdirectory (examples)

gtsam/3rdparty/ceres/CMakeLists.txt

@@ -0,0 +1,2 @@
+file(GLOB ceres_headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h")
+install(FILES ${ceres_headers} DESTINATION include/gtsam/3rdparty/ceres)

gtsam/3rdparty/ceres/autodiff.h

@@ -142,10 +142,10 @@
 
 #include <stddef.h>
 
-#include <gtsam_unstable/nonlinear/ceres_jet.h>
+#include <gtsam/3rdparty/ceres/jet.h>
-#include <gtsam_unstable/nonlinear/ceres_eigen.h>
+#include <gtsam/3rdparty/ceres/eigen.h>
-#include <gtsam_unstable/nonlinear/ceres_fixed_array.h>
+#include <gtsam/3rdparty/ceres/fixed_array.h>
-#include <gtsam_unstable/nonlinear/ceres_variadic_evaluate.h>
+#include <gtsam/3rdparty/ceres/variadic_evaluate.h>
 #define DCHECK assert
 #define DCHECK_GT(a,b) assert((a)>(b))
 

gtsam/3rdparty/ceres/example.h

@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include <gtsam_unstable/nonlinear/ceres_rotation.h>
+#include <gtsam/3rdparty/ceres/rotation.h>
 
 // Templated pinhole camera model for used with Ceres. The camera is
 // parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for

gtsam/3rdparty/ceres/fixed_array.h

@@ -34,8 +34,8 @@
 
 #include <cstddef>
 #include <gtsam/3rdparty/gtsam_eigen_includes.h>
-#include <gtsam_unstable/nonlinear/ceres_macros.h>
+#include <gtsam/3rdparty/ceres/macros.h>
-#include <gtsam_unstable/nonlinear/ceres_manual_constructor.h>
+#include <gtsam/3rdparty/ceres/manual_constructor.h>
 
 namespace ceres {
 namespace internal {

gtsam/3rdparty/ceres/jet.h

@@ -163,7 +163,7 @@
 #include <string>
 
 #include <gtsam/3rdparty/gtsam_eigen_includes.h>
-#include <gtsam_unstable/nonlinear/ceres_fpclassify.h>
+#include <gtsam/3rdparty/ceres/fpclassify.h>
 
 namespace ceres {
 

gtsam/3rdparty/ceres/rotation.h

@@ -47,6 +47,7 @@
 
 #include <algorithm>
 #include <cmath>
+#include <limits>
 #include <assert.h>
 #define DCHECK assert
 

gtsam/3rdparty/ceres/variadic_evaluate.h

@@ -34,9 +34,9 @@
 
 #include <stddef.h>
 
-#include <gtsam_unstable/nonlinear/ceres_jet.h>
+#include <gtsam/3rdparty/ceres/jet.h>
-#include <gtsam_unstable/nonlinear/ceres_eigen.h>
+#include <gtsam/3rdparty/ceres/eigen.h>
-#include <gtsam_unstable/nonlinear/ceres_fixed_array.h>
+#include <gtsam/3rdparty/ceres/fixed_array.h>
 
 namespace ceres {
 namespace internal {

gtsam/base/LieMatrix.h

@@ -19,6 +19,12 @@
 
 #include <cstdarg>
 
+#ifdef _MSC_VER
+#pragma message("LieMatrix.h is deprecated. Please use Eigen::Matrix instead.")
+#else
+#warning "LieMatrix.h is deprecated. Please use Eigen::Matrix instead."
+#endif
+
 #include <gtsam/base/DerivedValue.h>
 #include <gtsam/base/Lie.h>
 #include <gtsam/base/Matrix.h>
@@ -27,7 +33,9 @@
 namespace gtsam {
 
 /**
- * LieVector is a wrapper around vector to allow it to be a Lie type
+ * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
+ * we can directly add double, Vector, and Matrix into values now, because of
+ * gtsam::traits.
  */
 struct LieMatrix : public Matrix {
 

gtsam/base/LieScalar.h

@@ -17,6 +17,12 @@
 
 #pragma once
 
+#ifdef _MSC_VER
+#pragma message("LieScalar.h is deprecated. Please use double/float instead.")
+#else
+  #warning "LieScalar.h is deprecated. Please use double/float instead."
+#endif
+
 #include <gtsam/dllexport.h>
 #include <gtsam/base/DerivedValue.h>
 #include <gtsam/base/Lie.h>
@@ -24,7 +30,9 @@
 namespace gtsam {
 
   /**
-   * LieScalar is a wrapper around double to allow it to be a Lie type
+   * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
+   * we can directly add double, Vector, and Matrix into values now, because of
+   * gtsam::traits.
    */
   struct GTSAM_EXPORT LieScalar {
 

gtsam/base/LieVector.h

@@ -17,6 +17,12 @@
 
 #pragma once
 
+#ifdef _MSC_VER
+#pragma message("LieVector.h is deprecated. Please use Eigen::Vector instead.")
+#else
+#warning "LieVector.h is deprecated. Please use Eigen::Vector instead."
+#endif
+
 #include <gtsam/base/Lie.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/DerivedValue.h>
@@ -24,7 +30,9 @@
 namespace gtsam {
 
 /**
- * LieVector is a wrapper around vector to allow it to be a Lie type
+ * @deprecated: LieScalar, LieVector and LieMatrix are obsolete in GTSAM 4.0 as
+ * we can directly add double, Vector, and Matrix into values now, because of
+ * gtsam::traits.
  */
 struct LieVector : public Vector {
 

gtsam/base/Manifold.h

@@ -73,7 +73,6 @@ template<typename T>
 struct dimension: public Dynamic {
 };
 
-
 /**
  * zero<T>::value is intended to be the origin of a canonical coordinate system
  * with canonical(t) == DefaultChart<T>::local(zero<T>::value, t)
@@ -111,14 +110,16 @@ struct is_group<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : publi
 };
 
 template<int M, int N, int Options, int MaxRows, int MaxCols>
-struct is_manifold<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::true_type{
+struct is_manifold<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::true_type {
 };
 
 template<int M, int N, int Options, int MaxRows, int MaxCols>
-struct dimension<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::integral_constant<int,
+struct dimension<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::integral_constant<
-    M == Eigen::Dynamic ? Eigen::Dynamic : (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> {
+    int,
-    //TODO after switch to c++11 : the above should should be extracted to a constexpr function
+    M == Eigen::Dynamic ? Eigen::Dynamic :
-    // for readability and to reduce code duplication
+        (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> {
+  //TODO after switch to c++11 : the above should should be extracted to a constexpr function
+  // for readability and to reduce code duplication
 };
 
 template<int M, int N, int Options, int MaxRows, int MaxCols>
@@ -131,10 +132,10 @@ struct zero<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
 };
 
 template<int M, int N, int Options>
-struct identity<Eigen::Matrix<double, M, N, Options> > : public zero<Eigen::Matrix<double, M, N, Options> > {
+struct identity<Eigen::Matrix<double, M, N, Options> > : public zero<
+    Eigen::Matrix<double, M, N, Options> > {
 };
 
-
 template<typename T> struct is_chart: public boost::false_type {
 };
 
@@ -248,12 +249,16 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
    * This chart for the vector space of M x N matrices (represented by Eigen matrices) chooses as basis the one with respect to which the coordinates are exactly the matrix entries as laid out in memory (as determined by Options).
    * Computing coordinates for a matrix is then simply a reshape to the row vector of appropriate size.
    */
-	typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> type;
+  typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> type;
   typedef type T;
-  typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
+  typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
-      "DefaultChart has not been implemented yet for dynamically sized matrices");
+
+  BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
+      "Internal error: DefaultChart for Dynamic should be handled by template below");
+
   static vector local(const T& origin, const T& other) {
-    return reshape<vector::RowsAtCompileTime, 1, vector::Options>(other) - reshape<vector::RowsAtCompileTime, 1, vector::Options>(origin);
+    return reshape<vector::RowsAtCompileTime, 1, vector::Options>(other)
+        - reshape<vector::RowsAtCompileTime, 1, vector::Options>(origin);
   }
   static T retract(const T& origin, const vector& d) {
     return origin + reshape<M, N, Options>(d);
@@ -266,20 +271,36 @@ struct DefaultChart<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
 // Dynamically sized Vector
 template<>
 struct DefaultChart<Vector> {
-  typedef Vector T;
+  typedef Vector type;
-  typedef T type;
+  typedef Vector vector;
-  typedef T vector;
+  static vector local(const Vector& origin, const Vector& other) {
-  static vector local(const T& origin, const T& other) {
     return other - origin;
   }
-  static T retract(const T& origin, const vector& d) {
+  static Vector retract(const Vector& origin, const vector& d) {
     return origin + d;
   }
-  static int getDimension(const T& origin) {
+  static int getDimension(const Vector& origin) {
     return origin.size();
   }
 };
 
+// Dynamically sized Matrix
+template<>
+struct DefaultChart<Matrix> {
+  typedef Matrix type;
+  typedef Vector vector;
+  static vector local(const Matrix& origin, const Matrix& other) {
+    Matrix d = other - origin;
+    return Eigen::Map<Vector>(d.data(),getDimension(d));
+  }
+  static Matrix retract(const Matrix& origin, const vector& d) {
+    return origin + Eigen::Map<const Matrix>(d.data(),origin.rows(),origin.cols());
+  }
+  static int getDimension(const Matrix& m) {
+    return m.size();
+  }
+};
+
 /**
  * Old Concept check class for Manifold types
  * Requires a mapping between a linear tangent space and the underlying

gtsam/base/Matrix.h

@@ -37,27 +37,31 @@ namespace gtsam {
 typedef Eigen::MatrixXd Matrix;
 typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
 
-typedef Eigen::Matrix2d Matrix2;
+// Create handy typedefs and constants for square-size matrices
-typedef Eigen::Matrix3d Matrix3;
+// MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9
-typedef Eigen::Matrix4d Matrix4;
+#define GTSAM_MAKE_TYPEDEFS(SIZE, SUFFIX)   \
-typedef Eigen::Matrix<double,5,5> Matrix5;
+typedef Eigen::Matrix<double, SIZE, SIZE> Matrix##SUFFIX;  \
-typedef Eigen::Matrix<double,6,6> Matrix6;
+typedef Eigen::Matrix<double, 1, SIZE> Matrix1##SUFFIX;  \
+typedef Eigen::Matrix<double, 2, SIZE> Matrix2##SUFFIX;  \
+typedef Eigen::Matrix<double, 3, SIZE> Matrix3##SUFFIX;  \
+typedef Eigen::Matrix<double, 4, SIZE> Matrix4##SUFFIX;  \
+typedef Eigen::Matrix<double, 5, SIZE> Matrix5##SUFFIX;  \
+typedef Eigen::Matrix<double, 6, SIZE> Matrix6##SUFFIX;  \
+typedef Eigen::Matrix<double, 7, SIZE> Matrix7##SUFFIX;  \
+typedef Eigen::Matrix<double, 8, SIZE> Matrix8##SUFFIX;  \
+typedef Eigen::Matrix<double, 9, SIZE> Matrix9##SUFFIX;  \
+static const Eigen::MatrixBase<Matrix##SUFFIX>::IdentityReturnType I_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Identity(); \
+static const Eigen::MatrixBase<Matrix##SUFFIX>::ConstantReturnType Z_##SUFFIX##x##SUFFIX = Matrix##SUFFIX::Zero();
 
-typedef Eigen::Matrix<double,2,3> Matrix23;
+GTSAM_MAKE_TYPEDEFS(1,1);
-typedef Eigen::Matrix<double,2,4> Matrix24;
+GTSAM_MAKE_TYPEDEFS(2,2);
-typedef Eigen::Matrix<double,2,5> Matrix25;
+GTSAM_MAKE_TYPEDEFS(3,3);
-typedef Eigen::Matrix<double,2,6> Matrix26;
+GTSAM_MAKE_TYPEDEFS(4,4);
-typedef Eigen::Matrix<double,2,7> Matrix27;
+GTSAM_MAKE_TYPEDEFS(5,5);
-typedef Eigen::Matrix<double,2,8> Matrix28;
+GTSAM_MAKE_TYPEDEFS(6,6);
-typedef Eigen::Matrix<double,2,9> Matrix29;
+GTSAM_MAKE_TYPEDEFS(7,7);
-
+GTSAM_MAKE_TYPEDEFS(8,8);
-typedef Eigen::Matrix<double,3,2> Matrix32;
+GTSAM_MAKE_TYPEDEFS(9,9);
-typedef Eigen::Matrix<double,3,4> Matrix34;
-typedef Eigen::Matrix<double,3,5> Matrix35;
-typedef Eigen::Matrix<double,3,6> Matrix36;
-typedef Eigen::Matrix<double,3,7> Matrix37;
-typedef Eigen::Matrix<double,3,8> Matrix38;
-typedef Eigen::Matrix<double,3,9> Matrix39;
 
 // Matrix expressions for accessing parts of matrices
 typedef Eigen::Block<Matrix> SubMatrix;

gtsam/base/OptionalJacobian.h

@@ -0,0 +1,115 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    OptionalJacobian.h
+ * @brief   Special class for optional Matrix arguments
+ * @author  Frank Dellaert
+ * @author  Natesh Srinivasan
+ * @date    Nov 28, 2014
+ */
+
+#pragma once
+
+#include <gtsam/3rdparty/Eigen/Eigen/Dense>
+
+#ifndef OPTIONALJACOBIAN_NOBOOST
+#include <boost/optional.hpp>
+#endif
+
+namespace gtsam {
+
+/**
+ * OptionalJacobian is an Eigen::Ref like class that can take be constructed using
+ * either a fixed size or dynamic Eigen matrix. In the latter case, the dynamic
+ * matrix will be resized. Finally, there is a constructor that takes
+ * boost::none, the default constructor acts like boost::none, and
+ * boost::optional<Eigen::MatrixXd&> is also supported for backwards compatibility.
+ */
+template<int Rows, int Cols>
+class OptionalJacobian {
+
+public:
+
+  /// Fixed size type
+  typedef Eigen::Matrix<double, Rows, Cols> Fixed;
+
+private:
+
+  Eigen::Map<Fixed> map_; /// View on constructor argument, if given
+
+  // Trick from http://eigen.tuxfamily.org/dox/group__TutorialMapClass.html
+  // uses "placement new" to make map_ usurp the memory of the fixed size matrix
+  void usurp(double* data) {
+    new (&map_) Eigen::Map<Fixed>(data);
+  }
+
+public:
+
+  /// Default constructor acts like boost::none
+  OptionalJacobian() :
+      map_(NULL) {
+  }
+
+  /// Constructor that will usurp data of a fixed-size matrix
+  OptionalJacobian(Fixed& fixed) :
+      map_(NULL) {
+    usurp(fixed.data());
+  }
+
+  /// Constructor that will usurp data of a fixed-size matrix, pointer version
+  OptionalJacobian(Fixed* fixedPtr) :
+      map_(NULL) {
+    if (fixedPtr)
+      usurp(fixedPtr->data());
+  }
+
+  /// Constructor that will resize a dynamic matrix (unless already correct)
+  OptionalJacobian(Eigen::MatrixXd& dynamic) :
+      map_(NULL) {
+    dynamic.resize(Rows, Cols); // no malloc if correct size
+    usurp(dynamic.data());
+  }
+
+#ifndef OPTIONALJACOBIAN_NOBOOST
+
+  /// Constructor with boost::none just makes empty
+  OptionalJacobian(boost::none_t none) :
+      map_(NULL) {
+  }
+
+  /// Constructor compatible with old-style derivatives
+  OptionalJacobian(const boost::optional<Eigen::MatrixXd&> optional) :
+      map_(NULL) {
+    if (optional) {
+      optional->resize(Rows, Cols);
+      usurp(optional->data());
+    }
+  }
+
+#endif
+
+  /// Return true is allocated, false if default constructor was used
+  operator bool() const {
+    return map_.data();
+  }
+
+  /// De-reference, like boost optional
+  Eigen::Map<Fixed>& operator*() {
+    return map_;
+  }
+
+  /// TODO: operator->()
+  Eigen::Map<Fixed>* operator->(){ return &map_; }
+};
+
+} // namespace gtsam
+

gtsam/base/Vector.h

@@ -34,6 +34,7 @@ namespace gtsam {
 typedef Eigen::VectorXd Vector;
 
 // Commonly used fixed size vectors
+typedef Eigen::Matrix<double, 1, 1> Vector1;
 typedef Eigen::Vector2d Vector2;
 typedef Eigen::Vector3d Vector3;
 typedef Eigen::Matrix<double, 4, 1> Vector4;
@@ -42,6 +43,7 @@ typedef Eigen::Matrix<double, 6, 1> Vector6;
 typedef Eigen::Matrix<double, 7, 1> Vector7;
 typedef Eigen::Matrix<double, 8, 1> Vector8;
 typedef Eigen::Matrix<double, 9, 1> Vector9;
+typedef Eigen::Matrix<double, 10, 1> Vector10;
 
 typedef Eigen::VectorBlock<Vector> SubVector;
 typedef Eigen::VectorBlock<const Vector> ConstSubVector;

gtsam/base/debug.cpp

@@ -17,9 +17,32 @@
  */
 
 #include <gtsam/base/debug.h>
+#ifdef GTSAM_USE_TBB
+#include <tbb/mutex.h>
+#endif
 
 namespace gtsam {
 
-  GTSAM_EXPORT FastMap<std::string, ValueWithDefault<bool,false> > debugFlags;
+GTSAM_EXPORT FastMap<std::string, ValueWithDefault<bool, false> > debugFlags;
+
+#ifdef GTSAM_USE_TBB
+tbb::mutex debugFlagsMutex;
+#endif
+
+/* ************************************************************************* */
+bool guardedIsDebug(const std::string& s) {
+#ifdef GTSAM_USE_TBB
+  tbb::mutex::scoped_lock lock(debugFlagsMutex);
+#endif
+  return gtsam::debugFlags[s];
+}
+
+/* ************************************************************************* */
+void guardedSetDebug(const std::string& s, const bool v) {
+#ifdef GTSAM_USE_TBB
+  tbb::mutex::scoped_lock lock(debugFlagsMutex);
+#endif
+  gtsam::debugFlags[s] = v;
+}
 
 }

gtsam/base/debug.h

@@ -43,6 +43,10 @@
 
 namespace gtsam {
   GTSAM_EXTERN_EXPORT FastMap<std::string, ValueWithDefault<bool,false> > debugFlags;
+
+  // Non-guarded use led to crashes, and solved in commit cd35db2
+  bool GTSAM_EXPORT guardedIsDebug(const std::string& s);
+  void GTSAM_EXPORT guardedSetDebug(const std::string& s, const bool v);
 }
 
 #undef ISDEBUG
@@ -50,8 +54,8 @@ namespace gtsam {
 
 #ifdef GTSAM_ENABLE_DEBUG
 
-#define ISDEBUG(S) (gtsam::debugFlags[S])
+#define ISDEBUG(S) (gtsam::guardedIsDebug(S))
-#define SETDEBUG(S,V) ((void)(gtsam::debugFlags[S] = (V)))
+#define SETDEBUG(S,V) ((void)(gtsam::guardedSetDebug(S,V)))
 
 #else
 

gtsam/base/tests/testOptionalJacobian.cpp

@@ -0,0 +1,105 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testOptionalJacobian.cpp
+ * @brief   Unit test for OptionalJacobian
+ * @author  Frank Dellaert
+ * @date    Nov 28, 2014
+ **/
+
+#include <gtsam/base/Matrix.h>
+#include <gtsam/base/OptionalJacobian.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+//******************************************************************************
+TEST( OptionalJacobian, Constructors ) {
+  Matrix23 fixed;
+
+  OptionalJacobian<2, 3> H1;
+  EXPECT(!H1);
+
+  OptionalJacobian<2, 3> H2(fixed);
+  EXPECT(H2);
+
+  OptionalJacobian<2, 3> H3(&fixed);
+  EXPECT(H3);
+
+  Matrix dynamic;
+  OptionalJacobian<2, 3> H4(dynamic);
+  EXPECT(H4);
+
+  OptionalJacobian<2, 3> H5(boost::none);
+  EXPECT(!H5);
+
+  boost::optional<Matrix&> optional(dynamic);
+  OptionalJacobian<2, 3> H6(optional);
+  EXPECT(H6);
+}
+
+//******************************************************************************
+void test(OptionalJacobian<2, 3> H = boost::none) {
+  if (H)
+    *H = Matrix23::Zero();
+}
+
+void testPtr(Matrix23* H = NULL) {
+  if (H)
+    *H = Matrix23::Zero();
+}
+
+TEST( OptionalJacobian, Ref2) {
+
+  Matrix expected;
+  expected = Matrix23::Zero();
+
+  // Default argument does nothing
+  test();
+
+  // Fixed size, no copy
+  Matrix23 fixed1;
+  fixed1.setOnes();
+  test(fixed1);
+  EXPECT(assert_equal(expected,fixed1));
+
+  // Fixed size, no copy, pointer style
+  Matrix23 fixed2;
+  fixed2.setOnes();
+  test(&fixed2);
+  EXPECT(assert_equal(expected,fixed2));
+
+  // Empty is no longer a sign we don't want a matrix, we want it resized
+  Matrix dynamic0;
+  test(dynamic0);
+  EXPECT(assert_equal(expected,dynamic0));
+
+  // Dynamic wrong size
+  Matrix dynamic1(3, 5);
+  dynamic1.setOnes();
+  test(dynamic1);
+  EXPECT(assert_equal(expected,dynamic0));
+
+  // Dynamic right size
+  Matrix dynamic2(2, 5);
+  dynamic2.setOnes();
+  test(dynamic2);
+  EXPECT(assert_equal(dynamic2,dynamic0));
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************

gtsam/geometry/Cal3Bundler.cpp

@@ -34,20 +34,22 @@ Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) :
 }
 
 /* ************************************************************************* */
-Matrix Cal3Bundler::K() const {
+Matrix3 Cal3Bundler::K() const {
   Matrix3 K;
   K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1;
   return K;
 }
 
 /* ************************************************************************* */
-Vector Cal3Bundler::k() const {
+Vector4 Cal3Bundler::k() const {
-  return (Vector(4) << k1_, k2_, 0, 0).finished();
+  Vector4 rvalue_;
+  rvalue_ << k1_, k2_, 0, 0;
+  return rvalue_;
 }
 
 /* ************************************************************************* */
 Vector3 Cal3Bundler::vector() const {
-  return (Vector(3) << f_, k1_, k2_).finished();
+  return Vector3(f_, k1_, k2_);
 }
 
 /* ************************************************************************* */
@@ -64,21 +66,9 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
   return true;
 }
 
-/* ************************************************************************* */
-Point2 Cal3Bundler::uncalibrate(const Point2& p) const {
-  //  r = x^2 + y^2;
-  //  g = (1 + k(1)*r + k(2)*r^2);
-  //  pi(:,i) = g * pn(:,i)
-  const double x = p.x(), y = p.y();
-  const double r = x * x + y * y;
-  const double g = 1. + (k1_ + k2_ * r) * r;
-  const double u = g * x, v = g * y;
-  return Point2(u0_ + f_ * u, v0_ + f_ * v);
-}
-
 /* ************************************************************************* */
 Point2 Cal3Bundler::uncalibrate(const Point2& p, //
-    boost::optional<Matrix23&> Dcal, boost::optional<Matrix2&> Dp) const {
+    OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 2> Dp) const {
   //  r = x^2 + y^2;
   //  g = (1 + k(1)*r + k(2)*r^2);
   //  pi(:,i) = g * pn(:,i)
@@ -103,35 +93,6 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
   return Point2(u0_ + f_ * u, v0_ + f_ * v);
 }
 
-/* ************************************************************************* */
-Point2 Cal3Bundler::uncalibrate(const Point2& p, //
-    boost::optional<Matrix&> Dcal, boost::optional<Matrix&> Dp) const {
-  //  r = x^2 + y^2;
-  //  g = (1 + k(1)*r + k(2)*r^2);
-  //  pi(:,i) = g * pn(:,i)
-  const double x = p.x(), y = p.y();
-  const double r = x * x + y * y;
-  const double g = 1. + (k1_ + k2_ * r) * r;
-  const double u = g * x, v = g * y;
-
-  // Derivatives make use of intermediate variables above
-  if (Dcal) {
-    double rx = r * x, ry = r * y;
-    Dcal->resize(2, 3);
-    *Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry;
-  }
-
-  if (Dp) {
-    const double a = 2. * (k1_ + 2. * k2_ * r);
-    const double axx = a * x * x, axy = a * x * y, ayy = a * y * y;
-    Dp->resize(2,2);
-    *Dp << g + axx, axy, axy, g + ayy;
-    *Dp *= f_;
-  }
-
-  return Point2(u0_ + f_ * u, v0_ + f_ * v);
-}
-
 /* ************************************************************************* */
 Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
   // Copied from Cal3DS2 :-(
@@ -161,24 +122,25 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
 }
 
 /* ************************************************************************* */
-Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const {
+Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const {
-  Matrix Dp;
+  Matrix2 Dp;
   uncalibrate(p, boost::none, Dp);
   return Dp;
 }
 
 /* ************************************************************************* */
-Matrix Cal3Bundler::D2d_calibration(const Point2& p) const {
+Matrix23 Cal3Bundler::D2d_calibration(const Point2& p) const {
-  Matrix Dcal;
+  Matrix23 Dcal;
   uncalibrate(p, Dcal, boost::none);
   return Dcal;
 }
 
 /* ************************************************************************* */
-Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
+Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
-  Matrix Dcal, Dp;
+  Matrix23 Dcal;
+  Matrix2 Dp;
   uncalibrate(p, Dcal, Dp);
-  Matrix H(2, 5);
+  Matrix25 H;
   H << Dp, Dcal;
   return H;
 }

gtsam/geometry/Cal3Bundler.h

@@ -69,8 +69,8 @@ public:
   /// @name Standard Interface
   /// @{
 
-  Matrix K() const; ///< Standard 3*3 calibration matrix
+  Matrix3 K() const; ///< Standard 3*3 calibration matrix
-  Vector k() const; ///< Radial distortion parameters (4 of them, 2 0)
+  Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
 
   Vector3 vector() const;
 
@@ -106,43 +106,27 @@ public:
 
 
   /**
-   * convert intrinsic coordinates xy to image coordinates uv
+   * @brief: convert intrinsic coordinates xy to image coordinates uv
-   * @param p point in intrinsic coordinates
+   * Version of uncalibrate with derivatives
-   * @return point in image coordinates
-   */
-  Point2 uncalibrate(const Point2& p) const;
-
-  /**
-   * Version of uncalibrate with fixed derivatives
    * @param p point in intrinsic coordinates
    * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in image coordinates
    */
-  Point2 uncalibrate(const Point2& p, boost::optional<Matrix23&> Dcal,
+  Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 3> Dcal = boost::none,
-      boost::optional<Matrix2&> Dp) const;
+      OptionalJacobian<2, 2> Dp = boost::none) const;
-
-  /**
-   * Version of uncalibrate with dynamic derivatives
-   * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
-   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
-   * @return point in image coordinates
-   */
-  Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
-      boost::optional<Matrix&> Dp) const;
 
   /// Conver a pixel coordinate to ideal coordinate
   Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;
 
   /// @deprecated might be removed in next release, use uncalibrate
-  Matrix D2d_intrinsic(const Point2& p) const;
+  Matrix2 D2d_intrinsic(const Point2& p) const;
 
   /// @deprecated might be removed in next release, use uncalibrate
-  Matrix D2d_calibration(const Point2& p) const;
+  Matrix23 D2d_calibration(const Point2& p) const;
 
   /// @deprecated might be removed in next release, use uncalibrate
-  Matrix D2d_intrinsic_calibration(const Point2& p) const;
+  Matrix25 D2d_intrinsic_calibration(const Point2& p) const;
 
   /// @}
   /// @name Manifold

gtsam/geometry/Cal3DS2_Base.cpp

@@ -28,18 +28,22 @@ Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
     fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
 
 /* ************************************************************************* */
-Matrix Cal3DS2_Base::K() const {
+Matrix3 Cal3DS2_Base::K() const {
-  return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished();
+    Matrix3 K;
+    K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
+    return K;
 }
 
 /* ************************************************************************* */
-Vector Cal3DS2_Base::vector() const {
+Vector9 Cal3DS2_Base::vector() const {
-  return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_).finished();
+  Vector9 v;
+  v << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_;
+  return v;
 }
 
 /* ************************************************************************* */
 void Cal3DS2_Base::print(const std::string& s_) const {
-  gtsam::print(K(), s_ + ".K");
+  gtsam::print((Matrix)K(), s_ + ".K");
   gtsam::print(Vector(k()), s_ + ".k");
 }
 
@@ -91,8 +95,7 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
 
 /* ************************************************************************* */
 Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
-    boost::optional<Matrix29&> H1,
+    OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const {
-    boost::optional<Matrix2&> H2) const {
 
   //  rr = x^2 + y^2;
   //  g = (1 + k(1)*rr + k(2)*rr^2);
@@ -126,26 +129,6 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
   return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
 }
 
-/* ************************************************************************* */
-Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
-    boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H2) const {
-
-  if (H1 || H2) {
-    Matrix29 D1;
-    Matrix2 D2;
-    Point2 pu = uncalibrate(p, D1, D2);
-    if (H1)
-      *H1 = D1;
-    if (H2)
-      *H2 = D2;
-    return pu;
-
-  } else {
-    return uncalibrate(p);
-  }
-}
-
 /* ************************************************************************* */
 Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
   // Use the following fixed point iteration to invert the radial distortion.
@@ -177,7 +160,7 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
 }
 
 /* ************************************************************************* */
-Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
+Matrix2 Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
   const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
   const double rr = xx + yy;
   const double r4 = rr * rr;
@@ -188,7 +171,7 @@ Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
 }
 
 /* ************************************************************************* */
-Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const {
+Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const {
   const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
   const double rr = xx + yy;
   const double r4 = rr * rr;

gtsam/geometry/Cal3DS2_Base.h

@@ -45,9 +45,9 @@ protected:
   double p1_, p2_ ; // tangential distortion
 
 public:
-  Matrix K() const ;
+  Matrix3 K() const ;
-  Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
+  Vector4 k() const { return Vector4(k1_, k2_, p1_, p2_); }
-  Vector vector() const ;
+  Vector9 vector() const ;
 
   /// @name Standard Constructors
   /// @{
@@ -113,23 +113,18 @@ public:
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in (distorted) image coordinates
    */
-
   Point2 uncalibrate(const Point2& p,
-       boost::optional<Matrix29&> Dcal = boost::none,
+       OptionalJacobian<2,9> Dcal = boost::none,
-       boost::optional<Matrix2&> Dp = boost::none) const ;
+       OptionalJacobian<2,2> Dp = boost::none) const ;
-
-  Point2 uncalibrate(const Point2& p,
-       boost::optional<Matrix&> Dcal,
-       boost::optional<Matrix&> Dp) const ;
 
   /// Convert (distorted) image coordinates uv to intrinsic coordinates xy
   Point2 calibrate(const Point2& p, const double tol=1e-5) const;
 
   /// Derivative of uncalibrate wrpt intrinsic coordinates
-  Matrix D2d_intrinsic(const Point2& p) const ;
+  Matrix2 D2d_intrinsic(const Point2& p) const ;
 
   /// Derivative of uncalibrate wrpt the calibration parameters
-  Matrix D2d_calibration(const Point2& p) const ;
+  Matrix29 D2d_calibration(const Point2& p) const ;
 
 private:
 

gtsam/geometry/Cal3Unified.cpp

@@ -29,8 +29,10 @@ Cal3Unified::Cal3Unified(const Vector &v):
     Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {}
 
 /* ************************************************************************* */
-Vector Cal3Unified::vector() const {
+Vector10 Cal3Unified::vector() const {
-  return (Vector(10) << Base::vector(), xi_).finished();
+  Vector10 v;
+  v << Base::vector(), xi_;
+  return v;
 }
 
 /* ************************************************************************* */
@@ -52,8 +54,8 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
 /* ************************************************************************* */
 // todo: make a fixed sized jacobian version of this
 Point2 Cal3Unified::uncalibrate(const Point2& p,
-       boost::optional<Matrix&> H1,
+       OptionalJacobian<2,10> H1,
-       boost::optional<Matrix&> H2) const {
+       OptionalJacobian<2,2> H2) const {
 
   // this part of code is modified from Cal3DS2,
   // since the second part of this model (after project to normalized plane)
@@ -81,10 +83,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
     Vector2 DU;
     DU << -xs * sqrt_nx * xi_sqrt_nx2, //
         -ys * sqrt_nx * xi_sqrt_nx2;
-
+    *H1 << H1base, H2base * DU;
-    H1->resize(2,10);
-    H1->block<2,9>(0,0) = H1base;
-    H1->block<2,1>(0,9) = H2base * DU;
   }
 
   // Inlined derivative for points
@@ -96,7 +95,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
     DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
         mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
 
-    *H2 = H2base * DU;
+    *H2 << H2base * DU;
   }
 
   return puncalib;
@@ -136,7 +135,7 @@ Cal3Unified Cal3Unified::retract(const Vector& d) const {
 }
 
 /* ************************************************************************* */
-Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
+Vector10 Cal3Unified::localCoordinates(const Cal3Unified& T2) const {
   return T2.vector() - vector();
 }
 

gtsam/geometry/Cal3Unified.h

@@ -51,7 +51,7 @@ private:
 
 public:
 
-    Vector vector() const ;
+    Vector10 vector() const ;
 
   /// @name Standard Constructors
   /// @{
@@ -96,8 +96,8 @@ public:
    * @return point in image coordinates
    */
   Point2 uncalibrate(const Point2& p,
-      boost::optional<Matrix&> Dcal = boost::none,
+      OptionalJacobian<2,10> Dcal = boost::none,
-      boost::optional<Matrix&> Dp = boost::none) const ;
+      OptionalJacobian<2,2> Dp = boost::none) const ;
 
   /// Conver a pixel coordinate to ideal coordinate
   Point2 calibrate(const Point2& p, const double tol=1e-5) const;
@@ -116,7 +116,7 @@ public:
   Cal3Unified retract(const Vector& d) const ;
 
   /// Given a different calibration, calculate update to obtain it
-  Vector localCoordinates(const Cal3Unified& T2) const ;
+  Vector10 localCoordinates(const Cal3Unified& T2) const ;
 
   /// Return dimensions of calibration manifold object
   virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)

gtsam/geometry/Cal3_S2.cpp

@@ -53,7 +53,7 @@ Cal3_S2::Cal3_S2(const std::string &path) :
 
 /* ************************************************************************* */
 void Cal3_S2::print(const std::string& s) const {
-  gtsam::print(matrix(), s);
+  gtsam::print((Matrix)matrix(), s);
 }
 
 /* ************************************************************************* */
@@ -72,32 +72,13 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
 }
 
 /* ************************************************************************* */
-Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
+Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal,
-    boost::optional<Matrix&> Dp) const {
+    OptionalJacobian<2, 2> Dp) const {
   const double x = p.x(), y = p.y();
-  if (Dcal) {
+  if (Dcal)
-    Dcal->resize(2,5);
     *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
-  }
+  if (Dp)
-  if (Dp) {
-    Dp->resize(2,2);
     *Dp << fx_, s_, 0.0, fy_;
-  }
-  return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
-}
-
-/* ************************************************************************* */
-Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
-    boost::optional<Matrix2&> Dp) const {
-  const double x = p.x(), y = p.y();
-  if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
-  if (Dp) *Dp << fx_, s_, 0.0, fy_;
-  return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
-}
-
-/* ************************************************************************* */
-Point2 Cal3_S2::uncalibrate(const Point2& p) const {
-  const double x = p.x(), y = p.y();
   return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
 }
 

gtsam/geometry/Cal3_S2.h

@@ -117,37 +117,33 @@ public:
   }
 
   /// vectorized form (column-wise)
-  Vector vector() const {
+  Vector5 vector() const {
-    double r[] = { fx_, fy_, s_, u0_, v0_ };
+    Vector5 v;
-    Vector v(5);
+    v << fx_, fy_, s_, u0_, v0_;
-    std::copy(r, r + 5, v.data());
     return v;
   }
 
   /// return calibration matrix K
-  Matrix K() const {
+  Matrix3 K() const {
-    return (Matrix(3, 3) <<  fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished();
+    Matrix3 K;
+    K <<  fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
+    return K;
   }
 
   /** @deprecated The following function has been deprecated, use K above */
-  Matrix matrix() const {
+  Matrix3 matrix() const {
     return K();
   }
 
   /// return inverted calibration matrix inv(K)
-  Matrix matrix_inverse() const {
+  Matrix3 matrix_inverse() const {
     const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
-    return (Matrix(3, 3) << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
+    Matrix3 K_inverse;
-        1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0).finished();
+    K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
+        1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0;
+    return K_inverse;
   }
 
-  /**
-   * convert intrinsic coordinates xy to image coordinates uv
-   * @param p point in intrinsic coordinates
-   * @return point in image coordinates
-   */
-  Point2 uncalibrate(const Point2& p) const;
-
   /**
    * convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
    * @param p point in intrinsic coordinates
@@ -155,18 +151,8 @@ public:
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in image coordinates
    */
-  Point2 uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
+  Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none,
-      boost::optional<Matrix2&> Dp) const;
+      OptionalJacobian<2,2> Dp = boost::none) const;
-
-  /**
-   * convert intrinsic coordinates xy to image coordinates uv, dynamic derivaitves
-   * @param p point in intrinsic coordinates
-   * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
-   * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
-   * @return point in image coordinates
-   */
-  Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
-      boost::optional<Matrix&> Dp) const;
 
   /**
    * convert image coordinates uv to intrinsic coordinates xy
@@ -184,10 +170,10 @@ public:
 
   /// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
   inline Cal3_S2 between(const Cal3_S2& q,
-      boost::optional<Matrix&> H1=boost::none,
+      OptionalJacobian<5,5> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const {
+      OptionalJacobian<5,5> H2=boost::none) const {
-    if(H1) *H1 = -eye(5);
+    if(H1) *H1 = -I_5x5;
-    if(H2) *H2 = eye(5);
+    if(H2) *H2 =  I_5x5;
     return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
   }
 
@@ -212,7 +198,7 @@ public:
   }
 
   /// Unretraction for the calibration
-  Vector localCoordinates(const Cal3_S2& T2) const {
+  Vector5 localCoordinates(const Cal3_S2& T2) const {
     return T2.vector() - vector();
   }
 

gtsam/geometry/Cal3_S2Stereo.h

@@ -103,6 +103,38 @@ namespace gtsam {
     /// return baseline
     inline double baseline() const { return b_; }
 
+    /// vectorized form (column-wise)
+    Vector6 vector() const {
+      Vector6 v;
+      v << K_.vector(), b_;
+      return v;
+    }
+
+    /// @}
+    /// @name Manifold
+    /// @{
+
+    /// return DOF, dimensionality of tangent space
+    inline size_t dim() const {
+      return 6;
+    }
+
+    /// return DOF, dimensionality of tangent space
+    static size_t Dim() {
+      return 6;
+    }
+
+    /// Given 6-dim tangent vector, create new calibration
+    inline Cal3_S2Stereo retract(const Vector& d) const {
+      return Cal3_S2Stereo(K_.fx() + d(0), K_.fy() + d(1), K_.skew() + d(2), K_.px() + d(3), K_.py() + d(4), b_ + d(5));
+    }
+
+    /// Unretraction for the calibration
+    Vector6 localCoordinates(const Cal3_S2Stereo& T2) const {
+      return T2.vector() - vector();
+    }
+
+
     /// @}
     /// @name Advanced Interface
     /// @{
@@ -119,4 +151,23 @@ namespace gtsam {
     /// @}
 
   };
+
+  // Define GTSAM traits
+  namespace traits {
+
+  template<>
+  struct GTSAM_EXPORT is_manifold<Cal3_S2Stereo> : public boost::true_type{
+  };
+
+  template<>
+  struct GTSAM_EXPORT dimension<Cal3_S2Stereo> : public boost::integral_constant<int, 6>{
+  };
+
+  template<>
+  struct GTSAM_EXPORT zero<Cal3_S2Stereo> {
+    static Cal3_S2Stereo value() { return Cal3_S2Stereo();}
+  };
+
+  }
+
 } // \ namespace gtsam

gtsam/geometry/CalibratedCamera.cpp

@@ -33,10 +33,10 @@ CalibratedCamera::CalibratedCamera(const Vector &v) :
 
 /* ************************************************************************* */
 Point2 CalibratedCamera::project_to_camera(const Point3& P,
-    boost::optional<Matrix&> H1) {
+    OptionalJacobian<2,3> H1) {
   if (H1) {
     double d = 1.0 / P.z(), d2 = d * d;
-    *H1 = (Matrix(2, 3) <<  d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2).finished();
+    *H1 <<  d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2;
   }
   return Point2(P.x() / P.z(), P.y() / P.z());
 }
@@ -59,10 +59,12 @@ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
 
 /* ************************************************************************* */
 Point2 CalibratedCamera::project(const Point3& point,
-    boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) const {
+    OptionalJacobian<2,6> Dpose, OptionalJacobian<2,3> Dpoint) const {
 
 #ifdef CALIBRATEDCAMERA_CHAIN_RULE
-  Point3 q = pose_.transform_to(point, Dpose, Dpoint);
+  Matrix36 Dpose_;
+  Matrix3 Dpoint_;
+  Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0);
 #else
   Point3 q = pose_.transform_to(point);
 #endif
@@ -75,23 +77,26 @@ Point2 CalibratedCamera::project(const Point3& point,
   if (Dpose || Dpoint) {
 #ifdef CALIBRATEDCAMERA_CHAIN_RULE
     // just implement chain rule
-    Matrix H;
+    if(Dpose && Dpoint) {
-    project_to_camera(q,H);
+      Matrix23 H;
-    if (Dpose) *Dpose = H * (*Dpose);
+      project_to_camera(q,H);
-    if (Dpoint) *Dpoint = H * (*Dpoint);
+      if (Dpose) *Dpose = H * (*Dpose_);
+      if (Dpoint) *Dpoint = H * (*Dpoint_);
+    }
 #else
     // optimized version, see CalibratedCamera.nb
     const double z = q.z(), d = 1.0 / z;
     const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
     if (Dpose)
-      *Dpose = (Matrix(2, 6) <<  uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
+      *Dpose <<  uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
-          -uv, -u, 0., -d, d * v).finished();
+          -uv, -u, 0., -d, d * v;
     if (Dpoint) {
-      const Matrix R(pose_.rotation().matrix());
+      const Matrix3 R(pose_.rotation().matrix());
-      *Dpoint = d
+      Matrix23 Dpoint_;
-          * (Matrix(2, 3) <<  R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
+      Dpoint_ << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
               R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2),
-              R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2)).finished();
+              R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
+      *Dpoint = d * Dpoint_;
     }
 #endif
   }

gtsam/geometry/CalibratedCamera.h

@@ -18,9 +18,8 @@
 
 #pragma once
 
-#include <gtsam/base/DerivedValue.h>
-#include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Point2.h>
 
 namespace gtsam {
 
@@ -88,26 +87,6 @@ public:
     return pose_;
   }
 
-  /// compose the two camera poses: TODO Frank says this might not make sense
-  inline const CalibratedCamera compose(const CalibratedCamera &c,
-      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
-          boost::none) const {
-    return CalibratedCamera(pose_.compose(c.pose(), H1, H2));
-  }
-
-  /// between the two camera poses: TODO Frank says this might not make sense
-  inline const CalibratedCamera between(const CalibratedCamera& c,
-      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
-          boost::none) const {
-    return CalibratedCamera(pose_.between(c.pose(), H1, H2));
-  }
-
-  /// invert the camera pose: TODO Frank says this might not make sense
-  inline const CalibratedCamera inverse(boost::optional<Matrix&> H1 =
-      boost::none) const {
-    return CalibratedCamera(pose_.inverse(H1));
-  }
-
   /**
    * Create a level camera at the given 2D pose and height
    * @param pose2 specifies the location and viewing direction
@@ -152,8 +131,8 @@ public:
    * @return the intrinsic coordinates of the projected point
    */
   Point2 project(const Point3& point,
-      boost::optional<Matrix&> Dpose = boost::none,
+      OptionalJacobian<2, 6> Dpose = boost::none,
-      boost::optional<Matrix&> Dpoint = boost::none) const;
+      OptionalJacobian<2, 3> Dpoint = boost::none) const;
 
   /**
    * projects a 3-dimensional point in camera coordinates into the
@@ -161,7 +140,7 @@ public:
    * With optional 2by3 derivative
    */
   static Point2 project_to_camera(const Point3& cameraPoint,
-      boost::optional<Matrix&> H1 = boost::none);
+      OptionalJacobian<2, 3> H1 = boost::none);
 
   /**
    * backproject a 2-dimensional point to a 3-dimension point
@@ -175,8 +154,8 @@ public:
    * @param H2 optionally computed Jacobian with respect to the 3D point
    * @return range (double)
    */
-  double range(const Point3& point, boost::optional<Matrix&> H1 = boost::none,
+  double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none) const {
+      OptionalJacobian<1, 3> H2 = boost::none) const {
     return pose_.range(point, H1, H2);
   }
 
@@ -187,8 +166,8 @@ public:
    * @param H2 optionally computed Jacobian with respect to the 3D point
    * @return range (double)
    */
-  double range(const Pose3& pose, boost::optional<Matrix&> H1 = boost::none,
+  double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none) const {
+      OptionalJacobian<1, 6> H2 = boost::none) const {
     return pose_.range(pose, H1, H2);
   }
 
@@ -199,8 +178,8 @@ public:
    * @param H2 optionally computed Jacobian with respect to the 3D point
    * @return range (double)
    */
-  double range(const CalibratedCamera& camera, boost::optional<Matrix&> H1 =
+  double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 =
-      boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+      boost::none, OptionalJacobian<1, 6> H2 = boost::none) const {
     return pose_.range(camera.pose_, H1, H2);
   }
 
@@ -224,15 +203,16 @@ private:
 namespace traits {
 
 template<>
-struct GTSAM_EXPORT is_group<CalibratedCamera> : public boost::true_type{
+struct GTSAM_EXPORT is_group<CalibratedCamera> : public boost::true_type {
 };
 
 template<>
-struct GTSAM_EXPORT is_manifold<CalibratedCamera> : public boost::true_type{
+struct GTSAM_EXPORT is_manifold<CalibratedCamera> : public boost::true_type {
 };
 
 template<>
-struct GTSAM_EXPORT dimension<CalibratedCamera> : public boost::integral_constant<int, 6>{
+struct GTSAM_EXPORT dimension<CalibratedCamera> : public boost::integral_constant<
+    int, 6> {
 };
 
 }

gtsam/geometry/EssentialMatrix.cpp

@@ -14,7 +14,7 @@ namespace gtsam {
 
 /* ************************************************************************* */
 EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
-    boost::optional<Matrix&> H) {
+    OptionalJacobian<5, 6> H) {
   const Rot3& _1R2_ = _1P2_.rotation();
   const Point3& _1T2_ = _1P2_.translation();
   if (!H) {
@@ -25,13 +25,10 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
     // Calculate the 5*6 Jacobian H = D_E_1P2
     // D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation
     // First get 2*3 derivative from Unit3::FromPoint3
-    Matrix D_direction_1T2;
+    Matrix23 D_direction_1T2;
     Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2);
-    H->resize(5, 6);
+    *H << I_3x3, Z_3x3, //
-    H->block<3, 3>(0, 0) << Matrix::Identity(3, 3); // upper left
+    Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix();
-    H->block<2, 3>(3, 0) << Matrix::Zero(2, 3); // lower left
-    H->block<3, 3>(0, 3) << Matrix::Zero(3, 3); // upper right
-    H->block<2, 3>(3, 3) << D_direction_1T2 * _1R2_.matrix(); // lower right
     return EssentialMatrix(_1R2_, direction);
   }
 }
@@ -54,23 +51,26 @@ EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
 }
 
 /* ************************************************************************* */
-Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
+Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
-  return (Vector(5) <<
+  Vector5 v;
-      aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_)).finished();
+  v << aRb_.localCoordinates(other.aRb_),
+      aTb_.localCoordinates(other.aTb_);
+  return v;
 }
 
 /* ************************************************************************* */
-Point3 EssentialMatrix::transform_to(const Point3& p,
+Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE,
-    boost::optional<Matrix&> DE, boost::optional<Matrix&> Dpoint) const {
+    OptionalJacobian<3, 3> Dpoint) const {
   Pose3 pose(aRb_, aTb_.point3());
-  Point3 q = pose.transform_to(p, DE, Dpoint);
+  Matrix36 DE_;
+  Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint);
   if (DE) {
     // DE returned by pose.transform_to is 3*6, but we need it to be 3*5
     // The last 3 columns are derivative with respect to change in translation
     // The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
     // Duy made an educated guess that this needs to be rotated to the local frame
-    Matrix H(3, 5);
+    Matrix35 H;
-    H << DE->block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis();
+    H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis();
     *DE = H;
   }
   return q;
@@ -78,7 +78,7 @@ Point3 EssentialMatrix::transform_to(const Point3& p,
 
 /* ************************************************************************* */
 EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
-    boost::optional<Matrix&> HE, boost::optional<Matrix&> HR) const {
+    OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const {
 
   // The rotation must be conjugated to act in the camera frame
   Rot3 c1Rc2 = aRb_.conjugate(cRb);
@@ -89,18 +89,16 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
     return EssentialMatrix(c1Rc2, c1Tc2);
   } else {
     // Calculate derivatives
-    Matrix D_c1Tc2_cRb, D_c1Tc2_aTb; // 2*3 and 2*2
+    Matrix23 D_c1Tc2_cRb; // 2*3
+    Matrix2 D_c1Tc2_aTb; // 2*2
     Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
-    if (HE) {
+    if (HE)
-      *HE = zeros(5, 5);
+      *HE << cRb.matrix(), Matrix32::Zero(), //
-      HE->block<3, 3>(0, 0) << cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2
+      Matrix23::Zero(), D_c1Tc2_aTb;
-      HE->block<2, 2>(3, 3) << D_c1Tc2_aTb; // (2*2)
-    }
     if (HR) {
       throw runtime_error(
           "EssentialMatrix::rotate: derivative HR not implemented yet");
       /*
-       HR->resize(5, 3);
        HR->block<3, 3>(0, 0) << zeros(3, 3); // a change in the rotation yields ?
        HR->block<2, 3>(3, 0) << zeros(2, 3); // (2*3) * (3*3) ?
        */
@@ -110,13 +108,12 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
 }
 
 /* ************************************************************************* */
-double EssentialMatrix::error(const Vector& vA, const Vector& vB, //
+double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
-    boost::optional<Matrix&> H) const {
+    OptionalJacobian<1, 5> H) const {
   if (H) {
-    H->resize(1, 5);
     // See math.lyx
-    Matrix HR = vA.transpose() * E_ * skewSymmetric(-vB);
+    Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
-    Matrix HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
+    Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
         * aTb_.basis();
     *H << HR, HD;
   }

gtsam/geometry/EssentialMatrix.h

@@ -31,8 +31,8 @@ private:
 public:
 
   /// Static function to convert Point2 to homogeneous coordinates
-  static Vector Homogeneous(const Point2& p) {
+  static Vector3 Homogeneous(const Point2& p) {
-    return (Vector(3) << p.x(), p.y(), 1).finished();
+    return Vector3(p.x(), p.y(), 1);
   }
 
   /// @name Constructors and named constructors
@@ -50,7 +50,7 @@ public:
 
   /// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
   static EssentialMatrix FromPose3(const Pose3& _1P2_,
-      boost::optional<Matrix&> H = boost::none);
+      OptionalJacobian<5, 6> H = boost::none);
 
   /// Random, using Rot3::Random and Unit3::Random
   template<typename Engine>
@@ -84,15 +84,15 @@ public:
   }
 
   /// Return the dimensionality of the tangent space
-  virtual size_t dim() const {
+  size_t dim() const {
     return 5;
   }
 
   /// Retract delta to manifold
-  virtual EssentialMatrix retract(const Vector& xi) const;
+  EssentialMatrix retract(const Vector& xi) const;
 
   /// Compute the coordinates in the tangent space
-  virtual Vector localCoordinates(const EssentialMatrix& other) const;
+  Vector5 localCoordinates(const EssentialMatrix& other) const;
 
   /// @}
 
@@ -132,16 +132,16 @@ public:
    * @return point in pose coordinates
    */
   Point3 transform_to(const Point3& p,
-      boost::optional<Matrix&> DE = boost::none,
+      OptionalJacobian<3,5> DE = boost::none,
-      boost::optional<Matrix&> Dpoint = boost::none) const;
+      OptionalJacobian<3,3> Dpoint = boost::none) const;
 
   /**
    * Given essential matrix E in camera frame B, convert to body frame C
    * @param cRb rotation from body frame to camera frame
    * @param E essential matrix E in camera frame C
    */
-  EssentialMatrix rotate(const Rot3& cRb, boost::optional<Matrix&> HE =
+  EssentialMatrix rotate(const Rot3& cRb, OptionalJacobian<5, 5> HE =
-      boost::none, boost::optional<Matrix&> HR = boost::none) const;
+      boost::none, OptionalJacobian<5, 3> HR = boost::none) const;
 
   /**
    * Given essential matrix E in camera frame B, convert to body frame C
@@ -153,8 +153,8 @@ public:
   }
 
   /// epipolar error, algebraic
-  double error(const Vector& vA, const Vector& vB, //
+  double error(const Vector3& vA, const Vector3& vB, //
-      boost::optional<Matrix&> H = boost::none) const;
+      OptionalJacobian<1,5> H = boost::none) const;
 
   /// @}
 

gtsam/geometry/PinholeCamera.h

@@ -18,16 +18,9 @@
 
 #pragma once
 
-#include <cmath>
-#include <boost/optional.hpp>
-#include <boost/serialization/nvp.hpp>
-#include <gtsam/base/DerivedValue.h>
-#include <gtsam/base/Vector.h>
-#include <gtsam/base/Matrix.h>
-#include <gtsam/geometry/Point2.h>
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/Pose2.h>
+#include <cmath>
 
 namespace gtsam {
 
@@ -42,7 +35,9 @@ private:
   Pose3 pose_;
   Calibration K_;
 
-  static const int DimK = traits::dimension<Calibration>::value;
+  // Get dimensions of calibration type and This at compile time
+  static const int DimK = traits::dimension<Calibration>::value, //
+      DimC = 6 + DimK;
 
 public:
 
@@ -114,9 +109,9 @@ public:
   /// @{
 
   explicit PinholeCamera(const Vector &v) {
-    pose_ = Pose3::Expmap(v.head(Pose3::Dim()));
+    pose_ = Pose3::Expmap(v.head(6));
-    if (v.size() > Pose3::Dim()) {
+    if (v.size() > 6) {
-      K_ = Calibration(v.tail(Calibration::Dim()));
+      K_ = Calibration(v.tail(DimK));
     }
   }
 
@@ -167,82 +162,30 @@ public:
     return K_;
   }
 
-  /// @}
-  /// @name Group ?? Frank says this might not make sense
-  /// @{
-
-  /// compose two cameras: TODO Frank says this might not make sense
-  inline const PinholeCamera compose(const PinholeCamera &c,
-      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
-          boost::none) const {
-    PinholeCamera result(pose_.compose(c.pose(), H1, H2), K_);
-    if (H1) {
-      H1->conservativeResize(Dim(), Dim());
-      H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
-          Calibration::Dim());
-      H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
-    }
-    if (H2) {
-      H2->conservativeResize(Dim(), Dim());
-      H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
-          Calibration::Dim());
-      H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
-    }
-    return result;
-  }
-
-  /// between two cameras: TODO Frank says this might not make sense
-  inline const PinholeCamera between(const PinholeCamera& c,
-      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
-          boost::none) const {
-    PinholeCamera result(pose_.between(c.pose(), H1, H2), K_);
-    if (H1) {
-      H1->conservativeResize(Dim(), Dim());
-      H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
-          Calibration::Dim());
-      H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
-    }
-    if (H2) {
-      H2->conservativeResize(Dim(), Dim());
-      H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
-          Calibration::Dim());
-      H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
-    }
-    return result;
-  }
-
-  /// inverse camera: TODO Frank says this might not make sense
-  inline const PinholeCamera inverse(
-      boost::optional<Matrix&> H1 = boost::none) const {
-    PinholeCamera result(pose_.inverse(H1), K_);
-    if (H1) {
-      H1->conservativeResize(Dim(), Dim());
-      H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
-          Calibration::Dim());
-      H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
-    }
-    return result;
-  }
-
-  /// compose two cameras: TODO Frank says this might not make sense
-  inline const PinholeCamera compose(const Pose3 &c) const {
-    return PinholeCamera(pose_.compose(c), K_);
-  }
-
   /// @}
   /// @name Manifold
   /// @{
 
-  /// move a cameras according to d
+  /// Manifold dimension
-  PinholeCamera retract(const Vector& d) const {
+  inline size_t dim() const {
-    if ((size_t) d.size() == pose_.dim())
+    return DimC;
-      return PinholeCamera(pose().retract(d), calibration());
-    else
-      return PinholeCamera(pose().retract(d.head(pose().dim())),
-          calibration().retract(d.tail(calibration().dim())));
   }
 
-  typedef Eigen::Matrix<double,6+DimK,1> VectorK6;
+  /// Manifold dimension
+  inline static size_t Dim() {
+    return DimC;
+  }
+
+  typedef Eigen::Matrix<double, DimC, 1> VectorK6;
+
+  /// move a cameras according to d
+  PinholeCamera retract(const Vector& d) const {
+    if ((size_t) d.size() == 6)
+      return PinholeCamera(pose().retract(d), calibration());
+    else
+      return PinholeCamera(pose().retract(d.head(6)),
+          calibration().retract(d.tail(calibration().dim())));
+  }
 
   /// return canonical coordinate
   VectorK6 localCoordinates(const PinholeCamera& T2) const {
@@ -252,16 +195,6 @@ public:
     return d;
   }
 
-  /// Manifold dimension
-  inline size_t dim() const {
-    return pose_.dim() + K_.dim();
-  }
-
-  /// Manifold dimension
-  inline static size_t Dim() {
-    return Pose3::Dim() + Calibration::Dim();
-  }
-
   /// @}
   /// @name Transformations and measurement functions
   /// @{
@@ -272,8 +205,8 @@ public:
    * @param P A point in camera coordinates
    * @param Dpoint is the 2*3 Jacobian w.r.t. P
    */
-  inline static Point2 project_to_camera(const Point3& P,
+  static Point2 project_to_camera(const Point3& P, //
-      boost::optional<Matrix23&> Dpoint = boost::none) {
+      OptionalJacobian<2, 3> Dpoint = boost::none) {
 #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
     if (P.z() <= 0)
       throw CheiralityException();
@@ -292,21 +225,7 @@ public:
     return std::make_pair(K_.uncalibrate(pn), pc.z() > 0);
   }
 
-  /** project a point from world coordinate to the image
+  typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
-   *  @param pw is a point in world coordinates
-   */
-  inline Point2 project(const Point3& pw) const {
-
-    // Transform to camera coordinates and check cheirality
-    const Point3 pc = pose_.transform_to(pw);
-
-    // Project to normalized image coordinates
-    const Point2 pn = project_to_camera(pc);
-
-    return K_.uncalibrate(pn);
-  }
-
-  typedef Eigen::Matrix<double,2,DimK> Matrix2K;
 
   /** project a point from world coordinate to the image
    *  @param pw is a point in world coordinates
@@ -314,11 +233,9 @@ public:
    *  @param Dpoint is the Jacobian w.r.t. point3
    *  @param Dcal is the Jacobian w.r.t. calibration
    */
-  inline Point2 project(
+  inline Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose =
-      const Point3& pw, //
+      boost::none, OptionalJacobian<2, 3> Dpoint = boost::none,
-      boost::optional<Matrix26&> Dpose,
+      OptionalJacobian<2, DimK> Dcal = boost::none) const {
-      boost::optional<Matrix23&> Dpoint,
-      boost::optional<Matrix2K&> Dcal) const {
 
     // Transform to camera coordinates and check cheirality
     const Point3 pc = pose_.transform_to(pw);
@@ -340,46 +257,7 @@ public:
         calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
       return pi;
     } else
-      return K_.uncalibrate(pn, Dcal, boost::none);
+      return K_.uncalibrate(pn, Dcal);
-  }
-
-  /** project a point from world coordinate to the image
-   *  @param pw is a point in world coordinates
-   *  @param Dpose is the Jacobian w.r.t. pose3
-   *  @param Dpoint is the Jacobian w.r.t. point3
-   *  @param Dcal is the Jacobian w.r.t. calibration
-   */
-  inline Point2 project(
-      const Point3& pw, //
-      boost::optional<Matrix&> Dpose,
-      boost::optional<Matrix&> Dpoint,
-      boost::optional<Matrix&> Dcal) const {
-
-    // Transform to camera coordinates and check cheirality
-    const Point3 pc = pose_.transform_to(pw);
-
-    // Project to normalized image coordinates
-    const Point2 pn = project_to_camera(pc);
-
-    if (Dpose || Dpoint) {
-      const double z = pc.z(), d = 1.0 / z;
-
-      // uncalibration
-      Matrix Dpi_pn(2, 2);
-      const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
-
-      // chain the Jacobian matrices
-      if (Dpose) {
-        Dpose->resize(2, 6);
-        calculateDpose(pn, d, Dpi_pn, *Dpose);
-      }
-      if (Dpoint) {
-        Dpoint->resize(2, 3);
-        calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
-      }
-      return pi;
-    } else
-      return K_.uncalibrate(pn, Dcal, boost::none);
   }
 
   /** project a point at infinity from world coordinate to the image
@@ -388,11 +266,10 @@ public:
    *  @param Dpoint is the Jacobian w.r.t. point3
    *  @param Dcal is the Jacobian w.r.t. calibration
    */
-  inline Point2 projectPointAtInfinity(
+  inline Point2 projectPointAtInfinity(const Point3& pw,
-      const Point3& pw, //
+      OptionalJacobian<2, 6> Dpose = boost::none,
-      boost::optional<Matrix&> Dpose = boost::none,
+      OptionalJacobian<2, 2> Dpoint = boost::none,
-      boost::optional<Matrix&> Dpoint = boost::none,
+      OptionalJacobian<2, DimK> Dcal = boost::none) const {
-      boost::optional<Matrix&> Dcal = boost::none) const {
 
     if (!Dpose && !Dpoint && !Dcal) {
       const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
@@ -401,40 +278,30 @@ public:
     }
 
     // world to camera coordinate
-    Matrix Dpc_rot /* 3*3 */, Dpc_point /* 3*3 */;
+    Matrix3 Dpc_rot, Dpc_point;
     const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point);
 
-    Matrix Dpc_pose = Matrix::Zero(3, 6);
+    Matrix36 Dpc_pose;
-    Dpc_pose.block(0, 0, 3, 3) = Dpc_rot;
+    Dpc_pose.setZero();
+    Dpc_pose.leftCols<3>() = Dpc_rot;
 
     // camera to normalized image coordinate
     Matrix23 Dpn_pc; // 2*3
     const Point2 pn = project_to_camera(pc, Dpn_pc);
 
     // uncalibration
-    Matrix Dpi_pn; // 2*2
+    Matrix2 Dpi_pn; // 2*2
     const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
 
     // chain the Jacobian matrices
-    const Matrix Dpi_pc = Dpi_pn * Dpn_pc;
+    const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc;
     if (Dpose)
       *Dpose = Dpi_pc * Dpc_pose;
     if (Dpoint)
-      *Dpoint = (Dpi_pc * Dpc_point).block(0, 0, 2, 2); // only 2dof are important for the point (direction-only)
+      *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only)
     return pi;
   }
 
-  typedef Eigen::Matrix<double,2,6+DimK> Matrix2K6;
-
-  /** project a point from world coordinate to the image
-   *  @param pw is a point in the world coordinate
-   */
-  Point2 project2(const Point3& pw) const {
-    const Point3 pc = pose_.transform_to(pw);
-    const Point2 pn = project_to_camera(pc);
-    return K_.uncalibrate(pn);
-  }
-
   /** project a point from world coordinate to the image, fixed Jacobians
    *  @param pw is a point in the world coordinate
    *  @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
@@ -442,8 +309,8 @@ public:
    */
   Point2 project2(
       const Point3& pw, //
-      boost::optional<Matrix2K6&> Dcamera,
+      OptionalJacobian<2, DimC> Dcamera = boost::none,
-      boost::optional<Matrix23&> Dpoint) const {
+      OptionalJacobian<2, 3> Dpoint = boost::none) const {
 
     const Point3 pc = pose_.transform_to(pw);
     const Point2 pn = project_to_camera(pc);
@@ -459,8 +326,8 @@ public:
       const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
 
       if (Dcamera) { // TODO why does leftCols<6>() not compile ??
-        calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols(6));
+        calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6));
-        Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib
+        (*Dcamera).rightCols(DimK) = Dcal; // Jacobian wrt calib
       }
       if (Dpoint) {
         calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
@@ -469,40 +336,6 @@ public:
     }
   }
 
-  /** project a point from world coordinate to the image
-   *  @param pw is a point in the world coordinate
-   *  @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
-   *  @param Dpoint is the Jacobian w.r.t. point3
-   */
-  Point2 project2(const Point3& pw, //
-      boost::optional<Matrix&> Dcamera, boost::optional<Matrix&> Dpoint) const {
-
-    const Point3 pc = pose_.transform_to(pw);
-    const Point2 pn = project_to_camera(pc);
-
-    if (!Dcamera && !Dpoint) {
-      return K_.uncalibrate(pn);
-    } else {
-      const double z = pc.z(), d = 1.0 / z;
-
-      // uncalibration
-      Matrix2K Dcal;
-      Matrix2 Dpi_pn;
-      const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
-
-      if (Dcamera) {
-        Dcamera->resize(2, this->dim());
-        calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols<6>());
-        Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib
-      }
-      if (Dpoint) {
-        Dpoint->resize(2, 3);
-        calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
-      }
-      return pi;
-    }
-  }
-
   /// backproject a 2-dimensional point to a 3-dimensional point at given depth
   inline Point3 backproject(const Point2& p, double depth) const {
     const Point2 pn = K_.calibrate(p);
@@ -520,71 +353,64 @@ public:
   /**
    * Calculate range to a landmark
    * @param point 3D location of landmark
-   * @param Dpose the optionally computed Jacobian with respect to pose
+   * @param Dcamera the optionally computed Jacobian with respect to pose
    * @param Dpoint the optionally computed Jacobian with respect to the landmark
    * @return range (double)
    */
   double range(
       const Point3& point, //
-      boost::optional<Matrix&> Dpose = boost::none,
+      OptionalJacobian<1, DimC> Dcamera = boost::none,
-      boost::optional<Matrix&> Dpoint = boost::none) const {
+      OptionalJacobian<1, 3> Dpoint = boost::none) const {
-    double result = pose_.range(point, Dpose, Dpoint);
+    Matrix16 Dpose_;
-    if (Dpose) {
+    double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint);
-      // Add columns of zeros to Jacobian for calibration
+    if (Dcamera)
-      Matrix& H1r(*Dpose);
+      *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
-      H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim());
-      H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim());
-    }
     return result;
   }
 
   /**
    * Calculate range to another pose
    * @param pose Other SO(3) pose
-   * @param Dpose the optionally computed Jacobian with respect to pose
+   * @param Dcamera the optionally computed Jacobian with respect to pose
    * @param Dpose2 the optionally computed Jacobian with respect to the other pose
    * @return range (double)
    */
   double range(
       const Pose3& pose, //
-      boost::optional<Matrix&> Dpose = boost::none,
+      OptionalJacobian<1, DimC> Dcamera = boost::none,
-      boost::optional<Matrix&> Dpose2 = boost::none) const {
+      OptionalJacobian<1, 6> Dpose = boost::none) const {
-    double result = pose_.range(pose, Dpose, Dpose2);
+    Matrix16 Dpose_;
-    if (Dpose) {
+    double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose);
-      // Add columns of zeros to Jacobian for calibration
+    if (Dcamera)
-      Matrix& H1r(*Dpose);
+      *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
-      H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim());
-      H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim());
-    }
     return result;
   }
 
   /**
    * Calculate range to another camera
    * @param camera Other camera
-   * @param Dpose the optionally computed Jacobian with respect to pose
+   * @param Dcamera the optionally computed Jacobian with respect to pose
    * @param Dother the optionally computed Jacobian with respect to the other camera
    * @return range (double)
    */
   template<class CalibrationB>
   double range(
       const PinholeCamera<CalibrationB>& camera, //
-      boost::optional<Matrix&> Dpose = boost::none,
+      OptionalJacobian<1, DimC> Dcamera = boost::none,
-      boost::optional<Matrix&> Dother = boost::none) const {
+//      OptionalJacobian<1, 6 + traits::dimension<CalibrationB>::value> Dother =
-    double result = pose_.range(camera.pose_, Dpose, Dother);
+       boost::optional<Matrix&> Dother =
-    if (Dpose) {
+          boost::none) const {
-      // Add columns of zeros to Jacobian for calibration
+    Matrix16 Dcamera_, Dother_;
-      Matrix& H1r(*Dpose);
+    double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0,
-      H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim());
+        Dother ? &Dother_ : 0);
-      H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim());
+    if (Dcamera) {
+       Dcamera->resize(1, 6 + DimK);
+      *Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
     }
     if (Dother) {
-      // Add columns of zeros to Jacobian for calibration
+      Dother->resize(1, 6+traits::dimension<CalibrationB>::value);
-      Matrix& H2r(*Dother);
+      Dother->setZero();
-      H2r.conservativeResize(Eigen::NoChange,
+      Dother->block(0, 0, 1, 6) = Dother_;
-          camera.pose().dim() + camera.calibration().dim());
-      H2r.block(0, camera.pose().dim(), 1, camera.calibration().dim()) =
-          Matrix::Zero(1, camera.calibration().dim());
     }
     return result;
   }
@@ -592,15 +418,15 @@ public:
   /**
    * Calculate range to another camera
    * @param camera Other camera
-   * @param Dpose the optionally computed Jacobian with respect to pose
+   * @param Dcamera the optionally computed Jacobian with respect to pose
    * @param Dother the optionally computed Jacobian with respect to the other camera
    * @return range (double)
    */
   double range(
       const CalibratedCamera& camera, //
-      boost::optional<Matrix&> Dpose = boost::none,
+      OptionalJacobian<1, DimC> Dcamera = boost::none,
-      boost::optional<Matrix&> Dother = boost::none) const {
+      OptionalJacobian<1, 6> Dother = boost::none) const {
-    return pose_.range(camera.pose_, Dpose, Dother);
+    return range(camera.pose(), Dcamera, Dother);
   }
 
 private:
@@ -663,7 +489,7 @@ private:
 namespace traits {
 
 template<typename Calibration>
-struct GTSAM_EXPORT is_manifold<PinholeCamera<Calibration> > : public boost::true_type{
+struct GTSAM_EXPORT is_manifold<PinholeCamera<Calibration> > : public boost::true_type {
 };
 
 template<typename Calibration>

gtsam/geometry/Point2.cpp

@@ -38,15 +38,9 @@ bool Point2::equals(const Point2& q, double tol) const {
 }
 
 /* ************************************************************************* */
-double Point2::norm() const {
+double Point2::norm(OptionalJacobian<1,2> H) const {
-  return sqrt(x_ * x_ + y_ * y_);
+  double r = sqrt(x_ * x_ + y_ * y_);
-}
-
-/* ************************************************************************* */
-double Point2::norm(boost::optional<Matrix&> H) const {
-  double r = norm();
   if (H) {
-    H->resize(1,2);
     if (fabs(r) > 1e-10)
       *H << x_ / r, y_ / r;
     else
@@ -56,12 +50,11 @@ double Point2::norm(boost::optional<Matrix&> H) const {
 }
 
 /* ************************************************************************* */
-static const Matrix I2 = eye(2);
+double Point2::distance(const Point2& point, OptionalJacobian<1,2> H1,
-double Point2::distance(const Point2& point, boost::optional<Matrix&> H1,
+    OptionalJacobian<1,2> H2) const {
-    boost::optional<Matrix&> H2) const {
   Point2 d = point - *this;
   if (H1 || H2) {
-    Matrix H;
+    Matrix12 H;
     double r = d.norm(H);
     if (H1) *H1 = -H;
     if (H2) *H2 =  H;

gtsam/geometry/Point2.h

@@ -20,7 +20,7 @@
 #include <boost/serialization/nvp.hpp>
 
 #include <gtsam/base/DerivedValue.h>
-#include <gtsam/base/Matrix.h>
+#include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/base/Lie.h>
 
 namespace gtsam {
@@ -125,10 +125,10 @@ public:
 
   /// "Compose", just adds the coordinates of two points. With optional derivatives
   inline Point2 compose(const Point2& q,
-      boost::optional<Matrix&> H1=boost::none,
+      OptionalJacobian<2,2> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const {
+      OptionalJacobian<2,2> H2=boost::none) const {
-    if(H1) *H1 = eye(2);
+    if(H1) *H1 = I_2x2;
-    if(H2) *H2 = eye(2);
+    if(H2) *H2 = I_2x2;
     return *this + q;
   }
 
@@ -137,10 +137,10 @@ public:
 
   /// "Between", subtracts point coordinates. between(p,q) == compose(inverse(p),q)
   inline Point2 between(const Point2& q,
-      boost::optional<Matrix&> H1=boost::none,
+      OptionalJacobian<2,2> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const {
+      OptionalJacobian<2,2> H2=boost::none) const {
-    if(H1) *H1 = -eye(2);
+    if(H1) *H1 = -I_2x2;
-    if(H2) *H2 = eye(2);
+    if(H2) *H2 = I_2x2;
     return q - (*this);
   }
 
@@ -171,7 +171,7 @@ public:
   static inline Point2 Expmap(const Vector& v) { return Point2(v); }
 
   /// Log map around identity - just return the Point2 as a vector
-  static inline Vector Logmap(const Point2& dp) { return (Vector(2) << dp.x(), dp.y()).finished(); }
+  static inline Vector2 Logmap(const Point2& dp) { return Vector2(dp.x(), dp.y()); }
 
   /// @}
   /// @name Vector Space
@@ -180,15 +180,12 @@ public:
   /** creates a unit vector */
   Point2 unit() const { return *this/norm(); }
 
-  /** norm of point */
-  double norm() const;
-
   /** norm of point, with derivative */
-  double norm(boost::optional<Matrix&> H) const;
+  double norm(OptionalJacobian<1,2> H = boost::none) const;
 
   /** distance between two points */
-  double distance(const Point2& p2, boost::optional<Matrix&> H1 = boost::none,
+  double distance(const Point2& p2, OptionalJacobian<1,2> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none) const;
+      OptionalJacobian<1,2> H2 = boost::none) const;
 
   /** @deprecated The following function has been deprecated, use distance above */
   inline double dist(const Point2& p2) const {

gtsam/geometry/Point3.cpp

@@ -63,22 +63,18 @@ Point3 Point3::operator/(double s) const {
 }
 
 /* ************************************************************************* */
-Point3 Point3::add(const Point3 &q, boost::optional<Matrix&> H1,
+Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
-    boost::optional<Matrix&> H2) const {
+    OptionalJacobian<3,3> H2) const {
-  if (H1)
+  if (H1) *H1 = I_3x3;
-    *H1 = eye(3, 3);
+  if (H2) *H2 = I_3x3;
-  if (H2)
-    *H2 = eye(3, 3);
   return *this + q;
 }
 
 /* ************************************************************************* */
-Point3 Point3::sub(const Point3 &q, boost::optional<Matrix&> H1,
+Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
-    boost::optional<Matrix&> H2) const {
+    OptionalJacobian<3,3> H2) const {
-  if (H1)
+  if (H1) *H1 = I_3x3;
-    *H1 = eye(3, 3);
+  if (H2) *H2 = I_3x3;
-  if (H2)
-    *H2 = -eye(3, 3);
   return *this - q;
 }
 
@@ -94,26 +90,8 @@ double Point3::dot(const Point3 &q) const {
 }
 
 /* ************************************************************************* */
-double Point3::norm() const {
+double Point3::norm(OptionalJacobian<1,3> H) const {
-  return sqrt(x_ * x_ + y_ * y_ + z_ * z_);
+  double r = sqrt(x_ * x_ + y_ * y_ + z_ * z_);
-}
-
-/* ************************************************************************* */
-double Point3::norm(boost::optional<Matrix&> H) const {
-  double r = norm();
-  if (H) {
-    H->resize(1,3);
-    if (fabs(r) > 1e-10)
-      *H << x_ / r, y_ / r, z_ / r;
-    else
-      *H << 1, 1, 1; // really infinity, why 1 ?
-  }
-  return r;
-}
-
-/* ************************************************************************* */
-double Point3::norm(boost::optional<Eigen::Matrix<double,1,3>&> H) const {
-  double r = norm();
   if (H) {
     if (fabs(r) > 1e-10)
       *H << x_ / r, y_ / r, z_ / r;
@@ -124,13 +102,12 @@ double Point3::norm(boost::optional<Eigen::Matrix<double,1,3>&> H) const {
 }
 
 /* ************************************************************************* */
-Point3 Point3::normalize(boost::optional<Matrix&> H) const {
+Point3 Point3::normalize(OptionalJacobian<3,3> H) const {
   Point3 normalized = *this / norm();
   if (H) {
     // 3*3 Derivative
     double x2 = x_ * x_, y2 = y_ * y_, z2 = z_ * z_;
     double xy = x_ * y_, xz = x_ * z_, yz = y_ * z_;
-    H->resize(3, 3);
     *H << y2 + z2, -xy, -xz, /**/-xy, x2 + z2, -yz, /**/-xz, -yz, x2 + y2;
     *H /= pow(x2 + y2 + z2, 1.5);
   }

gtsam/geometry/Point3.h

@@ -21,9 +21,9 @@
 
 #pragma once
 
-#include <gtsam/base/Matrix.h>
 #include <gtsam/base/DerivedValue.h>
 #include <gtsam/base/Lie.h>
+#include <gtsam/base/OptionalJacobian.h>
 
 #include <boost/serialization/nvp.hpp>
 
@@ -93,10 +93,10 @@ namespace gtsam {
 
     /// "Compose" - just adds coordinates of two points
     inline Point3 compose(const Point3& p2,
-        boost::optional<Matrix&> H1=boost::none,
+        OptionalJacobian<3,3> H1=boost::none,
-        boost::optional<Matrix&> H2=boost::none) const {
+        OptionalJacobian<3,3> H2=boost::none) const {
-      if (H1) *H1 = eye(3);
+      if (H1) *H1 << I_3x3;
-      if (H2) *H2 = eye(3);
+      if (H2) *H2 << I_3x3;
       return *this + p2;
     }
 
@@ -105,10 +105,10 @@ namespace gtsam {
 
     /** Between using the default implementation */
     inline Point3 between(const Point3& p2,
-        boost::optional<Matrix&> H1=boost::none,
+        OptionalJacobian<3,3> H1=boost::none,
-        boost::optional<Matrix&> H2=boost::none) const {
+        OptionalJacobian<3,3> H2=boost::none) const {
-      if(H1) *H1 = -eye(3);
+      if(H1) *H1 = -I_3x3;
-      if(H2) *H2 = eye(3);
+      if(H2) *H2 = I_3x3;
       return p2 - *this;
     }
 
@@ -142,13 +142,13 @@ namespace gtsam {
     static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); }
 
     /// Left-trivialized derivative of the exponential map
-    static Matrix dexpL(const Vector& v) {
+    static Matrix3 dexpL(const Vector& v) {
-      return eye(3);
+      return I_3x3;
     }
 
     /// Left-trivialized derivative inverse of the exponential map
-    static Matrix dexpInvL(const Vector& v) {
+    static Matrix3 dexpInvL(const Vector& v) {
-      return eye(3);
+      return I_3x3;
     }
 
     /// @}
@@ -163,14 +163,16 @@ namespace gtsam {
 
     /** distance between two points */
     inline double distance(const Point3& p2,
-        boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+        OptionalJacobian<1,3> H1 = boost::none, OptionalJacobian<1,3> H2 = boost::none) const {
       double d = (p2 - *this).norm();
       if (H1) {
-        *H1 = (Matrix(1, 3) << x_-p2.x(), y_-p2.y(), z_-p2.z()).finished()*(1./d);
+        *H1 << x_-p2.x(), y_-p2.y(), z_-p2.z();
+        *H1 = *H1 *(1./d);
       }
 
       if (H2) {
-        *H2 = (Matrix(1, 3) << -x_+p2.x(), -y_+p2.y(), -z_+p2.z()).finished()*(1./d);
+        *H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z();
+        *H2 << *H2 *(1./d);
       }
       return d;
     }
@@ -180,17 +182,11 @@ namespace gtsam {
       return (p2 - *this).norm();
     }
 
-    /** Distance of the point from the origin */
-    double norm() const;
-
     /** Distance of the point from the origin, with Jacobian */
-    double norm(boost::optional<Matrix&> H) const;
+    double norm(OptionalJacobian<1,3> H = boost::none) const;
-
-    /** Distance of the point from the origin, with Jacobian */
-    double norm(boost::optional<Eigen::Matrix<double,1,3>&> H) const;
 
     /** normalize, with optional Jacobian */
-    Point3 normalize(boost::optional<Matrix&> H = boost::none) const;
+    Point3 normalize(OptionalJacobian<3, 3> H = boost::none) const;
 
     /** cross product @return this x q */
     Point3 cross(const Point3 &q) const;
@@ -219,11 +215,11 @@ namespace gtsam {
 
     /** add two points, add(this,q) is same as this + q */
     Point3 add (const Point3 &q,
-          boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
+          OptionalJacobian<3, 3> H1=boost::none, OptionalJacobian<3, 3> H2=boost::none) const;
 
     /** subtract two points, sub(this,q) is same as this - q */
     Point3 sub (const Point3 &q,
-          boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
+          OptionalJacobian<3,3> H1=boost::none, OptionalJacobian<3,3> H2=boost::none) const;
 
     /// @}
 

gtsam/geometry/Pose2.cpp

@@ -33,15 +33,20 @@ INSTANTIATE_LIE(Pose2);
 /** instantiate concept checks */
 GTSAM_CONCEPT_POSE_INST(Pose2);
 
-static const Matrix I3 = eye(3), Z12 = zeros(1,2);
 static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));
 
 /* ************************************************************************* */
-Matrix Pose2::matrix() const {
+Matrix3 Pose2::matrix() const {
-  Matrix R = r_.matrix();
+  Matrix2 R = r_.matrix();
-  R = stack(2, &R, &Z12);
+  Matrix32 R0;
-  Matrix T = (Matrix(3, 1) <<  t_.x(), t_.y(), 1.0).finished();
+  R0.block<2,2>(0,0) = R;
-  return collect(2, &R, &T);
+  R0.block<1,2>(2,0).setZero();
+  Matrix31 T;
+  T <<  t_.x(), t_.y(), 1.0;
+  Matrix3 RT_;
+  RT_.block<3,2>(0,0) = R0;
+  RT_.block<3,1>(0,2) = T;
+  return RT_;
 }
 
 /* ************************************************************************* */
@@ -70,18 +75,18 @@ Pose2 Pose2::Expmap(const Vector& xi) {
 }
 
 /* ************************************************************************* */
-Vector Pose2::Logmap(const Pose2& p) {
+Vector3 Pose2::Logmap(const Pose2& p) {
   const Rot2& R = p.r();
   const Point2& t = p.t();
   double w = R.theta();
   if (std::abs(w) < 1e-10)
-    return (Vector(3) << t.x(), t.y(), w).finished();
+    return Vector3(t.x(), t.y(), w);
   else {
     double c_1 = R.c()-1.0, s = R.s();
     double det = c_1*c_1 + s*s;
     Point2 p = R_PI_2 * (R.unrotate(t) - t);
     Point2 v = (w/det) * p;
-    return (Vector(3) << v.x(), v.y(), w).finished();
+    return Vector3(v.x(), v.y(), w);
   }
 }
 
@@ -96,117 +101,76 @@ Pose2 Pose2::retract(const Vector& v) const {
 }
 
 /* ************************************************************************* */
-Vector Pose2::localCoordinates(const Pose2& p2) const {
+Vector3 Pose2::localCoordinates(const Pose2& p2) const {
 #ifdef SLOW_BUT_CORRECT_EXPMAP
   return Logmap(between(p2));
 #else
   Pose2 r = between(p2);
-  return (Vector(3) << r.x(), r.y(), r.theta()).finished();
+  return Vector3(r.x(), r.y(), r.theta());
 #endif
 }
 
 /* ************************************************************************* */
 // Calculate Adjoint map
 // Ad_pose is 3*3 matrix that when applied to twist xi, returns Ad_pose(xi)
-Matrix Pose2::AdjointMap() const {
+Matrix3 Pose2::AdjointMap() const {
   double c = r_.c(), s = r_.s(), x = t_.x(), y = t_.y();
-  return (Matrix(3, 3) <<
+  Matrix3 rvalue;
+  rvalue <<
       c,  -s,   y,
       s,   c,  -x,
-      0.0, 0.0, 1.0
+      0.0, 0.0, 1.0;
-  ).finished();
+  return rvalue;
 }
 
 /* ************************************************************************* */
-Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
+Pose2 Pose2::inverse(OptionalJacobian<3,3> H1) const {
   if (H1) *H1 = -AdjointMap();
   return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
 }
 
-/* ************************************************************************* */
-// see doc/math.lyx, SE(2) section
-Point2 Pose2::transform_to(const Point2& point) const {
-  Point2 d = point - t_;
-  return r_.unrotate(d);
-}
-
 /* ************************************************************************* */
 // see doc/math.lyx, SE(2) section
 Point2 Pose2::transform_to(const Point2& point,
-    boost::optional<Matrix23&> H1, boost::optional<Matrix2&> H2) const {
+    OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const {
   Point2 d = point - t_;
   Point2 q = r_.unrotate(d);
   if (!H1 && !H2) return q;
   if (H1) *H1 <<
       -1.0, 0.0,  q.y(),
       0.0, -1.0, -q.x();
-  if (H2) *H2 = r_.transpose();
+  if (H2) *H2 << r_.transpose();
   return q;
 }
 
 /* ************************************************************************* */
 // see doc/math.lyx, SE(2) section
-Point2 Pose2::transform_to(const Point2& point,
+Pose2 Pose2::compose(const Pose2& p2, OptionalJacobian<3,3> H1,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+    OptionalJacobian<3,3> H2) const {
-  Point2 d = point - t_;
-  Point2 q = r_.unrotate(d);
-  if (!H1 && !H2) return q;
-  if (H1) *H1 = (Matrix(2, 3) <<
-      -1.0, 0.0,  q.y(),
-      0.0, -1.0, -q.x()).finished();
-  if (H2) *H2 = r_.transpose();
-  return q;
-}
-
-/* ************************************************************************* */
-// see doc/math.lyx, SE(2) section
-Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H2) const {
   // TODO: inline and reuse?
   if(H1) *H1 = p2.inverse().AdjointMap();
-  if(H2) *H2 = I3;
+  if(H2) *H2 = I_3x3;
   return (*this)*p2;
 }
 
 /* ************************************************************************* */
 // see doc/math.lyx, SE(2) section
 Point2 Pose2::transform_from(const Point2& p,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+    OptionalJacobian<2, 3> H1, OptionalJacobian<2, 2> H2) const {
   const Point2 q = r_ * p;
   if (H1 || H2) {
-    const Matrix R = r_.matrix();
+    const Matrix2 R = r_.matrix();
-    const Matrix Drotate1 = (Matrix(2, 1) <<  -q.y(), q.x()).finished();
+    Matrix21 Drotate1;
-    if (H1) *H1 = collect(2, &R, &Drotate1); // [R R_{pi/2}q]
+    Drotate1 <<  -q.y(), q.x();
-    if (H2) *H2 = R;                         // R
+    if (H1) *H1 << R, Drotate1;
+    if (H2) *H2 = R; // R
   }
   return q + t_;
 }
 
 /* ************************************************************************* */
-Pose2 Pose2::between(const Pose2& p2) const {
+Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1,
-  // get cosines and sines from rotation matrices
+    OptionalJacobian<3,3> H2) const {
-  const Rot2& R1 = r_, R2 = p2.r();
-  double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
-
-  // Assert that R1 and R2 are normalized
-  assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
-
-  // Calculate delta rotation = between(R1,R2)
-  double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
-  Rot2 R(Rot2::atan2(s,c)); // normalizes
-
-  // Calculate delta translation = unrotate(R1, dt);
-  Point2 dt = p2.t() - t_;
-  double x = dt.x(), y = dt.y();
-  // t = R1' * (t2-t1)
-  Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
-
-  return Pose2(R,t);
-}
-
-/* ************************************************************************* */
-Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix3&> H1,
-    boost::optional<Matrix3&> H2) const {
   // get cosines and sines from rotation matrices
   const Rot2& R1 = r_, R2 = p2.r();
   double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
@@ -233,97 +197,75 @@ Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix3&> H1,
         s,  -c,  dt2,
         0.0, 0.0,-1.0;
   }
-  if (H2) *H2 = I3;
+  if (H2) *H2 = I_3x3;
-
-  return Pose2(R,t);
-}
-
-/* ************************************************************************* */
-Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H2) const {
-  // get cosines and sines from rotation matrices
-  const Rot2& R1 = r_, R2 = p2.r();
-  double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
-
-  // Assert that R1 and R2 are normalized
-  assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
-
-  // Calculate delta rotation = between(R1,R2)
-  double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
-  Rot2 R(Rot2::atan2(s,c)); // normalizes
-
-  // Calculate delta translation = unrotate(R1, dt);
-  Point2 dt = p2.t() - t_;
-  double x = dt.x(), y = dt.y();
-  // t = R1' * (t2-t1)
-  Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
-
-  // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
-  if (H1) {
-    double dt1 = -s2 * x + c2 * y;
-    double dt2 = -c2 * x - s2 * y;
-    *H1 = (Matrix(3, 3) <<
-        -c,  -s,  dt1,
-        s,  -c,  dt2,
-        0.0, 0.0,-1.0).finished();
-  }
-  if (H2) *H2 = I3;
 
   return Pose2(R,t);
 }
 
 /* ************************************************************************* */
 Rot2 Pose2::bearing(const Point2& point,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+    OptionalJacobian<1, 3> H1, OptionalJacobian<1, 2> H2) const {
-  Point2 d = transform_to(point, H1, H2);
+  // make temporary matrices
+  Matrix23 D1; Matrix2 D2;
+  Point2 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0); // uses pointer version
   if (!H1 && !H2) return Rot2::relativeBearing(d);
-  Matrix D_result_d;
+  Matrix12 D_result_d;
   Rot2 result = Rot2::relativeBearing(d, D_result_d);
-  if (H1) *H1 = D_result_d * (*H1);
+  if (H1) *H1 = D_result_d * (D1);
-  if (H2) *H2 = D_result_d * (*H2);
+  if (H2) *H2 = D_result_d * (D2);
   return result;
 }
 
 /* ************************************************************************* */
-Rot2 Pose2::bearing(const Pose2& point,
+Rot2 Pose2::bearing(const Pose2& pose,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+    OptionalJacobian<1, 3> H1, OptionalJacobian<1, 3> H2) const {
-  Rot2 result = bearing(point.t(), H1, H2);
+  Matrix12 D2;
+  Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0);
   if (H2) {
-    Matrix H2_ = *H2 * point.r().matrix();
+    Matrix12 H2_ = D2 * pose.r().matrix();
-    *H2 = zeros(1, 3);
+    *H2 << H2_, Z_1x1;
-    insertSub(*H2, H2_, 0, 0);
   }
   return result;
 }
-
 /* ************************************************************************* */
 double Pose2::range(const Point2& point,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+    OptionalJacobian<1,3> H1, OptionalJacobian<1,2> H2) const {
   Point2 d = point - t_;
   if (!H1 && !H2) return d.norm();
-  Matrix H;
+  Matrix12 H;
   double r = d.norm(H);
-  if (H1) *H1 = H * (Matrix(2, 3) <<
+  if (H1) {
-      -r_.c(),  r_.s(),  0.0,
+      Matrix23 H1_;
-      -r_.s(), -r_.c(),  0.0).finished();
+      H1_ << -r_.c(),  r_.s(),  0.0,
+              -r_.s(), -r_.c(),  0.0;
+      *H1 = H * H1_;
+  }
   if (H2) *H2 = H;
   return r;
 }
 
 /* ************************************************************************* */
-double Pose2::range(const Pose2& pose2,
+double Pose2::range(const Pose2& pose,
-    boost::optional<Matrix&> H1,
+    OptionalJacobian<1,3> H1,
-    boost::optional<Matrix&> H2) const {
+    OptionalJacobian<1,3> H2) const {
-  Point2 d = pose2.t() - t_;
+  Point2 d = pose.t() - t_;
   if (!H1 && !H2) return d.norm();
-  Matrix H;
+  Matrix12 H;
   double r = d.norm(H);
-  if (H1) *H1 = H * (Matrix(2, 3) <<
+  if (H1) {
+      Matrix23 H1_;
+      H1_ <<
       -r_.c(),  r_.s(),  0.0,
-      -r_.s(), -r_.c(),  0.0).finished();
+      -r_.s(), -r_.c(),  0.0;
-  if (H2) *H2 = H * (Matrix(2, 3) <<
+      *H1 = H * H1_;
-      pose2.r_.c(), -pose2.r_.s(),  0.0,
+  }
-      pose2.r_.s(),  pose2.r_.c(),  0.0).finished();
+  if (H2) {
+      Matrix23 H2_;
+      H2_ <<
+      pose.r_.c(), -pose.r_.s(),  0.0,
+      pose.r_.s(),  pose.r_.c(),  0.0;
+      *H2 = H * H2_;
+  }
   return r;
 }
 

gtsam/geometry/Pose2.h

@@ -107,12 +107,12 @@ public:
   inline static Pose2 identity() { return Pose2(); }
 
   /// inverse transformation with derivatives
-  Pose2 inverse(boost::optional<Matrix&> H1=boost::none) const;
+  Pose2 inverse(OptionalJacobian<3, 3> H1=boost::none) const;
 
   /// compose this transformation onto another (first *this and then p2)
   Pose2 compose(const Pose2& p2,
-      boost::optional<Matrix&> H1 = boost::none,
+      OptionalJacobian<3, 3> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none) const;
+      OptionalJacobian<3, 3> H2 = boost::none) const;
 
   /// compose syntactic sugar
   inline Pose2 operator*(const Pose2& p2) const {
@@ -122,19 +122,8 @@ public:
   /**
    * Return relative pose between p1 and p2, in p1 coordinate frame
    */
-  Pose2 between(const Pose2& p2) const;
+  Pose2 between(const Pose2& p2, OptionalJacobian<3,3> H1 = boost::none,
-
+      OptionalJacobian<3,3> H = boost::none) const;
-  /**
-   * Return relative pose between p1 and p2, in p1 coordinate frame
-   */
-  Pose2 between(const Pose2& p2, boost::optional<Matrix3&> H1,
-      boost::optional<Matrix3&> H2) const;
-
-  /**
-   * Return relative pose between p1 and p2, in p1 coordinate frame
-   */
-  Pose2 between(const Pose2& p2, boost::optional<Matrix&> H1,
-      boost::optional<Matrix&> H2) const;
 
   /// @}
   /// @name Manifold
@@ -150,7 +139,7 @@ public:
   Pose2 retract(const Vector& v) const;
 
   /// Local 3D coordinates \f$ [T_x,T_y,\theta] \f$ of Pose2 manifold neighborhood around current pose
-  Vector localCoordinates(const Pose2& p2) const;
+  Vector3 localCoordinates(const Pose2& p2) const;
 
   /// @}
   /// @name Lie Group
@@ -160,13 +149,13 @@ public:
   static Pose2 Expmap(const Vector& xi);
 
   ///Log map at identity - return the canonical coordinates \f$ [T_x,T_y,\theta] \f$ of this rotation
-  static Vector Logmap(const Pose2& p);
+  static Vector3 Logmap(const Pose2& p);
 
   /**
    * Calculate Adjoint map
    * Ad_pose is 3*3 matrix that when applied to twist xi \f$ [T_x,T_y,\theta] \f$, returns Ad_pose(xi)
    */
-  Matrix AdjointMap() const;
+  Matrix3 AdjointMap() const;
   inline Vector Adjoint(const Vector& xi) const {
     assert(xi.size() == 3);
     return AdjointMap()*xi;
@@ -179,34 +168,27 @@ public:
    *  v (vx,vy) = 2D velocity
    * @return xihat, 3*3 element of Lie algebra that can be exponentiated
    */
-  static inline Matrix wedge(double vx, double vy, double w) {
+  static inline Matrix3 wedge(double vx, double vy, double w) {
-    return (Matrix(3,3) <<
+    Matrix3 m;
-        0.,-w,  vx,
+    m << 0.,-w,  vx,
-        w,  0., vy,
+         w,  0., vy,
-        0., 0.,  0.).finished();
+         0., 0.,  0.;
+    return m;
   }
 
   /// @}
   /// @name Group Action on Point2
   /// @{
 
-  /** Return point coordinates in pose coordinate frame */
-  Point2 transform_to(const Point2& point) const;
-
   /** Return point coordinates in pose coordinate frame */
   Point2 transform_to(const Point2& point,
-      boost::optional<Matrix23&> H1,
+      OptionalJacobian<2, 3> H1 = boost::none,
-      boost::optional<Matrix2&> H2) const;
+      OptionalJacobian<2, 2> H2 = boost::none) const;
-
-  /** Return point coordinates in pose coordinate frame */
-  Point2 transform_to(const Point2& point,
-      boost::optional<Matrix&> H1,
-      boost::optional<Matrix&> H2) const;
 
   /** Return point coordinates in global frame */
   Point2 transform_from(const Point2& point,
-      boost::optional<Matrix&> H1=boost::none,
+      OptionalJacobian<2, 3> H1 = boost::none,
-      boost::optional<Matrix&> H2=boost::none) const;
+      OptionalJacobian<2, 2> H2 = boost::none) const;
 
   /** syntactic sugar for transform_from */
   inline Point2 operator*(const Point2& point) const { return transform_from(point);}
@@ -237,7 +219,7 @@ public:
   inline const Rot2&   rotation() const { return r_; }
 
   //// return transformation matrix
-  Matrix matrix() const;
+  Matrix3 matrix() const;
 
   /**
    * Calculate bearing to a landmark
@@ -245,17 +227,15 @@ public:
    * @return 2D rotation \f$ \in SO(2) \f$
    */
   Rot2 bearing(const Point2& point,
-      boost::optional<Matrix&> H1=boost::none,
+               OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 2> H2=boost::none) const;
-      boost::optional<Matrix&> H2=boost::none) const;
 
   /**
    * Calculate bearing to another pose
    * @param point SO(2) location of other pose
    * @return 2D rotation \f$ \in SO(2) \f$
    */
-  Rot2 bearing(const Pose2& point,
+  Rot2 bearing(const Pose2& pose,
-      boost::optional<Matrix&> H1=boost::none,
+               OptionalJacobian<1, 3> H1=boost::none, OptionalJacobian<1, 3> H2=boost::none) const;
-      boost::optional<Matrix&> H2=boost::none) const;
 
   /**
    * Calculate range to a landmark
@@ -263,8 +243,8 @@ public:
    * @return range (double)
    */
   double range(const Point2& point,
-      boost::optional<Matrix&> H1=boost::none,
+      OptionalJacobian<1, 3> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const;
+      OptionalJacobian<1, 2> H2=boost::none) const;
 
   /**
    * Calculate range to another pose
@@ -272,8 +252,8 @@ public:
    * @return range (double)
    */
   double range(const Pose2& point,
-      boost::optional<Matrix&> H1=boost::none,
+      OptionalJacobian<1, 3> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const;
+      OptionalJacobian<1, 3> H2=boost::none) const;
 
   /// @}
   /// @name Advanced Interface

gtsam/geometry/Pose3.cpp

@@ -32,9 +32,6 @@ INSTANTIATE_LIE(Pose3);
 /** instantiate concept checks */
 GTSAM_CONCEPT_POSE_INST(Pose3);
 
-static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3 = -I3;
-static const Matrix6 I6 = eye(6);
-
 /* ************************************************************************* */
 Pose3::Pose3(const Pose2& pose2) :
     R_(Rot3::rodriguez(0, 0, pose2.theta())), t_(
@@ -50,59 +47,62 @@ Matrix6 Pose3::AdjointMap() const {
   const Vector3 t = t_.vector();
   Matrix3 A = skewSymmetric(t) * R;
   Matrix6 adj;
-  adj << R, Z3, A, R;
+  adj << R, Z_3x3, A, R;
   return adj;
 }
 
 /* ************************************************************************* */
-Matrix6 Pose3::adjointMap(const Vector& xi) {
+Matrix6 Pose3::adjointMap(const Vector6& xi) {
   Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
   Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
   Matrix6 adj;
-  adj << w_hat, Z3, v_hat, w_hat;
+  adj << w_hat, Z_3x3, v_hat, w_hat;
 
   return adj;
 }
 
 /* ************************************************************************* */
-Vector Pose3::adjoint(const Vector& xi, const Vector& y,
+Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
-    boost::optional<Matrix&> H) {
+    OptionalJacobian<6,6> H) {
   if (H) {
-    *H = zeros(6, 6);
+    H->setZero();
     for (int i = 0; i < 6; ++i) {
-      Vector dxi = zero(6);
+      Vector6 dxi;
+      dxi.setZero();
       dxi(i) = 1.0;
-      Matrix Gi = adjointMap(dxi);
+      Matrix6 Gi = adjointMap(dxi);
-      (*H).col(i) = Gi * y;
+      H->col(i) = Gi * y;
     }
   }
   return adjointMap(xi) * y;
 }
 
 /* ************************************************************************* */
-Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y,
+Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
-    boost::optional<Matrix&> H) {
+    OptionalJacobian<6,6> H) {
   if (H) {
-    *H = zeros(6, 6);
+    H->setZero();
     for (int i = 0; i < 6; ++i) {
-      Vector dxi = zero(6);
+      Vector6 dxi;
+      dxi.setZero();
       dxi(i) = 1.0;
-      Matrix GTi = adjointMap(dxi).transpose();
+      Matrix6 GTi = adjointMap(dxi).transpose();
-      (*H).col(i) = GTi * y;
+      H->col(i) = GTi * y;
     }
   }
-  Matrix adjT = adjointMap(xi).transpose();
   return adjointMap(xi).transpose() * y;
 }
 
 /* ************************************************************************* */
-Matrix6 Pose3::dExpInv_exp(const Vector& xi) {
+Matrix6 Pose3::dExpInv_exp(const Vector6& xi) {
   // Bernoulli numbers, from Wikipedia
   static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
       0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished();
   static const int N = 5; // order of approximation
-  Matrix res = I6;
+  Matrix6 res;
-  Matrix6 ad_i = I6;
+  res = I_6x6;
+  Matrix6 ad_i;
+  ad_i = I_6x6;
   Matrix6 ad_xi = adjointMap(xi);
   double fac = 1.0;
   for (int i = 1; i < N; ++i) {
@@ -225,7 +225,7 @@ Vector6 Pose3::localCoordinates(const Pose3& T,
 Matrix4 Pose3::matrix() const {
   const Matrix3 R = R_.matrix();
   const Vector3 T = t_.vector();
-  Eigen::Matrix<double, 1, 4> A14;
+  Matrix14 A14;
   A14 << 0.0, 0.0, 0.0, 1.0;
   Matrix4 mat;
   mat << R, T, A14;
@@ -240,12 +240,11 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
 }
 
 /* ************************************************************************* */
-Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
+Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
-    boost::optional<Matrix&> Dpoint) const {
+    OptionalJacobian<3,3> Dpoint) const {
   if (Dpose) {
     const Matrix3 R = R_.matrix();
     Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
-    Dpose->resize(3, 6);
     (*Dpose) << DR, R;
   }
   if (Dpoint)
@@ -254,13 +253,8 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
 }
 
 /* ************************************************************************* */
-Point3 Pose3::transform_to(const Point3& p) const {
+Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
-  return R_.unrotate(p - t_);
+    OptionalJacobian<3,3> Dpoint) const {
-}
-
-/* ************************************************************************* */
-Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
-    boost::optional<Matrix3&> Dpoint) const {
   // Only get transpose once, to avoid multiple allocations,
   // as well as multiple conversions in the Quaternion case
   const Matrix3 Rt = R_.transpose();
@@ -278,77 +272,57 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
 }
 
 /* ************************************************************************* */
-Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
+Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1,
-    boost::optional<Matrix&> Dpoint) const {
+    OptionalJacobian<6,6> H2) const {
-  const Matrix3 Rt = R_.transpose();
+  if (H1) *H1 = p2.inverse().AdjointMap();
-  const Point3 q(Rt*(p - t_).vector());
+  if (H2) *H2 = I_6x6;
-  if (Dpose) {
-    const double wx = q.x(), wy = q.y(), wz = q.z();
-    Dpose->resize(3, 6);
-    (*Dpose) <<
-        0.0, -wz, +wy,-1.0, 0.0, 0.0,
-        +wz, 0.0, -wx, 0.0,-1.0, 0.0,
-        -wy, +wx, 0.0, 0.0, 0.0,-1.0;
-  }
-  if (Dpoint)
-    *Dpoint = Rt;
-  return q;
-}
-
-/* ************************************************************************* */
-Pose3 Pose3::compose(const Pose3& p2, boost::optional<Matrix&> H1,
-    boost::optional<Matrix&> H2) const {
-  if (H1)
-    *H1 = p2.inverse().AdjointMap();
-  if (H2)
-    *H2 = I6;
   return (*this) * p2;
 }
 
 /* ************************************************************************* */
-Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
+Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const {
-  if (H1)
+  if (H1) *H1 = -AdjointMap();
-    *H1 = -AdjointMap();
   Rot3 Rt = R_.inverse();
   return Pose3(Rt, Rt * (-t_));
 }
 
 /* ************************************************************************* */
 // between = compose(p2,inverse(p1));
-Pose3 Pose3::between(const Pose3& p2, boost::optional<Matrix&> H1,
+Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1,
-    boost::optional<Matrix&> H2) const {
+    OptionalJacobian<6,6> H2) const {
   Pose3 result = inverse() * p2;
-  if (H1)
+  if (H1) *H1 = -result.inverse().AdjointMap();
-    *H1 = -result.inverse().AdjointMap();
+  if (H2) *H2 = I_6x6;
-  if (H2)
-    *H2 = I6;
   return result;
 }
 
 /* ************************************************************************* */
-double Pose3::range(const Point3& point, boost::optional<Matrix&> H1,
+double Pose3::range(const Point3& point, OptionalJacobian<1, 6> H1,
-    boost::optional<Matrix&> H2) const {
+    OptionalJacobian<1, 3> H2) const {
   if (!H1 && !H2)
     return transform_to(point).norm();
-  Point3 d = transform_to(point, H1, H2);
+  else {
-  double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt(
+    Matrix36 D1;
-      d2);
+    Matrix3 D2;
-  Matrix D_result_d = (Matrix(1, 3) << x / n, y / n, z / n).finished();
+    Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
-  if (H1)
+    const double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z,
-    *H1 = D_result_d * (*H1);
+        n = sqrt(d2);
-  if (H2)
+    Matrix13 D_result_d;
-    *H2 = D_result_d * (*H2);
+    D_result_d << x / n, y / n, z / n;
-  return n;
+    if (H1) *H1 = D_result_d * D1;
+    if (H2) *H2 = D_result_d * D2;
+    return n;
+  }
 }
 
 /* ************************************************************************* */
-double Pose3::range(const Pose3& point, boost::optional<Matrix&> H1,
+double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1,
-    boost::optional<Matrix&> H2) const {
+    OptionalJacobian<1,6> H2) const {
-  double r = range(point.translation(), H1, H2);
+  Matrix13 D2;
+  double r = range(pose.translation(), H1, H2? &D2 : 0);
   if (H2) {
-    Matrix H2_ = *H2 * point.rotation().matrix();
+    Matrix13 H2_ = D2 * pose.rotation().matrix();
-    *H2 = zeros(1, 6);
+    *H2 << Matrix13::Zero(), H2_;
-    insertSub(*H2, H2_, 0, 3);
   }
   return r;
 }
@@ -370,7 +344,7 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
   cq *= f;
 
   // Add to form H matrix
-  Matrix H = zeros(3, 3);
+  Matrix3 H = Eigen::Matrix3d::Zero();
   BOOST_FOREACH(const Point3Pair& pair, pairs){
   Vector dp = pair.first.vector() - cp;
   Vector dq = pair.second.vector() - cq;
@@ -378,13 +352,13 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
 }
 
 // Compute SVD
-  Matrix U, V;
+  Matrix U,V;
   Vector S;
   svd(H, U, S, V);
 
   // Recover transform with correction from Eggert97machinevisionandapplications
-  Matrix UVtranspose = U * V.transpose();
+  Matrix3 UVtranspose = U * V.transpose();
-  Matrix detWeighting = eye(3, 3);
+  Matrix3 detWeighting = I_3x3;
   detWeighting(2, 2) = UVtranspose.determinant();
   Rot3 R(Matrix(V * detWeighting * U.transpose()));
   Point3 t = Point3(cq) - R * Point3(cp);

gtsam/geometry/Pose3.h

@@ -70,6 +70,12 @@ public:
       R_(R), t_(t) {
   }
 
+  /** Construct from R,t, where t \in vector3 */
+  Pose3(const Rot3& R, const Vector3& t) :
+      R_(R), t_(t) {
+  }
+
+
   /** Construct from Pose2 */
   explicit Pose3(const Pose2& pose2);
 
@@ -99,11 +105,11 @@ public:
   }
 
   /// inverse transformation with derivatives
-  Pose3 inverse(boost::optional<Matrix&> H1 = boost::none) const;
+  Pose3 inverse(OptionalJacobian<6, 6> H1 = boost::none) const;
 
   ///compose this transformation onto another (first *this and then p2)
-  Pose3 compose(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
+  Pose3 compose(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none) const;
+      OptionalJacobian<6, 6> H2 = boost::none) const;
 
   /// compose syntactic sugar
   Pose3 operator*(const Pose3& T) const {
@@ -114,8 +120,8 @@ public:
    * Return relative pose between p1 and p2, in p1 coordinate frame
    * as well as optionally the derivatives
    */
-  Pose3 between(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none,
+  Pose3 between(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none) const;
+      OptionalJacobian<6, 6> H2 = boost::none) const;
 
   /// @}
   /// @name Manifold
@@ -186,17 +192,17 @@ public:
      * and its inverse transpose in the discrete Euler Poincare' (DEP) operator.
      *
      */
-    static Matrix6 adjointMap(const Vector& xi);
+    static Matrix6 adjointMap(const Vector6 &xi);
 
     /**
      * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
      */
-    static Vector adjoint(const Vector& xi, const Vector& y, boost::optional<Matrix&> H = boost::none);
+    static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, OptionalJacobian<6,6> = boost::none);
 
     /**
      * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
      */
-    static Vector adjointTranspose(const Vector& xi, const Vector& y, boost::optional<Matrix&> H = boost::none);
+    static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none);
 
     /**
      * Compute the inverse right-trivialized tangent (derivative) map of the exponential map,
@@ -208,7 +214,7 @@ public:
      *    Ernst Hairer, et al., Geometric Numerical Integration,
      *      Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006.
      */
-    static Matrix6 dExpInv_exp(const Vector&  xi);
+    static Matrix6 dExpInv_exp(const Vector6 &xi);
 
     /**
      * wedge for Pose3:
@@ -237,7 +243,7 @@ public:
      * @return point in world coordinates
      */
     Point3 transform_from(const Point3& p,
-        boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> Dpoint=boost::none) const;
+        OptionalJacobian<3,6> Dpose=boost::none, OptionalJacobian<3,3> Dpoint=boost::none) const;
 
     /** syntactic sugar for transform_from */
     inline Point3 operator*(const Point3& p) const { return transform_from(p); }
@@ -249,13 +255,9 @@ public:
      * @param Dpoint optional 3*3 Jacobian wrpt point
      * @return point in Pose coordinates
      */
-    Point3 transform_to(const Point3& p) const;
-
     Point3 transform_to(const Point3& p,
-        boost::optional<Matrix36&> Dpose, boost::optional<Matrix3&> Dpoint) const;
+        OptionalJacobian<3,6> Dpose = boost::none,
-
+        OptionalJacobian<3,3> Dpoint = boost::none) const;
-    Point3 transform_to(const Point3& p,
-        boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) const;
 
     /// @}
     /// @name Standard Interface
@@ -288,8 +290,8 @@ public:
      * @return range (double)
      */
     double range(const Point3& point,
-        boost::optional<Matrix&> H1=boost::none,
+        OptionalJacobian<1,6> H1=boost::none,
-        boost::optional<Matrix&> H2=boost::none) const;
+        OptionalJacobian<1,3> H2=boost::none) const;
 
     /**
      * Calculate range to another pose
@@ -297,8 +299,8 @@ public:
      * @return range (double)
      */
     double range(const Pose3& pose,
-        boost::optional<Matrix&> H1=boost::none,
+        OptionalJacobian<1,6> H1=boost::none,
-        boost::optional<Matrix&> H2=boost::none) const;
+        OptionalJacobian<1,6> H2=boost::none) const;
 
     /// @}
     /// @name Advanced Interface

gtsam/geometry/Rot2.cpp

@@ -64,21 +64,25 @@ Rot2& Rot2::normalize() {
 }
 
 /* ************************************************************************* */
-Matrix Rot2::matrix() const {
+Matrix2 Rot2::matrix() const {
-  return (Matrix(2, 2) <<  c_, -s_, s_, c_).finished();
+  Matrix2 rvalue_;
+  rvalue_ <<  c_, -s_, s_, c_;
+  return rvalue_;
 }
 
 /* ************************************************************************* */
-Matrix Rot2::transpose() const {
+Matrix2 Rot2::transpose() const {
-  return (Matrix(2, 2) <<  c_, s_, -s_, c_).finished();
+  Matrix2 rvalue_;
+  rvalue_ <<   c_, s_, -s_, c_;
+  return rvalue_;
 }
 
 /* ************************************************************************* */
 // see doc/math.lyx, SO(2) section
-Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
+Point2 Rot2::rotate(const Point2& p, OptionalJacobian<2, 1> H1,
-    boost::optional<Matrix&> H2) const {
+    OptionalJacobian<2, 2> H2) const {
   const Point2 q = Point2(c_ * p.x() + -s_ * p.y(), s_ * p.x() + c_ * p.y());
-  if (H1) *H1 = (Matrix(2, 1) <<  -q.y(), q.x()).finished();
+  if (H1) *H1 << -q.y(), q.x();
   if (H2) *H2 = matrix();
   return q;
 }
@@ -86,21 +90,23 @@ Point2 Rot2::rotate(const Point2& p, boost::optional<Matrix&> H1,
 /* ************************************************************************* */
 // see doc/math.lyx, SO(2) section
 Point2 Rot2::unrotate(const Point2& p,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+    OptionalJacobian<2, 1> H1, OptionalJacobian<2, 2> H2) const {
   const Point2 q = Point2(c_ * p.x() + s_ * p.y(), -s_ * p.x() + c_ * p.y());
-  if (H1) *H1 = (Matrix(2, 1) << q.y(), -q.x()).finished();  // R_{pi/2}q
+  if (H1) *H1 << q.y(), -q.x();
   if (H2) *H2 = transpose();
   return q;
 }
 
 /* ************************************************************************* */
-Rot2 Rot2::relativeBearing(const Point2& d, boost::optional<Matrix&> H) {
+Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
   double x = d.x(), y = d.y(), d2 = x * x + y * y, n = sqrt(d2);
   if(fabs(n) > 1e-5) {
-    if (H) *H = (Matrix(1, 2) << -y / d2, x / d2).finished();
+    if (H) {
+      *H << -y / d2, x / d2;
+    }
     return Rot2::fromCosSin(x / n, y / n);
   } else {
-    if (H) *H = (Matrix(1, 2) << 0.0, 0.0).finished();
+    if (H) *H << 0.0, 0.0;
     return Rot2();
   }
 }

gtsam/geometry/Rot2.h

@@ -87,7 +87,7 @@ namespace gtsam {
      * @param H optional reference for Jacobian
      * @return 2D rotation \f$ \in SO(2) \f$
      */
-    static Rot2 relativeBearing(const Point2& d, boost::optional<Matrix&> H =
+    static Rot2 relativeBearing(const Point2& d, OptionalJacobian<1,2> H =
         boost::none);
 
     /** Named constructor that behaves as atan2, i.e., y,x order (!) and normalizes */
@@ -116,10 +116,10 @@ namespace gtsam {
     }
 
     /** Compose - make a new rotation by adding angles */
-    inline Rot2 compose(const Rot2& R, boost::optional<Matrix&> H1 =
+    inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 =
-        boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+        boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
-      if (H1) *H1 = eye(1);
+      if (H1) *H1 = I_1x1; // set to 1x1 identity matrix
-      if (H2) *H2 = eye(1);
+      if (H2) *H2 = I_1x1; // set to 1x1 identity matrix
       return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
     }
 
@@ -129,10 +129,10 @@ namespace gtsam {
     }
 
     /** Between using the default implementation */
-    inline Rot2 between(const Rot2& R, boost::optional<Matrix&> H1 =
+    inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 =
-        boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+        boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
-      if (H1) *H1 = -eye(1);
+      if (H1) *H1 = -I_1x1; // set to 1x1 identity matrix
-      if (H2) *H2 = eye(1);
+      if (H2) *H2 =  I_1x1; // set to 1x1 identity matrix
       return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_);
     }
 
@@ -154,7 +154,7 @@ namespace gtsam {
     inline Rot2 retract(const Vector& v) const { return *this * Expmap(v); }
 
     /// Returns inverse retraction
-    inline Vector localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); }
+    inline Vector1 localCoordinates(const Rot2& t2) const { return Logmap(between(t2)); }
 
     /// @}
     /// @name Lie Group
@@ -169,8 +169,10 @@ namespace gtsam {
     }
 
     ///Log map at identity - return the canonical coordinates of this rotation
-    static inline Vector Logmap(const Rot2& r) {
+    static inline Vector1 Logmap(const Rot2& r) {
-      return (Vector(1) << r.theta()).finished();
+      Vector1 v;
+      v << r.theta();
+      return v;
     }
 
     /// @}
@@ -180,8 +182,8 @@ namespace gtsam {
     /**
      * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$
      */
-    Point2 rotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
+    Point2 rotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
-        boost::optional<Matrix&> H2 = boost::none) const;
+        OptionalJacobian<2, 2> H2 = boost::none) const;
 
     /** syntactic sugar for rotate */
     inline Point2 operator*(const Point2& p) const {
@@ -191,8 +193,8 @@ namespace gtsam {
     /**
      * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
      */
-    Point2 unrotate(const Point2& p, boost::optional<Matrix&> H1 = boost::none,
+    Point2 unrotate(const Point2& p, OptionalJacobian<2, 1> H1 = boost::none,
-        boost::optional<Matrix&> H2 = boost::none) const;
+        OptionalJacobian<2, 2> H2 = boost::none) const;
 
     /// @}
     /// @name Standard Interface
@@ -225,10 +227,10 @@ namespace gtsam {
     }
 
     /** return 2*2 rotation matrix */
-    Matrix matrix() const;
+    Matrix2 matrix() const;
 
     /** return 2*2 transpose (inverse) rotation matrix   */
-    Matrix transpose() const;
+    Matrix2 transpose() const;
 
   private:
     /** Serialization function */

gtsam/geometry/Rot3.cpp

@@ -27,8 +27,6 @@ using namespace std;
 
 namespace gtsam {
 
-static const Matrix3 I3 = Matrix3::Identity();
-
 /* ************************************************************************* */
 void Rot3::print(const std::string& s) const {
   gtsam::print((Matrix)matrix(), s);
@@ -54,7 +52,7 @@ Rot3 Rot3::Random(boost::mt19937 & rng) {
 }
 
 /* ************************************************************************* */
-Rot3 Rot3::rodriguez(const Vector& w) {
+Rot3 Rot3::rodriguez(const Vector3& w) {
   double t = w.norm();
   if (t < 1e-10) return Rot3();
   return rodriguez(w/t, t);
@@ -72,23 +70,21 @@ Point3 Rot3::operator*(const Point3& p) const {
 
 /* ************************************************************************* */
 Unit3 Rot3::rotate(const Unit3& p,
-    boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
+    OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
-  Unit3 q = Unit3(rotate(p.point3(Hp)));
+  Matrix32 Dp;
-  if (Hp)
+  Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0)));
-    (*Hp) = q.basis().transpose() * matrix() * (*Hp);
+  if (Hp) *Hp = q.basis().transpose() * matrix() * Dp;
-  if (HR)
+  if (HR) *HR = -q.basis().transpose() * matrix() * p.skew();
-    (*HR) = -q.basis().transpose() * matrix() * p.skew();
   return q;
 }
 
 /* ************************************************************************* */
 Unit3 Rot3::unrotate(const Unit3& p,
-    boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
+    OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
-  Unit3 q = Unit3(unrotate(p.point3(Hp)));
+  Matrix32 Dp;
-  if (Hp)
+  Unit3 q = Unit3(unrotate(p.point3(Dp)));
-    (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp);
+  if (Hp) *Hp = q.basis().transpose() * matrix().transpose () * Dp;
-  if (HR)
+  if (HR) *HR = q.basis().transpose() * q.skew();
-    (*HR) = q.basis().transpose() * q.skew();
   return q;
 }
 
@@ -99,8 +95,8 @@ Unit3 Rot3::operator*(const Unit3& p) const {
 
 /* ************************************************************************* */
 // see doc/math.lyx, SO(3) section
-Point3 Rot3::unrotate(const Point3& p, boost::optional<Matrix3&> H1,
+Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1,
-    boost::optional<Matrix3&> H2) const {
+    OptionalJacobian<3,3> H2) const {
   const Matrix3& Rt = transpose();
   Point3 q(Rt * p.vector()); // q = Rt*p
   const double wx = q.x(), wy = q.y(), wz = q.z();
@@ -111,32 +107,16 @@ Point3 Rot3::unrotate(const Point3& p, boost::optional<Matrix3&> H1,
   return q;
 }
 
-/* ************************************************************************* */
-// see doc/math.lyx, SO(3) section
-Point3 Rot3::unrotate(const Point3& p,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-  const Matrix3& Rt = transpose();
-  Point3 q(Rt * p.vector()); // q = Rt*p
-  const double wx = q.x(), wy = q.y(), wz = q.z();
-  if (H1) {
-    H1->resize(3,3);
-    *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
-  }
-  if (H2)
-    *H2 = Rt;
-  return q;
-}
-
 /* ************************************************************************* */
 /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
 Matrix3 Rot3::dexpL(const Vector3& v) {
   if(zero(v)) return eye(3);
-  Matrix x = skewSymmetric(v);
+  Matrix3 x = skewSymmetric(v);
-  Matrix x2 = x*x;
+  Matrix3 x2 = x*x;
   double theta = v.norm(), vi = theta/2.0;
   double s1 = sin(vi)/vi;
   double s2 = (theta - sin(theta))/(theta*theta*theta);
-  Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
+  Matrix3 res = I_3x3 - 0.5*s1*s1*x + s2*x2;
   return res;
 }
 
@@ -144,11 +124,11 @@ Matrix3 Rot3::dexpL(const Vector3& v) {
 /// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
 Matrix3 Rot3::dexpInvL(const Vector3& v) {
   if(zero(v)) return eye(3);
-  Matrix x = skewSymmetric(v);
+  Matrix3 x = skewSymmetric(v);
-  Matrix x2 = x*x;
+  Matrix3 x2 = x*x;
   double theta = v.norm(), vi = theta/2.0;
   double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
-  Matrix res = eye(3) + 0.5*x - s2*x2;
+  Matrix3 res = I_3x3 + 0.5*x - s2*x2;
   return res;
 }
 
@@ -167,7 +147,7 @@ Point3 Rot3::column(int index) const{
 
 /* ************************************************************************* */
 Vector3 Rot3::xyz() const {
-  Matrix I;Vector3 q;
+  Matrix3 I;Vector3 q;
   boost::tie(I,q)=RQ(matrix());
   return q;
 }
@@ -200,11 +180,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3(const Vector3& x)    {
   double normx = norm_2(x); // rotation angle
   Matrix3 Jr;
   if (normx < 10e-8){
-    Jr = Matrix3::Identity();
+    Jr = I_3x3;
   }
   else{
     const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
-    Jr = Matrix3::Identity() - ((1-cos(normx))/(normx*normx)) * X +
+    Jr = I_3x3 - ((1-cos(normx))/(normx*normx)) * X +
         ((normx-sin(normx))/(normx*normx*normx)) * X * X; // right Jacobian
   }
   return Jr;
@@ -217,11 +197,11 @@ Matrix3 Rot3::rightJacobianExpMapSO3inverse(const Vector3& x)    {
   Matrix3 Jrinv;
 
   if (normx < 10e-8){
-    Jrinv = Matrix3::Identity();
+    Jrinv = I_3x3;
   }
   else{
     const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
-    Jrinv = Matrix3::Identity() +
+    Jrinv = I_3x3 +
         0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx))   ) * X * X;
   }
   return Jrinv;
@@ -255,12 +235,6 @@ ostream &operator<<(ostream &os, const Rot3& R) {
   return os;
 }
 
-/* ************************************************************************* */
-Point3 Rot3::unrotate(const Point3& p) const {
-  // Eigen expression
-  return Point3(transpose()*p.vector()); // q = Rt*p
-}
-
 /* ************************************************************************* */
 Rot3 Rot3::slerp(double t, const Rot3& other) const {
   // amazingly simple in GTSAM :-)

gtsam/geometry/Rot3.h

@@ -183,7 +183,7 @@ namespace gtsam {
      * @param v a vector of incremental roll,pitch,yaw
      * @return incremental rotation matrix
      */
-    static Rot3 rodriguez(const Vector& v);
+    static Rot3 rodriguez(const Vector3& v);
 
     /**
      * Rodriguez' formula to compute an incremental rotation matrix
@@ -193,7 +193,7 @@ namespace gtsam {
      * @return incremental rotation matrix
      */
     static Rot3 rodriguez(double wx, double wy, double wz)
-      { return rodriguez((Vector(3) << wx, wy, wz).finished());}
+      { return rodriguez(Vector3(wx, wy, wz));}
 
     /// @}
     /// @name Testable
@@ -215,18 +215,11 @@ namespace gtsam {
     }
 
     /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
-    Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const;
+    Rot3 inverse(boost::optional<Matrix3&> H1=boost::none) const;
 
     /// Compose two rotations i.e., R= (*this) * R2
-    Rot3 compose(const Rot3& R2) const;
+    Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none,
-
+        OptionalJacobian<3, 3> H2 = boost::none) const;
-    /// Compose two rotations i.e., R= (*this) * R2
-    Rot3 compose(const Rot3& R2, boost::optional<Matrix3&> H1,
-        boost::optional<Matrix3&> H2) const;
-
-    /// Compose two rotations i.e., R= (*this) * R2
-    Rot3 compose(const Rot3& R2, boost::optional<Matrix&> H1,
-        boost::optional<Matrix&> H2) const;
 
     /** compose two rotations */
     Rot3 operator*(const Rot3& R2) const;
@@ -245,8 +238,8 @@ namespace gtsam {
      * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
      */
     Rot3 between(const Rot3& R2,
-        boost::optional<Matrix&> H1=boost::none,
+        OptionalJacobian<3,3> H1=boost::none,
-        boost::optional<Matrix&> H2=boost::none) const;
+        OptionalJacobian<3,3> H2=boost::none) const;
 
     /// @}
     /// @name Manifold
@@ -328,34 +321,27 @@ namespace gtsam {
     /**
      * rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$
      */
-    Point3 rotate(const Point3& p, boost::optional<Matrix&> H1 = boost::none,
+    Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
-        boost::optional<Matrix&> H2 = boost::none) const;
+        OptionalJacobian<3,3> H2 = boost::none) const;
 
     /// rotate point from rotated coordinate frame to world = R*p
     Point3 operator*(const Point3& p) const;
 
     /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
-    Point3 unrotate(const Point3& p) const;
+    Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
-
+        OptionalJacobian<3,3> H2=boost::none) const;
-    /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
-    Point3 unrotate(const Point3& p, boost::optional<Matrix3&> H1,
-        boost::optional<Matrix3&> H2) const;
-
-    /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
-    Point3 unrotate(const Point3& p, boost::optional<Matrix&> H1,
-        boost::optional<Matrix&> H2) const;
 
     /// @}
     /// @name Group Action on Unit3
     /// @{
 
     /// rotate 3D direction from rotated coordinate frame to world frame
-    Unit3 rotate(const Unit3& p, boost::optional<Matrix&> HR = boost::none,
+    Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
-        boost::optional<Matrix&> Hp = boost::none) const;
+        OptionalJacobian<2,2> Hp = boost::none) const;
 
     /// unrotate 3D direction from world frame to rotated coordinate frame
-    Unit3 unrotate(const Unit3& p, boost::optional<Matrix&> HR = boost::none,
+    Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
-        boost::optional<Matrix&> Hp = boost::none) const;
+        OptionalJacobian<2,2> Hp = boost::none) const;
 
     /// rotate 3D direction from rotated coordinate frame to world frame
     Unit3 operator*(const Unit3& p) const;

gtsam/geometry/Rot3M.cpp

@@ -30,10 +30,8 @@ using namespace std;
 
 namespace gtsam {
 
-static const Matrix3 I3 = Matrix3::Identity();
-
 /* ************************************************************************* */
-Rot3::Rot3() : rot_(Matrix3::Identity()) {}
+Rot3::Rot3() : rot_(I_3x3) {}
 
 /* ************************************************************************* */
 Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) {
@@ -142,23 +140,9 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) {
 }
 
 /* ************************************************************************* */
-Rot3 Rot3::compose (const Rot3& R2) const {
+Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
-  return *this * R2;
-}
-
-/* ************************************************************************* */
-Rot3 Rot3::compose (const Rot3& R2,
-    boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2) const {
   if (H1) *H1 = R2.transpose();
-  if (H2) *H2 = I3;
+  if (H2) *H2 = I_3x3;
-  return *this * R2;
-}
-
-/* ************************************************************************* */
-Rot3 Rot3::compose (const Rot3& R2,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-  if (H1) *H1 = R2.transpose();
-  if (H2) *H2 = I3;
   return *this * R2;
 }
 
@@ -174,23 +158,23 @@ Matrix3 Rot3::transpose() const {
 }
 
 /* ************************************************************************* */
-Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
+Rot3 Rot3::inverse(boost::optional<Matrix3 &> H1) const {
   if (H1) *H1 = -rot_;
   return Rot3(Matrix3(transpose()));
 }
 
 /* ************************************************************************* */
 Rot3 Rot3::between (const Rot3& R2,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+    OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
   if (H1) *H1 = -(R2.transpose()*rot_);
-  if (H2) *H2 = I3;
+  if (H2) *H2 = I_3x3;
   Matrix3 R12 = transpose()*R2.rot_;
   return Rot3(R12);
 }
 
 /* ************************************************************************* */
 Point3 Rot3::rotate(const Point3& p,
-    boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2) const {
+    OptionalJacobian<3,3> H1,  OptionalJacobian<3,3> H2) const {
   if (H1 || H2) {
       if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
       if (H2) *H2 = rot_;
@@ -257,7 +241,7 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
   } else if(mode == Rot3::CAYLEY) {
     return retractCayley(omega);
   } else if(mode == Rot3::SLOW_CAYLEY) {
-    Matrix Omega = skewSymmetric(omega);
+    Matrix3 Omega = skewSymmetric(omega);
     return (*this)*CayleyFixed<3>(-Omega/2);
   } else {
     assert(false);

gtsam/geometry/Rot3Q.cpp

@@ -87,22 +87,9 @@ namespace gtsam {
   Rot3 Rot3::rodriguez(const Vector& w, double theta) {
     return Quaternion(Eigen::AngleAxisd(theta, w)); }
 
-  /* ************************************************************************* */
-  Rot3 Rot3::compose(const Rot3& R2) const {
-    return Rot3(quaternion_ * R2.quaternion_);
-  }
-
   /* ************************************************************************* */
   Rot3 Rot3::compose(const Rot3& R2,
-  boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2) const {
+  OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
-    if (H1) *H1 = R2.transpose();
-    if (H2) *H2 = I3;
-    return Rot3(quaternion_ * R2.quaternion_);
-  }
-
-  /* ************************************************************************* */
-  Rot3 Rot3::compose(const Rot3& R2,
-  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
     if (H1) *H1 = R2.transpose();
     if (H2) *H2 = I3;
     return Rot3(quaternion_ * R2.quaternion_);
@@ -114,7 +101,7 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
+  Rot3 Rot3::inverse(boost::optional<Matrix3&> H1) const {
     if (H1) *H1 = -matrix();
     return Rot3(quaternion_.inverse());
   }
@@ -129,7 +116,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   Rot3 Rot3::between(const Rot3& R2,
-  boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+  OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
     if (H1) *H1 = -(R2.transpose()*matrix());
     if (H2) *H2 = I3;
     return between_default(*this, R2);
@@ -137,7 +124,7 @@ namespace gtsam {
 
   /* ************************************************************************* */
   Point3 Rot3::rotate(const Point3& p,
-        boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2) const {
+        OptionalJacobian<3,3> H1,  OptionalJacobian<3,3> H2) const {
     Matrix R = matrix();
     if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
     if (H2) *H2 = R;

gtsam/geometry/SimpleCamera.cpp

@@ -21,27 +21,27 @@
 
 namespace gtsam {
 
-  SimpleCamera simpleCamera(const Matrix& P) {
+  SimpleCamera simpleCamera(const Matrix34& P) {
 
     // P = [A|a] = s K cRw [I|-T], with s the unknown scale
-    Matrix A = P.topLeftCorner(3, 3);
+    Matrix3 A = P.topLeftCorner(3, 3);
-    Vector a = P.col(3);
+    Vector3 a = P.col(3);
 
     // do RQ decomposition to get s*K and cRw angles
-    Matrix sK;
+    Matrix3 sK;
-    Vector xyz;
+    Vector3 xyz;
     boost::tie(sK, xyz) = RQ(A);
 
     // Recover scale factor s and K
     double s = sK(2, 2);
-    Matrix K = sK / s;
+    Matrix3 K = sK / s;
 
     // Recover cRw itself, and its inverse
     Rot3 cRw = Rot3::RzRyRx(xyz);
     Rot3 wRc = cRw.inverse();
 
     // Now, recover T from a = - s K cRw T = - A T
-    Vector T = -(A.inverse() * a);
+    Vector3 T = -(A.inverse() * a);
     return SimpleCamera(Pose3(wRc, T),
         Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)));
   }

gtsam/geometry/SimpleCamera.h

@@ -27,5 +27,5 @@ namespace gtsam {
   typedef PinholeCamera<Cal3_S2> SimpleCamera;
 
   /// Recover camera from 3*4 camera matrix
-  GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix& P);
+  GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);
 }

gtsam/geometry/StereoCamera.cpp

@@ -30,8 +30,8 @@ namespace gtsam {
 
   /* ************************************************************************* */
   StereoPoint2 StereoCamera::project(const Point3& point,
-      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
+      OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2,
-      boost::optional<Matrix&> H3) const {
+      OptionalJacobian<3,6> H3) const {
 
 #ifdef STEREOCAMERA_CHAIN_RULE
     const Point3 q = leftCamPose_.transform_to(point, H1, H2);
@@ -58,28 +58,28 @@ namespace gtsam {
     if (H1 || H2) {
 #ifdef STEREOCAMERA_CHAIN_RULE
       // just implement chain rule
-      Matrix D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian
+      Matrix3 D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian
       if (H1) *H1 = D_project_point*(*H1);
       if (H2) *H2 = D_project_point*(*H2);
 #else
       // optimized version, see StereoCamera.nb
       if (H1) {
         const double v1 = v/fy, v2 = fx*v1, dx=d*x;
-        *H1 = (Matrix(3, 6) <<
+        *H1  << uL*v1, -fx-dx*uL,     v2, -dfx,  0.0, d*uL,
-                uL*v1, -fx-dx*uL,     v2, -dfx,  0.0, d*uL,
                 uR*v1, -fx-dx*uR,     v2, -dfx,  0.0, d*uR,
-            fy + v*v1,    -dx*v , -x*dfy,  0.0, -dfy, d*v
+                fy + v*v1,    -dx*v , -x*dfy,  0.0, -dfy, d*v;
-          ).finished();
       }
       if (H2) {
-        const Matrix R(leftCamPose_.rotation().matrix());
+        const Matrix3 R(leftCamPose_.rotation().matrix());
-        *H2 = d * (Matrix(3, 3) <<
+        *H2  << fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL,
-             fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL,
+                fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR,
-             fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR,
+                fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v;
-             fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v
+        *H2 << d * (*H2);
-         ).finished();
       }
 #endif
+      if (H3)
+        // TODO, this is set to zero, as Cal3_S2Stereo cannot be optimized yet
+        H3->setZero();
     }
 
     // finally translate
@@ -87,15 +87,15 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
+  Matrix3 StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
     double d = 1.0 / P.z(), d2 = d*d;
     const Cal3_S2Stereo& K = *K_;
     double f_x = K.fx(), f_y = K.fy(), b = K.baseline();
-    return (Matrix(3, 3) <<
+    Matrix3 m;
-         f_x*d,   0.0, -d2*f_x* P.x(),
+    m << f_x*d,   0.0, -d2*f_x* P.x(),
          f_x*d,   0.0, -d2*f_x*(P.x() - b),
-           0.0, f_y*d, -d2*f_y* P.y()
+           0.0, f_y*d, -d2*f_y* P.y();
-    ).finished();
+    return m;
   }
 
 }

gtsam/geometry/StereoCamera.h

@@ -90,14 +90,8 @@ public:
   }
 
   /// Local coordinates of manifold neighborhood around current value
-  inline Vector localCoordinates(const StereoCamera& t2) const {
+  inline Vector6 localCoordinates(const StereoCamera& t2) const {
-    return Vector(leftCamPose_.localCoordinates(t2.leftCamPose_));
+    return leftCamPose_.localCoordinates(t2.leftCamPose_);
-  }
-
-  Pose3 between(const StereoCamera &camera,
-      boost::optional<Matrix&> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const {
-    return leftCamPose_.between(camera.pose(), H1, H2);
   }
 
   /// @}
@@ -119,9 +113,9 @@ public:
    * @param H3 IGNORED (for calibration)
    */
   StereoPoint2 project(const Point3& point,
-      boost::optional<Matrix&> H1 = boost::none,
+      OptionalJacobian<3, 6> H1 = boost::none,
-      boost::optional<Matrix&> H2 = boost::none,
+      OptionalJacobian<3, 3> H2 = boost::none,
-      boost::optional<Matrix&> H3 = boost::none) const;
+      OptionalJacobian<3, 6> H3 = boost::none) const;
 
   /**
    *
@@ -139,7 +133,7 @@ public:
 
 private:
   /** utility functions */
-  Matrix Dproject_to_stereo_camera1(const Point3& P) const;
+  Matrix3 Dproject_to_stereo_camera1(const Point3& P) const;
 
   friend class boost::serialization::access;
   template<class Archive>

gtsam/geometry/Unit3.cpp

@@ -39,14 +39,13 @@ using namespace std;
 namespace gtsam {
 
 /* ************************************************************************* */
-Unit3 Unit3::FromPoint3(const Point3& point, boost::optional<Matrix&> H) {
+Unit3 Unit3::FromPoint3(const Point3& point, OptionalJacobian<2,3> H) {
   Unit3 direction(point);
   if (H) {
     // 3*3 Derivative of representation with respect to point is 3*3:
-    Matrix D_p_point;
+    Matrix3 D_p_point;
     point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
     // Calculate the 2*3 Jacobian
-    H->resize(2, 3);
     *H << direction.basis().transpose() * D_p_point;
   }
   return direction;
@@ -67,7 +66,7 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
 }
 
 /* ************************************************************************* */
-const Unit3::Matrix32& Unit3::basis() const {
+const Matrix32& Unit3::basis() const {
 
   // Return cached version if exists
   if (B_)
@@ -92,7 +91,7 @@ const Unit3::Matrix32& Unit3::basis() const {
   b2 = b2 / b2.norm();
 
   // Create the basis matrix
-  B_.reset(Unit3::Matrix32());
+  B_.reset(Matrix32());
   (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
   return *B_;
 }
@@ -104,38 +103,39 @@ void Unit3::print(const std::string& s) const {
 }
 
 /* ************************************************************************* */
-Matrix Unit3::skew() const {
+Matrix3 Unit3::skew() const {
   return skewSymmetric(p_.x(), p_.y(), p_.z());
 }
 
 /* ************************************************************************* */
-Vector Unit3::error(const Unit3& q, boost::optional<Matrix&> H) const {
+Vector2 Unit3::error(const Unit3& q, OptionalJacobian<2,2> H) const {
   // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
-  Matrix Bt = basis().transpose();
+  Matrix23 Bt = basis().transpose();
-  Vector xi = Bt * q.p_.vector();
+  Vector2 xi = Bt * q.p_.vector();
   if (H)
     *H = Bt * q.basis();
   return xi;
 }
 
 /* ************************************************************************* */
-double Unit3::distance(const Unit3& q, boost::optional<Matrix&> H) const {
+double Unit3::distance(const Unit3& q, OptionalJacobian<1,2> H) const {
-  Vector xi = error(q, H);
+  Matrix2 H_;
+  Vector2 xi = error(q, H_);
   double theta = xi.norm();
   if (H)
-    *H = (xi.transpose() / theta) * (*H);
+    *H = (xi.transpose() / theta) * H_;
   return theta;
 }
 
 /* ************************************************************************* */
-Unit3 Unit3::retract(const Vector& v) const {
+Unit3 Unit3::retract(const Vector2& v) const {
 
   // Get the vector form of the point and the basis matrix
-  Vector p = Point3::Logmap(p_);
+  Vector3 p = Point3::Logmap(p_);
-  Matrix B = basis();
+  Matrix32 B = basis();
 
   // Compute the 3D xi_hat vector
-  Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
+  Vector3 xi_hat = v(0) * B.col(0) + v(1) * B.col(1);
 
   double xi_hat_norm = xi_hat.norm();
 
@@ -147,28 +147,28 @@ Unit3 Unit3::retract(const Vector& v) const {
       return Unit3(-point3());
   }
 
-  Vector exp_p_xi_hat = cos(xi_hat_norm) * p
+  Vector3 exp_p_xi_hat = cos(xi_hat_norm) * p
       + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
   return Unit3(exp_p_xi_hat);
 
 }
 
 /* ************************************************************************* */
-Vector Unit3::localCoordinates(const Unit3& y) const {
+Vector2 Unit3::localCoordinates(const Unit3& y) const {
 
-  Vector p = Point3::Logmap(p_);
+  Vector3 p = Point3::Logmap(p_);
-  Vector q = Point3::Logmap(y.p_);
+  Vector3 q = Point3::Logmap(y.p_);
   double dot = p.dot(q);
 
   // Check for special cases
   if (std::abs(dot - 1.0) < 1e-16)
     return Vector2(0, 0);
   else if (std::abs(dot + 1.0) < 1e-16)
-    return (Vector(2) << M_PI, 0).finished();
+    return Vector2(M_PI, 0);
   else {
     // no special case
     double theta = acos(dot);
-    Vector result_hat = (theta / sin(theta)) * (q - p * dot);
+    Vector3 result_hat = (theta / sin(theta)) * (q - p * dot);
     return basis().transpose() * result_hat;
   }
 }

gtsam/geometry/Unit3.h

@@ -32,8 +32,6 @@ class GTSAM_EXPORT Unit3{
 
 private:
 
-  typedef Eigen::Matrix<double,3,2> Matrix32;
-
   Point3 p_; ///< The location of the point on the unit sphere
   mutable boost::optional<Matrix32> B_; ///< Cached basis
 
@@ -52,6 +50,11 @@ public:
       p_(p / p.norm()) {
   }
 
+  /// Construct from a vector3
+  explicit Unit3(const Vector3& p) :
+      p_(p / p.norm()) {
+  }
+
   /// Construct from x,y,z
   Unit3(double x, double y, double z) :
       p_(x, y, z) {
@@ -59,7 +62,7 @@ public:
   }
 
   /// Named constructor from Point3 with optional Jacobian
-  static Unit3 FromPoint3(const Point3& point, boost::optional<Matrix&> H =
+  static Unit3 FromPoint3(const Point3& point, OptionalJacobian<2,3> H =
       boost::none);
 
   /// Random direction, using boost::uniform_on_sphere
@@ -90,10 +93,10 @@ public:
   const Matrix32& basis() const;
 
   /// Return skew-symmetric associated with 3D point on unit sphere
-  Matrix skew() const;
+  Matrix3 skew() const;
 
   /// Return unit-norm Point3
-  const Point3& point3(boost::optional<Matrix&> H = boost::none) const {
+  const Point3& point3(OptionalJacobian<3,2> H = boost::none) const {
     if (H)
       *H = basis();
     return p_;
@@ -105,12 +108,12 @@ public:
   }
 
   /// Signed, vector-valued error between two directions
-  Vector error(const Unit3& q,
+  Vector2 error(const Unit3& q,
-      boost::optional<Matrix&> H = boost::none) const;
+      OptionalJacobian<2,2> H = boost::none) const;
 
   /// Distance between two directions
   double distance(const Unit3& q,
-      boost::optional<Matrix&> H = boost::none) const;
+      OptionalJacobian<1,2> H = boost::none) const;
 
   /// @}
 
@@ -133,10 +136,10 @@ public:
   };
 
   /// The retract function
-  Unit3 retract(const Vector& v) const;
+  Unit3 retract(const Vector2& v) const;
 
   /// The local coordinates function
-  Vector localCoordinates(const Unit3& s) const;
+  Vector2 localCoordinates(const Unit3& s) const;
 
   /// @}
 
@@ -144,7 +147,6 @@ private:
 
   /// @name Advanced Interface
   /// @{
-
   /** Serialization function */
   friend class boost::serialization::access;
   template<class ARCHIVE>

gtsam/geometry/tests/testCal3_S2.cpp

@@ -60,7 +60,7 @@ TEST( Cal3_S2, calibrate_homogeneous) {
 Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); }
 TEST( Cal3_S2, Duncalibrate1)
 {
-  Matrix computed; K.uncalibrate(p, computed, boost::none);
+  Matrix25 computed; K.uncalibrate(p, computed, boost::none);
   Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
   CHECK(assert_equal(numerical,computed,1e-8));
 }

gtsam/geometry/tests/testCalibratedCamera.cpp

@@ -15,12 +15,14 @@
  * @brief test CalibratedCamera class
  */
 
-#include <iostream>
+#include <gtsam/geometry/CalibratedCamera.h>
-
+#include <gtsam/geometry/Pose2.h>
-#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/geometry/CalibratedCamera.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <iostream>
 
 using namespace std;
 using namespace gtsam;

gtsam/geometry/tests/testPinholeCamera.cpp

@@ -15,29 +15,28 @@
  * @brief test PinholeCamera class
  */
 
-#include <cmath>
-#include <iostream>
-
 #include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+
+#include <cmath>
+#include <iostream>
 
 using namespace std;
 using namespace gtsam;
 
+typedef PinholeCamera<Cal3_S2> Camera;
+
 static const Cal3_S2 K(625, 625, 0, 0, 0);
 
-static const Pose3 pose1((Matrix)(Matrix(3,3) <<
+static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
-              1., 0., 0.,
+static const Camera camera(pose, K);
-              0.,-1., 0.,
-              0., 0.,-1.
-              ).finished(),
-            Point3(0,0,0.5));
 
-typedef PinholeCamera<Cal3_S2> Camera;
+static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
-static const Camera camera(pose1, K);
+static const Camera camera1(pose1, K);
 
 static const Point3 point1(-0.08,-0.08, 0.0);
 static const Point3 point2(-0.08, 0.08, 0.0);
@@ -52,8 +51,8 @@ static const Point3 point4_inf( 0.16,-0.16, -1.0);
 /* ************************************************************************* */
 TEST( PinholeCamera, constructor)
 {
-  CHECK(assert_equal( camera.calibration(), K));
+  EXPECT(assert_equal( camera.calibration(), K));
-  CHECK(assert_equal( camera.pose(), pose1));
+  EXPECT(assert_equal( camera.pose(), pose));
 }
 
 /* ************************************************************************* */
@@ -67,7 +66,7 @@ TEST( PinholeCamera, level2)
   Point3 x(1,0,0),y(0,0,-1),z(0,1,0);
   Rot3 wRc(x,y,z);
   Pose3 expected(wRc,Point3(0.4,0.3,0.1));
-  CHECK(assert_equal( camera.pose(), expected));
+  EXPECT(assert_equal( camera.pose(), expected));
 }
 
 /* ************************************************************************* */
@@ -80,72 +79,72 @@ TEST( PinholeCamera, lookat)
   // expected
   Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
   Pose3 expected(Rot3(xc,yc,zc),C);
-  CHECK(assert_equal( camera.pose(), expected));
+  EXPECT(assert_equal( camera.pose(), expected));
 
   Point3 C2(30.0,0.0,10.0);
   Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0));
 
   Matrix R = camera2.pose().rotation().matrix();
   Matrix I = trans(R)*R;
-  CHECK(assert_equal(I, eye(3)));
+  EXPECT(assert_equal(I, eye(3)));
 }
 
 /* ************************************************************************* */
 TEST( PinholeCamera, project)
 {
-  CHECK(assert_equal( camera.project(point1), Point2(-100,  100) ));
+  EXPECT(assert_equal( camera.project(point1), Point2(-100,  100) ));
-  CHECK(assert_equal( camera.project(point2), Point2(-100, -100) ));
+  EXPECT(assert_equal( camera.project(point2), Point2(-100, -100) ));
-  CHECK(assert_equal( camera.project(point3), Point2( 100, -100) ));
+  EXPECT(assert_equal( camera.project(point3), Point2( 100, -100) ));
-  CHECK(assert_equal( camera.project(point4), Point2( 100,  100) ));
+  EXPECT(assert_equal( camera.project(point4), Point2( 100,  100) ));
 }
 
 /* ************************************************************************* */
 TEST( PinholeCamera, backproject)
 {
-  CHECK(assert_equal( camera.backproject(Point2(-100,  100), 0.5),  point1));
+  EXPECT(assert_equal( camera.backproject(Point2(-100,  100), 0.5),  point1));
-  CHECK(assert_equal( camera.backproject(Point2(-100, -100), 0.5),  point2));
+  EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5),  point2));
-  CHECK(assert_equal( camera.backproject(Point2( 100, -100), 0.5),  point3));
+  EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5),  point3));
-  CHECK(assert_equal( camera.backproject(Point2( 100,  100), 0.5),  point4));
+  EXPECT(assert_equal( camera.backproject(Point2( 100,  100), 0.5),  point4));
 }
 
 /* ************************************************************************* */
 TEST( PinholeCamera, backprojectInfinity)
 {
-  CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100,  100)),  point1_inf));
+  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100,  100)),  point1_inf));
-  CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)),  point2_inf));
+  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)),  point2_inf));
-  CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)),  point3_inf));
+  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)),  point3_inf));
-  CHECK(assert_equal( camera.backprojectPointAtInfinity(Point2( 100,  100)),  point4_inf));
+  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100,  100)),  point4_inf));
 }
 
 /* ************************************************************************* */
 TEST( PinholeCamera, backproject2)
 {
   Point3 origin;
-  Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down
+  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 expected(0., 1., 0.);
   pair<Point2, bool> x = camera.projectSafe(expected);
 
-  CHECK(assert_equal(expected, actual));
+  EXPECT(assert_equal(expected, actual));
-  CHECK(assert_equal(Point2(), x.first));
+  EXPECT(assert_equal(Point2(), x.first));
-  CHECK(x.second);
+  EXPECT(x.second);
 }
 
 /* ************************************************************************* */
 TEST( PinholeCamera, backprojectInfinity2)
 {
   Point3 origin;
-  Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera looking down
+  Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
   Camera camera(Pose3(rot, origin), K);
 
   Point3 actual = camera.backprojectPointAtInfinity(Point2());
   Point3 expected(0., 1., 0.);
   Point2 x = camera.projectPointAtInfinity(expected);
 
-  CHECK(assert_equal(expected, actual));
+  EXPECT(assert_equal(expected, actual));
-  CHECK(assert_equal(Point2(), x));
+  EXPECT(assert_equal(Point2(), x));
 }
 
 /* ************************************************************************* */
@@ -159,8 +158,8 @@ TEST( PinholeCamera, backprojectInfinity3)
   Point3 expected(0., 0., 1.);
   Point2 x = camera.projectPointAtInfinity(expected);
 
-  CHECK(assert_equal(expected, actual));
+  EXPECT(assert_equal(expected, actual));
-  CHECK(assert_equal(Point2(), x));
+  EXPECT(assert_equal(Point2(), x));
 }
 
 /* ************************************************************************* */
@@ -173,13 +172,13 @@ TEST( PinholeCamera, Dproject)
 {
   Matrix Dpose, Dpoint, Dcal;
   Point2 result = camera.project(point1, Dpose, Dpoint, Dcal);
-  Matrix numerical_pose  = numericalDerivative31(project3, pose1, point1, K);
+  Matrix numerical_pose  = numericalDerivative31(project3, pose, point1, K);
-  Matrix numerical_point = numericalDerivative32(project3, pose1, point1, K);
+  Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K);
-  Matrix numerical_cal   = numericalDerivative33(project3, pose1, point1, K);
+  Matrix numerical_cal   = numericalDerivative33(project3, pose, point1, K);
-  CHECK(assert_equal(Point2(-100,  100), result));
+  EXPECT(assert_equal(Point2(-100,  100), result));
-  CHECK(assert_equal(numerical_pose,  Dpose,  1e-7));
+  EXPECT(assert_equal(numerical_pose,  Dpose,  1e-7));
-  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
+  EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
-  CHECK(assert_equal(numerical_cal,   Dcal,   1e-7));
+  EXPECT(assert_equal(numerical_cal,   Dcal,   1e-7));
 }
 
 /* ************************************************************************* */
@@ -190,21 +189,21 @@ static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const C
 TEST( PinholeCamera, Dproject_Infinity)
 {
   Matrix Dpose, Dpoint, Dcal;
-  Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera
+  Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera1
 
   // test Projection
   Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal);
   Point2 expected(-5.0, 5.0);
-  CHECK(assert_equal(actual, expected,  1e-7));
+  EXPECT(assert_equal(actual, expected,  1e-7));
 
   // test Jacobians
-  Matrix numerical_pose     = numericalDerivative31(projectInfinity3, pose1, point3D, K);
+  Matrix numerical_pose     = numericalDerivative31(projectInfinity3, pose, point3D, K);
-  Matrix numerical_point    = numericalDerivative32(projectInfinity3, pose1, point3D, K);
+  Matrix Hexpected2    = numericalDerivative32(projectInfinity3, pose, point3D, K);
-  Matrix numerical_point2x2 = numerical_point.block(0,0,2,2); // only the direction to the point matters
+  Matrix numerical_point2x2 = Hexpected2.block(0,0,2,2); // only the direction to the point matters
-  Matrix numerical_cal      = numericalDerivative33(projectInfinity3, pose1, point3D, K);
+  Matrix numerical_cal      = numericalDerivative33(projectInfinity3, pose, point3D, K);
-  CHECK(assert_equal(numerical_pose,     Dpose,  1e-7));
+  EXPECT(assert_equal(numerical_pose,     Dpose,  1e-7));
-  CHECK(assert_equal(numerical_point2x2, Dpoint, 1e-7));
+  EXPECT(assert_equal(numerical_point2x2, Dpoint, 1e-7));
-  CHECK(assert_equal(numerical_cal,      Dcal,   1e-7));
+  EXPECT(assert_equal(numerical_cal,      Dcal,   1e-7));
 }
 
 /* ************************************************************************* */
@@ -217,11 +216,80 @@ TEST( PinholeCamera, Dproject2)
 {
   Matrix Dcamera, Dpoint;
   Point2 result = camera.project2(point1, Dcamera, Dpoint);
-  Matrix numerical_camera = numericalDerivative21(project4, camera, point1);
+  Matrix Hexpected1 = numericalDerivative21(project4, camera, point1);
-  Matrix numerical_point  = numericalDerivative22(project4, camera, point1);
+  Matrix Hexpected2  = numericalDerivative22(project4, camera, point1);
-  CHECK(assert_equal(result, Point2(-100,  100) ));
+  EXPECT(assert_equal(result, Point2(-100,  100) ));
-  CHECK(assert_equal(numerical_camera, Dcamera, 1e-7));
+  EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7));
-  CHECK(assert_equal(numerical_point,  Dpoint,  1e-7));
+  EXPECT(assert_equal(Hexpected2,  Dpoint,  1e-7));
+}
+
+/* ************************************************************************* */
+static double range0(const Camera& camera, const Point3& point) {
+  return camera.range(point);
+}
+
+/* ************************************************************************* */
+TEST( PinholeCamera, range0) {
+  Matrix D1; Matrix D2;
+  double result = camera.range(point1, D1, D2);
+  Matrix Hexpected1 = numericalDerivative21(range0, camera, point1);
+  Matrix Hexpected2 = numericalDerivative22(range0, camera, point1);
+  EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result,
+      1e-9);
+  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
+  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
+}
+
+/* ************************************************************************* */
+static double range1(const Camera& camera, const Pose3& pose) {
+  return camera.range(pose);
+}
+
+/* ************************************************************************* */
+TEST( PinholeCamera, range1) {
+  Matrix D1; Matrix D2;
+  double result = camera.range(pose1, D1, D2);
+  Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1);
+  Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1);
+  EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
+  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
+  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
+}
+
+/* ************************************************************************* */
+typedef PinholeCamera<Cal3Bundler> Camera2;
+static const Cal3Bundler K2(625, 1e-3, 1e-3);
+static const Camera2 camera2(pose1, K2);
+static double range2(const Camera& camera, const Camera2& camera2) {
+  return camera.range<Cal3Bundler>(camera2);
+}
+
+/* ************************************************************************* */
+TEST( PinholeCamera, range2) {
+  Matrix D1; Matrix D2;
+  double result = camera.range<Cal3Bundler>(camera2, D1, D2);
+  Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2);
+  Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2);
+  EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
+  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
+  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
+}
+
+/* ************************************************************************* */
+static const CalibratedCamera camera3(pose1);
+static double range3(const Camera& camera, const CalibratedCamera& camera3) {
+  return camera.range(camera3);
+}
+
+/* ************************************************************************* */
+TEST( PinholeCamera, range3) {
+  Matrix D1; Matrix D2;
+  double result = camera.range(camera3, D1, D2);
+  Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3);
+  Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3);
+  EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
+  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
+  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testPose2.cpp

@@ -44,7 +44,7 @@ TEST(Pose2, constructors) {
   Pose2 origin;
   assert_equal(pose,origin);
   Pose2 t(M_PI/2.0+0.018, Point2(1.015, 2.01));
-  EXPECT(assert_equal(t,Pose2(t.matrix())));
+  EXPECT(assert_equal(t,Pose2((Matrix)t.matrix())));
 }
 
 /* ************************************************************************* */
@@ -519,7 +519,7 @@ TEST( Pose2, bearing )
   expectedH1 = numericalDerivative21(bearing_proxy, x2, l3);
   EXPECT(assert_equal(expectedH1,actualH1));
   expectedH2 = numericalDerivative22(bearing_proxy, x2, l3);
-  EXPECT(assert_equal(expectedH1,actualH1));
+  EXPECT(assert_equal(expectedH2,actualH2));
 
   // establish bearing is indeed 45 degrees even if rotated
   Rot2 actual34 = x3.bearing(l4, actualH1, actualH2);
@@ -529,7 +529,7 @@ TEST( Pose2, bearing )
   expectedH1 = numericalDerivative21(bearing_proxy, x3, l4);
   expectedH2 = numericalDerivative22(bearing_proxy, x3, l4);
   EXPECT(assert_equal(expectedH1,actualH1));
-  EXPECT(assert_equal(expectedH1,actualH1));
+  EXPECT(assert_equal(expectedH2,actualH2));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testRot2.cpp

@@ -49,7 +49,9 @@ TEST( Rot2, unit)
 /* ************************************************************************* */
 TEST( Rot2, transpose)
 {
-  CHECK(assert_equal(R.inverse().matrix(),R.transpose()));
+  Matrix expected = R.inverse().matrix();
+  Matrix actual = R.transpose();
+  CHECK(assert_equal(expected,actual));
 }
 
 /* ************************************************************************* */

gtsam/geometry/tests/testRot3.cpp

@@ -37,7 +37,6 @@ GTSAM_CONCEPT_LIE_INST(Rot3)
 static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
 static Point3 P(0.2, 0.7, -2.0);
 static double error = 1e-9, epsilon = 0.001;
-static const Matrix I3 = eye(3);
 
 /* ************************************************************************* */
 TEST( Rot3, chart)
@@ -50,7 +49,7 @@ TEST( Rot3, chart)
 /* ************************************************************************* */
 TEST( Rot3, constructor)
 {
-  Rot3 expected(I3);
+  Rot3 expected((Matrix)I_3x3);
   Point3 r1(1,0,0), r2(0,1,0), r3(0,0,1);
   Rot3 actual(r1, r2, r3);
   CHECK(assert_equal(actual,expected));
@@ -95,7 +94,7 @@ Rot3 slow_but_correct_rodriguez(const Vector& w) {
   double t = norm_2(w);
   Matrix J = skewSymmetric(w / t);
   if (t < 1e-5) return Rot3();
-  Matrix R = I3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
+  Matrix R = I_3x3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
   return R;
 }
 
@@ -359,7 +358,7 @@ TEST( Rot3, inverse )
   Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
 
   Rot3 I;
-  Matrix actualH;
+  Matrix3 actualH;
   Rot3 actual = R.inverse(actualH);
   CHECK(assert_equal(I,R*actual));
   CHECK(assert_equal(I,actual*R));
@@ -482,7 +481,7 @@ TEST( Rot3, RQ)
   Vector actual;
   boost::tie(actualK, actual) = RQ(R.matrix());
   Vector expected = Vector3(0.14715, 0.385821, 0.231671);
-  CHECK(assert_equal(I3,actualK));
+  CHECK(assert_equal(I_3x3,actualK));
   CHECK(assert_equal(expected,actual,1e-6));
 
   // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z]
@@ -516,7 +515,7 @@ TEST( Rot3, expmapStability ) {
                           w(2), 0.0, -w(0),
                           -w(1), w(0), 0.0 ).finished();
   Matrix W2 = W*W;
-  Matrix Rmat = I3 + (1.0-theta2/6.0 + theta2*theta2/120.0
+  Matrix Rmat = I_3x3 + (1.0-theta2/6.0 + theta2*theta2/120.0
       - theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ;
   Rot3 expectedR( Rmat );
   CHECK(assert_equal(expectedR, actualR, 1e-10));
@@ -578,7 +577,7 @@ TEST(Rot3, quaternion) {
 TEST( Rot3, Cayley ) {
   Matrix A = skewSymmetric(1,2,-3);
   Matrix Q = Cayley(A);
-  EXPECT(assert_equal(I3, trans(Q)*Q));
+  EXPECT(assert_equal((Matrix)I_3x3, trans(Q)*Q));
   EXPECT(assert_equal(A, Cayley(Q)));
 }
 

gtsam/geometry/tests/testRot3M.cpp

@@ -37,7 +37,6 @@ GTSAM_CONCEPT_LIE_INST(Rot3)
 
 static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
 static Point3 P(0.2, 0.7, -2.0);
-static const Matrix I3 = eye(3);
 
 /* ************************************************************************* */
 TEST(Rot3, manifold_cayley)

gtsam/geometry/tests/testStereoCamera.cpp

@@ -74,7 +74,7 @@ TEST( StereoCamera, project)
 
 /* ************************************************************************* */
 
-static Pose3 camera1((Matrix)(Matrix(3,3) <<
+static Pose3 camPose((Matrix)(Matrix(3,3) <<
            1., 0., 0.,
            0.,-1., 0.,
            0., 0.,-1.
@@ -82,33 +82,41 @@ static Pose3 camera1((Matrix)(Matrix(3,3) <<
         Point3(0,0,6.25));
 
 static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
-static StereoCamera stereoCam(Pose3(), K);
+static StereoCamera stereoCam(camPose, K);
 
 // point X Y Z in meters
-static Point3 p(0, 0, 5);
+static Point3 landmark(0, 0, 5);
 
 /* ************************************************************************* */
-static StereoPoint2 project_(const StereoCamera& cam, const Point3& point) { return cam.project(point); }
+static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_S2Stereo& K) {
-TEST( StereoCamera, Dproject_stereo_pose)
+  return StereoCamera(pose, boost::make_shared<Cal3_S2Stereo>(K)).project(point);
-{
-  Matrix expected = numericalDerivative21<StereoPoint2,StereoCamera,Point3>(project_,stereoCam, p);
-  Matrix actual; stereoCam.project(p, actual, boost::none);
-  CHECK(assert_equal(expected,actual,1e-7));
 }
 
 /* ************************************************************************* */
-TEST( StereoCamera, Dproject_stereo_point)
+TEST( StereoCamera, Dproject)
 {
-  Matrix expected = numericalDerivative22<StereoPoint2,StereoCamera,Point3>(project_,stereoCam, p);
+  Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K);
-  Matrix actual; stereoCam.project(p, boost::none, actual);
+  Matrix actual1; stereoCam.project(landmark, actual1, boost::none, boost::none);
-  CHECK(assert_equal(expected,actual,1e-8));
+  CHECK(assert_equal(expected1,actual1,1e-7));
+
+  Matrix expected2 = numericalDerivative32(project3,camPose, landmark, *K);
+  Matrix actual2; stereoCam.project(landmark, boost::none, actual2, boost::none);
+  CHECK(assert_equal(expected2,actual2,1e-7));
+
+  Matrix expected3 = numericalDerivative33(project3,camPose, landmark, *K);
+  Matrix actual3; stereoCam.project(landmark, boost::none, boost::none, actual3);
+//  CHECK(assert_equal(expected3,actual3,1e-8));
 }
 
+/* ************************************************************************* */
 TEST( StereoCamera, backproject)
 {
+  Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5));
+  StereoCamera stereoCam2(Pose3(), K2);
+
   Point3 expected(1.2, 2.3, 4.5);
-  StereoPoint2 stereo_point = stereoCam.project(expected);
+  StereoPoint2 stereo_point = stereoCam2.project(expected);
-  Point3 actual = stereoCam.backproject(stereo_point);
+  Point3 actual = stereoCam2.backproject(stereo_point);
   CHECK(assert_equal(expected,actual,1e-8));
 }
 

gtsam/geometry/tests/testTriangulation.cpp

@@ -262,25 +262,6 @@ TEST( triangulation, twoIdenticalPoses) {
  }
  */
 
-//******************************************************************************
-TEST( triangulation, TriangulationFactor ) {
-  // Create the factor with a measurement that is 3 pixels off in x
-  Key pointKey(1);
-  SharedNoiseModel model;
-  typedef TriangulationFactor<> Factor;
-  Factor factor(camera1, z1, model, pointKey);
-
-  // Use the factor to calculate the Jacobians
-  Matrix HActual;
-  factor.evaluateError(landmark, HActual);
-
-  Matrix HExpected = numericalDerivative11<Vector,Point3>(
-      boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);
-
-  // Verify the Jacobians are correct
-  CHECK(assert_equal(HExpected, HActual, 1e-3));
-}
-
 //******************************************************************************
 int main() {
   TestResult tr;

gtsam/geometry/tests/testUnit3.cpp

@@ -108,9 +108,9 @@ TEST(Unit3, unrotate) {
 TEST(Unit3, error) {
   Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
   r = p.retract(Vector2(0.8, 0));
-  EXPECT(assert_equal(Vector2(0, 0), p.error(p), 1e-8));
+  EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-8));
-  EXPECT(assert_equal(Vector2(0.479426, 0), p.error(q), 1e-5));
+  EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5));
-  EXPECT(assert_equal(Vector2(0.717356, 0), p.error(r), 1e-5));
+  EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5));
 
   Matrix actual, expected;
   // Use numerical derivatives to calculate the expected Jacobian

gtsam/geometry/triangulation.cpp

@@ -30,7 +30,7 @@ namespace gtsam {
  * @param rank_tol SVD rank tolerance
  * @return Triangulated Point3
  */
-Point3 triangulateDLT(const std::vector<Matrix>& projection_matrices,
+Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices,
     const std::vector<Point2>& measurements, double rank_tol) {
 
   // number of cameras
@@ -41,7 +41,7 @@ Point3 triangulateDLT(const std::vector<Matrix>& projection_matrices,
 
   for (size_t i = 0; i < m; i++) {
     size_t row = i * 2;
-    const Matrix& projection = projection_matrices.at(i);
+    const Matrix34& projection = projection_matrices.at(i);
     const Point2& p = measurements.at(i);
 
     // build system of equations

gtsam/geometry/triangulation.h

@@ -19,12 +19,10 @@
 #pragma once
 
 
-#include <gtsam/geometry/TriangulationFactor.h>
+#include <gtsam/slam/TriangulationFactor.h>
+#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/inference/Symbol.h>
-#include <gtsam/slam/PriorFactor.h>
-
-#include <vector>
 
 namespace gtsam {
 
@@ -53,7 +51,7 @@ public:
  * @return Triangulated Point3
  */
 GTSAM_EXPORT Point3 triangulateDLT(
-    const std::vector<Matrix>& projection_matrices,
+    const std::vector<Matrix34>& projection_matrices,
     const std::vector<Point2>& measurements, double rank_tol);
 
 ///
@@ -189,12 +187,11 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
     throw(TriangulationUnderconstrainedException());
 
   // construct projection matrices from poses & calibration
-  std::vector<Matrix> projection_matrices;
+  std::vector<Matrix34> projection_matrices;
   BOOST_FOREACH(const Pose3& pose, poses) {
     projection_matrices.push_back(
-        sharedCal->K() * sub(pose.inverse().matrix(), 0, 3, 0, 4));
+        sharedCal->K() * (pose.inverse().matrix()).block<3,4>(0,0));
   }
-
   // Triangulate linearly
   Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
 
@@ -240,12 +237,11 @@ Point3 triangulatePoint3(
 
   // construct projection matrices from poses & calibration
   typedef PinholeCamera<CALIBRATION> Camera;
-  std::vector<Matrix> projection_matrices;
+  std::vector<Matrix34> projection_matrices;
-  BOOST_FOREACH(const Camera& camera, cameras)
+  BOOST_FOREACH(const Camera& camera, cameras) {
-    projection_matrices.push_back(
+  Matrix P_ = (camera.pose().inverse().matrix());
-        camera.calibration().K()
+    projection_matrices.push_back(camera.calibration().K()* (P_.block<3,4>(0,0)) );
-            * sub(camera.pose().inverse().matrix(), 0, 3, 0, 4));
+   }
-
   Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
 
   // The n refine using non-linear optimization

gtsam/linear/IterativeSolver.h

@@ -106,9 +106,9 @@ namespace gtsam {
     typedef boost::tuple<Key,size_t,Key> Base;
     KeyInfoEntry(){}
     KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {}
-    const size_t index() const { return this->get<0>(); }
+    size_t index() const { return this->get<0>(); }
-    const size_t dim() const { return this->get<1>(); }
+    size_t dim() const { return this->get<1>(); }
-    const size_t colstart() const { return this->get<2>(); }
+    size_t colstart() const { return this->get<2>(); }
   };
 
   /************************************************************************************/

gtsam/navigation/CombinedImuFactor.cpp

@@ -0,0 +1,497 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  CombinedImuFactor.cpp
+ *  @author Luca Carlone
+ *  @author Stephen Williams
+ *  @author Richard Roberts
+ *  @author Vadim Indelman
+ *  @author David Jensen
+ *  @author Frank Dellaert
+ **/
+
+#include <gtsam/navigation/CombinedImuFactor.h>
+
+/* External or standard includes */
+#include <ostream>
+
+namespace gtsam {
+
+using namespace std;
+
+//------------------------------------------------------------------------------
+// Inner class CombinedPreintegratedMeasurements
+//------------------------------------------------------------------------------
+CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasurements(
+    const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
+    const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
+    const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
+    const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration) :
+            biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()),
+            deltaRij_(Rot3()), deltaTij_(0.0),
+            delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
+            delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
+            delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration)
+{
+  measurementCovariance_.setZero();
+  measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance;
+  measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance;
+  measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance;
+  measurementCovariance_.block<3,3>(9,9) = biasAccCovariance;
+  measurementCovariance_.block<3,3>(12,12) = biasOmegaCovariance;
+  measurementCovariance_.block<6,6>(15,15) = biasAccOmegaInit;
+  PreintMeasCov_.setZero();
+}
+
+//------------------------------------------------------------------------------
+void CombinedImuFactor::CombinedPreintegratedMeasurements::print(const string& s) const{
+  cout << s << endl;
+  biasHat_.print("  biasHat");
+  cout << "  deltaTij " << deltaTij_ << endl;
+  cout << "  deltaPij [ " << deltaPij_.transpose() << " ]" << endl;
+  cout << "  deltaVij [ " << deltaVij_.transpose() << " ]" << endl;
+  deltaRij_.print("  deltaRij ");
+  cout << "  measurementCovariance [ " << measurementCovariance_ << " ]" << endl;
+  cout << "  PreintMeasCov [ " << PreintMeasCov_ << " ]" << endl;
+}
+
+//------------------------------------------------------------------------------
+bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(const CombinedPreintegratedMeasurements& expected, double tol) const{
+  return biasHat_.equals(expected.biasHat_, tol)
+  && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
+  && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
+  && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
+  && deltaRij_.equals(expected.deltaRij_, tol)
+  && fabs(deltaTij_ - expected.deltaTij_) < tol
+  && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
+  && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
+  && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
+  && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
+  && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
+}
+
+//------------------------------------------------------------------------------
+void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration(){
+  deltaPij_ = Vector3::Zero();
+  deltaVij_ = Vector3::Zero();
+  deltaRij_ = Rot3();
+  deltaTij_ = 0.0;
+  delPdelBiasAcc_ = Z_3x3;
+  delPdelBiasOmega_ = Z_3x3;
+  delVdelBiasAcc_ = Z_3x3;
+  delVdelBiasOmega_ = Z_3x3;
+  delRdelBiasOmega_ = Z_3x3;
+  PreintMeasCov_.setZero();
+}
+
+//------------------------------------------------------------------------------
+void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
+    const Vector3& measuredAcc, const Vector3& measuredOmega,
+    double deltaT, boost::optional<const Pose3&> body_P_sensor) {
+
+  // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
+  // (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
+
+  // First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty
+  Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
+  Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
+
+  // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
+  if(body_P_sensor){
+    Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
+    correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
+    Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
+    correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
+    // linear acceleration vector in the body frame
+  }
+
+  const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
+  const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
+  const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
+
+  // Update Jacobians
+  /* ----------------------------------------------------------------------------------------------------------------------- */
+  if(!use2ndOrderIntegration_){
+    delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
+    delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
+  }else{
+    delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
+    delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
+                                            * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
+  }
+
+  delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
+  delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
+  delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr  * deltaT;
+
+  // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
+  // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
+  // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
+  /* ----------------------------------------------------------------------------------------------------------------------- */
+  const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
+  const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
+
+  Rot3 Rot_j = deltaRij_ * Rincr;
+  const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
+  const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
+
+  // Single Jacobians to propagate covariance
+  Matrix3 H_pos_pos    = I_3x3;
+  Matrix3 H_pos_vel    = I_3x3 * deltaT;
+  Matrix3 H_pos_angles = Z_3x3;
+
+  Matrix3 H_vel_pos    = Z_3x3;
+  Matrix3 H_vel_vel    = I_3x3;
+  Matrix3 H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
+  // analytic expression corresponding to the following numerical derivative
+  // Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
+  Matrix3 H_vel_biasacc = - deltaRij_.matrix() * deltaT;
+
+  Matrix3 H_angles_pos   = Z_3x3;
+  Matrix3 H_angles_vel    = Z_3x3;
+  Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
+  Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT;
+  // analytic expression corresponding to the following numerical derivative
+  // Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
+
+  // overall Jacobian wrt preintegrated measurements (df/dx)
+  Matrix F(15,15);
+  F << H_pos_pos,    H_pos_vel,     H_pos_angles,          Z_3x3,                     Z_3x3,
+      H_vel_pos,     H_vel_vel,     H_vel_angles,      H_vel_biasacc,              Z_3x3,
+      H_angles_pos,  H_angles_vel,  H_angles_angles,   Z_3x3,                         H_angles_biasomega,
+      Z_3x3,         Z_3x3,         Z_3x3,             I_3x3,                         Z_3x3,
+      Z_3x3,         Z_3x3,         Z_3x3,             Z_3x3,                         I_3x3;
+
+  // first order uncertainty propagation
+  // Optimized matrix multiplication   (1/deltaT) * G * measurementCovariance * G.transpose()
+
+  Matrix G_measCov_Gt = Matrix::Zero(15,15);
+  // BLOCK DIAGONAL TERMS
+  G_measCov_Gt.block<3,3>(0,0) = deltaT * measurementCovariance_.block<3,3>(0,0);
+
+  G_measCov_Gt.block<3,3>(3,3) = (1/deltaT) * (H_vel_biasacc)  *
+      (measurementCovariance_.block<3,3>(3,3)  +  measurementCovariance_.block<3,3>(15,15) ) *
+      (H_vel_biasacc.transpose());
+
+  G_measCov_Gt.block<3,3>(6,6) = (1/deltaT) *  (H_angles_biasomega) *
+      (measurementCovariance_.block<3,3>(6,6)  +  measurementCovariance_.block<3,3>(18,18) ) *
+      (H_angles_biasomega.transpose());
+
+  G_measCov_Gt.block<3,3>(9,9) = deltaT * measurementCovariance_.block<3,3>(9,9);
+
+  G_measCov_Gt.block<3,3>(12,12) = deltaT * measurementCovariance_.block<3,3>(12,12);
+
+  // NEW OFF BLOCK DIAGONAL TERMS
+  Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block<3,3>(18,15) *  H_angles_biasomega.transpose();
+  G_measCov_Gt.block<3,3>(3,6) = block23;
+  G_measCov_Gt.block<3,3>(6,3) = block23.transpose();
+
+  PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt;
+
+  // Update preintegrated measurements
+  /* ----------------------------------------------------------------------------------------------------------------------- */
+  if(!use2ndOrderIntegration_){
+    deltaPij_ += deltaVij_ * deltaT;
+  }else{
+    deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
+  }
+  deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
+  deltaRij_ = deltaRij_ * Rincr;
+  deltaTij_ += deltaT;
+}
+
+//------------------------------------------------------------------------------
+// CombinedImuFactor methods
+//------------------------------------------------------------------------------
+CombinedImuFactor::CombinedImuFactor() :
+    preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Matrix::Zero(6,6)) {}
+
+//------------------------------------------------------------------------------
+CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
+    const CombinedPreintegratedMeasurements& preintegratedMeasurements,
+    const Vector3& gravity, const Vector3& omegaCoriolis,
+    boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis) :
+          Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
+          preintegratedMeasurements_(preintegratedMeasurements),
+          gravity_(gravity),
+          omegaCoriolis_(omegaCoriolis),
+          body_P_sensor_(body_P_sensor),
+          use2ndOrderCoriolis_(use2ndOrderCoriolis){
+}
+
+//------------------------------------------------------------------------------
+gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
+  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+      gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+}
+
+//------------------------------------------------------------------------------
+void CombinedImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
+  cout << s << "CombinedImuFactor("
+      << keyFormatter(this->key1()) << ","
+      << keyFormatter(this->key2()) << ","
+      << keyFormatter(this->key3()) << ","
+      << keyFormatter(this->key4()) << ","
+      << keyFormatter(this->key5()) << ","
+      << keyFormatter(this->key6()) << ")\n";
+  preintegratedMeasurements_.print("  preintegrated measurements:");
+  cout << "  gravity: [ " << gravity_.transpose() << " ]" << endl;
+  cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
+  this->noiseModel_->print("  noise model: ");
+  if(this->body_P_sensor_)
+    this->body_P_sensor_->print("  sensor pose in body frame: ");
+}
+
+//------------------------------------------------------------------------------
+bool CombinedImuFactor::equals(const NonlinearFactor& expected, double tol) const {
+  const This *e =  dynamic_cast<const This*> (&expected);
+  return e != NULL && Base::equals(*e, tol)
+  && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
+  && equal_with_abs_tol(gravity_, e->gravity_, tol)
+  && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
+  && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
+}
+
+//------------------------------------------------------------------------------
+Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
+    const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
+    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
+    boost::optional<Matrix&> H3, boost::optional<Matrix&> H4,
+    boost::optional<Matrix&> H5, boost::optional<Matrix&> H6) const {
+
+  const double& deltaTij = preintegratedMeasurements_.deltaTij_;
+  const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
+  const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
+
+  // we give some shorter name to rotations and translations
+  const Rot3 Rot_i = pose_i.rotation();
+  const Rot3 Rot_j = pose_j.rotation();
+  const Vector3 pos_i = pose_i.translation().vector();
+  const Vector3 pos_j = pose_j.translation().vector();
+
+  // We compute factor's Jacobians, according to [3]
+  /* ---------------------------------------------------------------------------------------------------- */
+  const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
+  // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
+
+  Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
+
+  Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
+      Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
+
+  const Rot3 deltaRij_biascorrected_corioliscorrected =
+      Rot3::Expmap( theta_biascorrected_corioliscorrected );
+
+  const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
+
+  const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
+
+  const Matrix3 Jtheta = -Jr_theta_bcc  * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
+
+  const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
+
+  if(H1) {
+    H1->resize(15,6);
+
+    Matrix3 dfPdPi;
+    Matrix3 dfVdPi;
+    if(use2ndOrderCoriolis_){
+      dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
+      dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
+    }
+    else{
+      dfPdPi = - Rot_i.matrix();
+      dfVdPi = Z_3x3;
+    }
+
+    (*H1) <<
+        // dfP/dRi
+        Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
+        + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
+        // dfP/dPi
+        dfPdPi,
+        // dfV/dRi
+        Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
+        + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
+        // dfV/dPi
+        dfVdPi,
+        // dfR/dRi
+        Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
+        // dfR/dPi
+        Z_3x3,
+        //dBiasAcc/dPi
+        Z_3x3, Z_3x3,
+        //dBiasOmega/dPi
+        Z_3x3, Z_3x3;
+  }
+
+  if(H2) {
+    H2->resize(15,3);
+    (*H2) <<
+        // dfP/dVi
+        - I_3x3 * deltaTij
+        + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij,  // Coriolis term - we got rid of the 2 wrt ins paper
+        // dfV/dVi
+        - I_3x3
+        + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
+        // dfR/dVi
+        Z_3x3,
+        //dBiasAcc/dVi
+        Z_3x3,
+        //dBiasOmega/dVi
+        Z_3x3;
+  }
+
+  if(H3) {
+    H3->resize(15,6);
+    (*H3) <<
+        // dfP/dPosej
+        Z_3x3, Rot_j.matrix(),
+        // dfV/dPosej
+        Matrix::Zero(3,6),
+        // dfR/dPosej
+        Jrinv_fRhat *  ( I_3x3 ), Z_3x3,
+        //dBiasAcc/dPosej
+        Z_3x3, Z_3x3,
+        //dBiasOmega/dPosej
+        Z_3x3, Z_3x3;
+  }
+
+  if(H4) {
+    H4->resize(15,3);
+    (*H4) <<
+        // dfP/dVj
+        Z_3x3,
+        // dfV/dVj
+        I_3x3,
+        // dfR/dVj
+        Z_3x3,
+        //dBiasAcc/dVj
+        Z_3x3,
+        //dBiasOmega/dVj
+        Z_3x3;
+  }
+
+  if(H5) {
+    const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
+    const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
+    const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
+
+    H5->resize(15,6);
+    (*H5) <<
+        // dfP/dBias_i
+        - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
+        - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
+        // dfV/dBias_i
+        - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
+        - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
+        // dfR/dBias_i
+        Matrix::Zero(3,3),
+        Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega),
+        //dBiasAcc/dBias_i
+        -I_3x3, Z_3x3,
+        //dBiasOmega/dBias_i
+        Z_3x3, -I_3x3;
+  }
+
+  if(H6) {
+    H6->resize(15,6);
+    (*H6) <<
+        // dfP/dBias_j
+        Z_3x3, Z_3x3,
+        // dfV/dBias_j
+        Z_3x3, Z_3x3,
+        // dfR/dBias_j
+        Z_3x3, Z_3x3,
+        //dBiasAcc/dBias_j
+        I_3x3, Z_3x3,
+        //dBiasOmega/dBias_j
+        Z_3x3, I_3x3;
+  }
+
+  // Evaluate residual error, according to [3]
+  /* ---------------------------------------------------------------------------------------------------- */
+  const Vector3 fp =
+      pos_j - pos_i
+      - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
+          + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
+          + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
+          - vel_i * deltaTij
+          + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
+          - 0.5 * gravity_ * deltaTij*deltaTij;
+
+  const Vector3 fv =
+      vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
+          + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
+          + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
+          + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij  // Coriolis term
+          - gravity_ * deltaTij;
+
+  const Vector3 fR = Rot3::Logmap(fRhat);
+
+  const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer();
+
+  const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope();
+
+  Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15
+
+  return r;
+}
+
+//------------------------------------------------------------------------------
+PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
+    const imuBias::ConstantBias& bias_i,
+    const CombinedPreintegratedMeasurements& preintegratedMeasurements,
+    const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis){
+
+  const double& deltaTij = preintegratedMeasurements.deltaTij_;
+  const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
+  const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
+
+  const Rot3 Rot_i = pose_i.rotation();
+  const Vector3 pos_i = pose_i.translation().vector();
+
+  // Predict state at time j
+  /* ---------------------------------------------------------------------------------------------------- */
+  Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
+      + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
+      + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
+      + vel_i * deltaTij
+      - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
+      + 0.5 * gravity * deltaTij*deltaTij;
+
+  Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
+      + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
+      + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
+      - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
+      + gravity * deltaTij);
+
+  if(use2ndOrderCoriolis){
+    pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;  // 2nd order coriolis term for position
+    vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
+  }
+
+  const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
+  // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
+  Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
+  Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
+      Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
+  const Rot3 deltaRij_biascorrected_corioliscorrected =
+      Rot3::Expmap( theta_biascorrected_corioliscorrected );
+  const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  );
+
+  Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
+
+  return PoseVelocityBias(pose_j, vel_j, bias_i);
+}
+
+} /// namespace gtsam

gtsam/navigation/CombinedImuFactor.h

@@ -11,714 +11,291 @@
 
 /**
  *  @file  CombinedImuFactor.h
- *  @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen
+ *  @author Luca Carlone
+ *  @author Stephen Williams
+ *  @author Richard Roberts
+ *  @author Vadim Indelman
+ *  @author David Jensen
+ *  @author Frank Dellaert
  **/
 
 #pragma once
 
 /* GTSAM includes */
-#include <gtsam/navigation/ImuBias.h>
-#include <gtsam/geometry/Pose3.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/linear/GaussianFactor.h>
+#include <gtsam/navigation/ImuBias.h>
 #include <gtsam/base/debug.h>
 
-/* External or standard includes */
-#include <ostream>
-
-
 namespace gtsam {
 
-  /**
+/**
-   * Struct to hold all state variables of CombinedImuFactor
+ *
-   * returned by predict function
+ * @addtogroup SLAM
-   */
+ *
-  struct PoseVelocityBias {
+ * If you are using the factor, please cite:
-    Pose3 pose;
+ * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
-    Vector3 velocity;
+ * independent sets in factor graphs: a unifying perspective based on smart factors,
-    imuBias::ConstantBias bias;
+ * Int. Conf. on Robotics and Automation (ICRA), 2014.
+ *
+ * REFERENCES:
+ * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
+ * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built
+ * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
+ * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
+ */
 
-    PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
+/**
-        const imuBias::ConstantBias _bias) :
+ * Struct to hold all state variables of CombinedImuFactor returned by Predict function
+ */
+struct PoseVelocityBias {
+  Pose3 pose;
+  Vector3 velocity;
+  imuBias::ConstantBias bias;
+
+  PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
+      const imuBias::ConstantBias _bias) :
         pose(_pose), velocity(_velocity), bias(_bias) {
-    }
+  }
-  };
+};
 
-  /**
+/**
-   * 
+ * CombinedImuFactor is a 6-ways factor involving previous state (pose and velocity of the vehicle, as well as bias
-   * @addtogroup SLAM
+ * at previous time step), and current state (pose, velocity, bias at current time step). According to the
-   *
+ * preintegration scheme proposed in [2], the CombinedImuFactor includes many IMU measurements, which are
-   * If you are using the factor, please cite:
+ * "summarized" using the CombinedPreintegratedMeasurements class. There are 3 main differences wrt ImuFactor:
-   * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
+ * 1) The factor is 6-ways, meaning that it also involves both biases (previous and current time step).
-   * independent sets in factor graphs: a unifying perspective based on smart factors,
+ * Therefore, the factor internally imposes the biases to be slowly varying; in particular, the matrices
-   * Int. Conf. on Robotics and Automation (ICRA), 2014.
+ * "biasAccCovariance" and "biasOmegaCovariance" described the random walk that models bias evolution.
-   *
+ * 2) The preintegration covariance takes into account the noise in the bias estimate used for integration.
-   * REFERENCES:
+ * 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves the correlation between the bias uncertainty
-   * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
+ * and the preintegrated measurements uncertainty.
-   * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built
+ */
-   * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
+class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> {
-   * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
+public:
+
+  /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
+   * and the corresponding covariance matrix. The measurements are then used to build the CombinedPreintegrated IMU factor (CombinedImuFactor).
+   * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received
+   * from the IMU) so as to avoid costly integration at time of factor construction.
    */
+  class CombinedPreintegratedMeasurements {
 
-  class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> {
+    friend class CombinedImuFactor;
+
+  protected:
+    imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
+    Eigen::Matrix<double,21,21> measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector
+    ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
+
+    Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
+    Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
+    Rot3 deltaRij_;    ///< Preintegrated relative orientation (in frame i)
+    double deltaTij_;  ///< Time interval from i to j
+
+    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
+    Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
+
+    Eigen::Matrix<double,15,15> PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements
+    ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
+    ///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation
+    ///< between the preintegrated measurements and the biases
+
+    bool use2ndOrderIntegration_; ///< Controls the order of integration
 
   public:
 
-    /** Struct to store results of preintegrating IMU measurements.  Can be build
+    /**
-     * incrementally so as to avoid costly integration at time of factor construction. */
+     *  Default constructor, initializes the class with no measurements
+     *  @param bias                       Current estimate of acceleration and rotation rate biases
+     *  @param measuredAccCovariance      Covariance matrix of measuredAcc
+     *  @param measuredOmegaCovariance    Covariance matrix of measured Angular Rate
+     *  @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position)
+     *  @param biasAccCovariance          Covariance matrix of biasAcc (random walk describing BIAS evolution)
+     *  @param biasOmegaCovariance        Covariance matrix of biasOmega (random walk describing BIAS evolution)
+     *  @param biasAccOmegaInit           Covariance of biasAcc & biasOmega when preintegrating measurements
+     *  @param use2ndOrderIntegration     Controls the order of integration
+     *  (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
+     */
+    CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
+        const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
+        const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
+        const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration = false);
 
-    /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
+    /// print
-     * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/
+    void print(const std::string& s = "Preintegrated Measurements:") const;
-    class CombinedPreintegratedMeasurements {
-      friend class CombinedImuFactor;
-    protected:
-      imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
-      Matrix measurementCovariance_; ///< (Raw measurements uncertainty) Covariance of the vector
-      ///< [integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
 
-      Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
+    /// equals
-      Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
+    bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const;
-      Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
-      double deltaTij_; ///< Time interval from i to j
 
-      Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
+    /// Re-initialize CombinedPreintegratedMeasurements
-      Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
+    void resetIntegration();
-      Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
-      Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
-      Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
-      Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
-      bool use2ndOrderIntegration_; ///< Controls the order of integration
-//    public:
-      ///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases
-      ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
-      /** Default constructor, initialize with no IMU measurements */
-    public:
-      CombinedPreintegratedMeasurements(
-          const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
-          const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
-          const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
-          const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
-          const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
-          const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
-          const Matrix& biasAccOmegaInit, ///< Covariance of biasAcc & biasOmega when preintegrating measurements
-          const bool use2ndOrderIntegration = false ///< Controls the order of integration
-          ///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements)
-      ) : biasHat_(bias), measurementCovariance_(21,21), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
-      delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
-      delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
-      delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(15,15)),
-      use2ndOrderIntegration_(use2ndOrderIntegration)
-      {
-          // COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21)
-        measurementCovariance_ << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),      Matrix3::Zero(),
-                                       Matrix3::Zero(), measuredAccCovariance,  Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),     Matrix3::Zero(),
-                                       Matrix3::Zero(),   Matrix3::Zero(), measuredOmegaCovariance, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),  Matrix3::Zero(),
-                                       Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasAccCovariance, Matrix3::Zero(),   Matrix3::Zero(),        Matrix3::Zero(),
-                                       Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), biasOmegaCovariance, Matrix3::Zero(),        Matrix3::Zero(),
-                                       Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),     biasAccOmegaInit.block(0,0,3,3),    biasAccOmegaInit.block(0,3,3,3),
-                                       Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),     biasAccOmegaInit.block(3,0,3,3),        biasAccOmegaInit.block(3,3,3,3);
 
-      }
+    /**
+     * Add a single IMU measurement to the preintegration.
+     * @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
+     * @param measuredOmega Measured angular velocity (as given by the sensor)
+     * @param deltaT Time interval between two consecutive IMU measurements
+     * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
+     */
+    void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
+        boost::optional<const Pose3&> body_P_sensor = boost::none);
 
-      CombinedPreintegratedMeasurements() :
+    /// methods to access class variables
-      biasHat_(imuBias::ConstantBias()), measurementCovariance_(21,21), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
+    Matrix measurementCovariance() const {return measurementCovariance_;}
-      delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
+    Matrix deltaRij() const {return deltaRij_.matrix();}
-      delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
+    double deltaTij() const{return deltaTij_;}
-      delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(15,15)),
+    Vector deltaPij() const {return deltaPij_;}
-      use2ndOrderIntegration_(false) ///< Controls the order of integration
+    Vector deltaVij() const {return deltaVij_;}
-      {
+    Vector biasHat() const { return biasHat_.vector();}
-      }
+    Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;}
+    Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;}
+    Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;}
+    Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;}
+    Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
+    Matrix PreintMeasCov() const { return PreintMeasCov_;}
 
-      /** print */
+    /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
-      void print(const std::string& s = "Preintegrated Measurements:") const {
+    // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-        std::cout << s << std::endl;
+    static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
-        biasHat_.print("  biasHat");
+        const Vector3& delta_angles, const Vector& delta_vel_in_t0){
-        std::cout << "  deltaTij " << deltaTij_ << std::endl;
+      // Note: all delta terms refer to an IMU\sensor system at t0
-        std::cout << "  deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
+      Vector body_t_a_body = msr_acc_t;
-        std::cout << "  deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
+      Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-        deltaRij_.print("  deltaRij ");
+      return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
-        std::cout << "  measurementCovariance [ " << measurementCovariance_ << " ]" << std::endl;
+    }
-        std::cout << "  PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl;
-      }
 
-      /** equals */
+    // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-      bool equals(const CombinedPreintegratedMeasurements& expected, double tol=1e-9) const {
+    static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
-        return biasHat_.equals(expected.biasHat_, tol)
+        const Vector3& delta_angles){
-            && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
+      // Note: all delta terms refer to an IMU\sensor system at t0
-            && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
+      // Calculate the corrected measurements using the Bias object
-            && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
+      Vector body_t_omega_body= msr_gyro_t;
-            && deltaRij_.equals(expected.deltaRij_, tol)
+      Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-            && std::fabs(deltaTij_ - expected.deltaTij_) < tol
+      R_t_to_t0    = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
-            && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
+      return Rot3::Logmap(R_t_to_t0);
-            && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
+    }
-            && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
+    /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
-            && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
-            && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
-      }
-
-      void resetIntegration(){
-        deltaPij_ = Vector3::Zero();
-        deltaVij_ = Vector3::Zero();
-        deltaRij_ = Rot3();
-        deltaTij_ = 0.0;
-        delPdelBiasAcc_ = Matrix3::Zero();
-        delPdelBiasOmega_ = Matrix3::Zero();
-        delVdelBiasAcc_ = Matrix3::Zero();
-        delVdelBiasOmega_ = Matrix3::Zero();
-        delRdelBiasOmega_ = Matrix3::Zero();
-        PreintMeasCov_ = Matrix::Zero(15,15);
-      }
-
-      /** Add a single IMU measurement to the preintegration. */
-      void integrateMeasurement(
-          const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame)
-          const Vector3& measuredOmega, ///< Measured angular velocity (in body frame)
-          double deltaT, ///< Time step
-          boost::optional<const Pose3&> body_P_sensor = boost::none ///< Sensor frame
-      ) {
-        // NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
-        // First we compensate the measurements for the bias: since we have only an estimate of the bias, the covariance includes the corresponding uncertainty
-        Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
-        Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
-
-        // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
-        if(body_P_sensor){
-          Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
-          correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
-          Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
-          correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
-          // linear acceleration vector in the body frame
-        }
-
-        const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
-        const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
-        const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
-
-        // Update Jacobians
-        /* ----------------------------------------------------------------------------------------------------------------------- */
-        if(!use2ndOrderIntegration_){
-          delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
-          delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
-        }else{
-          delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
-          delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
-                                        * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
-        }
-
-        delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
-        delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
-        delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr  * deltaT;
-
-        // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
-        // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
-        // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
-        /* ----------------------------------------------------------------------------------------------------------------------- */
-        Matrix3 Z_3x3 = Matrix3::Zero();
-        Matrix3 I_3x3 = Matrix3::Identity();
-        const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
-        const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
-
-        Rot3 Rot_j = deltaRij_ * Rincr;
-        const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
-        const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
-
-        // Single Jacobians to propagate covariance
-        Matrix3 H_pos_pos    = I_3x3;
-        Matrix3 H_pos_vel    = I_3x3 * deltaT;
-        Matrix3 H_pos_angles = Z_3x3;
-
-        Matrix3 H_vel_pos    = Z_3x3;
-        Matrix3 H_vel_vel    = I_3x3;
-        Matrix3 H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
-        // analytic expression corresponding to the following numerical derivative
-        // Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
-        Matrix3 H_vel_biasacc = - deltaRij_.matrix() * deltaT;
-
-        Matrix3 H_angles_pos   = Z_3x3;
-        Matrix3 H_angles_vel    = Z_3x3;
-        Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
-        Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT;
-        // analytic expression corresponding to the following numerical derivative
-        // Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
-
-        // overall Jacobian wrt preintegrated measurements (df/dx)
-        Matrix F(15,15);
-        F << H_pos_pos,    H_pos_vel,     H_pos_angles,          Z_3x3,                     Z_3x3,
-                H_vel_pos,     H_vel_vel,     H_vel_angles,      H_vel_biasacc,              Z_3x3,
-                H_angles_pos,  H_angles_vel,  H_angles_angles,   Z_3x3,                         H_angles_biasomega,
-                Z_3x3,         Z_3x3,         Z_3x3,             I_3x3,                         Z_3x3,
-                Z_3x3,         Z_3x3,         Z_3x3,             Z_3x3,                         I_3x3;
-
-
-        // first order uncertainty propagation
-        // Optimized matrix multiplication   (1/deltaT) * G * measurementCovariance * G.transpose()
-
-        Matrix G_measCov_Gt = Matrix::Zero(15,15);
-        // BLOCK DIAGONAL TERMS
-        G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance_.block(0,0,3,3);
-
-        G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc)  *
-            (measurementCovariance_.block(3,3,3,3)  +  measurementCovariance_.block(15,15,3,3) ) *
-            (H_vel_biasacc.transpose());
-
-        G_measCov_Gt.block(6,6,3,3) = (1/deltaT) *  (H_angles_biasomega) *
-            (measurementCovariance_.block(6,6,3,3)  +  measurementCovariance_.block(18,18,3,3) ) *
-            (H_angles_biasomega.transpose());
-
-        G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance_.block(9,9,3,3);
-
-        G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance_.block(12,12,3,3);
-
-        // NEW OFF BLOCK DIAGONAL TERMS
-        Matrix3 block23 = H_vel_biasacc * measurementCovariance_.block(18,15,3,3) *  H_angles_biasomega.transpose();
-        G_measCov_Gt.block(3,6,3,3) = block23;
-        G_measCov_Gt.block(6,3,3,3) = block23.transpose();
-
-        PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + G_measCov_Gt;
-
-        // Update preintegrated measurements
-        /* ----------------------------------------------------------------------------------------------------------------------- */
-        if(!use2ndOrderIntegration_){
-          deltaPij_ += deltaVij_ * deltaT;
-        }else{
-          deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
-        }
-        deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
-        deltaRij_ = deltaRij_ * Rincr;
-        deltaTij_ += deltaT;
-      }
-
-      /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
-      // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-      static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
-              const Vector3& delta_angles, const Vector& delta_vel_in_t0){
-
-          // Note: all delta terms refer to an IMU\sensor system at t0
-
-        Vector body_t_a_body = msr_acc_t;
-        Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-
-          return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
-      }
-
-      // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-      static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
-              const Vector3& delta_angles){
-
-          // Note: all delta terms refer to an IMU\sensor system at t0
-
-          // Calculate the corrected measurements using the Bias object
-        Vector body_t_omega_body= msr_gyro_t;
-
-          Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-
-          R_t_to_t0    = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
-          return Rot3::Logmap(R_t_to_t0);
-      }
-      /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
-      Matrix measurementCovariance() const {return measurementCovariance_;}
-      Matrix deltaRij() const {return deltaRij_.matrix();}
-      double deltaTij() const{return deltaTij_;}
-      Vector deltaPij() const {return deltaPij_;}
-      Vector deltaVij() const {return deltaVij_;}
-      Vector biasHat() const { return biasHat_.vector();}
-      Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;}
-      Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;}
-      Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;}
-      Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;}
-      Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
-      Matrix PreintMeasCov() const { return PreintMeasCov_;}
-
-    private:
-      /** Serialization function */
-      friend class boost::serialization::access;
-      template<class ARCHIVE>
-      void serialize(ARCHIVE & ar, const unsigned int version) {
-        ar & BOOST_SERIALIZATION_NVP(biasHat_);
-        ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
-        ar & BOOST_SERIALIZATION_NVP(deltaPij_);
-        ar & BOOST_SERIALIZATION_NVP(deltaVij_);
-        ar & BOOST_SERIALIZATION_NVP(deltaRij_);
-        ar & BOOST_SERIALIZATION_NVP(deltaTij_);
-        ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
-        ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
-        ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
-        ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
-        ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
-      }
-    };
 
   private:
-
-    typedef CombinedImuFactor This;
-    typedef NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> Base;
-
-    CombinedPreintegratedMeasurements preintegratedMeasurements_;
-    Vector3 gravity_;
-    Vector3 omegaCoriolis_;
-    boost::optional<Pose3> body_P_sensor_;        ///< The pose of the sensor in the body frame
-
-    bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
-
-  public:
-
-    /** Shorthand for a smart pointer to a factor */
-#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
-    typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
-#else
-      typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
-#endif
-
-    /** Default constructor - only use for serialization */
-    CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {}
-
-    /** Constructor */
-    CombinedImuFactor(
-      Key pose_i, ///< previous pose key
-      Key vel_i, ///< previous velocity key
-      Key pose_j, ///< current pose key
-      Key vel_j, ///< current velocity key
-      Key bias_i, ///< previous bias key
-      Key bias_j, ///< current bias key
-        const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements
-        const Vector3& gravity, ///< gravity vector
-        const Vector3& omegaCoriolis, ///< rotation rate of inertial frame
-        boost::optional<const Pose3&> body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame
-        const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect
-    ) :
-      Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
-      preintegratedMeasurements_(preintegratedMeasurements),
-      gravity_(gravity),
-      omegaCoriolis_(omegaCoriolis),
-      body_P_sensor_(body_P_sensor),
-      use2ndOrderCoriolis_(use2ndOrderCoriolis){
-    }
-
-    virtual ~CombinedImuFactor() {}
-
-    /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
-      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
-
-    /** implement functions needed for Testable */
-
-    /** print */
-    virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-      std::cout << s << "CombinedImuFactor("
-          << keyFormatter(this->key1()) << ","
-          << keyFormatter(this->key2()) << ","
-          << keyFormatter(this->key3()) << ","
-          << keyFormatter(this->key4()) << ","
-          << keyFormatter(this->key5()) << ","
-          << keyFormatter(this->key6()) << ")\n";
-      preintegratedMeasurements_.print("  preintegrated measurements:");
-      std::cout << "  gravity: [ " << gravity_.transpose() << " ]" << std::endl;
-      std::cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
-      this->noiseModel_->print("  noise model: ");
-      if(this->body_P_sensor_)
-        this->body_P_sensor_->print("  sensor pose in body frame: ");
-    }
-
-    /** equals */
-    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
-      const This *e =  dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol)
-          && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
-          && equal_with_abs_tol(gravity_, e->gravity_, tol)
-          && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
-          && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
-    }
-
-    /** Access the preintegrated measurements. */
-    const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
-      return preintegratedMeasurements_; }
-
-    const Vector3& gravity() const { return gravity_; }
-
-    const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
-
-    /** implement functions needed to derive from Factor */
-
-    /** vector of errors */
-    Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
-        const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
-        boost::optional<Matrix&> H1 = boost::none,
-        boost::optional<Matrix&> H2 = boost::none,
-        boost::optional<Matrix&> H3 = boost::none,
-        boost::optional<Matrix&> H4 = boost::none,
-        boost::optional<Matrix&> H5 = boost::none,
-        boost::optional<Matrix&> H6 = boost::none) const
-    {
-
-      const double& deltaTij = preintegratedMeasurements_.deltaTij_;
-      const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
-      const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
-
-      // we give some shorter name to rotations and translations
-      const Rot3 Rot_i = pose_i.rotation();
-      const Rot3 Rot_j = pose_j.rotation();
-      const Vector3 pos_i = pose_i.translation().vector();
-      const Vector3 pos_j = pose_j.translation().vector();
-
-      // We compute factor's Jacobians, according to [3]
-      /* ---------------------------------------------------------------------------------------------------- */
-      const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
-      // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
-
-      Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-
-      Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-          Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
-
-      const Rot3 deltaRij_biascorrected_corioliscorrected =
-          Rot3::Expmap( theta_biascorrected_corioliscorrected );
-
-      const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
-
-      const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
-
-      const Matrix3 Jtheta = -Jr_theta_bcc  * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
-
-      const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
-
-      /*
-        (*H1) <<
-            // dfP/dRi
-            Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
-                + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
-                // dfP/dPi
-                - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij,
-                // dfV/dRi
-                Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
-                    + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
-                    // dfV/dPi
-                    skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij,
-                    // dfR/dRi
-                    Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
-                    // dfR/dPi
-                    Matrix3::Zero(),
-                    //dBiasAcc/dPi
-                    Matrix3::Zero(), Matrix3::Zero(),
-                    //dBiasOmega/dPi
-                    Matrix3::Zero(), Matrix3::Zero();
-          */
-      if(H1) {
-        H1->resize(15,6);
-
-        Matrix3 dfPdPi;
-        Matrix3 dfVdPi;
-        if(use2ndOrderCoriolis_){
-          dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
-          dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
-        }
-        else{
-          dfPdPi = - Rot_i.matrix();
-          dfVdPi = Matrix3::Zero();
-        }
-
-    (*H1) <<
-      // dfP/dRi
-      Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
-        + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
-      // dfP/dPi
-      dfPdPi,
-      // dfV/dRi
-      Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
-        + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
-      // dfV/dPi
-      dfVdPi,
-      // dfR/dRi
-      Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
-      // dfR/dPi
-      Matrix3::Zero(),
-      //dBiasAcc/dPi
-      Matrix3::Zero(), Matrix3::Zero(),
-      //dBiasOmega/dPi
-      Matrix3::Zero(), Matrix3::Zero();
-      }
-
-      if(H2) {
-        H2->resize(15,3);
-        (*H2) <<
-            // dfP/dVi
-            - Matrix3::Identity() * deltaTij
-            + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij,  // Coriolis term - we got rid of the 2 wrt ins paper
-            // dfV/dVi
-            - Matrix3::Identity()
-      + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
-      // dfR/dVi
-      Matrix3::Zero(),
-      //dBiasAcc/dVi
-      Matrix3::Zero(),
-      //dBiasOmega/dVi
-      Matrix3::Zero();
-      }
-
-      if(H3) {
-
-        H3->resize(15,6);
-        (*H3) <<
-            // dfP/dPosej
-            Matrix3::Zero(), Rot_j.matrix(),
-            // dfV/dPosej
-            Matrix::Zero(3,6),
-            // dfR/dPosej
-            Jrinv_fRhat *  ( Matrix3::Identity() ), Matrix3::Zero(),
-            //dBiasAcc/dPosej
-            Matrix3::Zero(), Matrix3::Zero(),
-            //dBiasOmega/dPosej
-            Matrix3::Zero(), Matrix3::Zero();
-      }
-
-      if(H4) {
-        H4->resize(15,3);
-        (*H4) <<
-            // dfP/dVj
-            Matrix3::Zero(),
-            // dfV/dVj
-            Matrix3::Identity(),
-            // dfR/dVj
-            Matrix3::Zero(),
-            //dBiasAcc/dVj
-            Matrix3::Zero(),
-            //dBiasOmega/dVj
-            Matrix3::Zero();
-      }
-
-      if(H5) {
-        const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
-        const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
-        const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
-
-        H5->resize(15,6);
-        (*H5) <<
-            // dfP/dBias_i
-            - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
-            - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
-            // dfV/dBias_i
-            - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
-            - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
-            // dfR/dBias_i
-            Matrix::Zero(3,3),
-            Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega),
-            //dBiasAcc/dBias_i
-            -Matrix3::Identity(), Matrix3::Zero(),
-            //dBiasOmega/dBias_i
-            Matrix3::Zero(), -Matrix3::Identity();
-      }
-
-      if(H6) {
-
-          H6->resize(15,6);
-          (*H6) <<
-                  // dfP/dBias_j
-                  Matrix3::Zero(), Matrix3::Zero(),
-                  // dfV/dBias_j
-                  Matrix3::Zero(), Matrix3::Zero(),
-                  // dfR/dBias_j
-                  Matrix3::Zero(), Matrix3::Zero(),
-                  //dBiasAcc/dBias_j
-                  Matrix3::Identity(), Matrix3::Zero(),
-                  //dBiasOmega/dBias_j
-                  Matrix3::Zero(), Matrix3::Identity();
-      }
-
-      // Evaluate residual error, according to [3]
-      /* ---------------------------------------------------------------------------------------------------- */
-      const Vector3 fp =
-          pos_j - pos_i
-          - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
-              + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
-              + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
-              - vel_i * deltaTij
-              + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-              - 0.5 * gravity_ * deltaTij*deltaTij;
-
-      const Vector3 fv =
-          vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
-              + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
-              + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
-              + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij  // Coriolis term
-              - gravity_ * deltaTij;
-
-      const Vector3 fR = Rot3::Logmap(fRhat);
-
-      const Vector3 fbiasAcc = bias_j.accelerometer() - bias_i.accelerometer();
-
-      const Vector3 fbiasOmega = bias_j.gyroscope() - bias_i.gyroscope();
-
-      Vector r(15); r << fp, fv, fR, fbiasAcc, fbiasOmega; // vector of size 15
-
-      return r;
-    }
-
-
-    /** predicted states from IMU */
-    static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i,
-        const imuBias::ConstantBias& bias_i,
-        const CombinedPreintegratedMeasurements& preintegratedMeasurements,
-        const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
-        const bool use2ndOrderCoriolis = false)
-    {
-
-      const double& deltaTij = preintegratedMeasurements.deltaTij_;
-      const Vector3 biasAccIncr = bias_i.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
-      const Vector3 biasOmegaIncr = bias_i.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
-
-      const Rot3 Rot_i = pose_i.rotation();
-      const Vector3 pos_i = pose_i.translation().vector();
-
-      // Predict state at time j
-      /* ---------------------------------------------------------------------------------------------------- */
-      Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
-          + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
-          + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
-          + vel_i * deltaTij
-          - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-          + 0.5 * gravity * deltaTij*deltaTij;
-
-      Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
-          + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
-          + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
-          - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
-          + gravity * deltaTij);
-
-      if(use2ndOrderCoriolis){
-        pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;  // 2nd order coriolis term for position
-        vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
-      }
-
-      const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
-      // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
-      Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-      Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-          Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
-      const Rot3 deltaRij_biascorrected_corioliscorrected =
-          Rot3::Expmap( theta_biascorrected_corioliscorrected );
-      const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  );
-
-      Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
-
-      return PoseVelocityBias(pose_j, vel_j, bias_i);
-    }
-
-
-  private:
-
     /** Serialization function */
     friend class boost::serialization::access;
     template<class ARCHIVE>
     void serialize(ARCHIVE & ar, const unsigned int version) {
-      ar & boost::serialization::make_nvp("NoiseModelFactor6",
+      ar & BOOST_SERIALIZATION_NVP(biasHat_);
-          boost::serialization::base_object<Base>(*this));
+      ar & BOOST_SERIALIZATION_NVP(measurementCovariance_);
-      ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
+      ar & BOOST_SERIALIZATION_NVP(deltaPij_);
-      ar & BOOST_SERIALIZATION_NVP(gravity_);
+      ar & BOOST_SERIALIZATION_NVP(deltaVij_);
-      ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
+      ar & BOOST_SERIALIZATION_NVP(deltaRij_);
-      ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
+      ar & BOOST_SERIALIZATION_NVP(deltaTij_);
+      ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
+      ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
+      ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
+      ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
+      ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
     }
-  }; // \class CombinedImuFactor
+  };
 
-  typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements;
+private:
+
+  typedef CombinedImuFactor This;
+  typedef NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> Base;
+
+  CombinedPreintegratedMeasurements preintegratedMeasurements_;
+  Vector3 gravity_;
+  Vector3 omegaCoriolis_;
+  boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
+
+  bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
+
+public:
+
+  /** Shorthand for a smart pointer to a factor */
+#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
+  typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
+#else
+  typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
+#endif
+
+  /** Default constructor - only use for serialization */
+  CombinedImuFactor();
+
+  /**
+   * Constructor
+   * @param pose_i Previous pose key
+   * @param vel_i  Previous velocity key
+   * @param pose_j Current pose key
+   * @param vel_j  Current velocity key
+   * @param bias_i Previous bias key
+   * @param bias_j Current bias key
+   * @param CombinedPreintegratedMeasurements CombinedPreintegratedMeasurements IMU measurements
+   * @param gravity Gravity vector expressed in the global frame
+   * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
+   * @param body_P_sensor Optional pose of the sensor frame in the body frame
+   * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
+   */
+  CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
+      const CombinedPreintegratedMeasurements& preintegratedMeasurements,
+      const Vector3& gravity, const Vector3& omegaCoriolis,
+      boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false);
+
+  virtual ~CombinedImuFactor() {}
+
+  /// @return a deep copy of this factor
+  virtual gtsam::NonlinearFactor::shared_ptr clone() const;
+
+  /** implement functions needed for Testable */
+
+  /// print
+  virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+
+  /// equals
+  virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const;
+
+  /** Access the preintegrated measurements. */
+
+  const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
+    return preintegratedMeasurements_; }
+
+  const Vector3& gravity() const { return gravity_; }
+
+  const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
+
+  /** implement functions needed to derive from Factor */
+
+  /// vector of errors
+  Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
+      const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
+      boost::optional<Matrix&> H1 = boost::none,
+      boost::optional<Matrix&> H2 = boost::none,
+      boost::optional<Matrix&> H3 = boost::none,
+      boost::optional<Matrix&> H4 = boost::none,
+      boost::optional<Matrix&> H5 = boost::none,
+      boost::optional<Matrix&> H6 = boost::none) const;
+
+  /// predicted states from IMU
+  static PoseVelocityBias Predict(const Pose3& pose_i, const Vector3& vel_i,
+      const imuBias::ConstantBias& bias_i,
+      const CombinedPreintegratedMeasurements& preintegratedMeasurements,
+      const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
+
+private:
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & boost::serialization::make_nvp("NoiseModelFactor6",
+        boost::serialization::base_object<Base>(*this));
+    ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
+    ar & BOOST_SERIALIZATION_NVP(gravity_);
+    ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
+    ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
+  }
+}; // class CombinedImuFactor
+
+typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements;
 
 } /// namespace gtsam

gtsam/navigation/ImuFactor.cpp

@@ -0,0 +1,438 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  ImuFactor.cpp
+ *  @author Luca Carlone
+ *  @author Stephen Williams
+ *  @author Richard Roberts
+ *  @author Vadim Indelman
+ *  @author David Jensen
+ *  @author Frank Dellaert
+ **/
+
+#include <gtsam/navigation/ImuFactor.h>
+
+/* External or standard includes */
+#include <ostream>
+
+namespace gtsam {
+
+using namespace std;
+
+//------------------------------------------------------------------------------
+// Inner class PreintegratedMeasurements
+//------------------------------------------------------------------------------
+ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements(
+    const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
+    const Matrix3& measuredOmegaCovariance, const Matrix3& integrationErrorCovariance,
+    const bool use2ndOrderIntegration) :
+                  biasHat_(bias), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()),
+                  deltaRij_(Rot3()), deltaTij_(0.0),
+                  delPdelBiasAcc_(Z_3x3), delPdelBiasOmega_(Z_3x3),
+                  delVdelBiasAcc_(Z_3x3), delVdelBiasOmega_(Z_3x3),
+                  delRdelBiasOmega_(Z_3x3), use2ndOrderIntegration_(use2ndOrderIntegration)
+{
+  measurementCovariance_.setZero();
+  measurementCovariance_.block<3,3>(0,0) = integrationErrorCovariance;
+  measurementCovariance_.block<3,3>(3,3) = measuredAccCovariance;
+  measurementCovariance_.block<3,3>(6,6) = measuredOmegaCovariance;
+  PreintMeasCov_.setZero(9,9);
+}
+
+//------------------------------------------------------------------------------
+void ImuFactor::PreintegratedMeasurements::print(const string& s) const {
+  cout << s << endl;
+  biasHat_.print("  biasHat");
+  cout << "  deltaTij " << deltaTij_ << endl;
+  cout << "  deltaPij [ " << deltaPij_.transpose() << " ]" << endl;
+  cout << "  deltaVij [ " << deltaVij_.transpose() << " ]" << endl;
+  deltaRij_.print("  deltaRij ");
+  cout << "  measurementCovariance = \n [ " << measurementCovariance_ << " ]" << endl;
+  cout << "  PreintMeasCov = \n [ " << PreintMeasCov_ << " ]" << endl;
+}
+
+//------------------------------------------------------------------------------
+bool ImuFactor::PreintegratedMeasurements::equals(const PreintegratedMeasurements& expected, double tol) const {
+  return biasHat_.equals(expected.biasHat_, tol)
+  && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
+  && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
+  && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
+  && deltaRij_.equals(expected.deltaRij_, tol)
+  && fabs(deltaTij_ - expected.deltaTij_) < tol
+  && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
+  && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
+  && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
+  && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
+  && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
+}
+
+//------------------------------------------------------------------------------
+void ImuFactor::PreintegratedMeasurements::resetIntegration(){
+  deltaPij_ = Vector3::Zero();
+  deltaVij_ = Vector3::Zero();
+  deltaRij_ = Rot3();
+  deltaTij_ = 0.0;
+  delPdelBiasAcc_ = Z_3x3;
+  delPdelBiasOmega_ = Z_3x3;
+  delVdelBiasAcc_ = Z_3x3;
+  delVdelBiasOmega_ = Z_3x3;
+  delRdelBiasOmega_ = Z_3x3;
+  PreintMeasCov_.setZero();
+}
+
+//------------------------------------------------------------------------------
+void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
+    const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
+    boost::optional<const Pose3&> body_P_sensor) {
+
+  // NOTE: order is important here because each update uses old values (i.e., we have to update
+  // jacobians and covariances before updating preintegrated measurements).
+
+  // First we compensate the measurements for the bias
+  Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
+  Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
+
+  // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
+  if(body_P_sensor){
+    Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
+    correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
+    Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
+    correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
+    // linear acceleration vector in the body frame
+  }
+
+  const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
+  const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
+
+  const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
+
+  // Update Jacobians
+  /* ----------------------------------------------------------------------------------------------------------------------- */
+  if(!use2ndOrderIntegration_){
+    delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
+    delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
+  }else{
+    delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
+    delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
+                                            * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
+  }
+  delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
+  delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
+  delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr  * deltaT;
+
+  // Update preintegrated measurements covariance
+  // as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework
+  /* ----------------------------------------------------------------------------------------------------------------------- */
+  const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
+  const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
+
+  Rot3 Rot_j = deltaRij_ * Rincr;
+  const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
+  const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
+
+  Matrix H_pos_pos    = I_3x3;
+  Matrix H_pos_vel    = I_3x3 * deltaT;
+  Matrix H_pos_angles = Z_3x3;
+
+  Matrix H_vel_pos    = Z_3x3;
+  Matrix H_vel_vel    = I_3x3;
+  Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
+  // analytic expression corresponding to the following numerical derivative
+  // Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
+
+  Matrix H_angles_pos   = Z_3x3;
+  Matrix H_angles_vel    = Z_3x3;
+  Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
+  // analytic expression corresponding to the following numerical derivative
+  // Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
+
+  // overall Jacobian wrt preintegrated measurements (df/dx)
+  Matrix F(9,9);
+  F << H_pos_pos, H_pos_vel,  H_pos_angles,
+      H_vel_pos, H_vel_vel, H_vel_angles,
+      H_angles_pos, H_angles_vel, H_angles_angles;
+
+  // first order uncertainty propagation:
+  // the deltaT allows to pass from continuous time noise to discrete time noise
+  // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
+  // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
+  PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ;
+
+  // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
+  // This in only kept for documentation.
+  //
+  // Matrix G(9,9);
+  // G << I_3x3 * deltaT, Z_3x3,  Z_3x3,
+  //      Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
+  //      Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
+  //
+  // PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
+
+  // Update preintegrated measurements (this has to be done after the update of covariances and jacobians!)
+  /* ----------------------------------------------------------------------------------------------------------------------- */
+  if(!use2ndOrderIntegration_){
+    deltaPij_ += deltaVij_ * deltaT;
+  }else{
+    deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
+  }
+  deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
+  deltaRij_ = deltaRij_ * Rincr;
+  deltaTij_ += deltaT;
+}
+
+//------------------------------------------------------------------------------
+// ImuFactor methods
+//------------------------------------------------------------------------------
+ImuFactor::ImuFactor() :
+    preintegratedMeasurements_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3), use2ndOrderCoriolis_(false){}
+
+//------------------------------------------------------------------------------
+ImuFactor::ImuFactor(
+    Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
+    const PreintegratedMeasurements& preintegratedMeasurements,
+    const Vector3& gravity, const Vector3& omegaCoriolis,
+    boost::optional<const Pose3&> body_P_sensor,
+    const bool use2ndOrderCoriolis) :
+        Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias),
+        preintegratedMeasurements_(preintegratedMeasurements),
+        gravity_(gravity),
+        omegaCoriolis_(omegaCoriolis),
+        body_P_sensor_(body_P_sensor),
+        use2ndOrderCoriolis_(use2ndOrderCoriolis){
+}
+
+//------------------------------------------------------------------------------
+gtsam::NonlinearFactor::shared_ptr ImuFactor::clone() const {
+  return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+      gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+}
+
+//------------------------------------------------------------------------------
+void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
+  cout << s << "ImuFactor("
+      << keyFormatter(this->key1()) << ","
+      << keyFormatter(this->key2()) << ","
+      << keyFormatter(this->key3()) << ","
+      << keyFormatter(this->key4()) << ","
+      << keyFormatter(this->key5()) << ")\n";
+  preintegratedMeasurements_.print("  preintegrated measurements:");
+  cout << "  gravity: [ " << gravity_.transpose() << " ]" << endl;
+  cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
+  this->noiseModel_->print("  noise model: ");
+  if(this->body_P_sensor_)
+    this->body_P_sensor_->print("  sensor pose in body frame: ");
+}
+
+//------------------------------------------------------------------------------
+bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const {
+  const This *e =  dynamic_cast<const This*> (&expected);
+  return e != NULL && Base::equals(*e, tol)
+  && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
+  && equal_with_abs_tol(gravity_, e->gravity_, tol)
+  && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
+  && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
+}
+
+//------------------------------------------------------------------------------
+Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
+    const imuBias::ConstantBias& bias,
+    boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2,
+    boost::optional<Matrix&> H3,  boost::optional<Matrix&> H4,
+    boost::optional<Matrix&> H5) const
+{
+
+  const double& deltaTij = preintegratedMeasurements_.deltaTij_;
+  const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
+  const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
+
+  // we give some shorter name to rotations and translations
+  const Rot3 Rot_i = pose_i.rotation();
+  const Rot3 Rot_j = pose_j.rotation();
+  const Vector3 pos_i = pose_i.translation().vector();
+  const Vector3 pos_j = pose_j.translation().vector();
+
+  // We compute factor's Jacobians
+  /* ---------------------------------------------------------------------------------------------------- */
+  const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
+  // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
+
+  Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
+
+  Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
+      Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
+
+  const Rot3 deltaRij_biascorrected_corioliscorrected =
+      Rot3::Expmap( theta_biascorrected_corioliscorrected );
+
+  const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
+
+  const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
+
+  const Matrix3 Jtheta = -Jr_theta_bcc  * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
+
+  const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
+
+  if(H1) {
+    H1->resize(9,6);
+
+    Matrix3 dfPdPi;
+    Matrix3 dfVdPi;
+    if(use2ndOrderCoriolis_){
+      dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
+      dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
+    }
+    else{
+      dfPdPi = - Rot_i.matrix();
+      dfVdPi = Z_3x3;
+    }
+
+    (*H1) <<
+        // dfP/dRi
+        Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
+        + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
+        // dfP/dPi
+        dfPdPi,
+        // dfV/dRi
+        Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
+        + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
+        // dfV/dPi
+        dfVdPi,
+        // dfR/dRi
+        Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
+        // dfR/dPi
+        Z_3x3;
+  }
+
+  if(H2) {
+    H2->resize(9,3);
+    (*H2) <<
+        // dfP/dVi
+        - I_3x3 * deltaTij
+        + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij,  // Coriolis term - we got rid of the 2 wrt ins paper
+        // dfV/dVi
+        - I_3x3
+        + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
+        // dfR/dVi
+        Z_3x3;
+  }
+
+  if(H3) {
+    H3->resize(9,6);
+    (*H3) <<
+        // dfP/dPosej
+        Z_3x3, Rot_j.matrix(),
+        // dfV/dPosej
+        Matrix::Zero(3,6),
+        // dfR/dPosej
+        Jrinv_fRhat *  ( I_3x3 ), Z_3x3;
+  }
+
+  if(H4) {
+    H4->resize(9,3);
+    (*H4) <<
+        // dfP/dVj
+        Z_3x3,
+        // dfV/dVj
+        I_3x3,
+        // dfR/dVj
+        Z_3x3;
+  }
+
+  if(H5) {
+    const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
+    const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
+    const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
+
+    H5->resize(9,6);
+    (*H5) <<
+        // dfP/dBias
+        - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
+        - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
+        // dfV/dBias
+        - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
+        - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
+        // dfR/dBias
+        Matrix::Zero(3,3),
+        Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
+  }
+
+  // Evaluate residual error, according to [3]
+  /* ---------------------------------------------------------------------------------------------------- */
+  const Vector3 fp =
+      pos_j - pos_i
+      - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
+          + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
+          + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
+          - vel_i * deltaTij
+          + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
+          - 0.5 * gravity_ * deltaTij*deltaTij;
+
+  const Vector3 fv =
+      vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
+          + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
+          + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
+          + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij  // Coriolis term
+          - gravity_ * deltaTij;
+
+  const Vector3 fR = Rot3::Logmap(fRhat);
+
+  Vector r(9); r << fp, fv, fR;
+  return r;
+}
+
+//------------------------------------------------------------------------------
+PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
+    const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
+    const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis)
+{
+
+  const double& deltaTij = preintegratedMeasurements.deltaTij_;
+  const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
+  const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
+
+  const Rot3 Rot_i = pose_i.rotation();
+  const Vector3 pos_i = pose_i.translation().vector();
+
+  // Predict state at time j
+  /* ---------------------------------------------------------------------------------------------------- */
+  Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
+      + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
+      + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
+      + vel_i * deltaTij
+      - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
+      + 0.5 * gravity * deltaTij*deltaTij;
+
+  Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
+      + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
+      + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
+      - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
+      + gravity * deltaTij);
+
+  if(use2ndOrderCoriolis){
+    pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;  // 2nd order coriolis term for position
+    vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
+  }
+
+  const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
+  // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
+  Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
+  Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
+      Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
+  const Rot3 deltaRij_biascorrected_corioliscorrected =
+      Rot3::Expmap( theta_biascorrected_corioliscorrected );
+  const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  );
+
+  Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
+  return PoseVelocity(pose_j, vel_j);
+}
+
+} /// namespace gtsam

gtsam/navigation/ImuFactor.h

@@ -11,23 +11,39 @@
 
 /**
  *  @file  ImuFactor.h
- *  @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen
+ *  @author Luca Carlone
+ *  @author Stephen Williams
+ *  @author Richard Roberts
+ *  @author Vadim Indelman
+ *  @author David Jensen
+ *  @author Frank Dellaert
  **/
 
 #pragma once
 
 /* GTSAM includes */
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/navigation/ImuBias.h>
-#include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/debug.h>
 
-/* External or standard includes */
-#include <ostream>
-
-
 namespace gtsam {
+
+/**
+ *
+ * @addtogroup SLAM
+ *
+ * If you are using the factor, please cite:
+ * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
+ * independent sets in factor graphs: a unifying perspective based on smart factors,
+ * Int. Conf. on Robotics and Automation (ICRA), 2014.
+ *
+ ** REFERENCES:
+ * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
+ * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built
+ * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
+ * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
+ */
+
 /**
  * Struct to hold return variables by the Predict Function
  */
@@ -35,265 +51,121 @@ struct PoseVelocity {
   Pose3 pose;
   Vector3 velocity;
   PoseVelocity(const Pose3& _pose, const Vector3& _velocity) :
-      pose(_pose), velocity(_velocity) {
+    pose(_pose), velocity(_velocity) {
   }
 };
 
+/**
+ * ImuFactor is a 5-ways factor involving previous state (pose and velocity of the vehicle at previous time step),
+ * current state (pose and velocity at current time step), and the bias estimate. According to the
+ * preintegration scheme proposed in [2], the ImuFactor includes many IMU measurements, which are
+ * "summarized" using the PreintegratedMeasurements class.
+ * Note that this factor does not force "temporal consistency" of the biases (which are usually
+ * slowly varying quantities), see also CombinedImuFactor for more details.
+ */
+class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> {
+public:
+
   /**
-   * 
+   * PreintegratedMeasurements accumulates (integrates) the IMU measurements
-   * @addtogroup SLAM
+   * (rotation rates and accelerations) and the corresponding covariance matrix.
-   *
+   * The measurements are then used to build the Preintegrated IMU factor (ImuFactor).
-   * If you are using the factor, please cite:
+   * Integration is done incrementally (ideally, one integrates the measurement as soon as it is received
-   * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
+   * from the IMU) so as to avoid costly integration at time of factor construction.
-   * independent sets in factor graphs: a unifying perspective based on smart factors,
-   * Int. Conf. on Robotics and Automation (ICRA), 2014.
-   *
-   ** REFERENCES:
-   * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
-   * [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built
-   * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
-   * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
    */
+  class PreintegratedMeasurements {
 
-  class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> {
+    friend class ImuFactor;
-  public:
 
-    /** Struct to store results of preintegrating IMU measurements.  Can be build
+  protected:
-     * incrementally so as to avoid costly integration at time of factor construction. */
+    imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
+    Eigen::Matrix<double,9,9> measurementCovariance_; ///< (continuous-time uncertainty) "Covariance" of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
 
-    /** CombinedPreintegratedMeasurements accumulates (integrates) the IMU measurements (rotation rates and accelerations)
+    Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
-         * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated IMU factor*/
+    Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
-    class PreintegratedMeasurements {
+    Rot3 deltaRij_;    ///< Preintegrated relative orientation (in frame i)
-      friend class ImuFactor;
+    double deltaTij_;  ///< Time interval from i to j
-    protected:
-      imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
-      Matrix measurementCovariance_; ///< (continuous-time uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
 
-      Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
+    Matrix3 delPdelBiasAcc_;   ///< Jacobian of preintegrated position w.r.t. acceleration bias
-      Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
+    Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
-      Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
+    Matrix3 delVdelBiasAcc_;   ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
-      double deltaTij_; ///< Time interval from i to j
+    Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
+    Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
+
+    Eigen::Matrix<double,9,9> PreintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
+    ///< (first-order propagation from *measurementCovariance*).
+
+    bool use2ndOrderIntegration_; ///< Controls the order of integration
 
-      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
-      Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
-      Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
-      bool use2ndOrderIntegration_; ///< Controls the order of integration
     public:
-      /** Default constructor, initialize with no IMU measurements */
-      PreintegratedMeasurements(
-          const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
-          const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
-          const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measured Angular Rate
-          const Matrix3& integrationErrorCovariance, ///< Covariance matrix of integration errors
-          const bool use2ndOrderIntegration = false ///< Controls the order of integration
-      ) : biasHat_(bias), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
-      delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
-      delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
-      delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(use2ndOrderIntegration)
-      {
-        measurementCovariance_ << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(),
-                                       Matrix3::Zero(), measuredAccCovariance,  Matrix3::Zero(),
-                                       Matrix3::Zero(),   Matrix3::Zero(), measuredOmegaCovariance;
-        PreintMeasCov_ = Matrix::Zero(9,9);
-      }
 
-      PreintegratedMeasurements() :
+    /**
-      biasHat_(imuBias::ConstantBias()), measurementCovariance_(9,9), deltaPij_(Vector3::Zero()), deltaVij_(Vector3::Zero()), deltaTij_(0.0),
+     *  Default constructor, initializes the class with no measurements
-      delPdelBiasAcc_(Matrix3::Zero()), delPdelBiasOmega_(Matrix3::Zero()),
+     *  @param bias Current estimate of acceleration and rotation rate biases
-      delVdelBiasAcc_(Matrix3::Zero()), delVdelBiasOmega_(Matrix3::Zero()),
+     *  @param measuredAccCovariance      Covariance matrix of measuredAcc
-      delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(9,9), use2ndOrderIntegration_(false)
+     *  @param measuredOmegaCovariance    Covariance matrix of measured Angular Rate
-      {
+     *  @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position)
-          measurementCovariance_ =  Matrix::Zero(9,9);
+     *  @param use2ndOrderIntegration     Controls the order of integration
-          PreintMeasCov_ = Matrix::Zero(9,9);
+     *  (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
-      }
+     */
+    PreintegratedMeasurements(const imuBias::ConstantBias& bias,
+        const Matrix3& measuredAccCovariance, const Matrix3& measuredOmegaCovariance,
+        const Matrix3& integrationErrorCovariance, const bool use2ndOrderIntegration = false);
 
-      /** print */
+    /// print
-      void print(const std::string& s = "Preintegrated Measurements:") const {
+    void print(const std::string& s = "Preintegrated Measurements:") const;
-        std::cout << s << std::endl;
-        biasHat_.print("  biasHat");
-        std::cout << "  deltaTij " << deltaTij_ << std::endl;
-        std::cout << "  deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
-        std::cout << "  deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
-        deltaRij_.print("  deltaRij ");
-        std::cout << "  measurementCovariance [ " << measurementCovariance_ << " ]" << std::endl;
-        std::cout << "  PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl;
-      }
 
-      /** equals */
+    /// equals
-      bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const {
+    bool equals(const PreintegratedMeasurements& expected, double tol=1e-9) const;
-        return biasHat_.equals(expected.biasHat_, tol)
-            && equal_with_abs_tol(measurementCovariance_, expected.measurementCovariance_, tol)
-            && equal_with_abs_tol(deltaPij_, expected.deltaPij_, tol)
-            && equal_with_abs_tol(deltaVij_, expected.deltaVij_, tol)
-            && deltaRij_.equals(expected.deltaRij_, tol)
-            && std::fabs(deltaTij_ - expected.deltaTij_) < tol
-            && equal_with_abs_tol(delPdelBiasAcc_, expected.delPdelBiasAcc_, tol)
-            && equal_with_abs_tol(delPdelBiasOmega_, expected.delPdelBiasOmega_, tol)
-            && equal_with_abs_tol(delVdelBiasAcc_, expected.delVdelBiasAcc_, tol)
-            && equal_with_abs_tol(delVdelBiasOmega_, expected.delVdelBiasOmega_, tol)
-            && equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
-      }
-      Matrix measurementCovariance() const {return measurementCovariance_;}
-      Matrix deltaRij() const {return deltaRij_.matrix();}
-      double deltaTij() const{return deltaTij_;}
-      Vector deltaPij() const {return deltaPij_;}
-      Vector deltaVij() const {return deltaVij_;}
-      Vector biasHat() const { return biasHat_.vector();}
-      Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;}
-      Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;}
-      Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;}
-      Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;}
-      Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
-      Matrix preintMeasCov() const { return PreintMeasCov_;}
 
+    /// Re-initialize PreintegratedMeasurements
+    void resetIntegration();
 
-      void resetIntegration(){
+    /**
-        deltaPij_ = Vector3::Zero();
+     * Add a single IMU measurement to the preintegration.
-        deltaVij_ = Vector3::Zero();
+     * @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
-        deltaRij_ = Rot3();
+     * @param measuredOmega Measured angular velocity (as given by the sensor)
-        deltaTij_ = 0.0;
+     * @param deltaT Time interval between two consecutive IMU measurements
-        delPdelBiasAcc_ = Matrix3::Zero();
+     * @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
-        delPdelBiasOmega_ = Matrix3::Zero();
+     */
-        delVdelBiasAcc_ = Matrix3::Zero();
+    void integrateMeasurement(const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
-        delVdelBiasOmega_ = Matrix3::Zero();
+        boost::optional<const Pose3&> body_P_sensor = boost::none);
-        delRdelBiasOmega_ = Matrix3::Zero();
-        PreintMeasCov_ = Matrix::Zero(9,9);
-      }
 
-      /** Add a single IMU measurement to the preintegration. */
+    /// methods to access class variables
-      void integrateMeasurement(
+    Matrix measurementCovariance() const {return measurementCovariance_;}
-          const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame)
+    Matrix deltaRij() const {return deltaRij_.matrix();}
-          const Vector3& measuredOmega, ///< Measured angular velocity (in body frame)
+    double deltaTij() const{return deltaTij_;}
-          double deltaT, ///< Time step
+    Vector deltaPij() const {return deltaPij_;}
-          boost::optional<const Pose3&> body_P_sensor = boost::none ///< Sensor frame
+    Vector deltaVij() const {return deltaVij_;}
-      ) {
+    Vector biasHat() const { return biasHat_.vector();}
+    Matrix delPdelBiasAcc() const { return delPdelBiasAcc_;}
+    Matrix delPdelBiasOmega() const { return delPdelBiasOmega_;}
+    Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;}
+    Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;}
+    Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
+    Matrix preintMeasCov() const { return PreintMeasCov_;}
 
-        // NOTE: order is important here because each update uses old values.
+    /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
-        // First we compensate the measurements for the bias
+    // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-        Vector3 correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
+    static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
-        Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
+        const Vector3& delta_angles, const Vector& delta_vel_in_t0){
+      // Note: all delta terms refer to an IMU\sensor system at t0
+      Vector body_t_a_body = msr_acc_t;
+      Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
+      return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
+    }
 
-        // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
+    // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-        if(body_P_sensor){
+    static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
-          Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
+        const Vector3& delta_angles){
-          correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
+      // Note: all delta terms refer to an IMU\sensor system at t0
-          Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
+      // Calculate the corrected measurements using the Bias object
-          correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
+      Vector body_t_omega_body= msr_gyro_t;
-          // linear acceleration vector in the body frame
+      Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-        }
+      R_t_to_t0    = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
-
+      return Rot3::Logmap(R_t_to_t0);
-        const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
+    }
-        const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
+    /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
-
-        const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
-
-        // Update Jacobians
-        /* ----------------------------------------------------------------------------------------------------------------------- */
-        if(!use2ndOrderIntegration_){
-          delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
-          delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
-        }else{
-          delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
-          delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
-                                    * skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
-        }
-        delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
-        delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
-        delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr  * deltaT;
-
-        // Update preintegrated measurements covariance
-        /* ----------------------------------------------------------------------------------------------------------------------- */
-        Matrix3 Z_3x3 = Matrix3::Zero();
-        Matrix3 I_3x3 = Matrix3::Identity();
-        const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
-        const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3(theta_i);
-
-        Rot3 Rot_j = deltaRij_ * Rincr;
-        const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization of so(3)
-        const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
-
-        // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
-        // can be seen as a prediction phase in an EKF framework
-        Matrix H_pos_pos    = I_3x3;
-        Matrix H_pos_vel    = I_3x3 * deltaT;
-        Matrix H_pos_angles = Z_3x3;
-
-        Matrix H_vel_pos    = Z_3x3;
-        Matrix H_vel_vel    = I_3x3;
-        Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
-        // analytic expression corresponding to the following numerical derivative
-        // Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
-
-        Matrix H_angles_pos   = Z_3x3;
-        Matrix H_angles_vel    = Z_3x3;
-        Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
-        // analytic expression corresponding to the following numerical derivative
-        // Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
-
-        // overall Jacobian wrt preintegrated measurements (df/dx)
-        Matrix F(9,9);
-        F << H_pos_pos, H_pos_vel,  H_pos_angles,
-            H_vel_pos, H_vel_vel, H_vel_angles,
-            H_angles_pos, H_angles_vel, H_angles_angles;
-
-        // first order uncertainty propagation:
-        // the deltaT allows to pass from continuous time noise to discrete time noise
-        // measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
-        // Gt * Qt * G =(approx)= measurementCovariance_discrete * deltaT^2 = measurementCovariance_contTime * deltaT
-        PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() + measurementCovariance_ * deltaT ;
-
-        // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
-        //
-        //        Matrix G(9,9);
-        //        G << I_3x3 * deltaT, Z_3x3,  Z_3x3,
-        //            Z_3x3, deltaRij.matrix() * deltaT, Z_3x3,
-        //            Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;
-        //
-        //        PreintMeasCov = F * PreintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
-
-        // Update preintegrated measurements
-        /* ----------------------------------------------------------------------------------------------------------------------- */
-        if(!use2ndOrderIntegration_){
-          deltaPij_ += deltaVij_ * deltaT;
-        }else{
-          deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
-        }
-        deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
-        deltaRij_ = deltaRij_ * Rincr;
-        deltaTij_ += deltaT;
-      }
-
-      /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
-      // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-      static inline Vector PreIntegrateIMUObservations_delta_vel(const Vector& msr_gyro_t, const Vector& msr_acc_t, const double msr_dt,
-              const Vector3& delta_angles, const Vector& delta_vel_in_t0){
-
-          // Note: all delta terms refer to an IMU\sensor system at t0
-
-        Vector body_t_a_body = msr_acc_t;
-        Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-
-          return delta_vel_in_t0 + R_t_to_t0.matrix() * body_t_a_body * msr_dt;
-      }
-
-      // This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
-      static inline Vector PreIntegrateIMUObservations_delta_angles(const Vector& msr_gyro_t, const double msr_dt,
-              const Vector3& delta_angles){
-
-          // Note: all delta terms refer to an IMU\sensor system at t0
-
-          // Calculate the corrected measurements using the Bias object
-        Vector body_t_omega_body= msr_gyro_t;
-
-          Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
-
-          R_t_to_t0    = R_t_to_t0 * Rot3::Expmap( body_t_omega_body*msr_dt );
-          return Rot3::Logmap(R_t_to_t0);
-      }
-      /* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
 
     private:
       /** Serialization function */
@@ -336,65 +208,41 @@ struct PoseVelocity {
 #endif
 
     /** Default constructor - only use for serialization */
-    ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()), use2ndOrderCoriolis_(false){}
+    ImuFactor();
 
-    /** Constructor */
+    /**
-    ImuFactor(
+     * Constructor
-      Key pose_i, ///< previous pose key
+     * @param pose_i Previous pose key
-      Key vel_i, ///< previous velocity key
+     * @param vel_i  Previous velocity key
-      Key pose_j, ///< current pose key
+     * @param pose_j Current pose key
-      Key vel_j, ///< current velocity key
+     * @param vel_j  Current velocity key
-      Key bias, ///< previous bias key
+     * @param bias   Previous bias key
-        const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements
+     * @param preintegratedMeasurements Preintegrated IMU measurements
-        const Vector3& gravity, ///< gravity vector
+     * @param gravity Gravity vector expressed in the global frame
-        const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame
+     * @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
-        boost::optional<const Pose3&> body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame
+     * @param body_P_sensor Optional pose of the sensor frame in the body frame
-        const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect
+     * @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
-    ) :
+     */
-      Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov_), pose_i, vel_i, pose_j, vel_j, bias),
+    ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
-      preintegratedMeasurements_(preintegratedMeasurements),
+        const PreintegratedMeasurements& preintegratedMeasurements,
-      gravity_(gravity),
+        const Vector3& gravity, const Vector3& omegaCoriolis,
-      omegaCoriolis_(omegaCoriolis),
+        boost::optional<const Pose3&> body_P_sensor = boost::none, const bool use2ndOrderCoriolis = false);
-      body_P_sensor_(body_P_sensor),
-      use2ndOrderCoriolis_(use2ndOrderCoriolis){
-    }
 
     virtual ~ImuFactor() {}
 
     /// @return a deep copy of this factor
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    virtual gtsam::NonlinearFactor::shared_ptr clone() const;
-      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
 
     /** implement functions needed for Testable */
 
-    /** print */
+    /// print
-    virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+    virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
-      std::cout << s << "ImuFactor("
-          << keyFormatter(this->key1()) << ","
-          << keyFormatter(this->key2()) << ","
-          << keyFormatter(this->key3()) << ","
-          << keyFormatter(this->key4()) << ","
-          << keyFormatter(this->key5()) << ")\n";
-      preintegratedMeasurements_.print("  preintegrated measurements:");
-      std::cout << "  gravity: [ " << gravity_.transpose() << " ]" << std::endl;
-      std::cout << "  omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << std::endl;
-      this->noiseModel_->print("  noise model: ");
-      if(this->body_P_sensor_)
-        this->body_P_sensor_->print("  sensor pose in body frame: ");
-    }
 
-    /** equals */
+    /// equals
-    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const;
-      const This *e =  dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol)
-          && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
-          && equal_with_abs_tol(gravity_, e->gravity_, tol)
-          && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
-          && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
-    }
 
     /** Access the preintegrated measurements. */
+
     const PreintegratedMeasurements& preintegratedMeasurements() const {
       return preintegratedMeasurements_; }
 
@@ -404,205 +252,19 @@ struct PoseVelocity {
 
     /** implement functions needed to derive from Factor */
 
-    /** vector of errors */
+    /// vector of errors
     Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
         const imuBias::ConstantBias& bias,
         boost::optional<Matrix&> H1 = boost::none,
         boost::optional<Matrix&> H2 = boost::none,
         boost::optional<Matrix&> H3 = boost::none,
         boost::optional<Matrix&> H4 = boost::none,
-        boost::optional<Matrix&> H5 = boost::none) const
+        boost::optional<Matrix&> H5 = boost::none) const;
-    {
 
-      const double& deltaTij = preintegratedMeasurements_.deltaTij_;
+    /// predicted states from IMU
-      const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements_.biasHat_.accelerometer();
-      const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements_.biasHat_.gyroscope();
-
-      // we give some shorter name to rotations and translations
-      const Rot3 Rot_i = pose_i.rotation();
-      const Rot3 Rot_j = pose_j.rotation();
-      const Vector3 pos_i = pose_i.translation().vector();
-      const Vector3 pos_j = pose_j.translation().vector();
-
-      // We compute factor's Jacobians
-      /* ---------------------------------------------------------------------------------------------------- */
-      const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
-      // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
-
-      Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-
-      Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-          Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
-
-      const Rot3 deltaRij_biascorrected_corioliscorrected =
-          Rot3::Expmap( theta_biascorrected_corioliscorrected );
-
-      const Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(Rot_i.between(Rot_j));
-
-      const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_biascorrected_corioliscorrected);
-
-      const Matrix3 Jtheta = -Jr_theta_bcc  * skewSymmetric(Rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
-
-      const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
-
-      if(H1) {
-        H1->resize(9,6);
-
-        Matrix3 dfPdPi;
-        Matrix3 dfVdPi;
-        if(use2ndOrderCoriolis_){
-          dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
-          dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
-        }
-        else{
-          dfPdPi = - Rot_i.matrix();
-          dfVdPi = Matrix3::Zero();
-        }
-
-    (*H1) <<
-      // dfP/dRi
-      Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij_
-        + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr),
-      // dfP/dPi
-      dfPdPi,
-      // dfV/dRi
-      Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij_
-        + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr),
-      // dfV/dPi
-      dfVdPi,
-      // dfR/dRi
-      Jrinv_fRhat *  (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
-      // dfR/dPi
-      Matrix3::Zero();
-      }
-
-      if(H2) {
-        H2->resize(9,3);
-        (*H2) <<
-            // dfP/dVi
-            - Matrix3::Identity() * deltaTij
-            + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij,  // Coriolis term - we got rid of the 2 wrt ins paper
-            // dfV/dVi
-            - Matrix3::Identity()
-            + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
-            // dfR/dVi
-            Matrix3::Zero();
-      }
-
-      if(H3) {
-
-        H3->resize(9,6);
-        (*H3) <<
-            // dfP/dPosej
-            Matrix3::Zero(), Rot_j.matrix(),
-            // dfV/dPosej
-            Matrix::Zero(3,6),
-            // dfR/dPosej
-            Jrinv_fRhat *  ( Matrix3::Identity() ), Matrix3::Zero();
-      }
-
-      if(H4) {
-        H4->resize(9,3);
-        (*H4) <<
-            // dfP/dVj
-            Matrix3::Zero(),
-            // dfV/dVj
-            Matrix3::Identity(),
-            // dfR/dVj
-            Matrix3::Zero();
-      }
-
-      if(H5) {
-
-        const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
-        const Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
-        const Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc * Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
-
-        H5->resize(9,6);
-        (*H5) <<
-            // dfP/dBias
-            - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasAcc_,
-            - Rot_i.matrix() * preintegratedMeasurements_.delPdelBiasOmega_,
-            // dfV/dBias
-            - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasAcc_,
-            - Rot_i.matrix() * preintegratedMeasurements_.delVdelBiasOmega_,
-            // dfR/dBias
-            Matrix::Zero(3,3),
-            Jrinv_fRhat * ( - fRhat.inverse().matrix() * JbiasOmega);
-      }
-
-      // Evaluate residual error, according to [3]
-      /* ---------------------------------------------------------------------------------------------------- */
-      const Vector3 fp =
-          pos_j - pos_i
-          - Rot_i.matrix() * (preintegratedMeasurements_.deltaPij_
-              + preintegratedMeasurements_.delPdelBiasAcc_ * biasAccIncr
-              + preintegratedMeasurements_.delPdelBiasOmega_ * biasOmegaIncr)
-              - vel_i * deltaTij
-              + skewSymmetric(omegaCoriolis_) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-              - 0.5 * gravity_ * deltaTij*deltaTij;
-
-      const Vector3 fv =
-          vel_j - vel_i - Rot_i.matrix() * (preintegratedMeasurements_.deltaVij_
-              + preintegratedMeasurements_.delVdelBiasAcc_ * biasAccIncr
-              + preintegratedMeasurements_.delVdelBiasOmega_ * biasOmegaIncr)
-              + 2 * skewSymmetric(omegaCoriolis_) * vel_i * deltaTij  // Coriolis term
-              - gravity_ * deltaTij;
-
-      const Vector3 fR = Rot3::Logmap(fRhat);
-
-      Vector r(9); r << fp, fv, fR;
-      return r;
-    }
-
-
-    /** predicted states from IMU */
     static PoseVelocity Predict(const Pose3& pose_i, const Vector3& vel_i,
         const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
-        const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
+        const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
-        const bool use2ndOrderCoriolis = false)
-    {
-
-      const double& deltaTij = preintegratedMeasurements.deltaTij_;
-      const Vector3 biasAccIncr = bias.accelerometer() - preintegratedMeasurements.biasHat_.accelerometer();
-      const Vector3 biasOmegaIncr = bias.gyroscope() - preintegratedMeasurements.biasHat_.gyroscope();
-
-      const Rot3 Rot_i = pose_i.rotation();
-      const Vector3 pos_i = pose_i.translation().vector();
-
-      // Predict state at time j
-      /* ---------------------------------------------------------------------------------------------------- */
-    Vector3 pos_j =  pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij_
-      + preintegratedMeasurements.delPdelBiasAcc_ * biasAccIncr
-      + preintegratedMeasurements.delPdelBiasOmega_ * biasOmegaIncr)
-      + vel_i * deltaTij
-      - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij  // Coriolis term - we got rid of the 2 wrt ins paper
-      + 0.5 * gravity * deltaTij*deltaTij;
-
-    Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
-      + preintegratedMeasurements.delVdelBiasAcc_ * biasAccIncr
-      + preintegratedMeasurements.delVdelBiasOmega_ * biasOmegaIncr)
-      - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij  // Coriolis term
-      + gravity * deltaTij);
-
-      if(use2ndOrderCoriolis){
-        pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij;  // 2nd order coriolis term for position
-        vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
-      }
-
-      const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP);
-      // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
-      Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
-      Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected  -
-          Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
-      const Rot3 deltaRij_biascorrected_corioliscorrected =
-          Rot3::Expmap( theta_biascorrected_corioliscorrected );
-      const Rot3 Rot_j = Rot_i.compose( deltaRij_biascorrected_corioliscorrected  );
-
-      Pose3 pose_j = Pose3( Rot_j, Point3(pos_j) );
-      return PoseVelocity(pose_j, vel_j);
-    }
-
 
   private:
 
@@ -617,7 +279,7 @@ struct PoseVelocity {
       ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
       ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
     }
-  }; // \class ImuFactor
+  }; // class ImuFactor
 
   typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements;
 

gtsam/navigation/tests/testCombinedImuFactor.cpp

@@ -10,19 +10,22 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    testImuFactor.cpp
+ * @file    testCombinedImuFactor.cpp
- * @brief   Unit test for ImuFactor
+ * @brief   Unit test for Lupton-style combined IMU factor
- * @author  Luca Carlone, Stephen Williams, Richard Roberts
+ * @author  Luca Carlone
+ * @author  Stephen Williams
+ * @author  Richard Roberts
  */
 
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/inference/Symbol.h>
 #include <gtsam/navigation/ImuFactor.h>
 #include <gtsam/navigation/CombinedImuFactor.h>
 #include <gtsam/navigation/ImuBias.h>
 #include <gtsam/geometry/Pose3.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/inference/Symbol.h>
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/base/numericalDerivative.h>
+
 #include <CppUnitLite/TestHarness.h>
 
 #include <boost/bind.hpp>

gtsam/navigation/tests/testImuFactor.cpp

@@ -39,15 +39,13 @@ using symbol_shorthand::B;
 namespace {
 Vector callEvaluateError(const ImuFactor& factor,
     const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
-    const imuBias::ConstantBias& bias)
+    const imuBias::ConstantBias& bias){
-{
   return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias);
 }
 
 Rot3 evaluateRotationError(const ImuFactor& factor,
     const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
-    const imuBias::ConstantBias& bias)
+    const imuBias::ConstantBias& bias){
-{
   return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ;
 }
 
@@ -56,9 +54,7 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
     const list<Vector3>& measuredAccs,
     const list<Vector3>& measuredOmegas,
     const list<double>& deltaTs,
-    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)
+    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0)  ){
-    )
-{
   ImuFactor::PreintegratedMeasurements result(bias, Matrix3::Identity(),
       Matrix3::Identity(), Matrix3::Identity());
 
@@ -68,7 +64,6 @@ ImuFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
   for( ; itAcc != measuredAccs.end(); ++itAcc, ++itOmega, ++itDeltaT) {
     result.integrateMeasurement(*itAcc, *itOmega, *itDeltaT);
   }
-
   return result;
 }
 
@@ -77,8 +72,7 @@ Vector3 evaluatePreintegratedMeasurementsPosition(
     const list<Vector3>& measuredAccs,
     const list<Vector3>& measuredOmegas,
     const list<double>& deltaTs,
-    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
+    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
-{
   return evaluatePreintegratedMeasurements(bias,
       measuredAccs, measuredOmegas, deltaTs).deltaPij();
 }
@@ -99,20 +93,16 @@ Rot3 evaluatePreintegratedMeasurementsRotation(
     const list<Vector3>& measuredAccs,
     const list<Vector3>& measuredOmegas,
     const list<double>& deltaTs,
-    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) )
+    const Vector3& initialRotationRate = Vector3(0.0,0.0,0.0) ){
-{
   return Rot3(evaluatePreintegratedMeasurements(bias,
       measuredAccs, measuredOmegas, deltaTs, initialRotationRate).deltaRij());
 }
 
-Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT)
+Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, const double deltaT){
-{
   return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
 }
 
-
+Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){
-Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta)
-{
   return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) );
 }
 
@@ -212,7 +202,6 @@ TEST( ImuFactor, Error )
   Matrix H1a, H2a, H3a, H4a, H5a;
   (void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
 
-
   // positions and velocities
   Matrix H1etop6 =  H1e.topRows(6);
   Matrix H1atop6 =  H1a.topRows(6);
@@ -230,7 +219,7 @@ TEST( ImuFactor, Error )
   EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5));  // 1e-5 needs to be added only when using quaternions for rotations
 
   EXPECT(assert_equal(H4e, H4a));
-//  EXPECT(assert_equal(H5e, H5a));
+  // EXPECT(assert_equal(H5e, H5a));
 }
 
 /* ************************************************************************* */
@@ -243,7 +232,6 @@ TEST( ImuFactor, ErrorWithBiases )
 //  Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
 //  Vector3 v2(Vector3(0.5, 0.0, 0.0));
 
-
   imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
   Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
   Vector3 v1(Vector3(0.5, 0.0, 0.0));
@@ -260,8 +248,8 @@ TEST( ImuFactor, ErrorWithBiases )
   ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
     pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
-//  ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3));
+  //  ImuFactor::PreintegratedMeasurements pre_int_data(bias.head(3), bias.tail(3));
-//    pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
+  //    pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
     // Create factor
     ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
@@ -272,7 +260,7 @@ TEST( ImuFactor, ErrorWithBiases )
 
     // Expected error
     Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
-//    EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
+    // EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
 
     // Expected Jacobians
     Matrix H1e = numericalDerivative11<Vector,Pose3>(
@@ -315,7 +303,6 @@ TEST( ImuFactor, PartialDerivativeExpmap )
   Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
   double deltaT = 0.5;
 
-
   // Compute numerical derivatives
   Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(boost::bind(
       &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega));
@@ -326,7 +313,6 @@ TEST( ImuFactor, PartialDerivativeExpmap )
 
   // Compare Jacobians
   EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3));  // 1e-3 needs to be added only when using quaternions for rotations
-
 }
 
 /* ************************************************************************* */
@@ -349,9 +335,6 @@ TEST( ImuFactor, PartialDerivativeLogmap )
   const Matrix3  actualDelFdeltheta = Matrix3::Identity() +
        0.5 * X + (1/(normx*normx) - (1+cos(normx))/(2*normx * sin(normx)) ) * X * X;
 
-//  std::cout << "actualDelFdeltheta" << actualDelFdeltheta << std::endl;
-//  std::cout << "expectedDelFdeltheta" << expectedDelFdeltheta << std::endl;
-
   // Compare Jacobians
   EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
 
@@ -361,30 +344,30 @@ TEST( ImuFactor, PartialDerivativeLogmap )
 TEST( ImuFactor, fistOrderExponential )
 {
   // Linearization point
-    Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias
+  Vector3 biasOmega; biasOmega << 0,0,0; ///< Current estimate of rotation rate bias
 
-    // Measurements
+  // Measurements
-    Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
+  Vector3 measuredOmega; measuredOmega << 0.1, 0, 0;
-    double deltaT = 1.0;
+  double deltaT = 1.0;
 
-    // change w.r.t. linearization point
+  // change w.r.t. linearization point
-    double alpha = 0.0;
+  double alpha = 0.0;
-    Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha;
+  Vector3 deltabiasOmega; deltabiasOmega << alpha,alpha,alpha;
 
 
-    const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT);
+  const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT);
 
-     Matrix3  delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
+  Matrix3  delRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
 
-     const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
+  const Matrix expectedRot = Rot3::Expmap((measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
 
-     const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
+  const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
-     const Matrix3 actualRot =
+  const Matrix3 actualRot =
-         hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
+      hatRot * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
-         //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
+  //hatRot * (Matrix3::Identity() + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
 
-    // Compare Jacobians
+  // Compare Jacobians
-    EXPECT(assert_equal(expectedRot, actualRot));
+  EXPECT(assert_equal(expectedRot, actualRot));
 }
 
 /* ************************************************************************* */
@@ -495,7 +478,6 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
 //  tictoc_print_();
 //}
 
-
 /* ************************************************************************* */
 TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
 {
@@ -515,15 +497,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
 
   const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0,0.10,0.10)), Point3(1,0,0));
 
-//  ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
-//        Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredOmega);
-
-
   ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
         Vector3(0.0, 0.0, 0.0)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
 
-
-
   pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
 
     // Create factor
@@ -560,6 +536,7 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
     EXPECT(assert_equal(H5e, H5a));
 }
 
+/* ************************************************************************* */
 TEST(ImuFactor, PredictPositionAndVelocity){
   imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
 
@@ -593,6 +570,7 @@ TEST(ImuFactor, PredictPositionAndVelocity){
 
 }
 
+/* ************************************************************************* */
 TEST(ImuFactor, PredictRotation) {
   imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
 

gtsam/nonlinear/AdaptAutoDiff.h

@@ -18,8 +18,9 @@
 
 #pragma once
 
-#include <gtsam_unstable/nonlinear/ceres_autodiff.h>
+#include <gtsam/3rdparty/ceres/autodiff.h>
 #include <gtsam/base/Manifold.h>
+#include <gtsam/base/OptionalJacobian.h>
 
 namespace gtsam {
 
@@ -50,11 +51,8 @@ class AdaptAutoDiff {
 
 public:
 
-  typedef Eigen::Matrix<double, N, M1> JacobianTA1;
+  T operator()(const A1& a1, const A2& a2, OptionalJacobian<N, M1> H1 = boost::none,
-  typedef Eigen::Matrix<double, N, M2> JacobianTA2;
+      OptionalJacobian<N, M2> H2 = boost::none) {
-
-  T operator()(const A1& a1, const A2& a2, boost::optional<JacobianTA1&> H1 =
-      boost::none, boost::optional<JacobianTA2&> H2 = boost::none) {
 
     using ceres::internal::AutoDiff;
 

gtsam/nonlinear/Expression-inl.h

@@ -23,6 +23,7 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Manifold.h>
+#include <gtsam/base/OptionalJacobian.h>
 
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
@@ -42,21 +43,21 @@ namespace gtsam {
 
 const unsigned TraceAlignment = 16;
 
-template <typename T>
+template<typename T>
-T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment){
+T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) {
   // right now only word sized types are supported.
   // Easy to extend if needed,
   //   by somehow inferring the unsigned integer of same size
   BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t));
   size_t & uiValue = reinterpret_cast<size_t &>(value);
   size_t misAlignment = uiValue % requiredAlignment;
-  if(misAlignment) {
+  if (misAlignment) {
     uiValue += requiredAlignment - misAlignment;
   }
   return value;
 }
-template <typename T>
+template<typename T>
-T upAligned(T value, unsigned requiredAlignment = TraceAlignment){
+T upAligned(T value, unsigned requiredAlignment = TraceAlignment) {
   return upAlign(value, requiredAlignment);
 }
 
@@ -88,33 +89,34 @@ public:
 
 namespace internal {
 
-template <bool UseBlock, typename Derived>
+template<bool UseBlock, typename Derived>
 struct UseBlockIf {
   static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA,
-                            JacobianMap& jacobians, Key key){
+      JacobianMap& jacobians, Key key) {
     // block makes HUGE difference
-    jacobians(key).block<Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>(0, 0) += dTdA;
+    jacobians(key).block<Derived::RowsAtCompileTime, Derived::ColsAtCompileTime>(
-  };
+        0, 0) += dTdA;
+  }
+  ;
 };
 /// Handle Leaf Case for Dynamic Matrix type (slower)
-template <typename Derived>
+template<typename Derived>
 struct UseBlockIf<false, Derived> {
   static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA,
-                            JacobianMap& jacobians, Key key) {
+      JacobianMap& jacobians, Key key) {
     jacobians(key) += dTdA;
   }
 };
 }
 
-/// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians
+/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians
 template<typename Derived>
 void handleLeafCase(const Eigen::MatrixBase<Derived>& dTdA,
     JacobianMap& jacobians, Key key) {
   internal::UseBlockIf<
-      Derived::RowsAtCompileTime != Eigen::Dynamic &&
+      Derived::RowsAtCompileTime != Eigen::Dynamic
-      Derived::ColsAtCompileTime != Eigen::Dynamic,
+          && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian(
-      Derived>
+      dTdA, jacobians, key);
-    ::addToJacobian(dTdA, jacobians, key);
 }
 
 //-----------------------------------------------------------------------------
@@ -186,28 +188,28 @@ public:
     }
   }
   /**
-   *  *** This is the main entry point for reverseAD, called from Expression ***
+   *  *** This is the main entry point for reverse AD, called from Expression ***
    * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function)
    */
   typedef Eigen::Matrix<double, Dim, Dim> JacobianTT;
-  void startReverseAD(JacobianMap& jacobians) const {
+  void startReverseAD1(JacobianMap& jacobians) const {
     if (kind == Leaf) {
       // This branch will only be called on trivial Leaf expressions, i.e. Priors
       static const JacobianTT I = JacobianTT::Identity();
       handleLeafCase(I, jacobians, content.key);
     } else if (kind == Function)
       // This is the more typical entry point, starting the AD pipeline
-      // Inside the startReverseAD that the correctly dimensioned pipeline is chosen.
+      // Inside startReverseAD2 the correctly dimensioned pipeline is chosen.
-      content.ptr->startReverseAD(jacobians);
+      content.ptr->startReverseAD2(jacobians);
   }
   // Either add to Jacobians (Leaf) or propagate (Function)
   template<typename DerivedMatrix>
-  void reverseAD(const Eigen::MatrixBase<DerivedMatrix> & dTdA,
+  void reverseAD1(const Eigen::MatrixBase<DerivedMatrix> & dTdA,
       JacobianMap& jacobians) const {
     if (kind == Leaf)
       handleLeafCase(dTdA, jacobians, content.key);
     else if (kind == Function)
-      content.ptr->reverseAD(dTdA, jacobians);
+      content.ptr->reverseAD2(dTdA, jacobians);
   }
 
   /// Define type so we can apply it as a meta-function
@@ -265,7 +267,7 @@ public:
 
   /// Construct an execution trace for reverse AD
   virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-     ExecutionTraceStorage* traceStorage) const = 0;
+      ExecutionTraceStorage* traceStorage) const = 0;
 };
 
 //-----------------------------------------------------------------------------
@@ -336,7 +338,7 @@ public:
 
   /// Construct an execution trace for reverse AD
   virtual const value_type& traceExecution(const Values& values,
-      ExecutionTrace<value_type>& trace, 
+      ExecutionTrace<value_type>& trace,
       ExecutionTraceStorage* traceStorage) const {
     trace.setLeaf(key_);
     return dynamic_cast<const value_type&>(values.at(key_));
@@ -454,10 +456,9 @@ struct Jacobian {
 
 /// meta-function to generate JacobianTA optional reference
 template<class T, class A>
-struct OptionalJacobian {
+struct MakeOptionalJacobian {
-  typedef Eigen::Matrix<double, traits::dimension<T>::value,
+  typedef OptionalJacobian<traits::dimension<T>::value,
-      traits::dimension<A>::value> Jacobian;
+      traits::dimension<A>::value> type;
-  typedef boost::optional<Jacobian&> type;
 };
 
 /**
@@ -470,10 +471,10 @@ struct FunctionalBase: ExpressionNode<T> {
   struct Record {
     void print(const std::string& indent) const {
     }
-    void startReverseAD(JacobianMap& jacobians) const {
+    void startReverseAD4(JacobianMap& jacobians) const {
     }
     template<typename SomeMatrix>
-    void reverseAD(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
+    void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
     }
   };
   /// Construct an execution trace for reverse AD
@@ -505,9 +506,9 @@ struct JacobianTrace {
   typename Jacobian<T, A>::type dTdA;
 };
 
-/**
+// Recursive Definition of Functional ExpressionNode
- * Recursive Definition of Functional ExpressionNode
+// The reason we inherit from Argument<T, A, N> is because we can then
- */
+// case to this unique signature to retrieve the expression at any level
 template<class T, class A, class Base>
 struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
 
@@ -528,7 +529,9 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
     This::expression->dims(map);
   }
 
-  /// Recursive Record Class for Functional Expressions
+  // Recursive Record Class for Functional Expressions
+  // The reason we inherit from JacobianTrace<T, A, N> is because we can then
+  // case to this unique signature to retrieve the value/trace at any level
   struct Record: JacobianTrace<T, A, N>, Base::Record {
 
     typedef T return_type;
@@ -543,22 +546,31 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
     }
 
     /// Start the reverse AD process
-    void startReverseAD(JacobianMap& jacobians) const {
+    void startReverseAD4(JacobianMap& jacobians) const {
-      Base::Record::startReverseAD(jacobians);
+      Base::Record::startReverseAD4(jacobians);
-      This::trace.reverseAD(This::dTdA, jacobians);
+      // This is the crucial point where the size of the AD pipeline is selected.
+      // One pipeline is started for each argument, but the number of rows in each
+      // pipeline is the same, namely the dimension of the output argument T.
+      // For example, if the entire expression is rooted by a binary function
+      // yielding a 2D result, then the matrix dTdA will have 2 rows.
+      // ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2
+      // which calls the correctly sized CallRecord::reverseAD3, which in turn
+      // calls reverseAD4 below.
+      This::trace.reverseAD1(This::dTdA, jacobians);
     }
 
     /// Given df/dT, multiply in dT/dA and continue reverse AD process
-    template<int Rows, int Cols>
+    // Cols is always known at compile time
-    void reverseAD(const Eigen::Matrix<double, Rows, Cols> & dFdT,
+    template<typename SomeMatrix>
+    void reverseAD4(const SomeMatrix & dFdT,
         JacobianMap& jacobians) const {
-      Base::Record::reverseAD(dFdT, jacobians);
+      Base::Record::reverseAD4(dFdT, jacobians);
-      This::trace.reverseAD(dFdT * This::dTdA, jacobians);
+      This::trace.reverseAD1(dFdT * This::dTdA, jacobians);
     }
   };
 
   /// Construct an execution trace for reverse AD
-  void trace(const Values& values, Record* record, 
+  void trace(const Values& values, Record* record,
       ExecutionTraceStorage*& traceStorage) const {
     Base::trace(values, record, traceStorage); // recurse
     // Write an Expression<A> execution trace in record->trace
@@ -614,8 +626,8 @@ struct FunctionalNode {
     struct Record: public internal::CallRecordImplementor<Record,
         traits::dimension<T>::value>, public Base::Record {
       using Base::Record::print;
-      using Base::Record::startReverseAD;
+      using Base::Record::startReverseAD4;
-      using Base::Record::reverseAD;
+      using Base::Record::reverseAD4;
 
       virtual ~Record() {
       }
@@ -634,7 +646,7 @@ struct FunctionalNode {
     };
 
     /// Construct an execution trace for reverse AD
-    Record* trace(const Values& values, 
+    Record* trace(const Values& values,
         ExecutionTraceStorage* traceStorage) const {
       assert(reinterpret_cast<size_t>(traceStorage) % TraceAlignment == 0);
 
@@ -659,7 +671,8 @@ class UnaryExpression: public FunctionalNode<T, boost::mpl::vector<A1> >::type {
 
 public:
 
-  typedef boost::function<T(const A1&, typename OptionalJacobian<T, A1>::type)> Function;
+  typedef boost::function<
+      T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> Function;
   typedef typename FunctionalNode<T, boost::mpl::vector<A1> >::type Base;
   typedef typename Base::Record Record;
 
@@ -704,8 +717,8 @@ class BinaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2> >::t
 public:
 
   typedef boost::function<
-      T(const A1&, const A2&, typename OptionalJacobian<T, A1>::type,
+      T(const A1&, const A2&, typename MakeOptionalJacobian<T, A1>::type,
-          typename OptionalJacobian<T, A2>::type)> Function;
+          typename MakeOptionalJacobian<T, A2>::type)> Function;
   typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2> >::type Base;
   typedef typename Base::Record Record;
 
@@ -758,9 +771,10 @@ class TernaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2, A3>
 public:
 
   typedef boost::function<
-      T(const A1&, const A2&, const A3&, typename OptionalJacobian<T, A1>::type,
+      T(const A1&, const A2&, const A3&,
-          typename OptionalJacobian<T, A2>::type,
+          typename MakeOptionalJacobian<T, A1>::type,
-          typename OptionalJacobian<T, A3>::type)> Function;
+          typename MakeOptionalJacobian<T, A2>::type,
+          typename MakeOptionalJacobian<T, A3>::type)> Function;
   typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type Base;
   typedef typename Base::Record Record;
 
@@ -776,7 +790,8 @@ private:
     this->template reset<A2, 2>(e2.root());
     this->template reset<A3, 3>(e3.root());
     ExpressionNode<T>::traceSize_ = //
-        upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() + e3.traceSize();
+        upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize()
+            + e3.traceSize();
   }
 
   friend class Expression<T> ;

gtsam/nonlinear/Expression.h

@@ -20,8 +20,8 @@
 #pragma once
 
 #include <gtsam/nonlinear/Expression-inl.h>
-#include <gtsam/base/FastVector.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/base/FastVector.h>
 
 #include <boost/bind.hpp>
 #include <boost/range/adaptor/map.hpp>
@@ -75,7 +75,7 @@ public:
   /// Construct a nullary method expression
   template<typename A>
   Expression(const Expression<A>& expression,
-      T (A::*method)(typename OptionalJacobian<T, A>::type) const) :
+      T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const) :
       root_(new UnaryExpression<T, A>(boost::bind(method, _1, _2), expression)) {
   }
 
@@ -89,8 +89,8 @@ public:
   /// Construct a unary method expression
   template<typename A1, typename A2>
   Expression(const Expression<A1>& expression1,
-      T (A1::*method)(const A2&, typename OptionalJacobian<T, A1>::type,
+      T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
-          typename OptionalJacobian<T, A2>::type) const,
+          typename MakeOptionalJacobian<T, A2>::type) const,
       const Expression<A2>& expression2) :
       root_(
           new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),
@@ -206,10 +206,24 @@ private:
     // with an execution trace, made up entirely of "Record" structs, see
     // the FunctionalNode class in expression-inl.h
     size_t size = traceSize();
+
+    // Windows does not support variable length arrays, so memory must be dynamically
+    // allocated on Visual Studio. For more information see the issue below
+    // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio
+#ifdef _MSC_VER
+    ExecutionTraceStorage* traceStorage = new ExecutionTraceStorage[size];
+#else
     ExecutionTraceStorage traceStorage[size];
+#endif
+
     ExecutionTrace<T> trace;
     T value(traceExecution(values, trace, traceStorage));
-    trace.startReverseAD(jacobians);
+    trace.startReverseAD1(jacobians);
+
+#ifdef _MSC_VER
+    delete[] traceStorage;
+#endif
+
     return value;
   }
 
@@ -224,9 +238,8 @@ template<class T>
 struct apply_compose {
   typedef T result_type;
   static const int Dim = traits::dimension<T>::value;
-  typedef Eigen::Matrix<double, Dim, Dim> Jacobian;
+  T operator()(const T& x, const T& y, OptionalJacobian<Dim, Dim> H1 =
-  T operator()(const T& x, const T& y, boost::optional<Jacobian&> H1,
+      boost::none, OptionalJacobian<Dim, Dim> H2 = boost::none) const {
-      boost::optional<Jacobian&> H2) const {
     return x.compose(y, H1, H2);
   }
 };

gtsam/nonlinear/NonlinearFactor.h

@@ -20,14 +20,14 @@
 
 #pragma once
 
-#include <boost/serialization/base_object.hpp>
-#include <boost/assign/list_of.hpp>
-
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/linear/NoiseModel.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/inference/Factor.h>
+#include <gtsam/base/OptionalJacobian.h>
 
+#include <boost/serialization/base_object.hpp>
+#include <boost/assign/list_of.hpp>
 
 /**
  * Macro to add a standard clone function to a derived factor

gtsam/nonlinear/Values.cpp

@@ -130,6 +130,24 @@ namespace gtsam {
       throw ValuesKeyAlreadyExists(j);
   }
 
+  /* ************************************************************************* */
+  void Values::insertFixed(Key j, const Vector& v, size_t n) {
+    switch (n) {
+    case 1: insert<Vector1>(j,v); break;
+    case 2: insert<Vector2>(j,v); break;
+    case 3: insert<Vector3>(j,v); break;
+    case 4: insert<Vector4>(j,v); break;
+    case 5: insert<Vector5>(j,v); break;
+    case 6: insert<Vector6>(j,v); break;
+    case 7: insert<Vector7>(j,v); break;
+    case 8: insert<Vector8>(j,v); break;
+    case 9: insert<Vector9>(j,v); break;
+    default:
+      throw runtime_error(
+          "Values::insert fixed size can only handle n in 1..9");
+    }
+  }
+
   /* ************************************************************************* */
   void Values::insert(const Values& values) {
     for(const_iterator key_value = values.begin(); key_value != values.end(); ++key_value) {

gtsam/nonlinear/Values.h

@@ -258,6 +258,10 @@ namespace gtsam {
     template <typename ValueType>
     void insert(Key j, const ValueType& val);
 
+    /// Special version for small fixed size vectors, for matlab/python
+    /// throws truntime error if n<1 || n>9
+    void insertFixed(Key j, const Vector& v, size_t n);
+
     /// overloaded insert version that also specifies a chart
     template <typename ValueType, typename Chart>
     void insert(Key j, const ValueType& val);

gtsam/nonlinear/tests/testAdaptAutoDiff.cpp

@@ -17,8 +17,8 @@
  * @brief unit tests for Block Automatic Differentiation
  */
 
-#include <gtsam_unstable/nonlinear/ceres_example.h>
+#include <gtsam/3rdparty/ceres/example.h>
-#include <gtsam_unstable/nonlinear/AdaptAutoDiff.h>
+#include <gtsam/nonlinear/AdaptAutoDiff.h>
 #include <gtsam/nonlinear/Expression.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Pose3.h>

gtsam/nonlinear/tests/testExpression.cpp

@@ -34,8 +34,8 @@ using namespace gtsam;
 
 /* ************************************************************************* */
 template<class CAL>
-Point2 uncalibrate(const CAL& K, const Point2& p,
+Point2 uncalibrate(const CAL& K, const Point2& p, OptionalJacobian<2, 5> Dcal,
-    boost::optional<Matrix25&> Dcal, boost::optional<Matrix2&> Dp) {
+    OptionalJacobian<2, 2> Dp) {
   return K.uncalibrate(p, Dcal, Dp);
 }
 
@@ -75,13 +75,13 @@ TEST(Expression, Leaves) {
 /* ************************************************************************* */
 // Unary(Leaf)
 namespace unary {
-Point2 f0(const Point3& p, boost::optional<Matrix23&> H) {
+Point2 f0(const Point3& p, OptionalJacobian<2,3> H) {
   return Point2();
 }
-LieScalar f1(const Point3& p, boost::optional<Eigen::Matrix<double, 1, 3>&> H) {
+LieScalar f1(const Point3& p, OptionalJacobian<1, 3> H) {
   return LieScalar(0.0);
 }
-double f2(const Point3& p, boost::optional<Eigen::Matrix<double, 1, 3>&> H) {
+double f2(const Point3& p, OptionalJacobian<1, 3> H) {
   return 0.0;
 }
 Expression<Point3> p(1);
@@ -117,7 +117,7 @@ TEST(Expression, NullaryMethod) {
   // Check dims as map
   std::map<Key, int> map;
   norm.dims(map);
-  LONGS_EQUAL(1,map.size());
+  LONGS_EQUAL(1, map.size());
 
   // Get value and Jacobians
   std::vector<Matrix> H(1);
@@ -133,9 +133,8 @@ TEST(Expression, NullaryMethod) {
 // Binary(Leaf,Leaf)
 namespace binary {
 // Create leaves
-double doubleF(const Pose3& pose, const Point3& point,
+double doubleF(const Pose3& pose, //
-    boost::optional<Eigen::Matrix<double, 1, 6>&> H1,
+    const Point3& point, OptionalJacobian<1, 6> H1, OptionalJacobian<1, 3> H2) {
-    boost::optional<Eigen::Matrix<double, 1, 3>&> H2) {
   return 0.0;
 }
 Expression<Pose3> x(1);
@@ -244,8 +243,7 @@ TEST(Expression, compose3) {
 /* ************************************************************************* */
 // Test with ternary function
 Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
-    boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2,
+    OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) {
-    boost::optional<Matrix3&> H3) {
   // return dummy derivatives (not correct, but that's ok for testing here)
   if (H1)
     *H1 = eye(3);

gtsam/nonlinear/tests/testValues.cpp

@@ -12,6 +12,8 @@
 /**
  * @file testValues.cpp
  * @author Richard Roberts
+ * @author Frank Dellaert
+ * @author Mike Bosse
  */
 
 #include <gtsam/nonlinear/Values.h>
@@ -168,9 +170,9 @@ TEST(Values, basic_functions)
   Values values;
   const Values& values_c = values;
   values.insert(2, Vector3());
-  values.insert(4, Vector3());
+  values.insert(4, Vector(3));
-  values.insert(6, Vector3());
+  values.insert(6, Matrix23());
-  values.insert(8, Vector3());
+  values.insert(8, Matrix(2,3));
 
   // find
   EXPECT_LONGS_EQUAL(4, values.find(4)->key);
@@ -468,6 +470,15 @@ TEST(Values, Destructors) {
   LONGS_EQUAL(4+2, (long)TestValueData::DestructorCount);
 }
 
+/* ************************************************************************* */
+TEST(Values, FixedSize) {
+  Values values;
+  Vector v(3); v << 5.0, 6.0, 7.0;
+  values.insertFixed(key1, v, 3);
+  Vector3 expected(5.0, 6.0, 7.0);
+  CHECK(assert_equal((Vector)expected, values.at<Vector3>(key1)));
+  CHECK_EXCEPTION(values.insertFixed(key1, v, 12),runtime_error);
+}
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/slam/BetweenFactor.h

@@ -74,25 +74,26 @@ namespace gtsam {
       std::cout << s << "BetweenFactor("
           << keyFormatter(this->key1()) << ","
           << keyFormatter(this->key2()) << ")\n";
-      measured_.print("  measured: ");
+      traits::print<T>()(measured_, "  measured: ");
       this->noiseModel_->print("  noise model: ");
     }
 
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This *e =  dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
+      return e != NULL && Base::equals(*e, tol) && traits::equals<T>()(this->measured_, e->measured_, tol);
     }
 
     /** implement functions needed to derive from Factor */
 
     /** vector of errors */
     Vector evaluateError(const T& p1, const T& p2,
-        boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
+        boost::optional<Matrix&> H1 = boost::none,boost::optional<Matrix&> H2 =
             boost::none) const {
       T hx = p1.between(p2, H1, H2); // h(x)
+      DefaultChart<T> chart;
       // manifold equivalent of h(x)-z -> log(z,h(x))
-      return measured_.localCoordinates(hx);
+      return chart.local(measured_, hx);
     }
 
     /** return the measured */
@@ -129,7 +130,9 @@ namespace gtsam {
 
     /** Syntactic sugar for constrained version */
     BetweenConstraint(const VALUE& measured, Key key1, Key key2, double mu = 1000.0) :
-      BetweenFactor<VALUE>(key1, key2, measured, noiseModel::Constrained::All(VALUE::Dim(), fabs(mu))) {}
+      BetweenFactor<VALUE>(key1, key2, measured,
+                           noiseModel::Constrained::All(DefaultChart<VALUE>::getDimension(measured), fabs(mu)))
+    {}
 
   private:
 

gtsam/slam/PriorFactor.h

@@ -67,23 +67,24 @@ namespace gtsam {
     /** print */
     virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
       std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
-      prior_.print("  prior mean: ");
+      traits::print<T>()(prior_, "  prior mean: ");
       this->noiseModel_->print("  noise model: ");
     }
 
     /** equals */
     virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
       const This* e = dynamic_cast<const This*> (&expected);
-      return e != NULL && Base::equals(*e, tol) && this->prior_.equals(e->prior_, tol);
+      return e != NULL && Base::equals(*e, tol) && traits::equals<T>()(prior_, e->prior_, tol);
     }
 
     /** implement functions needed to derive from Factor */
 
     /** vector of errors */
     Vector evaluateError(const T& p, boost::optional<Matrix&> H = boost::none) const {
-      if (H) (*H) = eye(p.dim());
+      DefaultChart<T> chart;
+      if (H) (*H) = eye(chart.getDimension(p));
       // manifold equivalent of h(x)-z -> log(z,h(x))
-      return prior_.localCoordinates(p);
+      return chart.local(prior_,p);
     }
 
     const VALUE & prior() const { return prior_; }

gtsam/slam/RangeFactor.h

@@ -16,99 +16,176 @@
 
 #pragma once
 
-#include <boost/lexical_cast.hpp>
-#include <gtsam/geometry/concepts.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/geometry/concepts.h>
+#include <boost/lexical_cast.hpp>
 
 namespace gtsam {
 
-  /**
+/**
-   * Binary factor for a range measurement
+ * Binary factor for a range measurement
-   * @addtogroup SLAM
+ * @addtogroup SLAM
-   */
+ */
-  template<class POSE, class POINT>
+template<class T1, class T2 = T1>
-  class RangeFactor: public NoiseModelFactor2<POSE, POINT> {
+class RangeFactor: public NoiseModelFactor2<T1, T2> {
-  private:
+private:
 
-    double measured_; /** measurement */
+  double measured_; /** measurement */
-    boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame
 
-    typedef RangeFactor<POSE, POINT> This;
+  typedef RangeFactor<T1, T2> This;
-    typedef NoiseModelFactor2<POSE, POINT> Base;
+  typedef NoiseModelFactor2<T1, T2> Base;
 
-    typedef POSE Pose;
+  // Concept requirements for this factor
-    typedef POINT Point;
+  GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(T1, T2)
 
-    // Concept requirements for this factor
+public:
-    GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, POINT)
 
-  public:
+  RangeFactor() {
+  } /* Default constructor */
 
-    RangeFactor() {} /* Default constructor */
+  RangeFactor(Key key1, Key key2, double measured,
+      const SharedNoiseModel& model) :
+      Base(model, key1, key2), measured_(measured) {
+  }
 
-    RangeFactor(Key poseKey, Key pointKey, double measured,
+  virtual ~RangeFactor() {
-        const SharedNoiseModel& model, boost::optional<POSE> body_P_sensor = boost::none) :
+  }
-          Base(model, poseKey, pointKey), measured_(measured), body_P_sensor_(body_P_sensor) {
+
+  /// @return a deep copy of this factor
+  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /** h(x)-z */
+  Vector evaluateError(const T1& t1, const T2& t2, boost::optional<Matrix&> H1 =
+      boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+    double hx;
+    hx = t1.range(t2, H1, H2);
+    return (Vector(1) << hx - measured_).finished();
+  }
+
+  /** return the measured */
+  double measured() const {
+    return measured_;
+  }
+
+  /** equals specialized to this factor */
+  virtual bool equals(const NonlinearFactor& expected,
+      double tol = 1e-9) const {
+    const This *e = dynamic_cast<const This*>(&expected);
+    return e != NULL && Base::equals(*e, tol)
+        && fabs(this->measured_ - e->measured_) < tol;
+  }
+
+  /** print contents */
+  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
+      DefaultKeyFormatter) const {
+    std::cout << s << "RangeFactor, range = " << measured_ << std::endl;
+    Base::print("", keyFormatter);
+  }
+
+private:
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar
+        & boost::serialization::make_nvp("NoiseModelFactor2",
+            boost::serialization::base_object<Base>(*this));
+    ar & BOOST_SERIALIZATION_NVP(measured_);
+  }
+};
+// RangeFactor
+
+/**
+ * Binary factor for a range measurement, with a transform applied
+ * @addtogroup SLAM
+ */
+template<class POSE, class T2 = POSE>
+class RangeFactorWithTransform: public NoiseModelFactor2<POSE, T2> {
+private:
+
+  double measured_; /** measurement */
+  POSE body_P_sensor_; ///< The pose of the sensor in the body frame
+
+  typedef RangeFactorWithTransform<POSE, T2> This;
+  typedef NoiseModelFactor2<POSE, T2> Base;
+
+  // Concept requirements for this factor
+  GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, T2)
+
+public:
+
+  RangeFactorWithTransform() {
+  } /* Default constructor */
+
+  RangeFactorWithTransform(Key key1, Key key2, double measured,
+      const SharedNoiseModel& model, const POSE& body_P_sensor) :
+      Base(model, key1, key2), measured_(measured), body_P_sensor_(
+          body_P_sensor) {
+  }
+
+  virtual ~RangeFactorWithTransform() {
+  }
+
+  /// @return a deep copy of this factor
+  virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+    return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
+
+  /** h(x)-z */
+  Vector evaluateError(const POSE& t1, const T2& t2,
+      boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
+          boost::none) const {
+    double hx;
+    if (H1) {
+      Matrix H0;
+      hx = t1.compose(body_P_sensor_, H0).range(t2, H1, H2);
+      *H1 = *H1 * H0;
+    } else {
+      hx = t1.compose(body_P_sensor_).range(t2, H1, H2);
     }
+    return (Vector(1) << hx - measured_).finished();
+  }
 
-    virtual ~RangeFactor() {}
+  /** return the measured */
+  double measured() const {
+    return measured_;
+  }
 
-    /// @return a deep copy of this factor
+  /** equals specialized to this factor */
-    virtual gtsam::NonlinearFactor::shared_ptr clone() const {
+  virtual bool equals(const NonlinearFactor& expected,
-      return boost::static_pointer_cast<gtsam::NonlinearFactor>(
+      double tol = 1e-9) const {
-          gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
+    const This *e = dynamic_cast<const This*>(&expected);
+    return e != NULL && Base::equals(*e, tol)
+        && fabs(this->measured_ - e->measured_) < tol
+        && body_P_sensor_.equals(e->body_P_sensor_);
+  }
 
-    /** h(x)-z */
+  /** print contents */
-    Vector evaluateError(const POSE& pose, const POINT& point,
+  void print(const std::string& s = "", const KeyFormatter& keyFormatter =
-        boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+      DefaultKeyFormatter) const {
-      double hx;
+    std::cout << s << "RangeFactor, range = " << measured_ << std::endl;
-      if(body_P_sensor_) {
+    this->body_P_sensor_.print("  sensor pose in body frame: ");
-        if(H1) {
+    Base::print("", keyFormatter);
-          Matrix H0;
+  }
-          hx = pose.compose(*body_P_sensor_, H0).range(point, H1, H2);
-          *H1 = *H1 * H0;
-        } else {
-          hx = pose.compose(*body_P_sensor_).range(point, H1, H2);
-        }
-      } else {
-        hx = pose.range(point, H1, H2);
-      }
-      return (Vector(1) << hx - measured_).finished();
-    }
 
-    /** return the measured */
+private:
-    double measured() const {
-      return measured_;
-    }
 
-    /** equals specialized to this factor */
+  /** Serialization function */
-    virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
+  friend class boost::serialization::access;
-      const This *e = dynamic_cast<const This*> (&expected);
+  template<class ARCHIVE>
-      return e != NULL
+  void serialize(ARCHIVE & ar, const unsigned int version) {
-          && Base::equals(*e, tol)
+    ar
-          && fabs(this->measured_ - e->measured_) < tol
+        & boost::serialization::make_nvp("NoiseModelFactor2",
-          && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
+            boost::serialization::base_object<Base>(*this));
-    }
+    ar & BOOST_SERIALIZATION_NVP(measured_);
+    ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
+  }
+};
+// RangeFactor
 
-    /** print contents */
+}// namespace gtsam
-    void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-      std::cout << s << "RangeFactor, range = " << measured_ << std::endl;
-      if(this->body_P_sensor_)
-        this->body_P_sensor_->print("  sensor pose in body frame: ");
-      Base::print("", keyFormatter);
-    }
-
-  private:
-
-    /** Serialization function */
-    friend class boost::serialization::access;
-    template<class ARCHIVE>
-    void serialize(ARCHIVE & ar, const unsigned int version) {
-      ar & boost::serialization::make_nvp("NoiseModelFactor2",
-          boost::serialization::base_object<Base>(*this));
-      ar & BOOST_SERIALIZATION_NVP(measured_);
-      ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
-    }
-  }; // RangeFactor
-
-} // namespace gtsam

gtsam/slam/TriangulationFactor.h

@@ -10,14 +10,13 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * testTriangulationFactor.h
+ * triangulationFactor.h
  * @date March 2, 2014
  * @author Frank Dellaert
  */
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/geometry/SimpleCamera.h>
-#include <gtsam/base/numericalDerivative.h>
 #include <boost/optional.hpp>
 #include <boost/make_shared.hpp>
 

gtsam/slam/tests/testBetweenFactor.cpp

@@ -44,6 +44,30 @@ TEST(BetweenFactor, Rot3) {
   EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
 }
 
+/* ************************************************************************* */
+/*
+// Constructor scalar
+TEST(BetweenFactor, ConstructorScalar) {
+  SharedNoiseModel model;
+  double measured_value = 0.0;
+  BetweenFactor<double> factor(1, 2, measured_value, model);
+}
+
+// Constructor vector3
+TEST(BetweenFactor, ConstructorVector3) {
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
+  Vector3 measured_value(1, 2, 3);
+  BetweenFactor<Vector3> factor(1, 2, measured_value, model);
+}
+
+// Constructor dynamic sized vector
+TEST(BetweenFactor, ConstructorDynamicSizeVector) {
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
+  Vector measured_value(5); measured_value << 1, 2, 3, 4, 5;
+  BetweenFactor<Vector> factor(1, 2, measured_value, model);
+}
+*/
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/slam/tests/testPriorFactor.cpp

@@ -5,8 +5,8 @@
  * @date   Nov 4, 2014
  */
 
+#include <gtsam/base/Vector.h>
 #include <gtsam/slam/PriorFactor.h>
-#include <gtsam/base/LieScalar.h>
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;
@@ -14,10 +14,23 @@ using namespace gtsam;
 
 /* ************************************************************************* */
 
-// Constructor
+// Constructor scalar
-TEST(PriorFactor, Constructor) {
+TEST(PriorFactor, ConstructorScalar) {
   SharedNoiseModel model;
-  PriorFactor<LieScalar> factor(1, LieScalar(1.0), model);
+  PriorFactor<double> factor(1, 1.0, model);
+}
+
+// Constructor vector3
+TEST(PriorFactor, ConstructorVector3) {
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
+  PriorFactor<Vector3> factor(1, Vector3(1,2,3), model);
+}
+
+// Constructor dynamic sized vector
+TEST(PriorFactor, ConstructorDynamicSizeVector) {
+  Vector v(5); v << 1, 2, 3, 4, 5;
+  SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
+  PriorFactor<Vector> factor(1, v, model);
 }
 
 /* ************************************************************************* */

gtsam/slam/tests/testRangeFactor.cpp

@@ -34,14 +34,30 @@ static SharedNoiseModel model(noiseModel::Unit::Create(1));
 
 typedef RangeFactor<Pose2, Point2> RangeFactor2D;
 typedef RangeFactor<Pose3, Point3> RangeFactor3D;
+typedef RangeFactorWithTransform<Pose2, Point2> RangeFactorWithTransform2D;
+typedef RangeFactorWithTransform<Pose3, Point3> RangeFactorWithTransform3D;
 
 /* ************************************************************************* */
-Vector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) {
+Vector factorError2D(const Pose2& pose, const Point2& point,
+    const RangeFactor2D& factor) {
   return factor.evaluateError(pose, point);
 }
 
 /* ************************************************************************* */
-Vector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) {
+Vector factorError3D(const Pose3& pose, const Point3& point,
+    const RangeFactor3D& factor) {
+  return factor.evaluateError(pose, point);
+}
+
+/* ************************************************************************* */
+Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point,
+    const RangeFactorWithTransform2D& factor) {
+  return factor.evaluateError(pose, point);
+}
+
+/* ************************************************************************* */
+Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point,
+    const RangeFactorWithTransform3D& factor) {
   return factor.evaluateError(pose, point);
 }
 
@@ -61,10 +77,13 @@ TEST( RangeFactor, ConstructorWithTransform) {
   Key pointKey(2);
   double measurement(10.0);
   Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
-  Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
+  Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
+      Point3(0.25, -0.10, 1.0));
 
-  RangeFactor2D factor2D(poseKey, pointKey, measurement, model, body_P_sensor_2D);
+  RangeFactorWithTransform2D factor2D(poseKey, pointKey, measurement, model,
-  RangeFactor3D factor3D(poseKey, pointKey, measurement, model, body_P_sensor_3D);
+      body_P_sensor_2D);
+  RangeFactorWithTransform3D factor3D(poseKey, pointKey, measurement, model,
+      body_P_sensor_3D);
 }
 
 /* ************************************************************************* */
@@ -90,14 +109,19 @@ TEST( RangeFactor, EqualsWithTransform ) {
   Key pointKey(2);
   double measurement(10.0);
   Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
-  Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
+  Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
+      Point3(0.25, -0.10, 1.0));
 
-  RangeFactor2D factor2D_1(poseKey, pointKey, measurement, model, body_P_sensor_2D);
+  RangeFactorWithTransform2D factor2D_1(poseKey, pointKey, measurement, model,
-  RangeFactor2D factor2D_2(poseKey, pointKey, measurement, model, body_P_sensor_2D);
+      body_P_sensor_2D);
+  RangeFactorWithTransform2D factor2D_2(poseKey, pointKey, measurement, model,
+      body_P_sensor_2D);
   CHECK(assert_equal(factor2D_1, factor2D_2));
 
-  RangeFactor3D factor3D_1(poseKey, pointKey, measurement, model, body_P_sensor_3D);
+  RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model,
-  RangeFactor3D factor3D_2(poseKey, pointKey, measurement, model, body_P_sensor_3D);
+      body_P_sensor_3D);
+  RangeFactorWithTransform3D factor3D_2(poseKey, pointKey, measurement, model,
+      body_P_sensor_3D);
   CHECK(assert_equal(factor3D_1, factor3D_2));
 }
 
@@ -130,7 +154,8 @@ TEST( RangeFactor, Error2DWithTransform ) {
   Key pointKey(2);
   double measurement(10.0);
   Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
-  RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor);
+  RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model,
+      body_P_sensor);
 
   // Set the linearization point
   Rot2 R(0.57);
@@ -176,8 +201,10 @@ TEST( RangeFactor, Error3DWithTransform ) {
   Key poseKey(1);
   Key pointKey(2);
   double measurement(10.0);
-  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
+  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
-  RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor);
+      Point3(0.25, -0.10, 1.0));
+  RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model,
+      body_P_sensor);
 
   // Set the linearization point
   Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
@@ -213,8 +240,10 @@ TEST( RangeFactor, Jacobian2D ) {
 
   // Use numerical derivatives to calculate the Jacobians
   Matrix H1Expected, H2Expected;
-  H1Expected = numericalDerivative11<Vector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
+  H1Expected = numericalDerivative11<Vector, Pose2>(
-  H2Expected = numericalDerivative11<Vector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
+      boost::bind(&factorError2D, _1, point, factor), pose);
+  H2Expected = numericalDerivative11<Vector, Point2>(
+      boost::bind(&factorError2D, pose, _1, factor), point);
 
   // Verify the Jacobians are correct
   CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@@ -228,7 +257,8 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
   Key pointKey(2);
   double measurement(10.0);
   Pose2 body_P_sensor(0.25, -0.10, -M_PI_2);
-  RangeFactor2D factor(poseKey, pointKey, measurement, model, body_P_sensor);
+  RangeFactorWithTransform2D factor(poseKey, pointKey, measurement, model,
+      body_P_sensor);
 
   // Set the linearization point
   Rot2 R(0.57);
@@ -242,8 +272,10 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
 
   // Use numerical derivatives to calculate the Jacobians
   Matrix H1Expected, H2Expected;
-  H1Expected = numericalDerivative11<Vector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
+  H1Expected = numericalDerivative11<Vector, Pose2>(
-  H2Expected = numericalDerivative11<Vector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
+      boost::bind(&factorErrorWithTransform2D, _1, point, factor), pose);
+  H2Expected = numericalDerivative11<Vector, Point2>(
+      boost::bind(&factorErrorWithTransform2D, pose, _1, factor), point);
 
   // Verify the Jacobians are correct
   CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@@ -268,8 +300,10 @@ TEST( RangeFactor, Jacobian3D ) {
 
   // Use numerical derivatives to calculate the Jacobians
   Matrix H1Expected, H2Expected;
-  H1Expected = numericalDerivative11<Vector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
+  H1Expected = numericalDerivative11<Vector, Pose3>(
-  H2Expected = numericalDerivative11<Vector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
+      boost::bind(&factorError3D, _1, point, factor), pose);
+  H2Expected = numericalDerivative11<Vector, Point3>(
+      boost::bind(&factorError3D, pose, _1, factor), point);
 
   // Verify the Jacobians are correct
   CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@@ -282,8 +316,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
   Key poseKey(1);
   Key pointKey(2);
   double measurement(10.0);
-  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
+  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
-  RangeFactor3D factor(poseKey, pointKey, measurement, model, body_P_sensor);
+      Point3(0.25, -0.10, 1.0));
+  RangeFactorWithTransform3D factor(poseKey, pointKey, measurement, model,
+      body_P_sensor);
 
   // Set the linearization point
   Rot3 R = Rot3::RzRyRx(0.2, -0.3, 1.75);
@@ -297,8 +333,10 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
 
   // Use numerical derivatives to calculate the Jacobians
   Matrix H1Expected, H2Expected;
-  H1Expected = numericalDerivative11<Vector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
+  H1Expected = numericalDerivative11<Vector, Pose3>(
-  H2Expected = numericalDerivative11<Vector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
+      boost::bind(&factorErrorWithTransform3D, _1, point, factor), pose);
+  H2Expected = numericalDerivative11<Vector, Point3>(
+      boost::bind(&factorErrorWithTransform3D, pose, _1, factor), point);
 
   // Verify the Jacobians are correct
   CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@@ -306,6 +344,9 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */
 

gtsam/slam/tests/testTriangulationFactor.cpp

@@ -0,0 +1,70 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * testTriangulationFactor.cpp
+ *
+ *  Created on: July 30th, 2013
+ *      Author: cbeall3
+ */
+
+#include <gtsam/geometry/triangulation.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/assign.hpp>
+#include <boost/assign/std/vector.hpp>
+
+using namespace std;
+using namespace gtsam;
+using namespace boost::assign;
+
+// Some common constants
+static const boost::shared_ptr<Cal3_S2> sharedCal = //
+    boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
+
+// Looking along X-axis, 1 meter above ground plane (x-y)
+static const Rot3 upright = Rot3::ypr(-M_PI / 2, 0., -M_PI / 2);
+static const Pose3 pose1 = Pose3(upright, gtsam::Point3(0, 0, 1));
+PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
+
+// landmark ~5 meters infront of camera
+static const Point3 landmark(5, 0.5, 1.2);
+
+// 1. Project two landmarks into two cameras and triangulate
+Point2 z1 = camera1.project(landmark);
+
+//******************************************************************************
+TEST( triangulation, TriangulationFactor ) {
+  // Create the factor with a measurement that is 3 pixels off in x
+  Key pointKey(1);
+  SharedNoiseModel model;
+  typedef TriangulationFactor<> Factor;
+  Factor factor(camera1, z1, model, pointKey);
+
+  // Use the factor to calculate the Jacobians
+  Matrix HActual;
+  factor.evaluateError(landmark, HActual);
+
+  Matrix HExpected = numericalDerivative11<Vector,Point3>(
+      boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);
+
+  // Verify the Jacobians are correct
+  CHECK(assert_equal(HExpected, HActual, 1e-3));
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************

gtsam_unstable/nonlinear/CallRecord.h

@@ -32,12 +32,6 @@ class JacobianMap;
 // forward declaration
 
 //-----------------------------------------------------------------------------
-/**
- * MaxVirtualStaticRows defines how many separate virtual reverseAD with specific
- * static rows (1..MaxVirtualStaticRows) methods will be part of the CallRecord interface.
- */
-const int MaxVirtualStaticRows = 4;
-
 namespace internal {
 
 /**
@@ -57,7 +51,8 @@ struct ConvertToVirtualFunctionSupportedMatrixType {
 template<>
 struct ConvertToVirtualFunctionSupportedMatrixType<false> {
   template<typename Derived>
-  static const Eigen::Matrix<double, Derived::RowsAtCompileTime, Derived::ColsAtCompileTime> convert(
+  static const Eigen::Matrix<double, Derived::RowsAtCompileTime,
+      Derived::ColsAtCompileTime> convert(
       const Eigen::MatrixBase<Derived> & x) {
     return x;
   }
@@ -69,126 +64,132 @@ struct ConvertToVirtualFunctionSupportedMatrixType<false> {
   }
 };
 
-/**
- * Recursive definition of an interface having several purely
- * virtual _reverseAD(const Eigen::Matrix<double, Rows, Cols> &, JacobianMap&)
- * with Rows in 1..MaxSupportedStaticRows
- */
-template<int MaxSupportedStaticRows, int Cols>
-struct ReverseADInterface: ReverseADInterface<MaxSupportedStaticRows - 1, Cols> {
-  using ReverseADInterface<MaxSupportedStaticRows - 1, Cols>::_reverseAD;
-  virtual void _reverseAD(
-      const Eigen::Matrix<double, MaxSupportedStaticRows, Cols> & dFdT,
-      JacobianMap& jacobians) const = 0;
-};
-
-template<int Cols>
-struct ReverseADInterface<0, Cols> {
-  virtual void _reverseAD(
-      const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
-      JacobianMap& jacobians) const = 0;
-  virtual void _reverseAD(const Matrix & dFdT,
-      JacobianMap& jacobians) const = 0;
-};
-
-/**
- * ReverseADImplementor is a utility class used by CallRecordImplementor to
- * implementing the recursive ReverseADInterface interface.
- */
-template<typename Derived, int MaxSupportedStaticRows, int Cols>
-struct ReverseADImplementor: ReverseADImplementor<Derived,
-    MaxSupportedStaticRows - 1, Cols> {
-private:
-  using ReverseADImplementor<Derived, MaxSupportedStaticRows - 1, Cols>::_reverseAD;
-  virtual void _reverseAD(
-      const Eigen::Matrix<double, MaxSupportedStaticRows, Cols> & dFdT,
-      JacobianMap& jacobians) const {
-    static_cast<const Derived *>(this)->reverseAD(dFdT, jacobians);
-  }
-  friend struct internal::ReverseADImplementor<Derived,
-      MaxSupportedStaticRows + 1, Cols>;
-};
-
-template<typename Derived, int Cols>
-struct ReverseADImplementor<Derived, 0, Cols> : virtual internal::ReverseADInterface<
-    MaxVirtualStaticRows, Cols> {
-private:
-  using internal::ReverseADInterface<MaxVirtualStaticRows, Cols>::_reverseAD;
-  const Derived & derived() const {
-    return static_cast<const Derived&>(*this);
-  }
-  virtual void _reverseAD(
-      const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
-      JacobianMap& jacobians) const {
-    derived().reverseAD(dFdT, jacobians);
-  }
-  virtual void _reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const {
-    derived().reverseAD(dFdT, jacobians);
-  }
-  friend struct internal::ReverseADImplementor<Derived, 1, Cols>;
-};
-
 } // namespace internal
 
 /**
- * The CallRecord class stores the Jacobians of applying a function
+ * The CallRecord is an abstract base class for the any class that stores
- * with respect to each of its arguments. It also stores an execution trace
+ * the Jacobians of applying a function with respect to each of its arguments,
- * (defined below) for each of its arguments.
+ * as well as an execution trace for each of its arguments.
- *
- * It is implemented in the function-style ExpressionNode's nested Record class below.
  */
 template<int Cols>
-struct CallRecord: virtual private internal::ReverseADInterface<
+struct CallRecord {
-    MaxVirtualStaticRows, Cols> {
 
+  // Print entire record, recursively
   inline void print(const std::string& indent) const {
     _print(indent);
   }
 
-  inline void startReverseAD(JacobianMap& jacobians) const {
+  // Main entry point for the reverse AD process of a functional expression.
-    _startReverseAD(jacobians);
+  // Called *once* by the main AD entry point, ExecutionTrace::startReverseAD1
+  // This function then calls ExecutionTrace::reverseAD for every argument
+  // which will in turn call the reverseAD method below.
+  // This non-virtual function _startReverseAD3, implemented in derived
+  inline void startReverseAD2(JacobianMap& jacobians) const {
+    _startReverseAD3(jacobians);
   }
 
+  // Dispatch the reverseAD2 calls issued by ExecutionTrace::reverseAD1
+  // Here we convert to dynamic if the
   template<typename Derived>
-  inline void reverseAD(const Eigen::MatrixBase<Derived> & dFdT,
+  inline void reverseAD2(const Eigen::MatrixBase<Derived> & dFdT,
       JacobianMap& jacobians) const {
-    _reverseAD(
+    _reverseAD3(
-        internal::ConvertToVirtualFunctionSupportedMatrixType<(Derived::RowsAtCompileTime > MaxVirtualStaticRows)>::convert(
+        internal::ConvertToVirtualFunctionSupportedMatrixType<
-            dFdT), jacobians);
+            (Derived::RowsAtCompileTime > 5)>::convert(dFdT),
+        jacobians);
   }
 
-  inline void reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const {
+  // This overload supports matrices with both rows and columns dynamically sized.
-    _reverseAD(dFdT, jacobians);
+  // The template version above would be slower by introducing an extra conversion
+  // to statically sized columns.
+  inline void reverseAD2(const Matrix & dFdT, JacobianMap& jacobians) const {
+    _reverseAD3(dFdT, jacobians);
   }
 
   virtual ~CallRecord() {
   }
 
 private:
+
   virtual void _print(const std::string& indent) const = 0;
-  virtual void _startReverseAD(JacobianMap& jacobians) const = 0;
+  virtual void _startReverseAD3(JacobianMap& jacobians) const = 0;
-  using internal::ReverseADInterface<MaxVirtualStaticRows, Cols>::_reverseAD;
+
+  virtual void _reverseAD3(const Matrix & dFdT,
+      JacobianMap& jacobians) const = 0;
+
+  virtual void _reverseAD3(
+      const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
+      JacobianMap& jacobians) const = 0;
+
+  virtual void _reverseAD3(const Eigen::Matrix<double, 1, Cols> & dFdT,
+      JacobianMap& jacobians) const = 0;
+  virtual void _reverseAD3(const Eigen::Matrix<double, 2, Cols> & dFdT,
+      JacobianMap& jacobians) const = 0;
+  virtual void _reverseAD3(const Eigen::Matrix<double, 3, Cols> & dFdT,
+      JacobianMap& jacobians) const = 0;
+  virtual void _reverseAD3(const Eigen::Matrix<double, 4, Cols> & dFdT,
+      JacobianMap& jacobians) const = 0;
+  virtual void _reverseAD3(const Eigen::Matrix<double, 5, Cols> & dFdT,
+      JacobianMap& jacobians) const = 0;
 };
 
+/**
+ * CallRecordMaxVirtualStaticRows tells which separate virtual reverseAD with specific
+ * static rows (1..CallRecordMaxVirtualStaticRows) methods are part of the CallRecord 
+ * interface. It is used to keep the testCallRecord unit test in sync.
+ */
+const int CallRecordMaxVirtualStaticRows = 5;
+
 namespace internal {
 /**
  * The CallRecordImplementor implements the CallRecord interface for a Derived class by
  * delegating to its corresponding (templated) non-virtual methods.
  */
 template<typename Derived, int Cols>
-struct CallRecordImplementor: public CallRecord<Cols>,
+struct CallRecordImplementor: public CallRecord<Cols> {
-    private ReverseADImplementor<Derived, MaxVirtualStaticRows, Cols> {
 private:
+
   const Derived & derived() const {
     return static_cast<const Derived&>(*this);
   }
+
   virtual void _print(const std::string& indent) const {
     derived().print(indent);
   }
-  virtual void _startReverseAD(JacobianMap& jacobians) const {
+
-    derived().startReverseAD(jacobians);
+  virtual void _startReverseAD3(JacobianMap& jacobians) const {
+    derived().startReverseAD4(jacobians);
+  }
+
+  virtual void _reverseAD3(const Matrix & dFdT, JacobianMap& jacobians) const {
+    derived().reverseAD4(dFdT, jacobians);
+  }
+
+  virtual void _reverseAD3(
+      const Eigen::Matrix<double, Eigen::Dynamic, Cols> & dFdT,
+      JacobianMap& jacobians) const {
+    derived().reverseAD4(dFdT, jacobians);
+  }
+  virtual void _reverseAD3(const Eigen::Matrix<double, 1, Cols> & dFdT,
+      JacobianMap& jacobians) const {
+    derived().reverseAD4(dFdT, jacobians);
+  }
+  virtual void _reverseAD3(const Eigen::Matrix<double, 2, Cols> & dFdT,
+      JacobianMap& jacobians) const {
+    derived().reverseAD4(dFdT, jacobians);
+  }
+  virtual void _reverseAD3(const Eigen::Matrix<double, 3, Cols> & dFdT,
+      JacobianMap& jacobians) const {
+    derived().reverseAD4(dFdT, jacobians);
+  }
+  virtual void _reverseAD3(const Eigen::Matrix<double, 4, Cols> & dFdT,
+      JacobianMap& jacobians) const {
+    derived().reverseAD4(dFdT, jacobians);
+  }
+  virtual void _reverseAD3(const Eigen::Matrix<double, 5, Cols> & dFdT,
+      JacobianMap& jacobians) const {
+    derived().reverseAD4(dFdT, jacobians);
   }
-  template<typename D, int R, int C> friend struct ReverseADImplementor;
 };
 
 } // namespace internal

gtsam_unstable/nonlinear/ExpressionFactor.h

@@ -32,10 +32,11 @@ namespace gtsam {
 template<class T>
 class ExpressionFactor: public NoiseModelFactor {
 
+protected:
+
   T measurement_; ///< the measurement to be compared with the expression
   Expression<T> expression_; ///< the expression that is AD enabled
   FastVector<int> dims_; ///< dimensions of the Jacobian matrices
-  size_t augmentedCols_; ///< total number of columns + 1 (for RHS)
 
   static const int Dim = traits::dimension<T>::value;
 
@@ -55,15 +56,6 @@ public:
     // Get keys and dimensions for Jacobian matrices
     // An Expression is assumed unmutable, so we do this now
     boost::tie(keys_, dims_) = expression_.keysAndDims();
-
-    // Add sizes to know how much memory to allocate on stack in linearize
-    augmentedCols_ = std::accumulate(dims_.begin(), dims_.end(), 1);
-
-#ifdef DEBUG_ExpressionFactor
-    BOOST_FOREACH(size_t d, dims_)
-    std::cout << d << " ";
-    std::cout << " -> " << Dim << "x" << augmentedCols_ << std::endl;
-#endif
   }
 
   /**

gtsam_unstable/nonlinear/expressions.h

@@ -13,8 +13,7 @@
 namespace gtsam {
 
 // Generics
-
+template<typename T>
-template<class T>
 Expression<T> between(const Expression<T>& t1, const Expression<T>& t2) {
   return Expression<T>(t1, &T::between, t2);
 }

gtsam_unstable/nonlinear/tests/testBasisDecompositions.cpp

@@ -1,119 +0,0 @@
-/* ----------------------------------------------------------------------------
-
- * 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 testBasisDecompositions.cpp
- * @date November 23, 2014
- * @author Frank Dellaert
- * @brief unit tests for Basis Decompositions w Expressions
- */
-
-#include <gtsam_unstable/nonlinear/expressions.h>
-#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
-#include <gtsam/linear/VectorValues.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/base/numericalDerivative.h>
-#include <gtsam/base/Testable.h>
-
-#include <CppUnitLite/TestHarness.h>
-
-#include <boost/assign/list_of.hpp>
-using boost::assign::list_of;
-
-using namespace std;
-using namespace gtsam;
-
-noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
-
-/// Fourier
-template<int N>
-class Fourier {
-
-public:
-
-  typedef Eigen::Matrix<double, N, 1> Coefficients;
-  typedef Eigen::Matrix<double, 1, N> Jacobian;
-
-private:
-
-  double x_;
-  Jacobian H_;
-
-public:
-
-  /// Constructor
-  Fourier(double x) :
-      x_(x) {
-    H_(0, 0) = 1;
-    for (size_t i = 1; i < N; i += 2) {
-      H_(0, i) = cos(i * x);
-      H_(0, i + 1) = sin(i * x);
-    }
-  }
-
-  /// Given coefficients c, predict value for x
-  double operator()(const Coefficients& c, boost::optional<Jacobian&> H) {
-    if (H)
-      (*H) = H_;
-    return H_ * c;
-  }
-};
-
-//******************************************************************************
-TEST(BasisDecompositions, Fourier) {
-  Fourier<3> fx(0);
-  Eigen::Matrix<double, 1, 3> expectedH, actualH;
-  Vector3 c(1.5661, 1.2717, 1.2717);
-  expectedH = numericalDerivative11<double, Vector3>(
-      boost::bind(&Fourier<3>::operator(), fx, _1, boost::none), c);
-  EXPECT_DOUBLES_EQUAL(c[0]+c[1], fx(c,actualH), 1e-9);
-  EXPECT(assert_equal((Matrix)expectedH, actualH));
-}
-
-//******************************************************************************
-TEST(BasisDecompositions, FourierExpression) {
-
-  // Create linear factor graph
-  GaussianFactorGraph g;
-  Key key(1);
-  Vector3_ c(key);
-  for (size_t i = 0; i < 16; i++) {
-    double x = i * M_PI / 8, y = exp(sin(x) + cos(x));
-
-    // Manual JacobianFactor
-    Matrix A(1, 3);
-    A << 1, cos(x), sin(x);
-    Vector b(1);
-    b << y;
-    JacobianFactor f1(key, A, b, model);
-
-    // With ExpressionFactor
-    Expression<double> expression(Fourier<3>(x), c);
-    ExpressionFactor<double> f2(model, y, expression);
-
-    g.add(f1);
-  }
-
-  // Solve
-  VectorValues actual = g.optimize();
-
-  // Check
-  Vector3 expected(1.5661, 1.2717, 1.2717);
-  EXPECT(assert_equal((Vector) expected, actual.at(key),1e-4));
-}
-
-//******************************************************************************
-int main() {
-  TestResult tr;
-  return TestRegistry::runAllTests(tr);
-}
-//******************************************************************************
-