projection and SFM for 2d poses

gtsam/slam/PlanarProjectionFactor.h

@@ -0,0 +1,154 @@
+/**
+ * Similar to GenericProjectionFactor, but:
+ *
+ * * 2d pose variable (robot on the floor)
+ * * constant landmarks
+ * * batched input
+ * * numeric differentiation
+ *
+ * This factor is useful for high-school robotics competitions,
+ * which run robots on the floor, with use fixed maps and fiducial
+ * markers.  The camera offset and calibration are fixed, perhaps
+ * found using PlanarSFMFactor.
+ *
+ * The python version of this factor uses batches, to save on calls
+ * across the C++/python boundary, but here the only extra cost
+ * is instantiating the camera, so there's no batch.
+ *
+ * @see https://www.firstinspires.org/
+ * @see PlanarSFMFactor.h
+ *
+ * @file PlanarSFMFactor.h
+ * @brief for planar smoothing
+ * @date Dec 2, 2024
+ * @author joel@truher.org
+ */
+#pragma once
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/Lie.h>
+
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/base/numericalDerivative.h>
+
+
+namespace gtsam {
+    /**
+     * @class PlanarProjectionFactor
+     * @brief Camera projection for robot on the floor.
+     */
+    class PlanarProjectionFactor : public NoiseModelFactorN<Pose2> {
+        typedef NoiseModelFactorN<Pose2> Base;
+        static const Pose3 CAM_COORD;
+
+    protected:
+
+        Point3 landmark_; // landmark
+        Point2 measured_; // pixel measurement
+        Pose3 offset_; // camera offset to robot pose
+        Cal3DS2 calib_; // camera calibration
+
+    public:
+        // Provide access to the Matrix& version of evaluateError:
+        using Base::evaluateError;
+
+        PlanarProjectionFactor() {}
+
+        /**
+         * @param landmarks point in the world
+         * @param measured corresponding point in the camera frame
+         * @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 pose in the z=0 plane
+         */
+        PlanarProjectionFactor(
+            const Point3& landmark,
+            const Point2& measured,
+            const Pose3& offset,
+            const Cal3DS2& calib,
+            const SharedNoiseModel& model,
+            Key poseKey)
+            : NoiseModelFactorN(model, poseKey),
+            landmark_(landmark),
+            measured_(measured),
+            offset_(offset),
+            calib_(calib)
+        {
+            assert(2 == model->dim());
+        }
+
+        ~PlanarProjectionFactor() override {}
+
+        /// @return a deep copy of this factor
+        gtsam::NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<gtsam::NonlinearFactor>(
+                gtsam::NonlinearFactor::shared_ptr(new PlanarProjectionFactor(*this)));
+        }
+
+        Point2 h2(const Pose2& pose, OptionalMatrixType H1) const {
+            // this is x-forward z-up
+            gtsam::Matrix H0;
+            Pose3 offset_pose = Pose3(pose).compose(offset_, H0);
+            // this is z-forward y-down
+            gtsam::Matrix H00;
+            Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00);
+            PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib_);
+            if (H1) {
+                // Dpose is 2x6 (R,t)
+                // H1 is 2x3 (x, y, theta)
+                gtsam::Matrix Dpose;
+                Point2 result = camera.project(landmark_, Dpose);
+                Dpose = Dpose * H00 * H0;
+                *H1 = Matrix::Zero(2, 3);
+                (*H1)(0, 0) = Dpose(0, 3); // du/dx
+                (*H1)(1, 0) = Dpose(1, 3); // dv/dx
+                (*H1)(0, 1) = Dpose(0, 4); // du/dy
+                (*H1)(1, 1) = Dpose(1, 4); // dv/dy
+                (*H1)(0, 2) = Dpose(0, 2); // du/dyaw
+                (*H1)(1, 2) = Dpose(1, 2); // dv/dyaw
+                return result;
+            }
+            else {
+                return camera.project(landmark_, {}, {}, {});
+            }
+        }
+
+        Vector evaluateError(
+            const Pose2& pose,
+            OptionalMatrixType H1 = OptionalNone) const override {
+            try {
+                return h2(pose, H1) - measured_;
+            } catch (CheiralityException& e) {
+                std::cout << "****** CHIRALITY EXCEPTION ******\n";
+                std::cout << "landmark " << landmark_ << "\n";
+                std::cout << "pose " << pose << "\n";
+                std::cout << "offset " << offset_ << "\n";
+                std::cout << "calib " << calib_ << "\n";
+                if (H1) *H1 = Matrix::Zero(2, 3);
+                // return a large error
+                return Matrix::Constant(2, 1, 2.0 * calib_.fx());
+            }
+        }
+    };
+
+    // camera "zero" is facing +z; this turns it to face +x
+    const Pose3 PlanarProjectionFactor::CAM_COORD = Pose3(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+
+    template<>
+    struct traits<PlanarProjectionFactor> :
+        public Testable<PlanarProjectionFactor > {
+    };
+
+} // namespace gtsam

gtsam/slam/PlanarSFMFactor.h

@@ -0,0 +1,165 @@
+/**
+ * Similar to GeneralSFMFactor, but:
+ *
+ * * 2d pose variable (robot on the floor)
+ * * camera offset variable
+ * * constant landmarks
+ * * batched input
+ * * numeric differentiation
+ *
+ * This factor is useful to find camera calibration and placement, in
+ * a sort of "autocalibrate" mode.  Once a satisfactory solution is
+ * found, the PlanarProjectionFactor should be used for localization.
+ *
+ * The python version of this factor uses batches, to save on calls
+ * across the C++/python boundary, but here the only extra cost
+ * is instantiating the camera, so there's no batch.
+ *
+ * @see https://www.firstinspires.org/
+ * @see PlanarProjectionFactor.h
+ *
+ * @file PlanarSFMFactor.h
+ * @brief for planar smoothing with unknown calibration
+ * @date Dec 2, 2024
+ * @author joel@truher.org
+ */
+#pragma once
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/Lie.h>
+
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+#include <gtsam/base/numericalDerivative.h>
+
+
+namespace gtsam {
+    /**
+     * @class PlanarSFMFactor
+     * @brief Camera calibration for robot on the floor.
+     */
+    class PlanarSFMFactor : public NoiseModelFactorN<Pose2, Pose3, Cal3DS2> {
+    private:
+        typedef NoiseModelFactorN<Pose2, Pose3, Cal3DS2> Base;   
+        static const Pose3 CAM_COORD;
+
+    protected:
+
+        Point3 landmark_; // landmark
+        Point2 measured_; // pixel measurement
+
+    public:
+        // Provide access to the Matrix& version of evaluateError:
+        using Base::evaluateError;
+
+        PlanarSFMFactor() {}
+        /**
+         * @param landmarks point in the world
+         * @param measured corresponding point 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 the 3d camera offset from the robot pose
+         * @param calibKey index of the camera calibration
+         */
+        PlanarSFMFactor(
+            const Point3& landmark,
+            const Point2& measured,
+            const SharedNoiseModel& model,
+            Key poseKey,
+            Key offsetKey,
+            Key calibKey)
+            : NoiseModelFactorN(model, poseKey, offsetKey, calibKey),
+            landmark_(landmark), measured_(measured)
+        {
+            assert(2 == model->dim());
+        }
+
+        ~PlanarSFMFactor() override {}
+
+        /// @return a deep copy of this factor
+        gtsam::NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<gtsam::NonlinearFactor>(
+                gtsam::NonlinearFactor::shared_ptr(new PlanarSFMFactor(*this)));
+        }
+
+        Point2 h2(const Pose2& pose,
+            const Pose3& offset,
+            const Cal3DS2& calib,
+            OptionalMatrixType H1,
+            OptionalMatrixType H2,
+            OptionalMatrixType H3
+            ) const {
+            // this is x-forward z-up
+            gtsam::Matrix H0;
+            Pose3 offset_pose = Pose3(pose).compose(offset, H0);
+            // this is z-forward y-down
+            gtsam::Matrix H00;
+            Pose3 camera_pose = offset_pose.compose(CAM_COORD, H00);
+            PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(camera_pose, calib);
+            if (H1 || H2) {
+                gtsam::Matrix Dpose;
+                Point2 result = camera.project(landmark_, Dpose, {}, H3);
+                gtsam::Matrix DposeOffset = Dpose * H00;
+                if (H2)
+                    *H2 = DposeOffset; // a deep copy
+                if (H1) {
+                    gtsam::Matrix DposeOffsetFwd = DposeOffset * H0;
+                    *H1 = Matrix::Zero(2,3);
+                    (*H1)(0,0) = DposeOffsetFwd(0,3); // du/dx
+                    (*H1)(1,0) = DposeOffsetFwd(1,3); // dv/dx
+                    (*H1)(0,1) = DposeOffsetFwd(0,4); // du/dy
+                    (*H1)(1,1) = DposeOffsetFwd(1,4); // dv/dy
+                    (*H1)(0,2) = DposeOffsetFwd(0,2); // du/dyaw
+                    (*H1)(1,2) = DposeOffsetFwd(1,2); // dv/dyaw
+                }
+                return result;
+            } else {
+                return camera.project(landmark_, {}, {}, {});
+            }    
+        }
+
+        Vector evaluateError(
+            const Pose2& pose,
+            const Pose3& offset,
+            const Cal3DS2& calib,
+            OptionalMatrixType H1 = OptionalNone,
+            OptionalMatrixType H2 = OptionalNone,
+            OptionalMatrixType H3 = OptionalNone
+        ) const override {
+            try {
+                return h2(pose,offset,calib,H1,H2,H3) - measured_;
+            }
+            catch (CheiralityException& e) {
+                std::cout << "****** CHIRALITY EXCEPTION ******\n";
+                std::cout << "landmark " << landmark_ << "\n";
+                std::cout << "pose " << pose << "\n";
+                std::cout << "offset " << offset << "\n";
+                std::cout << "calib " << calib << "\n";
+                if (H1) *H1 = Matrix::Zero(2, 3);
+                if (H2) *H2 = Matrix::Zero(2, 6);
+                if (H3) *H3 = Matrix::Zero(2, 9);
+                // return a large error
+                return Matrix::Constant(2, 1, 2.0 * calib.fx());
+            }
+        }
+    };
+
+    // camera "zero" is facing +z; this turns it to face +x
+    const Pose3 PlanarSFMFactor::CAM_COORD = Pose3(
+        Rot3(0, 0, 1,//
+            -1, 0, 0, //
+            0, -1, 0),
+        Vector3(0, 0, 0)
+    );
+
+    template<>
+    struct traits<PlanarSFMFactor> :
+        public Testable<PlanarSFMFactor > {
+    };
+
+} // namespace gtsam

gtsam/slam/slam.i

@@ -24,6 +24,18 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
   void serialize() const;
 };
 
+#include <gtsam/slam/PlanarProjectionFactor.h>
+virtual class PlanarProjectionFactor : gtsam::NoiseModelFactor {
+  PlanarProjectionFactor(
+            const gtsam::Point3& landmark,
+            const gtsam::Point2& measured,
+            const gtsam::Pose3& offset,
+            const gtsam::Cal3DS2& calib,
+            const gtsam::noiseModel::Base* model,
+            size_t poseKey);
+  void serialize() const;
+};
+
 #include <gtsam/slam/ProjectionFactor.h>
 template <POSE, LANDMARK, CALIBRATION>
 virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
@@ -67,6 +79,18 @@ typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
                                        gtsam::Cal3Unified>
     GenericProjectionFactorCal3Unified;
 
+#include <gtsam/slam/PlanarSFMFactor.h>
+virtual class PlanarSFMFactor  : gtsam::NoiseModelFactor {
+  PlanarSFMFactor(
+            const gtsam::Point3& landmark,
+            const gtsam::Point2& measured,
+            const gtsam::noiseModel::Base* model,
+            size_t poseKey,
+            size_t offsetKey,
+            size_t calibKey);
+  void serialize() const;
+};
+
 #include <gtsam/slam/GeneralSFMFactor.h>
 template <CAMERA, LANDMARK>
 virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {

gtsam/slam/tests/testPlanarProjectionFactor.cpp

@@ -0,0 +1,156 @@
+/**
+ * @file testPlanarProjectionFactor.cpp
+ * @date Dec 3, 2024
+ * @author joel@truher.org
+ * @brief unit tests for PlanarProjectionFactor
+ */
+
+#include <random>
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot2.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/PlanarProjectionFactor.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using symbol_shorthand::X;
+
+TEST(PlanarProjectionFactor, error1) {
+    // landmark is on the camera bore (which faces +x)
+    Point3 landmark(1, 0, 0);
+    // so pixel measurement is (cx, cy)
+    Point2 measured(200, 200);
+    Pose3 offset;
+    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);
+
+    PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
+
+    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(PlanarProjectionFactor, error2) {
+    // landmark is in the upper left corner
+    Point3 landmark(1, 1, 1);
+    // upper left corner in pixels
+    Point2 measured(0, 0);
+    Pose3 offset;
+    Cal3DS2 calib(200, 200, 0, 200, 200, 0, 0);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
+    Values values;
+    Pose2 pose(0, 0, 0);
+
+    values.insert(X(0), pose);
+
+    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();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+}
+
+TEST(PlanarProjectionFactor, error3) {
+    // landmark is in the upper left corner
+    Point3 landmark(1, 1, 1);
+    // upper left corner in pixels
+    Point2 measured(0, 0);
+    Pose3 offset;
+    // distortion
+    Cal3DS2 calib(200, 200, 0, 200, 200, -0.2, 0.1);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+    PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
+    Values values;
+    Pose2 pose(0, 0, 0);
+
+    values.insert(X(0), pose);
+
+    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();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+}
+
+TEST(PlanarProjectionFactor, jacobian) {
+    // test many jacobians with many randoms
+
+    std::default_random_engine g;
+    std::uniform_real_distribution<double> s(-0.3, 0.3);
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+
+    for (int i = 0; i < 1000; ++i) {
+        Point3 landmark(2 + s(g), s(g), s(g));
+        Point2 measured(200 + 100*s(g), 200 + 100*s(g));
+        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);
+
+        PlanarProjectionFactor factor(landmark, measured, offset, calib, model, X(0));
+
+        Pose2 pose(s(g), s(g), s(g));
+
+        // actual H
+        Matrix H1;
+        factor.evaluateError(pose, H1);
+
+        Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
+            [&factor](const Pose2& p) {
+                return factor.evaluateError(p);},
+                pose);
+        CHECK(assert_equal(expectedH1, H1, 1e-6));
+    }
+
+
+}
+
+/* ************************************************************************* */
+int main() {
+    TestResult tr;
+    return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+

gtsam/slam/tests/testPlanarSFMFactor.cpp

@@ -0,0 +1,243 @@
+/**
+ * @file testPlanarSFMFactor.cpp
+ * @date Dec 3, 2024
+ * @author joel@truher.org
+ * @brief unit tests for PlanarSFMFactor
+ */
+
+#include <random>
+
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot2.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/PlanarSFMFactor.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using symbol_shorthand::X;
+using symbol_shorthand::C;
+using symbol_shorthand::K;
+
+TEST(PlanarSFMFactor, error1) {
+    // landmark is on the camera bore (facing +x)
+    Point3 landmark(1, 0, 0);
+    // so px is (cx, cy)
+    Point2 measured(200, 200);
+    // offset is identity
+    Pose3 offset;
+    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);
+
+    PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
+
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(3);
+
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+    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());
+
+    // du/dx etc for the pose2d
+    Matrix23 H1Expected = (Matrix23() <<//
+        0, 200, 200,//
+        0, 0, 0).finished();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+
+    // 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(PlanarSFMFactor, 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));
+    PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
+    Values values;
+    Pose2 pose(0, 0, 0);
+
+    values.insert(X(0), pose);
+    values.insert(C(0), offset);
+    values.insert(K(0), calib);
+
+
+    CHECK_EQUAL(2, model->dim());
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(3);
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+
+    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() <<//
+        -200, 200, 400,//
+        -200, 0, 200).finished();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+
+    // du/dx for the pose3d offset
+    // note this is (roll, pitch, yaw, x, y, z)
+    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();
+
+    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
+}
+
+TEST(PlanarSFMFactor, 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));
+    PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
+    Values values;
+    Pose2 pose(0, 0, 0);
+
+    values.insert(X(0), pose);
+    values.insert(C(0), offset);
+    values.insert(K(0), calib);
+
+
+    CHECK_EQUAL(2, model->dim());
+    CHECK_EQUAL(2, factor.dim());
+    CHECK(factor.active(values));
+    std::vector<Matrix> actualHs(3);
+    gtsam::Vector actual = factor.unwhitenedError(values, actualHs);
+
+    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();
+    CHECK(assert_equal(H1Expected, H1Actual, 1e-6));
+
+    // du/dx for the pose3d offset
+    // note this is (roll, pitch, yaw, x, y, z)
+    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();
+
+    CHECK(assert_equal(H3Expected, H3Actual, 1e-6));
+}
+
+
+
+TEST(PlanarSFMFactor, jacobian) {
+    // test many jacobians with many randoms
+
+    std::default_random_engine g;
+    std::uniform_real_distribution<double> s(-0.3, 0.3);    
+    SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(1, 1));
+
+    for (int i = 0; i < 1000; ++i) {
+        Point3 landmark(2 + s(g), s(g), s(g));
+        Point2 measured(200 + 100*s(g), 200 + 100*s(g));
+        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);
+
+        PlanarSFMFactor factor(landmark, measured, model, X(0), C(0), K(0));
+
+        Pose2 pose(s(g), s(g), s(g));
+
+        // actual H
+        Matrix H1, H2, H3;
+        factor.evaluateError(pose, offset, calib, H1, H2, H3);
+
+        Matrix expectedH1 = numericalDerivative31<Vector, Pose2, Pose3, Cal3DS2>(
+            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
+                return factor.evaluateError(p, o, c);},
+                pose, offset, calib);
+        Matrix expectedH2 = numericalDerivative32<Vector, Pose2, Pose3, Cal3DS2>(
+            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
+                return factor.evaluateError(p, o, c);},
+                pose, offset, calib);
+        Matrix expectedH3 = numericalDerivative33<Vector, Pose2, Pose3, Cal3DS2>(
+            [&factor](const Pose2& p, const Pose3& o, const Cal3DS2& c) {
+                return factor.evaluateError(p, o, c);},
+                pose, offset, calib);
+        CHECK(assert_equal(expectedH1, H1, 1e-6));
+        CHECK(assert_equal(expectedH2, H2, 1e-6));
+        CHECK(assert_equal(expectedH3, H3, 1e-6));
+    }
+}
+
+
+/* ************************************************************************* */
+int main() {
+    TestResult tr;
+    return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+