Merge pull request #1934 from truher/team100_frc_factors

dellaert · web-flow · commit a2f917aa09e7 · 2024-12-25T10:49:18.000-05:00
projection and SFM for 2d poses
diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp
@@ -43,6 +43,20 @@ Pose3 Pose3::Create(const Rot3& R, const Point3& t, OptionalJacobian<6, 3> HR,
   return Pose3(R, t);
 }
 
+// Pose2 constructor Jacobian is always the same.
+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);
+}
+
 /* ************************************************************************* */
 Pose3 Pose3::inverse() const {
   Rot3 Rt = R_.inverse();
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
@@ -78,6 +78,9 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
                       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 = {});
+
   /**
    *  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
diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h
@@ -0,0 +1,298 @@
+/**
+ * ProjectionFactor, but with pose2 (robot on the floor)
+ *
+ * This factor is useful for high-school robotics competitions,
+ * which run robots on the floor, with use fixed maps and fiducial
+ * markers.
+ *
+ * @see https://www.firstinspires.org/
+ *
+ * @file PlanarProjectionFactor.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 PlanarProjectionFactorBase
+     * @brief Camera projection for robot on the floor.  Measurement is camera pixels.
+     */
+    class PlanarProjectionFactorBase {
+    protected:
+        PlanarProjectionFactorBase() {}
+
+        /**
+         * @param measured pixels in the camera frame
+         */
+        PlanarProjectionFactorBase(const Point2& measured) : measured_(measured) {}
+
+        /**
+         * Predict the projection of the landmark in camera pixels.
+         *
+         * @param landmark point3 of the target
+         * @param wTb "world to body": planar pose2 of vehicle body frame in world frame
+         * @param bTc "body to camera": camera pose in body frame, oriented parallel to pose2 zero i.e. +x
+         * @param calib camera calibration with distortion
+         * @param Hlandmark jacobian
+         * @param HwTb jacobian
+         * @param HbTc jacobian
+         * @param Hcalib jacobian
+         */
+        Point2 predict(
+            const Point3& landmark,
+            const Pose2& wTb,
+            const Pose3& bTc,
+            const Cal3DS2& calib,
+            OptionalJacobian<2, 3> Hlandmark = {}, // (x, y, z)
+            OptionalJacobian<2, 3> HwTb = {}, // (x, y, theta)
+            OptionalJacobian<2, 6> HbTc = {}, // (rx, ry, rz, x, y, theta)
+            OptionalJacobian<2, 9> Hcalib = {}
+        ) const {
+#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION
+            try {
+#endif
+                Matrix63 Hp; // 6x3
+                Matrix66 H0; // 6x6
+                Pose3 wTc = Pose3::FromPose2(wTb, HwTb ? &Hp : nullptr).compose(bTc, HwTb ? &H0 : nullptr);
+                PinholeCamera<Cal3DS2> camera = PinholeCamera<Cal3DS2>(wTc, calib);
+                if (HwTb || HbTc) {
+                    // Dpose is for pose3 (R,t)
+                    Matrix26 Dpose;
+                    Point2 result = camera.project(landmark, Dpose, Hlandmark, Hcalib);
+                    if (HbTc)
+                        *HbTc = Dpose;
+                    if (HwTb)
+                        *HwTb = Dpose * H0 * Hp;
+                    return result;
+                } else {
+                    return camera.project(landmark, {}, {}, {});
+                }
+#ifndef GTSAM_THROW_CHEIRALITY_EXCEPTION
+            } catch (CheiralityException& e) {
+                std::cout << "****** CHIRALITY EXCEPTION ******\n";
+                if (Hlandmark) Hlandmark->setZero();
+                if (HwTb) HwTb->setZero();
+                if (HbTc) HbTc->setZero();
+                if (Hcalib) Hcalib->setZero();
+                // return a large error
+                return Matrix::Constant(2, 1, 2.0 * calib.fx());
+            }
+#endif
+        }
+
+        Point2 measured_; // pixel measurement
+    };
+
+
+    /**
+     * @class PlanarProjectionFactor1
+     * @brief One variable: the pose.
+     * Landmark, camera offset, camera calibration are constant.
+     * This is intended for localization within a known map.
+     */
+    class PlanarProjectionFactor1
+        : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2> {
+    public:
+        typedef NoiseModelFactorN<Pose2> Base;
+        using Base::evaluateError;
+        PlanarProjectionFactor1() {}
+
+        ~PlanarProjectionFactor1() override {}
+
+        /// @return a deep copy of this factor
+        NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<NonlinearFactor>(
+                NonlinearFactor::shared_ptr(new PlanarProjectionFactor1(*this)));
+        }
+
+
+        /**
+         * @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 bTc "body to camera": constant camera offset from pose
+         * @param calib constant camera calibration
+         * @param model stddev of the measurements, ~one pixel?
+         */
+        PlanarProjectionFactor1(
+            Key poseKey,
+            const Point3& landmark,
+            const Point2& measured,
+            const Pose3& bTc,
+            const Cal3DS2& calib,
+            const SharedNoiseModel& model = {})
+            : PlanarProjectionFactorBase(measured),
+            NoiseModelFactorN(model, poseKey),
+            landmark_(landmark),
+            bTc_(bTc),
+            calib_(calib) {}
+
+        /**
+         * @param wTb "world to body": estimated pose2
+         * @param HwTb jacobian
+         */
+        Vector evaluateError(const Pose2& wTb, OptionalMatrixType HwTb) const override {
+            return predict(landmark_, wTb, bTc_, calib_, {}, HwTb, {}, {}) - measured_;
+        }
+
+    private:
+        Point3 landmark_; // landmark
+        Pose3 bTc_; // "body to camera": camera offset to robot pose
+        Cal3DS2 calib_; // camera calibration
+    };
+
+    template<>
+    struct traits<PlanarProjectionFactor1> :
+        public Testable<PlanarProjectionFactor1> {};
+
+    /**
+     * @class PlanarProjectionFactor2
+     * @brief Two unknowns: the pose and the landmark.
+     * Camera offset and calibration are constant.
+     * This is similar to GeneralSFMFactor, used for SLAM.
+    */
+    class PlanarProjectionFactor2
+        : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2, Point3> {
+    public:
+        typedef NoiseModelFactorN<Pose2, Point3> Base;
+        using Base::evaluateError;
+
+        PlanarProjectionFactor2() {}
+
+        ~PlanarProjectionFactor2() override {}
+
+        /// @return a deep copy of this factor
+        NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<NonlinearFactor>(
+                NonlinearFactor::shared_ptr(new PlanarProjectionFactor2(*this)));
+        }
+
+        /**
+         * @brief constructor for variable landmark, known offset and calibration
+         * @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(
+            Key poseKey,
+            Key landmarkKey,
+            const Point2& measured,
+            const Pose3& bTc,
+            const Cal3DS2& calib,
+            const SharedNoiseModel& model = {})
+            : PlanarProjectionFactorBase(measured),
+            NoiseModelFactorN(model, landmarkKey, poseKey),
+            bTc_(bTc),
+            calib_(calib) {}
+
+        /**
+         * @param wTb "world to body": estimated pose2
+         * @param landmark estimated landmark
+         * @param HwTb jacobian
+         * @param Hlandmark jacobian
+         */
+        Vector evaluateError(
+            const Pose2& wTb,
+            const Point3& landmark,
+            OptionalMatrixType HwTb,
+            OptionalMatrixType Hlandmark) const override {
+            return predict(landmark, wTb, bTc_, calib_, Hlandmark, HwTb, {}, {}) - measured_;
+        }
+
+    private:
+        Pose3 bTc_; // "body to camera": camera offset to robot pose
+        Cal3DS2 calib_; // camera calibration
+    };
+
+    template<>
+    struct traits<PlanarProjectionFactor2> :
+        public Testable<PlanarProjectionFactor2> {};
+
+    /**
+     * @class PlanarProjectionFactor3
+     * @brief Three unknowns: the pose, the camera offset, and the camera calibration.
+     * Landmark is constant.
+     * This is intended to be used for camera calibration.
+    */
+    class PlanarProjectionFactor3 : public PlanarProjectionFactorBase, public NoiseModelFactorN<Pose2, Pose3, Cal3DS2> {
+    public:
+        typedef NoiseModelFactorN<Pose2, Pose3, Cal3DS2> Base;
+        using Base::evaluateError;
+
+        PlanarProjectionFactor3() {}
+
+        ~PlanarProjectionFactor3() override {}
+
+        /// @return a deep copy of this factor
+        NonlinearFactor::shared_ptr clone() const override {
+            return std::static_pointer_cast<NonlinearFactor>(
+                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?
+         */
+        PlanarProjectionFactor3(
+            Key poseKey,
+            Key offsetKey,
+            Key calibKey,
+            const Point3& landmark,
+            const Point2& measured,
+            const SharedNoiseModel& model = {})
+            : PlanarProjectionFactorBase(measured),
+            NoiseModelFactorN(model, poseKey, offsetKey, calibKey),
+            landmark_(landmark) {}
+
+        /**
+         * @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 HbTc offset jacobian
+         * @param Hcalib calibration jacobian
+         */
+        Vector evaluateError(
+            const Pose2& wTb,
+            const Pose3& bTc,
+            const Cal3DS2& calib,
+            OptionalMatrixType HwTb,
+            OptionalMatrixType HbTc,
+            OptionalMatrixType Hcalib) const override {
+            return predict(landmark_, wTb, bTc, calib, {}, HwTb, HbTc, Hcalib) - measured_;
+        }
+
+    private:
+        Point3 landmark_; // landmark
+    };
+
+    template<>
+    struct traits<PlanarProjectionFactor3> :
+        public Testable<PlanarProjectionFactor3> {};
+
+} // namespace gtsam
diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i
@@ -24,6 +24,38 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
   void serialize() const;
 };
 
+#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& bTc,
+    const gtsam::Cal3DS2& calib,
+    const gtsam::noiseModel::Base* model);
+  void serialize() const;
+};
+virtual class PlanarProjectionFactor2 : gtsam::NoiseModelFactor {
+  PlanarProjectionFactor2(
+    size_t poseKey,
+    size_t landmarkKey,
+    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(
+    size_t poseKey,
+    size_t offsetKey,
+    size_t calibKey,
+    const gtsam::Point3& landmark,
+    const gtsam::Point2& measured,
+    const gtsam::noiseModel::Base* model);
+  void serialize() const;
+};
+
 #include <gtsam/slam/ProjectionFactor.h>
 template <POSE, LANDMARK, CALIBRATION>
 virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
diff --git a/gtsam/slam/tests/testPlanarProjectionFactor.cpp b/gtsam/slam/tests/testPlanarProjectionFactor.cpp
