Fixed Jacobian versions

gtsam/geometry/Cal3_S2.cpp

@@ -95,6 +95,12 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
   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_);
+}
+
 /* ************************************************************************* */
 Point2 Cal3_S2::calibrate(const Point2& p) const {
   const double u = p.x(), v = p.y();

gtsam/geometry/Cal3_S2.h

@@ -150,11 +150,13 @@ public:
    * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
    * @return point in image coordinates
    */
-  Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal =
-      boost::none, boost::optional<Matrix&> Dp = boost::none) const;
+  Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
+      boost::optional<Matrix&> Dp) const;
 
-  Point2 uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal =
-      boost::none, boost::optional<Matrix2&> Dp = boost::none) const;
+  Point2 uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
+      boost::optional<Matrix2&> Dp) const;
+
+  Point2 uncalibrate(const Point2& p) const;
 
   /**
    * convert image coordinates uv to intrinsic coordinates xy

gtsam/geometry/PinholeCamera.h

@@ -325,7 +325,7 @@ public:
       }
       return pi;
     } else
-      return K_.uncalibrate(pn, Dcal);
+      return K_.uncalibrate(pn, Dcal, boost::none);
   }
 
   /** project a point at infinity from world coordinate to the image

gtsam/geometry/Pose2.cpp

@@ -123,6 +123,27 @@ Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
   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 {
+  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();
+  return q;
+}
+
 /* ************************************************************************* */
 // see doc/math.lyx, SE(2) section
 Point2 Pose2::transform_to(const Point2& point,

gtsam/geometry/Pose2.h

@@ -182,10 +182,18 @@ 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<Matrix&> H1=boost::none,
-      boost::optional<Matrix&> H2=boost::none) const;
+      boost::optional<Matrix23&> H1,
+      boost::optional<Matrix2&> H2) 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,

gtsam/geometry/Pose3.cpp

@@ -253,6 +253,34 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
   return R_ * p + t_;
 }
 
+/* ************************************************************************* */
+Point3 Pose3::transform_to(const Point3& p) const {
+  // Only get transpose once, to avoid multiple allocations,
+  // as well as multiple conversions in the Quaternion case
+  return R_.unrotate(p - t_);
+}
+
+/* ************************************************************************* */
+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
+  Matrix3 Rt(R_.transpose());
+  const Point3 q(Rt*(p - t_).vector());
+  if (Dpose) {
+    double wx = q.x();
+    double wy = q.y();
+    double wz = q.z();
+    (*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;
+}
+
 /* ************************************************************************* */
 Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
     boost::optional<Matrix&> Dpoint) const {

gtsam/geometry/Pose3.h

@@ -250,8 +250,13 @@ 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<Matrix&> Dpose=boost::none, boost::optional<Matrix&> Dpoint=boost::none) const;
+        boost::optional<Matrix36&> Dpose, boost::optional<Matrix3&> Dpoint) const;
+
+    Point3 transform_to(const Point3& p,
+        boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) const;
 
     /// @}
     /// @name Standard Interface

gtsam/geometry/Rot3.h

@@ -219,8 +219,15 @@ namespace gtsam {
     Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const;
 
     /// Compose two rotations i.e., R= (*this) * R2
-    Rot3 compose(const Rot3& R2,
-    boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
+    Rot3 compose(const Rot3& R2) 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;

gtsam/geometry/Rot3M.cpp

@@ -143,6 +143,19 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) {
      -swy + C02,  swx + C12,    c + C22);
 }
 
+/* ************************************************************************* */
+Rot3 Rot3::compose (const Rot3& R2) 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;
+  return *this * R2;
+}
+
 /* ************************************************************************* */
 Rot3 Rot3::compose (const Rot3& R2,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {

gtsam/geometry/Rot3Q.cpp

@@ -82,6 +82,19 @@ 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 {
+    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 {