Added AHRSPose3Factor by extending AHRSFactor to enable integration with Pose3 variables.

gtsam/geometry/DepthCamera3.h

@@ -0,0 +1,153 @@
+
+/**
+ * @file DepthCamera3.h
+ * @brief  Depth Camera.
+ * @author junlinp
+ * @date Nov 9, 2025
+ */
+
+#pragma once
+
+#include <iostream>
+#include <optional>
+#include <gtsam/base/Vector.h>
+#include <gtsam/base/Matrix.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
+#include <boost/serialization/nvp.hpp>
+#endif
+
+namespace gtsam {
+
+/**
+ * A pinhole camera class that has a Pose3 and a Calibration.
+ * @ingroup geometry
+ * \nosubgrouping
+ */
+template <class CALIBRATION>
+class DepthCamera3 {
+private:
+  Point3 measurement_;               ///< The depth measurement  (u, v, depth)
+  std::shared_ptr<CALIBRATION> k_;  ///< The fixed camera calibration
+  std::optional<Pose3> body_P_sensor_;  ///< The pose of the sensor in the body frame
+
+public:
+
+  /// @name Standard Constructors
+  /// @{
+
+  /** default constructor */
+  DepthCamera3() {}
+
+  /** constructor with pose and calibration */
+  DepthCamera3(const Point3& measurement, const std::shared_ptr<CALIBRATION>& k) :
+    measurement_{measurement}, k_(k) {}
+
+  /** constructor with pose, calibration, and body_P_sensor */
+  DepthCamera3(const Point3& measurement, const std::shared_ptr<CALIBRATION>& k,
+               const std::optional<Pose3>& body_P_sensor) :
+    measurement_{measurement}, k_(k), body_P_sensor_(body_P_sensor) {}
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  virtual ~DepthCamera3() {}
+
+
+  /// return calibration
+  inline const std::shared_ptr<CALIBRATION>& calibration() const {  return k_; }
+
+  /// print
+  void print(const std::string& s = "") const {
+    std::cout << s << "DepthCamera3:" << std::endl;
+    std::cout << "  measurement: " << measurement_.transpose() << std::endl;
+    if (k_) k_->print("  calibration: ");
+    if (body_P_sensor_) {
+      body_P_sensor_->print("  body_P_sensor: ");
+    }
+  }
+
+
+  /** project a point from world InvDepth parameterization to the image
+   *  @param pose is the body pose in world frame
+   *  @param landmark is a point in the world coordinate
+   *  @param H1 is the jacobian w.r.t. body pose
+   *  @param H2 is the jacobian w.r.t. landmark
+   */
+  inline gtsam::Point3 project(const Pose3& pose, const Point3& landmark,
+      OptionalJacobian<3,6> H1 = {},
+      OptionalJacobian<3,3> H2 = {}
+      ) const {
+    double fx = k_->fx(); 
+    double fy = k_->fy();
+    double cx = k_->px();
+    double cy = k_->py();
+    Matrix33 inv_intrinsics = (Matrix33() << 1.0 / fx, 0.0, -cx / fx,
+                                              0.0, 1.0 / fy, -cy / fy,
+                                              0.0, 0.0, 1.0).finished();
+
+    Point3 observation_raw(measurement_.x() * measurement_.z(),
+                        measurement_.y() * measurement_.z(),
+                        measurement_.z());
+
+    Point3 observation = inv_intrinsics * observation_raw;
+
+    // Convert body pose to camera pose using body_P_sensor_
+    Pose3 camera_pose_world;
+    if (body_P_sensor_) {
+      if (H1) {
+        Matrix HbodySensor;
+        camera_pose_world = pose.compose(*body_P_sensor_, HbodySensor);
+        // Transform landmark from world to camera frame
+        Matrix Hcam;
+        Point3 predicted_cam = camera_pose_world.transformTo(landmark, Hcam, H2);
+        *H1 = Hcam * HbodySensor;
+        return predicted_cam - observation;
+      } else {
+        camera_pose_world = pose.compose(*body_P_sensor_);
+      }
+    } else {
+      camera_pose_world = pose;
+    }
+
+    // Transform landmark from world to camera frame
+    Point3 predicted_cam;
+    if (H1 || H2) {
+      Matrix Hcam;
+      predicted_cam = camera_pose_world.transformTo(landmark, Hcam, H2);
+      if (H1) {
+        *H1 = Hcam;
+      }
+    } else {
+      predicted_cam = camera_pose_world.transformTo(landmark);
+    }
+
+    return predicted_cam - observation;
+  }
+
+private:
+
+  /// @}
+  /// @name Advanced Interface
+  /// @{
+
+#if GTSAM_ENABLE_BOOST_SERIALIZATION
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class Archive>
+  void serialize(Archive & ar, const unsigned int /*version*/) {
+    ar & BOOST_SERIALIZATION_NVP(measurement_);
+    ar & BOOST_SERIALIZATION_NVP(k_);
+    ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
+  }
+#endif
+  /// @}
+}; // \class DepthCamera3
+} // \namespace gtsam
+

gtsam/geometry/geometry.i

@@ -1312,6 +1312,23 @@ class PinholeCamera {
 typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
 typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
 typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
+
+#include <gtsam/geometry/DepthCamera3.h>
+template<CALIBRATION>
+class DepthCamera3 {
+  DepthCamera3();
+  DepthCamera3(const gtsam::Point3& measurement, const CALIBRATION::shared_ptr& k);
+  DepthCamera3(const gtsam::Point3& measurement, const CALIBRATION::shared_ptr& k, const gtsam::Pose3& body_P_sensor);
+
+  void print(string s = "") const;
+  const CALIBRATION::shared_ptr& calibration() const;
+  gtsam::Point3 project(const gtsam::Pose3& pose, const gtsam::Point3& landmark) const;
+  gtsam::Point3 project(const gtsam::Pose3& pose, const gtsam::Point3& landmark,
+      gtsam::OptionalJacobian<3,6> H1, gtsam::OptionalJacobian<3,3> H2) const;
+
+  void serializable() const; // enabling serialization functionality
+};
+typedef gtsam::DepthCamera3<gtsam::Cal3_S2> DepthCamera3Cal3_S2;
 typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
 typedef gtsam::PinholeCamera<gtsam::Cal3f> PinholeCameraCal3f;
 typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;

gtsam/geometry/tests/testDepthCamera3.cpp

@@ -0,0 +1,148 @@
+
+/*
+ * testDepthFactor.cpp
+ *
+ *  Created on: Nov 9, 2025
+ *      Author: junlinp
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3_S2.h>
+
+#include <gtsam/geometry/DepthCamera3.h>
+
+using namespace std;
+using namespace gtsam;
+
+static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
+Pose3 level_pose = Pose3(Rot3::Ypr(0, 0, 0), gtsam::Point3(0,0,1));
+PinholeCamera<Cal3_S2> level_camera(level_pose, *K);
+
+/* ************************************************************************* */
+TEST(DepthCamera3, Project1) {
+  double depth = 3.0;
+  // landmark directly in front of camera at depth
+  Point3 landmark(0, 0, level_pose.z() + depth);
+  Point2 expected_uv = level_camera.project(landmark);
+
+  Point3 measurement(expected_uv.x(), expected_uv.y(), depth); // z = depth
+  cout << "measurement: " << measurement.transpose() << endl;
+  DepthCamera3<Cal3_S2> depth_camera(measurement, K);
+
+  Point3 actual_error = depth_camera.project(level_pose, landmark);
+
+  // When landmark matches the measurement, error should be approximately zero
+  Vector3 expected_zero = Vector3::Zero();
+  EXPECT(assert_equal(expected_zero, Vector3(actual_error), 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(DepthCamera3, ProjectWithJacobians) {
+  double depth = 3.0;
+  Point3 landmark(5, 3, level_pose.z() + depth);
+  Point2 expected_uv = level_camera.project(landmark);
+  Point3 measurement(expected_uv.x(), expected_uv.y(), depth);
+
+  DepthCamera3<Cal3_S2> depth_camera(measurement, K);
+
+  // Compute analytic Jacobians
+  Matrix H1, H2;
+  depth_camera.project(level_pose, landmark, H1, H2);
+
+  // Numerical Jacobians
+  auto f1 = [&](const Pose3& pose) {
+    return depth_camera.project(pose, landmark);
+  };
+  auto f2 = [&](const Point3& lm) {
+    return depth_camera.project(level_pose, lm);
+  };
+
+  Matrix H1_num = numericalDerivative11<Point3, Pose3>(f1, level_pose, 1e-6);
+  Matrix H2_num = numericalDerivative11<Point3, Point3>(f2, landmark, 1e-6);
+
+  EXPECT(assert_equal(H1_num, H1, 1e-5));
+  EXPECT(assert_equal(H2_num, H2, 1e-5));
+}
+
+/* ************************************************************************* */
+TEST(DepthCamera3, ProjectWithBodyPSensor) {
+  // Create a body_P_sensor transform (rotation and translation)
+  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
+
+  // Body pose in world frame
+  Pose3 body_pose_world = level_pose;
+
+  // Camera pose in world frame (body pose composed with body_P_sensor)
+  Pose3 camera_pose_world = body_pose_world.compose(body_P_sensor);
+  PinholeCamera<Cal3_S2> camera_with_transform(camera_pose_world, *K);
+
+  double depth = 3.0;
+  // landmark directly in front of camera at depth (in camera frame: (0, 0, depth))
+  Point3 landmark_cam(0, 0, depth);
+  // Transform to world frame
+  Point3 landmark = camera_pose_world.transformFrom(landmark_cam);
+  Point2 expected_uv = camera_with_transform.project(landmark);
+
+  Point3 measurement(expected_uv.x(), expected_uv.y(), depth);
+
+  // Create DepthCamera3 with body_P_sensor
+  DepthCamera3<Cal3_S2> depth_camera(measurement, K, body_P_sensor);
+
+  // Evaluate error using body pose (not camera pose)
+  Point3 actual_error = depth_camera.project(body_pose_world, landmark);
+
+  // When landmark matches the measurement, error should be approximately zero
+  Vector3 expected_zero = Vector3::Zero();
+  EXPECT(assert_equal(expected_zero, Vector3(actual_error), 1e-6));
+}
+
+/* ************************************************************************* */
+TEST(DepthCamera3, ProjectWithBodyPSensorJacobians) {
+  // Create a body_P_sensor transform
+  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
+
+  // Body pose in world frame
+  Pose3 body_pose_world = level_pose;
+
+  // Camera pose in world frame
+  Pose3 camera_pose_world = body_pose_world.compose(body_P_sensor);
+  PinholeCamera<Cal3_S2> camera_with_transform(camera_pose_world, *K);
+
+  double depth = 3.0;
+  // landmark in camera frame: (5, 3, depth)
+  Point3 landmark_cam(5, 3, depth);
+  // Transform to world frame
+  Point3 landmark = camera_pose_world.transformFrom(landmark_cam);
+  Point2 expected_uv = camera_with_transform.project(landmark);
+  Point3 measurement(expected_uv.x(), expected_uv.y(), depth);
+
+  // Create DepthCamera3 with body_P_sensor
+  DepthCamera3<Cal3_S2> depth_camera(measurement, K, body_P_sensor);
+
+  // Compute analytic Jacobians (w.r.t. body pose)
+  Matrix H1, H2;
+  depth_camera.project(body_pose_world, landmark, H1, H2);
+
+  // Numerical Jacobians
+  auto f1 = [&](const Pose3& pose) {
+    return depth_camera.project(pose, landmark);
+  };
+  auto f2 = [&](const Point3& lm) {
+    return depth_camera.project(body_pose_world, lm);
+  };
+
+  Matrix H1_num = numericalDerivative11<Point3, Pose3>(f1, body_pose_world, 1e-6);
+  Matrix H2_num = numericalDerivative11<Point3, Point3>(f2, landmark, 1e-6);
+
+  EXPECT(assert_equal(H1_num, H1, 1e-5));
+  EXPECT(assert_equal(H2_num, H2, 1e-5));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
+/* ************************************************************************* */
+

gtsam/navigation/AHRSFactor.cpp

@@ -208,4 +208,62 @@ Rot3 AHRSFactor::predict(const Rot3& Ri, const Vector3& bias,
   return newPim.predict(Ri, bias);
 }
 
+
+gtsam::NonlinearFactor::shared_ptr AHRSPose3Factor::clone() const {
+  return std::static_pointer_cast<gtsam::NonlinearFactor>(
+      gtsam::NonlinearFactor::shared_ptr(new This(*this)));
+}
+
+AHRSPose3Factor::AHRSPose3Factor(gtsam::Key pose_i, gtsam::Key pose_j, gtsam::Key bias,
+                                 const PreintegratedAhrsMeasurements& pim)
+    : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, pose_j,
+           bias),
+      _PIM_(pim) {
+      }
+
+void AHRSPose3Factor::print(const std::string& s,
+                            const gtsam::KeyFormatter& keyFormatter) const {
+  cout << s << "AHRSPose3Factor(" << keyFormatter(this->key<1>()) << ","
+       << keyFormatter(this->key<2>()) << "," << keyFormatter(this->key<3>())
+       << ",";
+  _PIM_.print("  preintegrated measurements:");
+  noiseModel_->print("  noise model: ");
+}
+
+bool AHRSPose3Factor::equals(const NonlinearFactor& other, double tol) const {
+  const This* e = dynamic_cast<const This*>(&other);
+  return e != nullptr && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
+}
+
+//------------------------------------------------------------------------------
+Vector AHRSPose3Factor::evaluateError(const Pose3& Posei, const Pose3& Posej,
+                                 const Vector3& bias, OptionalMatrixType H1,
+                                 OptionalMatrixType H2,
+                                  OptionalMatrixType H3) const {
+  const Rot3 rot_i = Posei.rotation();
+  const Rot3 rot_j = Posej.rotation();
+
+  // Compute error and rotational Jacobians
+  Matrix3 Hr_i, Hr_j, Hb;
+  Vector error = _PIM_.computeError(rot_i, rot_j, bias,
+                                    H1 ? &Hr_i : nullptr,
+                                    H2 ? &Hr_j : nullptr,
+                                    H3 ? &Hb : nullptr);
+
+  // Lift 3x3 rotational Jacobians to 3x6 Pose3 Jacobians by zeroing translation
+  if (H1) {
+    H1->setZero(3, 6);
+    H1->block<3,3>(0,0) = Hr_i;
+  }
+  if (H2) {
+    H2->setZero(3, 6);
+    H2->block<3,3>(0,0) = Hr_j;
+  }
+  if (H3) {
+    *H3 = Hb;
+  }
+
+  return error;
+}
+
 }  // namespace gtsam