BIG CHANGE: the OptionalJacobian<M,N> obviates the need for the `optional<Matrix&> Jacobian arguments. They will continue to exist, for backwards compatibility, in the old-style factors, but everywhere else they should disappear. This PR by Natesh has eradicated all but one in gtsam/geometry. Great job !!!!

Merged in feature/fixedSizeDerivatives (pull request #54)

Proposed way to do Jacobians from now on via Eigen::Ref like type

.cproject

@@ -854,6 +854,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testSmartStereoProjectionPoseFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testSmartStereoProjectionPoseFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -2953,6 +2961,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testRangeFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testRangeFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>

.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

gtsam.h

@@ -758,10 +758,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);
@@ -2198,10 +2194,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>

gtsam/base/Matrix.h

@@ -37,36 +37,36 @@ 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;
 typedef Eigen::Block<const Matrix> ConstSubMatrix;
 
-// Two very commonly used matrices:
-const Matrix3 Z_3x3 = Matrix3::Zero();
-const Matrix3 I_3x3 = Matrix3::Identity();
-
 // Matlab-like syntax
 
 /**

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/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,15 +34,17 @@ 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_;
 }
 
 /* ************************************************************************* */
@@ -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,8 +28,10 @@ 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;
 }
 
 /* ************************************************************************* */
@@ -39,7 +41,7 @@ Vector Cal3DS2_Base::vector() const {
 
 /* ************************************************************************* */
 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 +93,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 +127,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 +158,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 +169,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,7 +45,7 @@ protected:
   double p1_, p2_ ; // tangential distortion
 
 public:
-  Matrix K() const ;
+  Matrix3 K() const ;
   Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
   Vector vector() const ;
 
@@ -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

@@ -52,8 +52,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 +81,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 +93,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;

gtsam/geometry/Cal3Unified.h

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

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

@@ -125,29 +125,26 @@ public:
   }
 
   /// 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 +152,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 +171,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_);
   }
 

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) {
+      Matrix23 H;
       project_to_camera(q,H);
-    if (Dpose) *Dpose = H * (*Dpose);
+      if (Dpose) *Dpose = H * (*Dpose_);
-    if (Dpoint) *Dpoint = H * (*Dpoint);
+      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);
   }
 
@@ -232,7 +211,8 @@ 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);
   }
 }
@@ -55,22 +52,23 @@ EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
 
 /* ************************************************************************* */
 Vector EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
-  return (Vector(5) <<
+  return (Vector(5) << aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(
-      aRb_.localCoordinates(other.aRb_), aTb_.localCoordinates(other.aTb_)).finished();
+      other.aTb_)).finished();
 }
 
 /* ************************************************************************* */
-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 +76,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 +87,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 +106,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

@@ -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>
@@ -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,20 +225,6 @@ public:
     return std::make_pair(K_.uncalibrate(pn), pc.z() > 0);
   }
 
-  /** project a point from world coordinate to the image
-   *  @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
@@ -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:

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);
   }
 
@@ -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_;
 }
 
 /* ************************************************************************* */
@@ -108,105 +113,64 @@ Vector Pose2::localCoordinates(const Pose2& p2) const {
 /* ************************************************************************* */
 // 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 (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
@@ -166,7 +155,7 @@ public:
    * 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,7 +168,7 @@ 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) <<
         0.,-w,  vx,
         w,  0., vy,
@@ -190,23 +179,15 @@ public:
   /// @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 +218,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 +226,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 +242,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 +251,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;
+    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;
@@ -383,8 +357,8 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
   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_);
     }
 
@@ -180,8 +180,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 +191,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 +225,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);
@@ -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

@@ -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,
-             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;
-         ).finished();
+        *H2 << d * (*H2);
       }
 #endif
+      if (H3)
+        // TODO, this is set to zero, as Cal3_S2Stereo cannot be optimized yet
+        H3->setZero();
     }
 
     // finally translate
@@ -87,7 +87,7 @@ 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();

gtsam/geometry/StereoCamera.h

@@ -95,8 +95,8 @@ public:
   }
 
   Pose3 between(const StereoCamera &camera,
-      boost::optional<Matrix&> H1=boost::none,
+      OptionalJacobian<6,6> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const {
+      OptionalJacobian<6,6> H2=boost::none) const {
     return leftCamPose_.between(camera.pose(), H1, H2);
   }
 
@@ -119,9 +119,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 +139,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,17 +147,17 @@ 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
@@ -168,7 +168,7 @@ Vector Unit3::localCoordinates(const Unit3& y) const {
   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/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/nonlinear/AdaptAutoDiff.h

@@ -20,6 +20,7 @@
 
 #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>
@@ -93,8 +94,10 @@ struct UseBlockIf {
   static void addToJacobian(const Eigen::MatrixBase<Derived>& dTdA,
       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>
@@ -111,10 +114,9 @@ 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);
 }
 
 //-----------------------------------------------------------------------------
@@ -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;
 };
 
 /**
@@ -670,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;
 
@@ -715,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;
 
@@ -769,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;
 
@@ -787,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),
@@ -224,9 +224,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/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);
@@ -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/slam/RangeFactor.h

@@ -16,9 +16,9 @@
 
 #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 {
 
@@ -26,52 +26,127 @@ namespace gtsam {
  * Binary factor for a range measurement
  * @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:
 
   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;
-    typedef POINT Point;
 
   // Concept requirements for this factor
-    GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(POSE, POINT)
+  GTSAM_CONCEPT_RANGE_MEASUREMENT_TYPE(T1, T2)
 
 public:
 
-    RangeFactor() {} /* Default constructor */
+  RangeFactor() {
+  } /* Default constructor */
 
-    RangeFactor(Key poseKey, Key pointKey, double measured,
+  RangeFactor(Key key1, Key key2, double measured,
-        const SharedNoiseModel& model, boost::optional<POSE> body_P_sensor = boost::none) :
+      const SharedNoiseModel& model) :
-          Base(model, poseKey, pointKey), measured_(measured), body_P_sensor_(body_P_sensor) {
+      Base(model, key1, key2), measured_(measured) {
   }
 
-    virtual ~RangeFactor() {}
+  virtual ~RangeFactor() {
+  }
 
   /// @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))); }
+        gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+  }
 
   /** h(x)-z */
-    Vector evaluateError(const POSE& pose, const POINT& point,
+  Vector evaluateError(const T1& t1, const T2& t2, boost::optional<Matrix&> H1 =
-        boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) const {
+      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(body_P_sensor_) {
     if (H1) {
       Matrix H0;
-          hx = pose.compose(*body_P_sensor_, H0).range(point, H1, H2);
+      hx = t1.compose(body_P_sensor_, H0).range(t2, H1, H2);
       *H1 = *H1 * H0;
     } else {
-          hx = pose.compose(*body_P_sensor_).range(point, H1, H2);
+      hx = t1.compose(body_P_sensor_).range(t2, H1, H2);
-        }
-      } else {
-        hx = pose.range(point, H1, H2);
     }
     return (Vector(1) << hx - measured_).finished();
   }
@@ -82,19 +157,19 @@ namespace gtsam {
   }
 
   /** equals specialized to this factor */
-    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
+    return e != NULL && Base::equals(*e, tol)
-          && Base::equals(*e, tol)
         && fabs(this->measured_ - e->measured_) < tol
-          && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
+        && body_P_sensor_.equals(e->body_P_sensor_);
   }
 
   /** print contents */
-    void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
+  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: ");
-        this->body_P_sensor_->print("  sensor pose in body frame: ");
     Base::print("", keyFormatter);
   }
 
@@ -104,11 +179,13 @@ namespace gtsam {
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int version) {
-      ar & boost::serialization::make_nvp("NoiseModelFactor2",
+    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
+};
+// 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/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/tests/testBasisDecompositions.cpp

@@ -1,231 +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/base/Matrix.h>
-
-namespace gtsam {
-
-/// 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;
-  }
-};
-
-}
-
-#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/VectorValues.h>
-
-namespace gtsam {
-
-/// For now, this is our sequence representation
-typedef std::map<double, double> Sequence;
-
-/**
- * Class that does Fourier Decomposition via least squares
- */
-class FourierDecomposition {
-public:
-
-  typedef Vector3 Coefficients; ///< Fourier coefficients
-
-private:
-  Coefficients c_;
-
-public:
-
-  /// Create nonlinear FG from Sequence
-  static NonlinearFactorGraph NonlinearGraph(const Sequence& sequence,
-      const SharedNoiseModel& model) {
-    NonlinearFactorGraph graph;
-    Expression<Coefficients> c(0);
-    typedef std::pair<double, double> Sample;
-    BOOST_FOREACH(Sample sample, sequence) {
-      Expression<double> expression(Fourier<3>(sample.first), c);
-      ExpressionFactor<double> factor(model, sample.second, expression);
-      graph.add(factor);
-    }
-    return graph;
-  }
-
-  /// Create linear FG from Sequence
-  static GaussianFactorGraph::shared_ptr LinearGraph(const Sequence& sequence,
-      const SharedNoiseModel& model) {
-    NonlinearFactorGraph graph = NonlinearGraph(sequence, model);
-    Values values;
-    values.insert<Coefficients>(0, Coefficients::Zero()); // does not matter
-    GaussianFactorGraph::shared_ptr gfg = graph.linearize(values);
-    return gfg;
-  }
-
-  /// Constructor
-  FourierDecomposition(const Sequence& sequence,
-      const SharedNoiseModel& model) {
-    GaussianFactorGraph::shared_ptr gfg = LinearGraph(sequence, model);
-    VectorValues solution = gfg->optimize();
-    c_ = solution.at(0);
-  }
-
-  /// Return Fourier coefficients
-  Coefficients coefficients() {
-    return c_;
-  }
-};
-
-}
-
-#include <gtsam_unstable/nonlinear/expressionTesting.h>
-#include <gtsam/base/Testable.h>
-#include <CppUnitLite/TestHarness.h>
-
-using namespace std;
-using namespace gtsam;
-
-noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
-
-//******************************************************************************
-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, ManualFourier) {
-
-  // Create linear factor graph
-  GaussianFactorGraph g;
-  Key key(1);
-  Expression<Vector3> c(key);
-  Values values;
-  values.insert<Vector3>(key, Vector3::Zero()); // does not matter
-  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);
-    g.add(f1);
-
-    // With ExpressionFactor
-    Expression<double> expression(Fourier<3>(x), c);
-    EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, 1e-5, 1e-9);
-    {
-      ExpressionFactor<double> f2(model, y, expression);
-      boost::shared_ptr<GaussianFactor> gf = f2.linearize(values);
-      boost::shared_ptr<JacobianFactor> jf = //
-          boost::dynamic_pointer_cast<JacobianFactor>(gf);
-      CHECK(jf);
-      EXPECT( assert_equal(f1, *jf, 1e-9));
-    }
-    {
-      ExpressionFactor<double> f2(model, y, expression);
-      boost::shared_ptr<GaussianFactor> gf = f2.linearize(values);
-      boost::shared_ptr<JacobianFactor> jf = //
-          boost::dynamic_pointer_cast<JacobianFactor>(gf);
-      CHECK(jf);
-      EXPECT( assert_equal(f1, *jf, 1e-9));
-    }
-    {
-      ExpressionFactor<double> f2(model, y, expression);
-      boost::shared_ptr<GaussianFactor> gf = f2.linearize(values);
-      boost::shared_ptr<JacobianFactor> jf = //
-          boost::dynamic_pointer_cast<JacobianFactor>(gf);
-      CHECK(jf);
-      EXPECT( assert_equal(f1, *jf, 1e-9));
-    }
-    {
-      ExpressionFactor<double> f2(model, y, expression);
-      boost::shared_ptr<GaussianFactor> gf = f2.linearize(values);
-      boost::shared_ptr<JacobianFactor> jf = //
-          boost::dynamic_pointer_cast<JacobianFactor>(gf);
-      CHECK(jf);
-      EXPECT( assert_equal(f1, *jf, 1e-9));
-    }
-  }
-
-  // Solve
-  VectorValues actual = g.optimize();
-
-  // Check
-  Vector3 expected(1.5661, 1.2717, 1.2717);
-  EXPECT(assert_equal((Vector) expected, actual.at(key),1e-4));
-}
-
-//******************************************************************************
-TEST(BasisDecompositions, FourierDecomposition) {
-
-  // Create example sequence
-  Sequence sequence;
-  for (size_t i = 0; i < 16; i++) {
-    double x = i * M_PI / 8, y = exp(sin(x) + cos(x));
-    sequence[x] = y;
-  }
-
-  // Do Fourier Decomposition
-  FourierDecomposition actual(sequence, model);
-
-  // Check
-  Vector3 expected(1.5661, 1.2717, 1.2717);
-  EXPECT(assert_equal((Vector) expected, actual.coefficients(),1e-4));
-}
-
-//******************************************************************************
-int main() {
-  TestResult tr;
-  return TestRegistry::runAllTests(tr);
-}
-//******************************************************************************
-

gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp

@@ -131,14 +131,14 @@ TEST(ExpressionFactor, Unary) {
 // Unary(Leaf)) and Unary(Unary(Leaf)))
 // wide version (not handled in fixed-size pipeline)
 typedef Eigen::Matrix<double,9,3> Matrix93;
-Vector9 wide(const Point3& p, boost::optional<Matrix93&> H) {
+Vector9 wide(const Point3& p, OptionalJacobian<9,3> H) {
   Vector9 v;
   v << p.vector(), p.vector(), p.vector();
   if (H) *H << eye(3), eye(3), eye(3);
   return v;
 }
 typedef Eigen::Matrix<double,9,9> Matrix9;
-Vector9 id9(const Vector9& v, boost::optional<Matrix9&> H) {
+Vector9 id9(const Vector9& v, OptionalJacobian<9,9> H) {
   if (H) *H = Matrix9::Identity();
   return v;
 }
@@ -161,7 +161,7 @@ TEST(ExpressionFactor, Wide) {
 
 /* ************************************************************************* */
 static Point2 myUncal(const Cal3_S2& K, const Point2& p,
-    boost::optional<Matrix25&> Dcal, boost::optional<Matrix2&> Dp) {
+    OptionalJacobian<2,5> Dcal, OptionalJacobian<2,2> Dp) {
   return K.uncalibrate(p, Dcal, Dp);
 }
 
@@ -427,8 +427,7 @@ TEST(ExpressionFactor, compose3) {
 /* ************************************************************************* */
 // Test compose with three arguments
 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_unstable/nonlinear/tests/testExpressionMeta.cpp

@@ -179,29 +179,29 @@ TEST(ExpressionFactor, InvokeDerivatives) {
 
   // Let's assign it it to a boost function object
   // cast is needed because Pose3::transform_to is overloaded
-  typedef boost::function<Point3(const Pose3&, const Point3&)> F;
+//  typedef boost::function<Point3(const Pose3&, const Point3&)> F;
-  F f = static_cast<Point3 (Pose3::*)(
+//  F f = static_cast<Point3 (Pose3::*)(
-      const Point3&) const >(&Pose3::transform_to);
+//      const Point3&) const >(&Pose3::transform_to);
-
+//
-  // Create arguments
+//  // Create arguments
-Pose3  pose;
+//  Pose3  pose;
-  Point3 point;
+//  Point3 point;
-  typedef boost::fusion::vector<Pose3, Point3> Arguments;
+//  typedef boost::fusion::vector<Pose3, Point3> Arguments;
-  Arguments args = boost::fusion::make_vector(pose, point);
+//  Arguments args = boost::fusion::make_vector(pose, point);
-
+//
-  // Create fused function (takes fusion vector) and call it
+//  // Create fused function (takes fusion vector) and call it
-  boost::fusion::fused<F> g(f);
+//  boost::fusion::fused<F> g(f);
-  Point3 actual = g(args);
+//  Point3 actual = g(args);
-  CHECK(assert_equal(point,actual));
+//  CHECK(assert_equal(point,actual));
-
+//
-  // We can *immediately* do this using invoke
+//  // We can *immediately* do this using invoke
-  Point3 actual2 = boost::fusion::invoke(f, args);
+//  Point3 actual2 = boost::fusion::invoke(f, args);
-  CHECK(assert_equal(point,actual2));
+//  CHECK(assert_equal(point,actual2));
 
   // Now, let's create the optional Jacobian arguments
   typedef Point3 T;
   typedef boost::mpl::vector<Pose3, Point3> TYPES;
-  typedef boost::mpl::transform<TYPES, OptionalJacobian<T, MPL::_1> >::type Optionals;
+  typedef boost::mpl::transform<TYPES, MakeOptionalJacobian<T, MPL::_1> >::type Optionals;
 
   // Unfortunately this is moot: we need a pointer to a function with the
   // optional derivatives; I don't see a way of calling a function that we
@@ -215,8 +215,8 @@ struct proxy {
     return pose.transform_to(point);
   }
   Point3 operator()(const Pose3& pose, const Point3& point,
-      boost::optional<Matrix36&> Dpose,
+      OptionalJacobian<3,6> Dpose,
-      boost::optional<Matrix3&> Dpoint) const {
+      OptionalJacobian<3,3> Dpoint) const {
     return pose.transform_to(point, Dpose, Dpoint);
   }
 };

gtsam_unstable/slam/AHRS.cpp

@@ -12,17 +12,14 @@ using namespace std;
 
 namespace gtsam {
 
-Matrix cov(const Matrix& m) {
+/* ************************************************************************* */
+Matrix3 AHRS::Cov(const Vector3s& m) {
   const double num_observations = m.cols();
-  const Vector mean = m.rowwise().sum() / num_observations;
+  const Vector3 mean = m.rowwise().sum() / num_observations;
-  Matrix D = m.colwise() - mean;
+  Vector3s D = m.colwise() - mean;
-  Matrix DDt = D * trans(D);
+  return D * trans(D) / (num_observations - 1);
-  return DDt / (num_observations - 1);
 }
 
-Matrix I3 = eye(3);
-Matrix Z3 = zeros(3, 3);
-
 /* ************************************************************************* */
 AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
     bool flat) :
@@ -34,11 +31,11 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
   size_t T = stationaryU.cols();
 
   // estimate standard deviation on gyroscope readings
-  Pg_ = cov(stationaryU);
+  Pg_ = Cov(stationaryU);
-  Vector sigmas_v_g = esqrt(Pg_.diagonal() * T);
+  Vector3 sigmas_v_g = esqrt(Pg_.diagonal() * T);
 
   // estimate standard deviation on accelerometer readings
-  Pa_ = cov(stationaryF);
+  Pa_ = Cov(stationaryF);
 
   // Quantities needed for integration
 
@@ -46,15 +43,13 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
   double tau_g = 730; // correlation time for gyroscope
   double tau_a = 730; // correlation time for accelerometer
 
-  F_g_ = -I3 / tau_g;
+  F_g_ = -I_3x3 / tau_g;
-  F_a_ = -I3 / tau_a;
+  F_a_ = -I_3x3 / tau_a;
   Vector3 var_omega_w = 0 * ones(3); // TODO
   Vector3 var_omega_g = (0.0034 * 0.0034) * ones(3);
   Vector3 var_omega_a = (0.034 * 0.034) * ones(3);
-  Vector3 sigmas_v_g_sq = emul(sigmas_v_g, sigmas_v_g);
+  Vector3 sigmas_v_g_sq = sigmas_v_g.cwiseProduct(sigmas_v_g);
-  Vector vars(12);
+  var_w_ << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a;
-  vars << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a;
-  var_w_ = diag(vars);
 
   // Quantities needed for aiding
   sigmas_v_a_ = esqrt(T * Pa_.diagonal());
@@ -95,15 +90,15 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::initialize(double g_e)
 
   Matrix P_plus_k2 = Matrix(9, 9);
   P_plus_k2.block<3,3>(0, 0) = P11;
-  P_plus_k2.block<3,3>(0, 3) = Z3;
+  P_plus_k2.block<3,3>(0, 3) = Z_3x3;
   P_plus_k2.block<3,3>(0, 6) = P12;
 
-  P_plus_k2.block<3,3>(3, 0) = Z3;
+  P_plus_k2.block<3,3>(3, 0) = Z_3x3;
   P_plus_k2.block<3,3>(3, 3) = Pg_;
-  P_plus_k2.block<3,3>(3, 6) = Z3;
+  P_plus_k2.block<3,3>(3, 6) = Z_3x3;
 
   P_plus_k2.block<3,3>(6, 0) = trans(P12);
-  P_plus_k2.block<3,3>(6, 3) = Z3;
+  P_plus_k2.block<3,3>(6, 3) = Z_3x3;
   P_plus_k2.block<3,3>(6, 6) = Pa;
 
   Vector dx = zero(9);
@@ -123,26 +118,26 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::integrate(
   // FIXME:
   //if nargout>1
   Matrix bRn = mech.bRn().matrix();
-  Matrix I3 = eye(3);
-  Matrix Z3 = zeros(3, 3);
 
-  Matrix F_k = zeros(9, 9);
+  Matrix9 F_k; F_k.setZero();
   F_k.block<3,3>(0, 3) = -bRn;
   F_k.block<3,3>(3, 3) = F_g_;
   F_k.block<3,3>(6, 6) = F_a_;
 
-  Matrix G_k = zeros(9, 12);
+  typedef Eigen::Matrix<double,9,12> Matrix9_12;
+  Matrix9_12 G_k; G_k.setZero();
   G_k.block<3,3>(0, 0) = -bRn;
   G_k.block<3,3>(0, 6) = -bRn;
-  G_k.block<3,3>(3, 3) = I3;
+  G_k.block<3,3>(3, 3) = I_3x3;
-  G_k.block<3,3>(6, 9) = I3;
+  G_k.block<3,3>(6, 9) = I_3x3;
 
-  Matrix Q_k = G_k * var_w_ * trans(G_k);
+  Matrix9 Q_k = G_k * var_w_.asDiagonal() * G_k.transpose();
-  Matrix Psi_k = eye(9) + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix
+  Matrix9 Psi_k = I_9x9 + dt * F_k; // +dt*dt*F_k*F_k/2; // transition matrix
 
   // This implements update in section 10.6
-  Matrix B = zeros(9, 9);
+  Matrix9 B; B.setZero();
-  Vector u2 = zero(9);
+  Vector9 u2; u2.setZero();
+  // TODO predictQ should be templated to also take fixed size matrices.
   KalmanFilter::State newState = KF_.predictQ(state, Psi_k,B,u2,dt*Q_k);
   return make_pair(newMech, newState);
 }
@@ -175,7 +170,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
   if (Farrell) {
     // calculate residual gravity measurement
     z = n_g_ - trans(bRn) * measured_b_g;
-    H = collect(3, &n_g_cross_, &Z3, &bRn);
+    H = collect(3, &n_g_cross_, &Z_3x3, &bRn);
     R = trans(bRn) * diag(emul(sigmas_v_a_, sigmas_v_a_)) * bRn;
   } else {
     // my measurement prediction (in body frame):
@@ -189,7 +184,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aid(
     z = bRn * n_g_ - measured_b_g;
     // Now the Jacobian H
     Matrix b_g = bRn * n_g_cross_;
-    H = collect(3, &b_g, &Z3, &I3);
+    H = collect(3, &b_g, &Z_3x3, &I_3x3);
     // And the measurement noise, TODO: should be created once where sigmas_v_a is given
     R = diag(emul(sigmas_v_a_, sigmas_v_a_));
   }
@@ -219,7 +214,7 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aidGeneral(
   Vector z = f - increment * f_previous;
   //Vector z = increment * f_previous - f;
   Matrix b_g = skewSymmetric(increment* f_previous);
-  Matrix H = collect(3, &b_g, &I3, &Z3);
+  Matrix H = collect(3, &b_g, &I_3x3, &Z_3x3);
 //  Matrix R = diag(emul(sigmas_v_a_, sigmas_v_a_));
 //  Matrix R = diag(Vector3(1.0, 0.2, 1.0)); // good for L_twice
   Matrix R = diag(Vector3(0.01, 0.0001, 0.01));
@@ -240,16 +235,16 @@ std::pair<Mechanization_bRn2, KalmanFilter::State> AHRS::aidGeneral(
 /* ************************************************************************* */
 void AHRS::print(const std::string& s) const {
   mech0_.print(s + ".mech0_");
-  gtsam::print(F_g_, s + ".F_g");
+  gtsam::print((Matrix)F_g_, s + ".F_g");
-  gtsam::print(F_a_, s + ".F_a");
+  gtsam::print((Matrix)F_a_, s + ".F_a");
-  gtsam::print(var_w_, s + ".var_w");
+  gtsam::print((Vector)var_w_, s + ".var_w");
 
-  gtsam::print(sigmas_v_a_, s + ".sigmas_v_a");
+  gtsam::print((Vector)sigmas_v_a_, s + ".sigmas_v_a");
-  gtsam::print(n_g_, s + ".n_g");
+  gtsam::print((Vector)n_g_, s + ".n_g");
-  gtsam::print(n_g_cross_, s + ".n_g_cross");
+  gtsam::print((Matrix)n_g_cross_, s + ".n_g_cross");
 
-  gtsam::print(Pg_, s + ".P_g");
+  gtsam::print((Matrix)Pg_, s + ".P_g");
-  gtsam::print(Pa_, s + ".P_a");
+  gtsam::print((Matrix)Pa_, s + ".P_a");
 
 }
 

gtsam_unstable/slam/AHRS.h

@@ -14,8 +14,6 @@
 
 namespace gtsam {
 
-GTSAM_UNSTABLE_EXPORT Matrix cov(const Matrix& m);
-
 class GTSAM_UNSTABLE_EXPORT AHRS {
 
 private:
@@ -25,18 +23,24 @@ private:
   KalmanFilter KF_;          ///< initial Kalman filter
 
   // Quantities needed for integration
-  Matrix F_g_;              ///< gyro bias dynamics
+  Matrix3 F_g_;              ///< gyro bias dynamics
-  Matrix F_a_;              ///< acc bias dynamics
+  Matrix3 F_a_;              ///< acc bias dynamics
-  Matrix var_w_;            ///< dynamic noise variances
+
+  typedef Eigen::Matrix<double,12,1> Variances;
+  Variances var_w_;            ///< dynamic noise variances
 
   // Quantities needed for aiding
-  Vector sigmas_v_a_;       ///< measurement sigma
+  Vector3 sigmas_v_a_;       ///< measurement sigma
-  Vector n_g_;              ///< gravity in nav frame
+  Vector3 n_g_;              ///< gravity in nav frame
-  Matrix n_g_cross_;        ///< and its skew-symmetric matrix
+  Matrix3 n_g_cross_;        ///< and its skew-symmetric matrix
 
-  Matrix Pg_, Pa_;
+  Matrix3 Pg_, Pa_;
 
 public:
+
+  typedef Eigen::Matrix<double,3,Eigen::Dynamic> Vector3s;
+  static Matrix3 Cov(const Vector3s& m);
+
   /**
    * AHRS constructor
    * @param stationaryU initial interval of gyro measurements, 3xn matrix

gtsam_unstable/slam/expressions.h

@@ -20,9 +20,8 @@ typedef Expression<Rot2> Rot2_;
 typedef Expression<Pose2> Pose2_;
 
 Point2_ transform_to(const Pose2_& x, const Point2_& p) {
-  Point2(Pose2::*transform)(const Point2& p,
+  Point2 (Pose2::*transform)(const Point2& p, OptionalJacobian<2, 3> H1,
-                           boost::optional<Matrix23&> H1,
+      OptionalJacobian<2, 2> H2) const = &Pose2::transform_to;
-                           boost::optional<Matrix2&> H2) const = &Pose2::transform_to;
 
   return Point2_(x, transform, p);
 }
@@ -35,9 +34,8 @@ typedef Expression<Pose3> Pose3_;
 
 Point3_ transform_to(const Pose3_& x, const Point3_& p) {
 
-  Point3(Pose3::*transform)(const Point3& p, 
+  Point3 (Pose3::*transform)(const Point3& p, OptionalJacobian<3, 6> Dpose,
-                           boost::optional<Matrix36&> Dpose,
+      OptionalJacobian<3, 3> Dpoint) const = &Pose3::transform_to;
-                           boost::optional<Matrix3&> Dpoint) const = &Pose3::transform_to;
 
   return Point3_(x, transform, p);
 }
@@ -51,8 +49,7 @@ Point2_ project(const Point3_& p_cam) {
 }
 
 Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K,
-    boost::optional<Matrix26&> Dpose, boost::optional<Matrix23&> Dpoint,
+    OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, OptionalJacobian<2, 5> Dcal) {
-    boost::optional<Matrix25&> Dcal) {
   return PinholeCamera<Cal3_S2>(x, K).project(p, Dpose, Dpoint, Dcal);
 }
 

gtsam_unstable/slam/tests/testAHRS.cpp

@@ -30,7 +30,7 @@ TEST (AHRS, cov) {
       9.0, 4.0, 7.0,
       6.0, 3.0, 2.0).finished();
 
-  Matrix actual = cov(trans(A));
+  Matrix actual = AHRS::Cov(trans(A));
   Matrix expected = (Matrix(3, 3) <<
       10.9167,    2.3333,    5.0000,
           2.3333,    4.6667,   -2.6667,
@@ -42,7 +42,7 @@ TEST (AHRS, cov) {
 /* ************************************************************************* */
 TEST (AHRS, covU) {
 
-  Matrix actual = cov(10000*stationaryU);
+  Matrix actual = AHRS::Cov(10000*stationaryU);
   Matrix expected = (Matrix(3, 3) <<
       33.3333333,    -1.66666667,    53.3333333,
       -1.66666667,    0.333333333,   -5.16666667,
@@ -54,7 +54,7 @@ TEST (AHRS, covU) {
 /* ************************************************************************* */
 TEST (AHRS, covF) {
 
-  Matrix actual = cov(100*stationaryF);
+  Matrix actual = AHRS::Cov(100*stationaryF);
   Matrix expected = (Matrix(3, 3) <<
       63.3808333, -0.432166667,  -48.1706667,
     -0.432166667,   8.31053333,  -16.6792667,