address some comments

gtsam/geometry/Pose3.cpp

@@ -42,6 +42,17 @@ Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR,
   return Pose3(R, t);
 }
 
+Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) {
+  if (H) *H << (gtsam::Matrix(6, 3) << //
+    0., 0., 0., //
+    0., 0., 0.,//
+    0., 0., 1.,//
+    1., 0., 0.,//
+    0., 1., 0.,//
+    0., 0., 0.).finished();
+  return Pose3(p);
+}
+
 /* ************************************************************************* */
 Pose3 Pose3::inverse() const {
   Rot3 Rt = R_.inverse();

gtsam/geometry/Pose3.h

@@ -78,6 +78,8 @@ public:
                       OptionalJacobian<6, 3> HR = {},
                       OptionalJacobian<6, 3> Ht = {});
 
+  static Pose3 FromPose2(const Pose2& p, OptionalJacobian<6,3> H = {});
+
   /**
    *  Create Pose3 by aligning two point pairs
    *  A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point

gtsam/slam/PlanarProjectionFactor.h

@@ -38,69 +38,67 @@ namespace gtsam {
         PlanarProjectionFactorBase() {}
 
         /**
-         * @param measured corresponding point in the camera frame
+         * @param measured pixels in the camera frame
-         * @param poseKey index of the robot pose in the z=0 plane
          */
         PlanarProjectionFactorBase(const Point2& measured) : measured_(measured) {}
 
         /**
-         * @param landmark point3
+         * Predict the projection of the landmark in camera pixels.
-         * @param pose
+         *
-         * @param offset oriented parallel to pose2 zero i.e. +x
+         * @param landmark point3 of the target
-         * @param calib
+         * @param wTb "world to body": planar pose2 of vehicle body frame in world frame
-         * @param H1 landmark jacobian
+         * @param bTc "body to camera": camera pose in body frame, oriented parallel to pose2 zero i.e. +x
-         * @param H2 pose jacobian
+         * @param calib camera calibration with distortion
-         * @param H3 offset jacobian
+         * @param Hlandmark jacobian
-         * @param H4 calib jacobian
+         * @param HwTb jacobian
+         * @param HbTc jacobian
+         * @param Hcalib jacobian
          */
-        Point2 h(
+        Point2 predict(
             const Point3& landmark,
-            const Pose2& pose,
+            const Pose2& wTb,
-            const Pose3& offset,
+            const Pose3& bTc,
             const Cal3DS2& calib,
-            OptionalMatrixType H1, // 2x3 (x, y, z)
+            OptionalMatrixType Hlandmark = nullptr, // 2x3 (x, y, z)
-            OptionalMatrixType H2, // 2x3 (x, y, theta)
+            OptionalMatrixType HwTb = nullptr, // 2x3 (x, y, theta)
-            OptionalMatrixType H3, // 2x6 (rx, ry, rz, x, y, theta)
+            OptionalMatrixType HbTc = nullptr, // 2x6 (rx, ry, rz, x, y, theta)
-            OptionalMatrixType H4  // 2x9
+            OptionalMatrixType Hcalib = nullptr // 2x9
         ) const {
+#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION
             try {
-                // this is x-forward z-up
+#endif
+                gtsam::Matrix Hp; // 6x3
                 gtsam::Matrix H0; // 6x6
-                Pose3 offset_pose = Pose3(pose).compose(offset, H0);
+                // this is x-forward z-up
+                Pose3 wTc = Pose3::FromPose2(wTb, Hp).compose(bTc, HwTb ? &H0 : nullptr);
                 // this is z-forward y-down
                 gtsam::Matrix H00; // 6x6
-                Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00);
+                Pose3 camera_pose = wTc.compose(CAM_COORD, H00);
                 PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib);
-                if (H2 || H3) {
+                if (HwTb || HbTc) {
                     // Dpose is for pose3, 2x6 (R,t)
                     gtsam::Matrix Dpose;
-                    Point2 result = camera.project(landmark, Dpose, H1, H4);
+                    Point2 result = camera.project(landmark, Dpose, Hlandmark, Hcalib);
                     gtsam::Matrix DposeOffset = Dpose * H00; // 2x6
-                    if (H3)
+                    if (HbTc)
-                        *H3 = DposeOffset; // with Eigen this is a deep copy (!)
+                        *HbTc = DposeOffset; // with Eigen this is a deep copy (!)
-                    if (H2) {
+                    if (HwTb) 
-                        gtsam::Matrix DposeOffsetFwd = DposeOffset * H0;
+                        *HwTb =  DposeOffset * H0 * Hp;
-                        *H2 = Matrix::Zero(2, 3);
-                        (*H2)(0, 0) = DposeOffsetFwd(0, 3); // du/dx
-                        (*H2)(1, 0) = DposeOffsetFwd(1, 3); // dv/dx
-                        (*H2)(0, 1) = DposeOffsetFwd(0, 4); // du/dy
-                        (*H2)(1, 1) = DposeOffsetFwd(1, 4); // dv/dy
-                        (*H2)(0, 2) = DposeOffsetFwd(0, 2); // du/dyaw
-                        (*H2)(1, 2) = DposeOffsetFwd(1, 2); // dv/dyaw
-                    }
                     return result;
                 } else {
                     return camera.project(landmark, {}, {}, {});
                 }
+#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION
             } catch (CheiralityException& e) {
                 std::cout << "****** CHIRALITY EXCEPTION ******\n";
-                if (H1) *H1 = Matrix::Zero(2, 3);
+                if (Hlandmark) *Hlandmark = Matrix::Zero(2, 3);
-                if (H2) *H2 = Matrix::Zero(2, 3);
+                if (HwTb) *HwTb = Matrix::Zero(2, 3);
-                if (H3) *H3 = Matrix::Zero(2, 6);
+                if (HbTc) *HbTc = Matrix::Zero(2, 6);
-                if (H4) *H4 = Matrix::Zero(2, 9);
+                if (Hcalib) *Hcalib = Matrix::Zero(2, 9);
                 // return a large error
                 return Matrix::Constant(2, 1, 2.0 * calib.fx());
             }
+#endif
         }
 
         Point2 measured_; // pixel measurement
@@ -140,41 +138,42 @@ namespace gtsam {
 
 
         /**
+         * @brief constructor for known landmark, offset, and calibration
+         * @param poseKey index of the robot pose2 in the z=0 plane
          * @param landmark point3 in the world
          * @param measured corresponding point2 in the camera frame
-         * @param offset constant camera offset from pose
+         * @param bTc "body to camera": constant camera offset from pose
          * @param calib constant camera calibration
          * @param model stddev of the measurements, ~one pixel?
-         * @param poseKey index of the robot pose2 in the z=0 plane
          */
         PlanarProjectionFactor1(
+            Key poseKey,
             const Point3& landmark,
             const Point2& measured,
-            const Pose3& offset,
+            const Pose3& bTc,
             const Cal3DS2& calib,
-            const SharedNoiseModel& model,
+            const SharedNoiseModel& model)
-            Key poseKey)
             : PlanarProjectionFactorBase(measured),
             NoiseModelFactorN(model, poseKey),
             landmark_(landmark),
-            offset_(offset),
+            bTc_(bTc),
             calib_(calib) {
             assert(2 == model->dim());
         }
 
         /**
-         * @param pose estimated pose2
+         * @param wTb "world to body": estimated pose2
-         * @param H1 pose jacobian
+         * @param HwTb jacobian
          */
         Vector evaluateError(
-            const Pose2& pose,
+            const Pose2& wTb,
-            OptionalMatrixType H1 = OptionalNone) const override {
+            OptionalMatrixType HwTb = OptionalNone) const override {
-            return h(landmark_, pose, offset_, calib_, {}, H1, {}, {}) - measured_;
+            return predict(landmark_, wTb, bTc_, calib_, {}, HwTb, {}, {}) - measured_;
         }
 
     private:
         Point3 landmark_; // landmark
-        Pose3 offset_; // camera offset to robot pose
+        Pose3 bTc_; // "body to camera": camera offset to robot pose
         Cal3DS2 calib_; // camera calibration
     };
 
@@ -188,9 +187,10 @@ namespace gtsam {
      * Camera offset and calibration are constant.
      * This is similar to GeneralSFMFactor, used for SLAM.
     */
-    class PlanarProjectionFactor2 : public PlanarProjectionFactorBase, public NoiseModelFactorN<Point3, Pose2> {
+    class PlanarProjectionFactor2 
+    : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2, Point3> {
     public:
-        typedef NoiseModelFactorN<Point3, Pose2> Base;
+        typedef NoiseModelFactorN<Pose2, Point3> Base;
         using Base::evaluateError;
 
         PlanarProjectionFactor2() {}
@@ -204,43 +204,44 @@ namespace gtsam {
         }
 
         /**
-         * @param measured corresponding point in the camera frame
+         * @brief constructor for variable landmark, known offset and calibration
-         * @param offset constant camera offset from pose
-         * @param calib constant camera calibration
-         * @param model stddev of the measurements, ~one pixel?
          * @param poseKey index of the robot pose2 in the z=0 plane
          * @param landmarkKey index of the landmark point3
+         * @param measured corresponding point in the camera frame
+         * @param bTc "body to camera": constant camera offset from pose
+         * @param calib constant camera calibration
+         * @param model stddev of the measurements, ~one pixel?
          */
         PlanarProjectionFactor2(
-            const Point2& measured,
+            Key poseKey,
-            const Pose3& offset,
-            const Cal3DS2& calib,
-            const SharedNoiseModel& model,
             Key landmarkKey,
-            Key poseKey)
+            const Point2& measured,
+            const Pose3& bTc,
+            const Cal3DS2& calib,
+            const SharedNoiseModel& model)
             : PlanarProjectionFactorBase(measured),
             NoiseModelFactorN(model, landmarkKey, poseKey),
-            offset_(offset),
+            bTc_(bTc),
             calib_(calib) {
             assert(2 == model->dim());
         }
 
         /**
+         * @param wTb "world to body": estimated pose2
          * @param landmark estimated landmark
-         * @param pose estimated pose2
+         * @param HwTb jacobian
-         * @param H1 landmark jacobian
+         * @param Hlandmark jacobian
-         * @param H2 pose jacobian
          */
         Vector evaluateError(
+            const Pose2& wTb,
             const Point3& landmark,
-            const Pose2& pose,
+            OptionalMatrixType HwTb = OptionalNone,
-            OptionalMatrixType H1 = OptionalNone,
+            OptionalMatrixType Hlandmark = OptionalNone) const override {
-            OptionalMatrixType H2 = OptionalNone) const override {
+            return predict(landmark, wTb, bTc_, calib_, Hlandmark, HwTb, {}, {}) - measured_;
-            return h(landmark, pose, offset_, calib_, H1, H2, {}, {}) - measured_;
         }
 
     private:
-        Pose3 offset_; // camera offset to robot pose
+        Pose3 bTc_; // "body to camera": camera offset to robot pose
         Cal3DS2 calib_; // camera calibration
     };
 
@@ -270,6 +271,7 @@ namespace gtsam {
         }
 
         /**
+         * @brief constructor for variable pose, offset, and calibration, known landmark.
          * @param landmark point3 in the world
          * @param measured corresponding point2 in the camera frame
          * @param model stddev of the measurements, ~one pixel?
@@ -277,12 +279,12 @@ namespace gtsam {
          * @param offsetKey index of camera offset from pose
          * @param calibKey index of camera calibration         */
         PlanarProjectionFactor3(
-            const Point3& landmark,
-            const Point2& measured,
-            const SharedNoiseModel& model,
             Key poseKey,
             Key offsetKey,
-            Key calibKey)
+            Key calibKey,
+            const Point3& landmark,
+            const Point2& measured,
+            const SharedNoiseModel& model)
             : PlanarProjectionFactorBase(measured),
             NoiseModelFactorN(model, poseKey, offsetKey, calibKey),
             landmark_(landmark) {
@@ -290,21 +292,21 @@ namespace gtsam {
         }
 
         /**
-         * @param pose estimated pose2
+         * @param wTb "world to body": estimated pose2
-         * @param offset pose3 offset from pose2 +x
+         * @param bTc "body to camera": pose3 offset from pose2 +x
          * @param calib calibration
-         * @param H1 pose jacobian
+         * @param HwTb pose jacobian
          * @param H2 offset jacobian
          * @param H3 calibration jacobian
          */
         Vector evaluateError(
-            const Pose2& pose,
+            const Pose2& wTb,
-            const Pose3& offset,
+            const Pose3& bTc,
             const Cal3DS2& calib,
-            OptionalMatrixType H1 = OptionalNone,
+            OptionalMatrixType HwTb = OptionalNone,
-            OptionalMatrixType H2 = OptionalNone,
+            OptionalMatrixType HbTc = OptionalNone,
-            OptionalMatrixType H3 = OptionalNone) const override {
+            OptionalMatrixType Hcalib = OptionalNone) const override {
-            return h(landmark_, pose, offset, calib, {}, H1, H2, H3) - measured_;
+            return predict(landmark_, wTb, bTc, calib, {}, HwTb, HbTc, Hcalib) - measured_;
         }
 
     private:

gtsam/slam/slam.i

@@ -27,32 +27,32 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
 #include <gtsam/slam/PlanarProjectionFactor.h>
 virtual class PlanarProjectionFactor1 : gtsam::NoiseModelFactor {
   PlanarProjectionFactor1(
+    size_t poseKey,
     const gtsam::Point3& landmark,
     const gtsam::Point2& measured,
-            const gtsam::Pose3& offset,
+    const gtsam::Pose3& bTc,
     const gtsam::Cal3DS2& calib,
-            const gtsam::noiseModel::Base* model,
+    const gtsam::noiseModel::Base* model);
-            size_t poseKey);
   void serialize() const;
 };
 virtual class PlanarProjectionFactor2 : gtsam::NoiseModelFactor {
   PlanarProjectionFactor2(
-            const gtsam::Point2& measured,
+    size_t poseKey,
-            const gtsam::Pose3& offset,
-            const gtsam::Cal3DS2& calib,
-            const gtsam::noiseModel::Base* model,
     size_t landmarkKey,
-            size_t poseKey);
+    const gtsam::Point2& measured,
+    const gtsam::Pose3& bTc,
+    const gtsam::Cal3DS2& calib,
+    const gtsam::noiseModel::Base* model);
   void serialize() const;
 };
 virtual class PlanarProjectionFactor3 : gtsam::NoiseModelFactor {
   PlanarProjectionFactor3(
-            const gtsam::Point3& landmark,
-            const gtsam::Point2& measured,
-            const gtsam::noiseModel::Base* model,
     size_t poseKey,
     size_t offsetKey,
-            size_t calibKey);
+    size_t calibKey,
+    const gtsam::Point3& landmark,
+    const gtsam::Point2& measured,
+    const gtsam::noiseModel::Base* model);
   void serialize() const;
 };
 

gtsam/slam/tests/testPlanarProjectionFactor.cpp

@@ -39,7 +39,8 @@ TEST(PlanarProjectionFactor1, error1) {
     Pose2 pose(0, 0, 0);
     values.insert(X(0), pose);
 
-    PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0));
+    PlanarProjectionFactor1 factor(
+        X(0), landmark, measured, offset, calib, model);
 
     CHECK_EQUAL(2, factor.dim());
     CHECK(factor.active(values));
@@ -65,7 +66,8 @@ TEST(PlanarProjectionFactor1, error2) {
     Pose3 offset;
     Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0));
+    PlanarProjectionFactor1 factor(
+        X(0), landmark, measured, offset, calib, model);
     Values values;
     Pose2 pose(0, 0, 0);
 
@@ -96,7 +98,8 @@ TEST(PlanarProjectionFactor1, error3) {
     // distortion
     Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0));
+    PlanarProjectionFactor1 factor(
+        X(0), landmark, measured, offset, calib, model);
     Values values;
     Pose2 pose(0, 0, 0);
 
@@ -131,7 +134,8 @@ TEST(PlanarProjectionFactor1, jacobian) {
         Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
         Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
 
-        PlanarProjectionFactor1 factor(landmark, measured, offset, calib, model, X(0));
+        PlanarProjectionFactor1 factor(
+            X(0), landmark, measured, offset, calib, model);
 
         Pose2 pose(s(g), s(g), s(g));
 
@@ -164,7 +168,8 @@ TEST(PlanarProjectionFactor3, error1) {
     values.insert(C(0), offset);
     values.insert(K(0), calib);
 
-    PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0));
+    PlanarProjectionFactor3 factor(
+        X(0), C(0), K(0), landmark, measured, model);
 
     CHECK_EQUAL(2, factor.dim());
     CHECK(factor.active(values));
@@ -213,7 +218,8 @@ TEST(PlanarProjectionFactor3, error2) {
     Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
 
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0));
+    PlanarProjectionFactor3 factor(
+        X(0), C(0), K(0), landmark, measured, model);
     Values values;
     Pose2 pose(0, 0, 0);
 
@@ -267,7 +273,8 @@ TEST(PlanarProjectionFactor3, error3) {
     Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
 
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0));
+    PlanarProjectionFactor3 factor(
+        X(0), C(0), K(0), landmark, measured, model);
     Values values;
     Pose2 pose(0, 0, 0);
 
@@ -328,7 +335,8 @@ TEST(PlanarProjectionFactor3, jacobian) {
         Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
         Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
 
-        PlanarProjectionFactor3 factor(landmark, measured, model, X(0), C(0), K(0));
+        PlanarProjectionFactor3 factor(
+            X(0), C(0), K(0), landmark, measured, model);
 
         Pose2 pose(s(g), s(g), s(g));