remove "cam coord" idea

gtsam/geometry/Pose3.cpp

@@ -42,14 +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) {
+// Pose2 constructor Jacobian is always the same.
-  if (H) *H << (gtsam::Matrix(6, 3) << //
+static const Matrix63 Hpose2 = (Matrix63() << //
     0., 0., 0., //
     0., 0., 0.,//
     0., 0., 1.,//
     1., 0., 0.,//
     0., 1., 0.,//
     0., 0., 0.).finished();
+
+Pose3 Pose3::FromPose2(const Pose2& p, OptionalJacobian<6, 3> H) {
+  if (H) *H << Hpose2;
   return Pose3(p);
 }
 
@@ -520,4 +523,4 @@ std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
   return os;
 }
 
-} // namespace gtsam
+} // namespace gtsam

gtsam/geometry/Pose3.h

@@ -78,6 +78,7 @@ public:
                       OptionalJacobian<6, 3> HR = {},
                       OptionalJacobian<6, 3> Ht = {});
 
+  /** Construct from Pose2 in the xy plane, with derivative. */
   static Pose3 FromPose2(const Pose2& p, OptionalJacobian<6,3> H = {});
 
   /**
@@ -460,4 +461,4 @@ struct Bearing<Pose3, Pose3> : HasBearing<Pose3, Pose3, Unit3> {};
 template <typename T>
 struct Range<Pose3, T> : HasRange<Pose3, T, double> {};
 
-}  // namespace gtsam
+}  // namespace gtsam

gtsam/slam/PlanarProjectionFactor.h

@@ -59,31 +59,26 @@ namespace gtsam {
             const Pose2& wTb,
             const Pose3& bTc,
             const Cal3DS2& calib,
-            OptionalMatrixType Hlandmark = nullptr, // 2x3 (x, y, z)
+            OptionalJacobian<2, 3> Hlandmark = {}, // (x, y, z)
-            OptionalMatrixType HwTb = nullptr, // 2x3 (x, y, theta)
+            OptionalJacobian<2, 3> HwTb = {}, // (x, y, theta)
-            OptionalMatrixType HbTc = nullptr, // 2x6 (rx, ry, rz, x, y, theta)
+            OptionalJacobian<2, 6> HbTc = {}, // (rx, ry, rz, x, y, theta)
-            OptionalMatrixType Hcalib = nullptr // 2x9
+            OptionalJacobian<2, 9> Hcalib = {}
         ) const {
 #ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION
             try {
 #endif
-                gtsam::Matrix Hp; // 6x3
+                Matrix63 Hp; // 6x3
-                gtsam::Matrix H0; // 6x6
+                Matrix66 H0; // 6x6
-                // this is x-forward z-up
+                Pose3 wTc = Pose3::FromPose2(wTb, HwTb ? &Hp : nullptr).compose(bTc, HwTb ? &H0 : nullptr);
-                Pose3 wTc = Pose3::FromPose2(wTb, Hp).compose(bTc, HwTb ? &H0 : nullptr);
+                PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(wTc, calib);
-                // this is z-forward y-down
-                gtsam::Matrix H00; // 6x6
-                Pose3 camera_pose = wTc.compose(CAM_COORD, H00);
-                PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib);
                 if (HwTb || HbTc) {
-                    // Dpose is for pose3, 2x6 (R,t)
+                    // Dpose is for pose3 (R,t)
-                    gtsam::Matrix Dpose;
+                    Matrix26 Dpose;
                     Point2 result = camera.project(landmark, Dpose, Hlandmark, Hcalib);
-                    gtsam::Matrix DposeOffset = Dpose * H00; // 2x6
                     if (HbTc)
-                        *HbTc = DposeOffset; // with Eigen this is a deep copy (!)
+                        *HbTc = Dpose;
-                    if (HwTb) 
+                    if (HwTb)
-                        *HwTb =  DposeOffset * H0 * Hp;
+                        *HwTb = Dpose * H0 * Hp;
                     return result;
                 } else {
                     return camera.project(landmark, {}, {}, {});
@@ -91,10 +86,10 @@ namespace gtsam {
 #ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION
             } catch (CheiralityException& e) {
                 std::cout << "****** CHIRALITY EXCEPTION ******\n";
-                if (Hlandmark) *Hlandmark = Matrix::Zero(2, 3);
+                if (Hlandmark) Hlandmark->setZero();
-                if (HwTb) *HwTb = Matrix::Zero(2, 3);
+                if (HwTb) HwTb->setZero();
-                if (HbTc) *HbTc = Matrix::Zero(2, 6);
+                if (HbTc) HbTc->setZero();
-                if (Hcalib) *Hcalib = Matrix::Zero(2, 9);
+                if (Hcalib) Hcalib->setZero();
                 // return a large error
                 return Matrix::Constant(2, 1, 2.0 * calib.fx());
             }
@@ -102,18 +97,8 @@ namespace gtsam {
         }
 
         Point2 measured_; // pixel measurement
-
-    private:
-        static const Pose3 CAM_COORD;
     };
 
-    // camera "zero" is facing +z; this turns it to face +x
-    const Pose3 PlanarProjectionFactorBase::CAM_COORD = Pose3(
-        Rot3(0, 0, 1,//
-            -1, 0, 0, //
-            0, -1, 0),
-        Vector3(0, 0, 0)
-    );
 
     /**
      * @class PlanarProjectionFactor1
@@ -131,9 +116,9 @@ namespace gtsam {
         ~PlanarProjectionFactor1() override {}
 
         /// @return a deep copy of this factor
-        gtsam::NonlinearFactor::shared_ptr clone() const override {
+        NonlinearFactor::shared_ptr clone() const override {
-            return std::static_pointer_cast<gtsam::NonlinearFactor>(
+            return std::static_pointer_cast<NonlinearFactor>(
-                gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor1(*this)));
+                NonlinearFactor::shared_ptr(new PlanarProjectionFactor1(*this)));
         }
 
 
@@ -152,22 +137,18 @@ namespace gtsam {
             const Point2& measured,
             const Pose3& bTc,
             const Cal3DS2& calib,
-            const SharedNoiseModel& model)
+            const SharedNoiseModel& model = {})
             : PlanarProjectionFactorBase(measured),
             NoiseModelFactorN(model, poseKey),
             landmark_(landmark),
             bTc_(bTc),
-            calib_(calib) {
+            calib_(calib) {}
-            assert(2 == model->dim());
-        }
 
         /**
          * @param wTb "world to body": estimated pose2
          * @param HwTb jacobian
          */
-        Vector evaluateError(
+        Vector evaluateError(const Pose2& wTb, OptionalMatrixType HwTb) const override {
-            const Pose2& wTb,
-            OptionalMatrixType HwTb = OptionalNone) const override {
             return predict(landmark_, wTb, bTc_, calib_, {}, HwTb, {}, {}) - measured_;
         }
 
@@ -187,8 +168,8 @@ namespace gtsam {
      * Camera offset and calibration are constant.
      * This is similar to GeneralSFMFactor, used for SLAM.
     */
-    class PlanarProjectionFactor2 
+    class PlanarProjectionFactor2
-    : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2, Point3> {
+        : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2, Point3> {
     public:
         typedef NoiseModelFactorN<Pose2, Point3> Base;
         using Base::evaluateError;
@@ -198,9 +179,9 @@ namespace gtsam {
         ~PlanarProjectionFactor2() override {}
 
         /// @return a deep copy of this factor
-        gtsam::NonlinearFactor::shared_ptr clone() const override {
+        NonlinearFactor::shared_ptr clone() const override {
-            return std::static_pointer_cast<gtsam::NonlinearFactor>(
+            return std::static_pointer_cast<NonlinearFactor>(
-                gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor2(*this)));
+                NonlinearFactor::shared_ptr(new PlanarProjectionFactor2(*this)));
         }
 
         /**
@@ -218,13 +199,11 @@ namespace gtsam {
             const Point2& measured,
             const Pose3& bTc,
             const Cal3DS2& calib,
-            const SharedNoiseModel& model)
+            const SharedNoiseModel& model = {})
             : PlanarProjectionFactorBase(measured),
             NoiseModelFactorN(model, landmarkKey, poseKey),
             bTc_(bTc),
-            calib_(calib) {
+            calib_(calib) {}
-            assert(2 == model->dim());
-        }
 
         /**
          * @param wTb "world to body": estimated pose2
@@ -235,8 +214,8 @@ namespace gtsam {
         Vector evaluateError(
             const Pose2& wTb,
             const Point3& landmark,
-            OptionalMatrixType HwTb = OptionalNone,
+            OptionalMatrixType HwTb,
-            OptionalMatrixType Hlandmark = OptionalNone) const override {
+            OptionalMatrixType Hlandmark) const override {
             return predict(landmark, wTb, bTc_, calib_, Hlandmark, HwTb, {}, {}) - measured_;
         }
 
@@ -265,47 +244,46 @@ namespace gtsam {
         ~PlanarProjectionFactor3() override {}
 
         /// @return a deep copy of this factor
-        gtsam::NonlinearFactor::shared_ptr clone() const override {
+        NonlinearFactor::shared_ptr clone() const override {
-            return std::static_pointer_cast<gtsam::NonlinearFactor>(
+            return std::static_pointer_cast<NonlinearFactor>(
-                gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor3(*this)));
+                NonlinearFactor::shared_ptr(new PlanarProjectionFactor3(*this)));
         }
 
         /**
          * @brief constructor for variable pose, offset, and calibration, known landmark.
+         * @param poseKey index of the robot pose2 in the z=0 plane
+         * @param offsetKey index of camera offset from pose
+         * @param calibKey index of camera calibration
          * @param landmark point3 in the world
          * @param measured corresponding point2 in the camera frame
          * @param model stddev of the measurements, ~one pixel?
-         * @param poseKey index of the robot pose2 in the z=0 plane
+         */
-         * @param offsetKey index of camera offset from pose
-         * @param calibKey index of camera calibration         */
         PlanarProjectionFactor3(
             Key poseKey,
             Key offsetKey,
             Key calibKey,
             const Point3& landmark,
             const Point2& measured,
-            const SharedNoiseModel& model)
+            const SharedNoiseModel& model = {})
             : PlanarProjectionFactorBase(measured),
             NoiseModelFactorN(model, poseKey, offsetKey, calibKey),
-            landmark_(landmark) {
+            landmark_(landmark) {}
-            assert(2 == model->dim());
-        }
 
         /**
          * @param wTb "world to body": estimated pose2
          * @param bTc "body to camera": pose3 offset from pose2 +x
          * @param calib calibration
          * @param HwTb pose jacobian
-         * @param H2 offset jacobian
+         * @param HbTc offset jacobian
-         * @param H3 calibration jacobian
+         * @param Hcalib calibration jacobian
          */
         Vector evaluateError(
             const Pose2& wTb,
             const Pose3& bTc,
             const Cal3DS2& calib,
-            OptionalMatrixType HwTb = OptionalNone,
+            OptionalMatrixType HwTb,
-            OptionalMatrixType HbTc = OptionalNone,
+            OptionalMatrixType HbTc,
-            OptionalMatrixType Hcalib = OptionalNone) const override {
+            OptionalMatrixType Hcalib) const override {
             return predict(landmark_, wTb, bTc, calib, {}, HwTb, HbTc, Hcalib) - measured_;
         }
 

gtsam/slam/tests/testPlanarProjectionFactor.cpp

@@ -16,6 +16,10 @@
 #include <gtsam/geometry/Rot2.h>
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/PriorFactor.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/slam/PlanarProjectionFactor.h>
 
@@ -26,124 +30,101 @@ using namespace gtsam;
 using symbol_shorthand::X;
 using symbol_shorthand::C;
 using symbol_shorthand::K;
+using symbol_shorthand::L;
 
+/* ************************************************************************* */
 TEST(PlanarProjectionFactor1, error1) {
-    // landmark is on the camera bore (which faces +x)
+    // Example: center projection and Jacobian
     Point3 landmark(1, 0, 0);
-    // so pixel measurement is (cx, cy)
     Point2 measured(200, 200);
-    Pose3 offset;
+    Pose3 offset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
     Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    Values values;
+    PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model);
     Pose2 pose(0, 0, 0);
-    values.insert(X(0), pose);
+    Matrix H;
-
+    CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6));
-    PlanarProjectionFactor1 factor(
+    CHECK(assert_equal((Matrix23() << //
-        X(0), landmark, measured, offset, calib, model);
+        0, 200, 200, //
-
+        0, 0, 0).finished(), H, 1e-6));
-    CHECK_EQUAL(2, factor.dim());
-    CHECK(factor.active(values));
-    std::vector<Matrix> actualHs(1);
-    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
-
-    CHECK(assert_equal(Vector2(0, 0), actual));
-
-    const Matrix& H1Actual = actualHs.at(0);
-    CHECK_EQUAL(2, H1Actual.rows());
-    CHECK_EQUAL(3, H1Actual.cols());
-    const Matrix23 H1Expected = (Matrix23() << //
-        0, 200, 200,//
-        0, 0, 0).finished();
-    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
 }
 
+/* ************************************************************************* */
 TEST(PlanarProjectionFactor1, error2) {
-    // landmark is in the upper left corner
+    // Example: upper left corner projection and Jacobian
     Point3 landmark(1, 1, 1);
-    // upper left corner in pixels
     Point2 measured(0, 0);
-    Pose3 offset;
+    Pose3 offset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
     Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor1 factor(
+    PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model);
-        X(0), landmark, measured, offset, calib, model);
-    Values values;
     Pose2 pose(0, 0, 0);
-
+    Matrix H;
-    values.insert(X(0), pose);
+    CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6));
-
+    CHECK(assert_equal((Matrix23() << //
-    CHECK_EQUAL(2, factor.dim());
-    CHECK(factor.active(values));
-    std::vector<Matrix> actualHs(1);
-    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
-
-    CHECK(assert_equal(Vector2(0, 0), actual));
-
-    const Matrix& H1Actual = actualHs.at(0);
-    CHECK_EQUAL(2, H1Actual.rows());
-    CHECK_EQUAL(3, H1Actual.cols());
-    Matrix23 H1Expected = (Matrix23() << //
         -200, 200, 400, //
-        -200, 0, 200).finished();
+        -200, 0, 200).finished(), H, 1e-6));
-    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
 }
 
+/* ************************************************************************* */
 TEST(PlanarProjectionFactor1, error3) {
-    // landmark is in the upper left corner
+    // Example: upper left corner projection and Jacobian with distortion
     Point3 landmark(1, 1, 1);
-    // upper left corner in pixels
     Point2 measured(0, 0);
-    Pose3 offset;
+    Pose3 offset(
-    // distortion
+        Rot3(0, 0, 1,//
-    Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+    Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1); // note distortion
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor1 factor(
+    PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model);
-        X(0), landmark, measured, offset, calib, model);
-    Values values;
     Pose2 pose(0, 0, 0);
-
+    Matrix H;
-    values.insert(X(0), pose);
+    CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, H), 1e-6));
-
+    CHECK(assert_equal((Matrix23() << //
-    CHECK_EQUAL(2, factor.dim());
-    CHECK(factor.active(values));
-    std::vector<Matrix> actualHs(1);
-    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
-
-    CHECK(assert_equal(Vector2(0, 0), actual));
-
-    const Matrix& H1Actual = actualHs.at(0);
-    CHECK_EQUAL(2, H1Actual.rows());
-    CHECK_EQUAL(3, H1Actual.cols());
-    Matrix23 H1Expected = (Matrix23() << //
         -360, 280, 640, //
-        -360, 80, 440).finished();
+        -360, 80, 440).finished(), H, 1e-6));
-    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
 }
 
+/* ************************************************************************* */
 TEST(PlanarProjectionFactor1, jacobian) {
-    // test many jacobians with many randoms
+    // Verify Jacobians with numeric derivative
-
+    std::default_random_engine rng(42);
-    std::default_random_engine g;
+    std::uniform_real_distribution<double> dist(-0.3, 0.3);
-    std::uniform_real_distribution<double> s(-0.3, 0.3);
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    // center of the random camera poses
+    Pose3 centerOffset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
 
     for (int i = 0; i < 1000; ++i) {
-        Point3 landmark(2 + s(g), s(g), s(g));
+        Point3 landmark(2 + dist(rng), dist(rng), dist(rng));
-        Point2 measured(200 + 100*s(g), 200 + 100*s(g));
+        Point2 measured(200 + 100 * dist(rng), 200 + 100 * dist(rng));
-        Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
+        Pose3 offset = centerOffset.compose(
+            Pose3(
+                Rot3::Ypr(dist(rng), dist(rng), dist(rng)),
+                Point3(dist(rng), dist(rng), dist(rng))));
         Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
-
+        PlanarProjectionFactor1 factor(X(0), landmark, measured, offset, calib, model);
-        PlanarProjectionFactor1 factor(
+        Pose2 pose(dist(rng), dist(rng), dist(rng));
-            X(0), landmark, measured, offset, calib, model);
-
-        Pose2 pose(s(g), s(g), s(g));
-
-        // actual H
         Matrix H1;
         factor.evaluateError(pose, H1);
-
+        auto expectedH1 = numericalDerivative11<Vector, Pose2>(
-        Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
             [&factor](const Pose2& p) {
                 return factor.evaluateError(p, {});},
                 pose);
@@ -151,194 +132,182 @@ TEST(PlanarProjectionFactor1, jacobian) {
     }
 }
 
+/* ************************************************************************* */
+TEST(PlanarProjectionFactor1, solve) {
+    // Example localization
 
+    SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    // pose model is wide, so the solver finds the right answer.
+    SharedNoiseModel xNoise = noiseModel::Diagonal::Sigmas(Vector3(10, 10, 10));
 
-TEST(PlanarProjectionFactor3, error1) {
+    // landmarks
-    // landmark is on the camera bore (facing +x)
+    Point3 l0(1, 0.1, 1);
-    Point3 landmark(1, 0, 0);
+    Point3 l1(1, -0.1, 1);
-    // so px is (cx, cy)
+
-    Point2 measured(200, 200);
+    // camera pixels
-    // offset is identity
+    Point2 p0(180, 0);
-    Pose3 offset;
+    Point2 p1(220, 0);
+
+    // body
+    Pose2 x0(0, 0, 0);
+
+    // camera z looking at +x with (xy) antiparallel to (yz)
+    Pose3 c0(
+        Rot3(0, 0, 1, //
+            -1, 0, 0, //
+            0, -1, 0), //
+        Vector3(0, 0, 0));
     Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
-    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    Values values;
-    Pose2 pose(0, 0, 0);
-    values.insert(X(0), pose);
-    values.insert(C(0), offset);
-    values.insert(K(0), calib);
 
-    PlanarProjectionFactor3 factor(
+    NonlinearFactorGraph graph;
-        X(0), C(0), K(0), landmark, measured, model);
+    graph.add(PlanarProjectionFactor1(X(0), l0, p0, c0, calib, pxModel));
+    graph.add(PlanarProjectionFactor1(X(0), l1, p1, c0, calib, pxModel));
+    graph.add(PriorFactor<Pose2>(X(0), x0, xNoise));
 
-    CHECK_EQUAL(2, factor.dim());
+    Values initialEstimate;
-    CHECK(factor.active(values));
+    initialEstimate.insert(X(0), x0);
-    std::vector<Matrix> actualHs(3);
 
-    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+    // run the optimizer
-    CHECK(assert_equal(Vector2(0, 0), actual));
+    LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
+    Values result = optimizer.optimize();
 
-    const Matrix& H1Actual = actualHs.at(0);
+    // verify that the optimizer found the right pose.
-    const Matrix& H2Actual = actualHs.at(1);
+    CHECK(assert_equal(x0, result.at<Pose2>(X(0)), 2e-3));
-    const Matrix& H3Actual = actualHs.at(2);
 
-    CHECK_EQUAL(2, H1Actual.rows());
+    // covariance
-    CHECK_EQUAL(3, H1Actual.cols());
+    Marginals marginals(graph, result);
-    CHECK_EQUAL(2, H2Actual.rows());
+    Matrix cov = marginals.marginalCovariance(X(0));
-    CHECK_EQUAL(6, H2Actual.cols());
+    CHECK(assert_equal((Matrix33() << //
-    CHECK_EQUAL(2, H3Actual.rows());
+        0.000012, 0.000000, 0.000000, //
-    CHECK_EQUAL(9, H3Actual.cols());
+        0.000000, 0.001287, -.001262, //
+        0.000000, -.001262, 0.001250).finished(), cov, 3e-6));
 
-    // du/dx etc for the pose2d
+    // pose stddev
-    Matrix23 H1Expected = (Matrix23() <<//
+    Vector3 sigma = cov.diagonal().cwiseSqrt();
-        0, 200, 200,//
+    CHECK(assert_equal((Vector3() << //
-        0, 0, 0).finished();
+        0.0035,
-    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+        0.0359,
+        0.0354
+        ).finished(), sigma, 1e-4));
 
-    // du/dx for the pose3d offset
-    // note this is (roll, pitch, yaw, x, y, z)
-    Matrix26 H2Expected = (Matrix26() <<//
-        0, 0, 200, 0, 200, 0,//
-        0, -200, 0, 0, 0, 200).finished();
-    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
-
-    // du wrt calibration
-    // on-bore, f doesn't matter
-    // but c does
-    Matrix29 H3Expected = (Matrix29() <<//
-        0, 0, 0, 1, 0, 0, 0, 0, 0,//
-        0, 0, 0, 0, 1, 0, 0, 0, 0).finished();
-    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
 }
 
+/* ************************************************************************* */
+TEST(PlanarProjectionFactor3, error1) {
+    // Example: center projection and Jacobian
+    Point3 landmark(1, 0, 0);
+    Point2 measured(200, 200);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model);
+    Pose2 pose(0, 0, 0);
+    Pose3 offset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+    Matrix H1;
+    Matrix H2;
+    Matrix H3;
+    CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, offset, calib, H1, H2, H3), 1e-6));
+    CHECK(assert_equal((Matrix23() <<//
+        0, 200, 200,//
+        0, 0, 0).finished(), H1, 1e-6));
+    CHECK(assert_equal((Matrix26() <<//
+        0, -200, 0, -200, 0, 0,//
+        200, -0, 0, 0, -200, 0).finished(), H2, 1e-6));
+    CHECK(assert_equal((Matrix29() <<//
+        0, 0, 0, 1, 0, 0, 0, 0, 0,//
+        0, 0, 0, 0, 1, 0, 0, 0, 0).finished(), H3, 1e-6));
+}
+
+/* ************************************************************************* */
 TEST(PlanarProjectionFactor3, error2) {
     Point3 landmark(1, 1, 1);
     Point2 measured(0, 0);
-    Pose3 offset;
-    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
-
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor3 factor(
+    PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model);
-        X(0), C(0), K(0), landmark, measured, model);
-    Values values;
     Pose2 pose(0, 0, 0);
-
+    Pose3 offset(
-    values.insert(X(0), pose);
+        Rot3(0, 0, 1,//
-    values.insert(C(0), offset);
+            -1, 0, 0, //
-    values.insert(K(0), calib);
+            0, -1, 0),
-
+        Vector3(0, 0, 0)
-
+    );
-    CHECK_EQUAL(2, model->dim());
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
-    CHECK_EQUAL(2, factor.dim());
+    Matrix H1;
-    CHECK(factor.active(values));
+    Matrix H2;
-    std::vector<Matrix> actualHs(3);
+    Matrix H3;
-    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+    gtsam::Vector actual = factor.evaluateError(pose, offset, calib, H1, H2, H3);
-
     CHECK(assert_equal(Vector2(0, 0), actual));
-
+    CHECK(assert_equal((Matrix23() <<//
-    const Matrix& H1Actual = actualHs.at(0);
-    const Matrix& H2Actual = actualHs.at(1);
-    const Matrix& H3Actual = actualHs.at(2);
-
-    CHECK_EQUAL(2, H1Actual.rows());
-    CHECK_EQUAL(3, H1Actual.cols());
-    CHECK_EQUAL(2, H2Actual.rows());
-    CHECK_EQUAL(6, H2Actual.cols());
-    CHECK_EQUAL(2, H3Actual.rows());
-    CHECK_EQUAL(9, H3Actual.cols());
-
-    Matrix23 H1Expected = (Matrix23() <<//
         -200, 200, 400,//
-        -200, 0, 200).finished();
+        -200, 0, 200).finished(), H1, 1e-6));
-    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+    CHECK(assert_equal((Matrix26() <<//
-
+        200, -400, -200, -200, 0, -200,//
-    // du/dx for the pose3d offset
+        400, -200, 200, 0, -200, -200).finished(), H2, 1e-6));
-    // note this is (roll, pitch, yaw, x, y, z)
+    CHECK(assert_equal((Matrix29() <<//
-    Matrix26 H2Expected = (Matrix26() <<//
-        -200, -200, 400, -200, 200, 0,//
-        200, -400, 200, -200, 0, 200).finished();
-    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
-
-    Matrix29 H3Expected = (Matrix29() <<//
         -1, 0, -1, 1, 0, -400, -800, 400, 800,//
-        0, -1, 0, 0, 1, -400, -800, 800, 400).finished();
+        0, -1, 0, 0, 1, -400, -800, 800, 400).finished(), H3, 1e-6));
-
-    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
 }
 
+/* ************************************************************************* */
 TEST(PlanarProjectionFactor3, error3) {
     Point3 landmark(1, 1, 1);
     Point2 measured(0, 0);
-    Pose3 offset;
-    Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
-
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
-    PlanarProjectionFactor3 factor(
+    PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model);
-        X(0), C(0), K(0), landmark, measured, model);
-    Values values;
     Pose2 pose(0, 0, 0);
-
+    Pose3 offset(
-    values.insert(X(0), pose);
+        Rot3(0, 0, 1,//
-    values.insert(C(0), offset);
+            -1, 0, 0, //
-    values.insert(K(0), calib);
+            0, -1, 0),
-
+        Vector3(0, 0, 0)
-
+    );
-    CHECK_EQUAL(2, model->dim());
+    Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
-    CHECK_EQUAL(2, factor.dim());
+    Matrix H1;
-    CHECK(factor.active(values));
+    Matrix H2;
-    std::vector<Matrix> actualHs(3);
+    Matrix H3;
-    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+    CHECK(assert_equal(Vector2(0, 0), factor.evaluateError(pose, offset, calib, H1, H2, H3), 1e-6));
-
+    CHECK(assert_equal((Matrix23() <<//
-    CHECK(assert_equal(Vector2(0, 0), actual));
-
-    const Matrix& H1Actual = actualHs.at(0);
-    const Matrix& H2Actual = actualHs.at(1);
-    const Matrix& H3Actual = actualHs.at(2);
-
-    CHECK_EQUAL(2, H1Actual.rows());
-    CHECK_EQUAL(3, H1Actual.cols());
-    CHECK_EQUAL(2, H2Actual.rows());
-    CHECK_EQUAL(6, H2Actual.cols());
-    CHECK_EQUAL(2, H3Actual.rows());
-    CHECK_EQUAL(9, H3Actual.cols());
-
-    Matrix23 H1Expected = (Matrix23() <<//
         -360, 280, 640,//
-        -360, 80, 440).finished();
+        -360, 80, 440).finished(), H1, 1e-6));
-    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+    CHECK(assert_equal((Matrix26() <<//
-
+        440, -640, -200, -280, -80, -360,//
-    // du/dx for the pose3d offset
+        640, -440, 200, -80, -280, -360).finished(), H2, 1e-6));
-    // note this is (roll, pitch, yaw, x, y, z)
+    CHECK(assert_equal((Matrix29() <<//
-    Matrix26 H2Expected = (Matrix26() <<//
-        -200, -440, 640, -360, 280, 80,//
-        200, -640, 440, -360, 80, 280).finished();
-    CHECK(assert_equal(H2Expected, H2Actual, 1e-6));
-
-    Matrix29 H3Expected = (Matrix29() <<//
         -1, 0, -1, 1, 0, -400, -800, 400, 800,//
-        0, -1, 0, 0, 1, -400, -800, 800, 400).finished();
+        0, -1, 0, 0, 1, -400, -800, 800, 400).finished(), H3, 1e-6));
-
-    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
 }
 
-
+/* ************************************************************************* */
 TEST(PlanarProjectionFactor3, jacobian) {
-    // test many jacobians with many randoms
+    // Verify Jacobians with numeric derivative
 
-    std::default_random_engine g;
+    std::default_random_engine rng(42);
-    std::uniform_real_distribution<double> s(-0.3, 0.3);    
+    std::uniform_real_distribution<double> dist(-0.3, 0.3);
     SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    // center of the random camera poses
+    Pose3 centerOffset(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
 
     for (int i = 0; i < 1000; ++i) {
-        Point3 landmark(2 + s(g), s(g), s(g));
+        Point3 landmark(2 + dist(rng), dist(rng), dist(rng));
-        Point2 measured(200 + 100*s(g), 200 + 100*s(g));
+        Point2 measured(200 + 100 * dist(rng), 200 + 100 * dist(rng));
-        Pose3 offset(Rot3::Ypr(s(g),s(g),s(g)), Point3(s(g),s(g),s(g)));
+        Pose3 offset = centerOffset.compose(
+            Pose3(
+                Rot3::Ypr(dist(rng), dist(rng), dist(rng)),
+                Point3(dist(rng), dist(rng), dist(rng))));
         Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
 
-        PlanarProjectionFactor3 factor(
+        PlanarProjectionFactor3 factor(X(0), C(0), K(0), landmark, measured, model);
-            X(0), C(0), K(0), landmark, measured, model);
 
-        Pose2 pose(s(g), s(g), s(g));
+        Pose2 pose(dist(rng), dist(rng), dist(rng));
 
         // actual H
         Matrix H1, H2, H3;
@@ -362,6 +331,104 @@ TEST(PlanarProjectionFactor3, jacobian) {
     }
 }
 
+/* ************************************************************************* */
+TEST(PlanarProjectionFactor3, solveOffset) {
+    // Example localization
+    SharedNoiseModel pxModel = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    SharedNoiseModel xNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01));
+    // offset model is wide, so the solver finds the right answer.
+    SharedNoiseModel cNoise = noiseModel::Diagonal::Sigmas(Vector6(10, 10, 10, 10, 10, 10));
+    SharedNoiseModel kNoise = noiseModel::Diagonal::Sigmas(Vector9(0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001));
+
+    // landmarks
+    Point3 l0(1, 0, 1);
+    Point3 l1(1, 0, 0);
+    Point3 l2(1, -1, 1);
+    Point3 l3(2, 2, 1);
+
+    // camera pixels
+    Point2 p0(200, 200);
+    Point2 p1(200, 400);
+    Point2 p2(400, 200);
+    Point2 p3(0, 200);
+
+    // body
+    Pose2 x0(0, 0, 0);
+
+    // camera z looking at +x with (xy) antiparallel to (yz)
+    Pose3 c0(
+        Rot3(0, 0, 1, //
+            -1, 0, 0, //
+            0, -1, 0), //
+        Vector3(0, 0, 1)); // note z offset
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+
+    NonlinearFactorGraph graph;
+    graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l0, p0, pxModel));
+    graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l1, p1, pxModel));
+    graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l2, p2, pxModel));
+    graph.add(PlanarProjectionFactor3(X(0), C(0), K(0), l3, p3, pxModel));
+    graph.add(PriorFactor<Pose2>(X(0), x0, xNoise));
+    graph.add(PriorFactor<Pose3>(C(0), c0, cNoise));
+    graph.add(PriorFactor<Cal3DS2>(K(0), calib, kNoise));
+
+    Values initialEstimate;
+    initialEstimate.insert(X(0), x0);
+    initialEstimate.insert(C(0), c0);
+    initialEstimate.insert(K(0), calib);
+
+    // run the optimizer
+    LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
+    Values result = optimizer.optimize();
+
+    // verify that the optimizer found the right pose.
+    CHECK(assert_equal(x0, result.at<Pose2>(X(0)), 2e-3));
+
+    // verify the camera is pointing at +x
+    Pose3 cc0 = result.at<Pose3>(C(0));
+    CHECK(assert_equal(c0, cc0, 5e-3));
+
+    // verify the calibration
+    CHECK(assert_equal(calib, result.at<Cal3DS2>(K(0)), 2e-3));
+
+    Marginals marginals(graph, result);
+    Matrix x0cov = marginals.marginalCovariance(X(0));
+
+    // narrow prior => ~zero cov
+    CHECK(assert_equal(Matrix33::Zero(), x0cov, 1e-4));
+
+    Matrix c0cov = marginals.marginalCovariance(C(0));
+
+    // invert the camera offset to get covariance in body coordinates
+    Matrix66 HcTb = cc0.inverse().AdjointMap().inverse();
+    Matrix c0cov2 = HcTb * c0cov * HcTb.transpose();
+
+    // camera-frame stddev
+    Vector6 c0sigma = c0cov.diagonal().cwiseSqrt();
+    CHECK(assert_equal((Vector6() << //
+        0.009,
+        0.011,
+        0.004,
+        0.012,
+        0.012,
+        0.011
+        ).finished(), c0sigma, 1e-3));
+
+    // body frame stddev
+    Vector6 bTcSigma = c0cov2.diagonal().cwiseSqrt();
+    CHECK(assert_equal((Vector6() << //
+        0.004,
+        0.009,
+        0.011,
+        0.012,
+        0.012,
+        0.012
+        ).finished(), bTcSigma, 1e-3));
+
+    // narrow prior => ~zero cov
+    CHECK(assert_equal(Matrix99::Zero(), marginals.marginalCovariance(K(0)), 3e-3));
+}
+
 /* ************************************************************************* */
 int main() {
     TestResult tr;