From 898fd9da0385e46e422e4bfa7549d90bc59d3b02 Mon Sep 17 00:00:00 2001 From: junlinp Date: Thu, 30 Oct 2025 22:08:49 +0800 Subject: [PATCH 01/15] AHRSFactor support Pose3. --- gtsam/navigation/AHRSFactor.cpp | 57 ++++++++++++++++ gtsam/navigation/AHRSFactor.h | 88 +++++++++++++++++++++++++ tests/testAHRSWithPose.cpp | 112 ++++++++++++++++++++++++++++++++ 3 files changed, 257 insertions(+) create mode 100644 tests/testAHRSWithPose.cpp diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 72513d1649..2b58f2f56e 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -208,4 +208,61 @@ 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::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(&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 diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 035c6206d7..a4db3a96ce 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -271,5 +271,93 @@ class GTSAM_EXPORT AHRSFactor : public NoiseModelFactorN { #endif }; // AHRSFactor +// +// +// +// +// +// +// +// + +class GTSAM_EXPORT AHRSPose3Factor : public NoiseModelFactorN { + typedef AHRSPose3Factor This; + typedef NoiseModelFactorN Base; + + PreintegratedAhrsMeasurements _PIM_; + + public: + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + /** Shorthand for a smart pointer to a factor */ +#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 + typedef typename std::shared_ptr shared_ptr; +#else + typedef std::shared_ptr shared_ptr; +#endif + + /** Default constructor - only use for serialization */ + AHRSPose3Factor() {} + + /** + * Constructor + * @param rot_i previous rot key + * @param rot_j current rot key + * @param bias previous bias key + * @param pim preintegrated measurements + */ + AHRSPose3Factor(Key pose_i, Key pose_j, Key bias, + const PreintegratedAhrsMeasurements& pim); + + ~AHRSPose3Factor() override {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override; + + /// print + void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + + /// equals + bool equals(const NonlinearFactor&, double tol = 1e-9) const override; + + /// Access the preintegrated measurements. + const PreintegratedAhrsMeasurements& preintegratedMeasurements() const { + return _PIM_; + } + + /** implement functions needed to derive from Factor */ + + /// vector of errors + Vector evaluateError(const Pose3& Ri, const Pose3& Rj, const Vector3& bias, + OptionalMatrixType H1, OptionalMatrixType H2, + OptionalMatrixType H3) const override; + + /// @deprecated constructor, but used in tests. + AHRSPose3Factor(gtsam::Key pose_i, gtsam::Key pose_j, gtsam::Key bias, + const PreintegratedAhrsMeasurements& pim, + const Vector3& omegaCoriolis, + const std::optional& body_P_sensor = {}); + + /// @deprecated static function, but used in tests. + static Pose3 predict(const Pose3& Ri, const Vector3& bias, + const PreintegratedAhrsMeasurements& pim, + const Vector3& omegaCoriolis, + const std::optional& body_P_sensor = {}); + + private: +#if GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + // NoiseModelFactor3 instead of NoiseModelFactorN for backward compatibility + ar& boost::serialization::make_nvp( + "NoiseModelFactor3", boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(_PIM_); + } +#endif +}; } // namespace gtsam diff --git a/tests/testAHRSWithPose.cpp b/tests/testAHRSWithPose.cpp new file mode 100644 index 0000000000..68a3aedb0b --- /dev/null +++ b/tests/testAHRSWithPose.cpp @@ -0,0 +1,112 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testAHRSWithPose.cpp + * @brief Unit tests for AHRSPose3Factor + */ + +#include + +#include +#include +#include +#include +#include + +using namespace gtsam; + +/* ************************************************************************* */ +TEST(AHRSPose3Factor, zeroErrorWhenConsistent) { + // Create preintegration params with small gyro noise + auto params = std::make_shared(); + params->gyroscopeCovariance = 1e-6 * I_3x3; + + // Zero bias used during preintegration + Vector3 biasHat = Vector3::Zero(); + PreintegratedAhrsMeasurements pim(params, biasHat); + + // Integrate a constant angular velocity around z for dt seconds + const double dt = 0.01; // 10 ms + const int steps = 100; // 1.0 s total + const Vector3 omega(0.0, 0.0, 1.0); // rad/s + for (int k = 0; k < steps; ++k) { + pim.integrateMeasurement(omega, dt); + } + + // Construct poses consistent with integrated rotation + const Rot3 Ri = Rot3(); + const Rot3 Rj = pim.predict(Ri, /*bias=*/Vector3::Zero()); + const Pose3 Posei(Ri, Point3(0, 0, 0)); + const Pose3 Posej(Rj, Point3(0, 0, 0)); + + // Build factor and evaluate error at consistent state + AHRSPose3Factor factor(/*pose_i*/ 1, /*pose_j*/ 2, /*bias*/ 3, pim); + Vector actual = factor.evaluateError(Posei, Posej, /*bias=*/Vector3::Zero()); + + EXPECT(assert_equal(Z_3x1, actual, 1e-9)); +} + +/* ************************************************************************* */ +TEST(AHRSPose3Factor, jacobiansAgreeWithNumerical) { + // Params with small gyro noise + auto params = std::make_shared(); + params->gyroscopeCovariance = 1e-6 * I_3x3; + + // Non-zero bias used during preintegration + Vector3 biasHat(0.01, -0.02, 0.03); + PreintegratedAhrsMeasurements pim(params, biasHat); + + // Integrate a short random motion + const double dt = 0.005; + const Vector3 omega1(0.2, -0.1, 0.3); + const Vector3 omega2(-0.05, 0.4, -0.2); + pim.integrateMeasurement(omega1, dt); + pim.integrateMeasurement(omega2, dt); + + // States to linearize at + const Pose3 Posei(Rot3::RzRyRx(0.1, -0.2, 0.05), Point3(0.0, 0.0, 0.0)); + const Pose3 Posej(Rot3::RzRyRx(-0.05, 0.1, -0.02), Point3(0.0, 0.0, 0.0)); + const Vector3 bias(0.02, -0.03, 0.01); + + // Factor + AHRSPose3Factor factor(/*pose_i*/ 1, /*pose_j*/ 2, /*bias*/ 3, pim); + + // Compute analytic Jacobians + Matrix H1, H2, H3; + factor.evaluateError(Posei, Posej, bias, H1, H2, H3); + + // Numerical Jacobians using GTSAM helpers + auto f1 = [&](const Pose3& Pi) { + return factor.evaluateError(Pi, Posej, bias); + }; + auto f2 = [&](const Pose3& Pj) { + return factor.evaluateError(Posei, Pj, bias); + }; + auto f3 = [&](const Vector3& b) { + return factor.evaluateError(Posei, Posej, b); + }; + + Matrix H1_num = numericalDerivative11(f1, Posei, 1e-6); + Matrix H2_num = numericalDerivative11(f2, Posej, 1e-6); + Matrix H3_num = numericalDerivative11(f3, bias, 1e-6); + + EXPECT(assert_equal(H1_num, H1, 1e-5)); + EXPECT(assert_equal(H2_num, H2, 1e-5)); + EXPECT(assert_equal(H3_num, H3, 1e-5)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From a35915037c16ac1006aba9105030db4dbb900003 Mon Sep 17 00:00:00 2001 From: junlinp Date: Fri, 31 Oct 2025 21:34:43 +0800 Subject: [PATCH 02/15] update interface for python --- CMakeLists.txt | 4 ++-- gtsam/navigation/navigation.i | 16 ++++++++++++++++ 2 files changed, 18 insertions(+), 2 deletions(-) mode change 100644 => 100755 CMakeLists.txt mode change 100644 => 100755 gtsam/navigation/navigation.i diff --git a/CMakeLists.txt b/CMakeLists.txt old mode 100644 new mode 100755 index c9dde1668d..7107763a4b --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,9 +5,9 @@ endif() # Set the version number for the library set (GTSAM_VERSION_MAJOR 4) -set (GTSAM_VERSION_MINOR 3) +set (GTSAM_VERSION_MINOR 4) set (GTSAM_VERSION_PATCH 0) -set (GTSAM_PRERELEASE_VERSION "a0") +set (GTSAM_PRERELEASE_VERSION "") math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") # Set the version string for the library. diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i old mode 100644 new mode 100755 index 7944c8d905..1707b7c316 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -310,6 +310,9 @@ class PreintegratedAhrsMeasurements { void integrateMeasurement(gtsam::Vector measuredOmega, double deltaT); void resetIntegration() ; + // Predict orientation at time j given orientation and bias at time i + gtsam::Rot3 predict(const gtsam::Rot3& Ri, gtsam::Vector bias) const; + // enable serialization functionality void serialize() const; }; @@ -337,6 +340,19 @@ virtual class AHRSFactor : gtsam::NonlinearFactor { const gtsam::Pose3& body_P_sensor); }; +virtual class AHRSPose3Factor : gtsam::NonlinearFactor { + AHRSPose3Factor(gtsam::Key pose_i, gtsam::Key pose_j, gtsam::Key bias, + const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements); + + // Standard Interface + gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; + gtsam::Vector evaluateError(const gtsam::Pose3& pose_i, const gtsam::Pose3& pose_j, + gtsam::Vector bias) const; + + // enable serialization functionality + void serialize() const; +}; + #include virtual class Rot3AttitudeFactor : gtsam::NoiseModelFactor { Rot3AttitudeFactor(gtsam::Key key, const gtsam::Unit3& nRef, const gtsam::noiseModel::Diagonal* model, From 474872a31010b6c64a97f4dc9350a43d24cb5591 Mon Sep 17 00:00:00 2001 From: junlinp Date: Sun, 2 Nov 2025 20:10:22 +0800 Subject: [PATCH 03/15] minor fixed. --- gtsam/navigation/AHRSFactor.h | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index a4db3a96ce..1aaf69134c 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -271,31 +271,19 @@ class GTSAM_EXPORT AHRSFactor : public NoiseModelFactorN { #endif }; // AHRSFactor -// -// -// -// -// -// -// -// - class GTSAM_EXPORT AHRSPose3Factor : public NoiseModelFactorN { typedef AHRSPose3Factor This; typedef NoiseModelFactorN Base; - PreintegratedAhrsMeasurements _PIM_; - public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; - /** Shorthand for a smart pointer to a factor */ #if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 typedef typename std::shared_ptr shared_ptr; #else - typedef std::shared_ptr shared_ptr; + typedef std::shared_ptr shared_ptr; #endif /** Default constructor - only use for serialization */ From 839aaa8a43c666eab9dce40377495f5edb127fe7 Mon Sep 17 00:00:00 2001 From: junlinp Date: Tue, 4 Nov 2025 14:25:00 +0800 Subject: [PATCH 04/15] reset CMakeLists.txt --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) mode change 100755 => 100644 CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt old mode 100755 new mode 100644 index 7107763a4b..c9dde1668d --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,9 +5,9 @@ endif() # Set the version number for the library set (GTSAM_VERSION_MAJOR 4) -set (GTSAM_VERSION_MINOR 4) +set (GTSAM_VERSION_MINOR 3) set (GTSAM_VERSION_PATCH 0) -set (GTSAM_PRERELEASE_VERSION "") +set (GTSAM_PRERELEASE_VERSION "a0") math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") # Set the version string for the library. From a637c0c2abfe2cec0f67713a64f3b972f4651b64 Mon Sep 17 00:00:00 2001 From: junlinp Date: Wed, 5 Nov 2025 09:55:08 +0800 Subject: [PATCH 05/15] Update gtsam/navigation/AHRSFactor.h Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- gtsam/navigation/AHRSFactor.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 1aaf69134c..1907709ee5 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -291,8 +291,8 @@ class GTSAM_EXPORT AHRSPose3Factor : public NoiseModelFactorN Date: Wed, 5 Nov 2025 10:48:20 +0800 Subject: [PATCH 06/15] update documents and api arugement naming. --- gtsam/navigation/AHRSFactor.cpp | 5 ++-- gtsam/navigation/AHRSFactor.h | 46 +++++++++++++++++++++++++++++++-- 2 files changed, 47 insertions(+), 4 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 2b58f2f56e..bfcf274d46 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -216,9 +216,10 @@ gtsam::NonlinearFactor::shared_ptr AHRSPose3Factor::clone() const { 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, + : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, pose_j, bias), - _PIM_(pim) {} + _PIM_(pim) { + } void AHRSPose3Factor::print(const std::string& s, const gtsam::KeyFormatter& keyFormatter) const { diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index 1907709ee5..dda33924a9 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -57,6 +57,7 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements ///< *measurementCovariance*) friend class AHRSFactor; + friend class AHRSPose3Factor; public: /// Default constructor, only for serialization and wrappers @@ -272,6 +273,35 @@ class GTSAM_EXPORT AHRSFactor : public NoiseModelFactorN { }; // AHRSFactor + +/** + * An AHRSPose3Factor is a three-way factor that is based on the preintegrated + * gyroscope measurements. + * + * @section math_notes Mathematical Formulation + * + * The factor relates the orientation of pose at two time steps, \f$ R_i \f$ and \f$ R_j \f$, + * and the gyroscope bias \f$ b_g \f$. The error function is given by: + * \f[ + * e(R_i, R_j, b_g) = \text{Log}\left( (\Delta \tilde{R}_{ij}(b_g))^{-1} R_i^{-1} R_j \right) + * \f] + * where \f$ \Delta \tilde{R}_{ij}(b_g) \f$ is the preintegrated rotation corrected + * for the current estimate of the gyroscope bias, and \f$ \text{Log}(\cdot) \f$ is + * the logarithmic map from SO(3) to \f$ \mathbb{R}^3 \f$. + * + * The preintegrated rotation \f$ \Delta R_{ij} \f$ is calculated as: + * \f[ + * \Delta R_{ij} = \prod_{k=i}^{j-1} \text{Exp}((\omega_k - \hat{b}_g) \Delta t) + * \f] + * where \f$ \hat{b}_g \f$ is the bias estimate used for preintegration. The + * bias-corrected preintegrated rotation \f$ \Delta \tilde{R}_{ij}(b_g) \f$ is + * then approximated using a first-order expansion: + * \f[ + * \Delta \tilde{R}_{ij}(b_g) \approx \Delta R_{ij} \text{Exp}(J_b (b_g - \hat{b}_g)) + * \f] + * where \f$ J_b \f$ is the Jacobian of the preintegrated rotation with respect + * to the gyroscope bias. + */ class GTSAM_EXPORT AHRSPose3Factor : public NoiseModelFactorN { typedef AHRSPose3Factor This; typedef NoiseModelFactorN Base; @@ -297,7 +327,9 @@ class GTSAM_EXPORT AHRSPose3Factor : public NoiseModelFactorN Date: Fri, 7 Nov 2025 10:22:42 +0800 Subject: [PATCH 07/15] minor --- gtsam/navigation/AHRSFactor.h | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.h b/gtsam/navigation/AHRSFactor.h index dda33924a9..a770c7554c 100644 --- a/gtsam/navigation/AHRSFactor.h +++ b/gtsam/navigation/AHRSFactor.h @@ -327,9 +327,7 @@ class GTSAM_EXPORT AHRSPose3Factor : public NoiseModelFactorN Date: Sun, 9 Nov 2025 15:23:34 +0800 Subject: [PATCH 08/15] add DepthCamera3 --- gtsam_unstable/geometry/DepthCamera3.h | 122 +++++++++++++++++ .../geometry/tests/testDepthCamera3.cpp | 73 ++++++++++ gtsam_unstable/slam/DepthFactor3.h | 125 ++++++++++++++++++ .../slam/tests/testDepthFactor3.cpp | 124 +++++++++++++++++ 4 files changed, 444 insertions(+) create mode 100644 gtsam_unstable/geometry/DepthCamera3.h create mode 100644 gtsam_unstable/geometry/tests/testDepthCamera3.cpp create mode 100644 gtsam_unstable/slam/DepthFactor3.h create mode 100644 gtsam_unstable/slam/tests/testDepthFactor3.cpp diff --git a/gtsam_unstable/geometry/DepthCamera3.h b/gtsam_unstable/geometry/DepthCamera3.h new file mode 100644 index 0000000000..800e9529ac --- /dev/null +++ b/gtsam_unstable/geometry/DepthCamera3.h @@ -0,0 +1,122 @@ + +/** + * @file DepthCamera3.h + * @brief Depth Camera. + * @author junlinp + * @date Nov 9, 2025 + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#if GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#endif + +namespace gtsam { + +/** + * A pinhole camera class that has a Pose3 and a Calibration. + * @ingroup geometry + * \nosubgrouping + */ +template +class DepthCamera3 { +private: + Point3 measurement_; ///< The depth measurement (u, v, depth) + std::shared_ptr k_; ///< The fixed camera calibration + +public: + + /// @name Standard Constructors + /// @{ + + /** default constructor */ + DepthCamera3() {} + + /** constructor with pose and calibration */ + DepthCamera3(const Point3& measurement, const std::shared_ptr& k) : + measurement_{measurement}, k_(k) {} + + /// @} + /// @name Standard Interface + /// @{ + + virtual ~DepthCamera3() {} + + + /// return calibration + inline const std::shared_ptr& 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: "); + } + + + /** project a point from world InvDepth parameterization to the image + * @param pw is a point in the world coordinate + * @param H1 is the jacobian w.r.t. [pose3 calibration] + * @param H2 is the jacobian w.r.t. inv_depth_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; + + // Compute predicted in camera frame + Point3 predicted_cam = pose.transformTo(landmark, H1, H2); + + // Negate Jacobians because we're subtracting predicted_cam + if (H1) *H1 = (*H1); + if (H2) *H2 = (*H2); + + return predicted_cam - observation; + } + +private: + + /// @} + /// @name Advanced Interface + /// @{ + +#if GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(measurement_); + ar & BOOST_SERIALIZATION_NVP(k_); + } +#endif + /// @} +}; // \class InvDepthCamera +} // \namespace gtsam + + + + + diff --git a/gtsam_unstable/geometry/tests/testDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testDepthCamera3.cpp new file mode 100644 index 0000000000..3078745d3c --- /dev/null +++ b/gtsam_unstable/geometry/tests/testDepthCamera3.cpp @@ -0,0 +1,73 @@ + +/* + * testDepthFactor.cpp + * + * Created on: Nov 9, 2025 + * Author: junlinp + */ + +#include + +#include +#include +#include +#include + +#include + +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 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 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 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(f1, level_pose, 1e-6); + Matrix H2_num = numericalDerivative11(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);} +/* ************************************************************************* */ diff --git a/gtsam_unstable/slam/DepthFactor3.h b/gtsam_unstable/slam/DepthFactor3.h new file mode 100644 index 0000000000..c9167ed0ea --- /dev/null +++ b/gtsam_unstable/slam/DepthFactor3.h @@ -0,0 +1,125 @@ + +/** + * @file DepthFactor3.h + * @brief Depth Factor for 3D landmarks with depth measurements. + * A factor that relates a camera pose, a 3D landmark, and a depth measurement + * (u, v, depth) in pixel coordinates. + * @author junlinp + * @date Nov 9, 2025 + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * Binary factor representing a visual measurement with depth + * @tparam POSE Camera pose type (typically Pose3) + * @tparam LANDMARK Landmark type (typically Point3) + */ +template +class DepthFactor3: public NoiseModelFactorN { +protected: + + // Keep a copy of measurement and calibration for I/O + Point3 measured_; ///< 3D measurement (u, v, depth) in pixel coordinates + std::shared_ptr K_; ///< shared pointer to calibration object + +public: + + /// shorthand for base class type + typedef NoiseModelFactorN Base; + + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + /// shorthand for this class + typedef DepthFactor3 This; + + /// shorthand for a smart pointer to a factor + typedef std::shared_ptr shared_ptr; + + /// Default constructor + DepthFactor3() : + measured_(0.0, 0.0, 0.0), K_(new Cal3_S2(444, 555, 666, 777, 888)) { + } + + /** + * Constructor + * @param measured is the 3D measurement (u, v, depth) in pixel coordinates + * @param model is the noise model + * @param poseKey is the index of the camera pose + * @param landmarkKey is the index of the landmark + * @param K shared pointer to the constant calibration + */ + DepthFactor3(const Point3& measured, const SharedNoiseModel& model, + const Key poseKey, Key landmarkKey, const Cal3_S2::shared_ptr& K) : + Base(model, poseKey, landmarkKey), measured_(measured), K_(K) {} + + /** Virtual destructor */ + ~DepthFactor3() override {} + + /** + * print + * @param s optional string naming the factor + * @param keyFormatter optional formatter useful for printing Symbols + */ + void print(const std::string& s = "DepthFactor3", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + Base::print(s, keyFormatter); + std::cout << " measurement: " << measured_.transpose() << std::endl; + if (K_) K_->print(" calibration: "); + } + + /// equals + bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { + const This *e = dynamic_cast(&p); + return e && Base::equals(p, tol) && + traits::Equals(this->measured_, e->measured_, tol) && + this->K_->equals(*e->K_, tol); + } + + /// Evaluate error h(x)-z and optionally derivatives + Vector evaluateError(const POSE& pose, const LANDMARK& landmark, + OptionalMatrixType H1, OptionalMatrixType H2) const override { + try { + DepthCamera3 camera(measured_, K_); + return camera.project(pose, landmark, H1, H2); + } catch( CheiralityException& e) { + if (H1) *H1 = Matrix::Zero(3, 6); + if (H2) *H2 = Matrix::Zero(3, 3); + std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) << + " moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl; + return Vector::Ones(3) * 2.0 * K_->fx(); + } + } + + /** return the measurement */ + const Point3& measurement() const { + return measured_; + } + + /** return the calibration object */ + inline const Cal3_S2::shared_ptr calibration() const { + return K_; + } + +private: + +#if GTSAM_ENABLE_BOOST_SERIALIZATION /// + /// Serialization function + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); + ar & BOOST_SERIALIZATION_NVP(measured_); + ar & BOOST_SERIALIZATION_NVP(K_); + } +#endif +}; +} // \ namespace gtsam + diff --git a/gtsam_unstable/slam/tests/testDepthFactor3.cpp b/gtsam_unstable/slam/tests/testDepthFactor3.cpp new file mode 100644 index 0000000000..d2d5c4f59a --- /dev/null +++ b/gtsam_unstable/slam/tests/testDepthFactor3.cpp @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------------- + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + * See LICENSE for the license information + * -------------------------------------------------------------------------- */ + +/** + * @file testDepthFactor3.cpp + * @brief Unit tests for depth factor with 3D landmarks + * + * @author junlinp + * @date Nov 9, 2025 + */ + +#include + +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); +static SharedNoiseModel sigma(noiseModel::Unit::Create(3)); + +// camera pose at (0,0,1) looking straight along the z-axis. +Pose3 level_pose = Pose3(Rot3::Ypr(0, 0, 0), gtsam::Point3(0,0,1)); +PinholeCamera level_camera(level_pose, *K); + +typedef DepthFactor3 DepthFactor; +typedef NonlinearEquality PoseConstraint; + +Vector factorError(const Pose3& pose, const Point3& landmark, + const DepthFactor& factor) { + return factor.evaluateError(pose, landmark); +} + +/* ************************************************************************* */ +TEST(DepthFactor3, zeroErrorWhenConsistent) { + 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 + + DepthFactor factor(measurement, sigma, Symbol('x',1), Symbol('l',1), K); + + Vector actual_error = factor.evaluateError(level_pose, landmark); + + // When landmark matches the measurement, error should be approximately zero + EXPECT(assert_equal(Vector3::Zero(), actual_error, 1e-6)); +} + +/* ************************************************************************* */ +TEST(DepthFactor3, Jacobians) { + 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); + + DepthFactor factor(measurement, sigma, Symbol('x',1), Symbol('l',1), K); + + // Compute analytic Jacobians + Matrix H1, H2; + factor.evaluateError(level_pose, landmark, H1, H2); + + // Numerical Jacobians + auto f1 = [&](const Pose3& pose) { + return factor.evaluateError(pose, landmark); + }; + auto f2 = [&](const Point3& lm) { + return factor.evaluateError(level_pose, lm); + }; + + Matrix H1_num = numericalDerivative11(f1, level_pose, 1e-6); + Matrix H2_num = numericalDerivative11(f2, landmark, 1e-6); + + EXPECT(assert_equal(H1_num, H1, 1e-5)); + EXPECT(assert_equal(H2_num, H2, 1e-5)); +} + +/* ************************************************************************* */ +TEST(DepthFactor3, optimize) { + // landmark 5 meters in front of camera (camera center at (0,0,1), looking along +z) + Point3 landmark(0, 0, level_pose.z() + 5.0); // 5 meters in front + double depth = 5.0; + + // get expected projection using pinhole camera + Point2 expected_uv = level_camera.project(landmark); + Point3 measurement(expected_uv.x(), expected_uv.y(), depth); + + gtsam::NonlinearFactorGraph graph; + Values initial; + + graph.emplace_shared(measurement, sigma, + Symbol('x',1), Symbol('l',1), K); + graph.emplace_shared(Symbol('x', 1), level_pose); + + // Initialize with slightly perturbed landmark + Point3 initial_landmark = Point3(0.1, 0.1, 0.1); + initial.insert(Symbol('x',1), level_pose); + initial.insert(Symbol('l',1), initial_landmark); + + LevenbergMarquardtParams lmParams; + Values result = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize(); + + // Verify that the landmark was optimized to the correct position + Point3 result_landmark = result.at(Symbol('l',1)); + EXPECT(assert_equal(landmark, result_landmark, 1e-6)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */ + From 55286a5ec58ca49aa737bfebb7f000a786e275d4 Mon Sep 17 00:00:00 2001 From: junlinp Date: Sun, 9 Nov 2025 15:37:33 +0800 Subject: [PATCH 09/15] export python --- gtsam_unstable/gtsam_unstable.i | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 10b196204d..f94c961c2f 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -162,6 +162,21 @@ class BearingS2 { void serializable() const; // enabling serialization functionality }; +#include +template +class DepthCamera3 { + DepthCamera3(); + DepthCamera3(const gtsam::Point3& measurement, const CALIBRATION::shared_ptr& k); + + 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 DepthCamera3Cal3_S2; #include class SimWall2D { From 47bb7e6a40744f635127f3c668bea4bf3426373c Mon Sep 17 00:00:00 2001 From: junlinp Date: Mon, 10 Nov 2025 10:18:21 +0800 Subject: [PATCH 10/15] add body_P_sensor_ for DepthCamera3 --- gtsam_unstable/geometry/DepthCamera3.h | 51 ++++++-- .../geometry/tests/testDepthCamera3.cpp | 74 +++++++++++ gtsam_unstable/gtsam_unstable.i | 18 +++ gtsam_unstable/slam/DepthFactor3.h | 21 +++- .../slam/tests/testDepthFactor3.cpp | 116 ++++++++++++++++++ 5 files changed, 268 insertions(+), 12 deletions(-) diff --git a/gtsam_unstable/geometry/DepthCamera3.h b/gtsam_unstable/geometry/DepthCamera3.h index 800e9529ac..74d1d8a2de 100644 --- a/gtsam_unstable/geometry/DepthCamera3.h +++ b/gtsam_unstable/geometry/DepthCamera3.h @@ -9,6 +9,7 @@ #pragma once #include +#include #include #include #include @@ -33,6 +34,7 @@ class DepthCamera3 { private: Point3 measurement_; ///< The depth measurement (u, v, depth) std::shared_ptr k_; ///< The fixed camera calibration + std::optional body_P_sensor_; ///< The pose of the sensor in the body frame public: @@ -46,6 +48,11 @@ class DepthCamera3 { DepthCamera3(const Point3& measurement, const std::shared_ptr& k) : measurement_{measurement}, k_(k) {} + /** constructor with pose, calibration, and body_P_sensor */ + DepthCamera3(const Point3& measurement, const std::shared_ptr& k, + const std::optional& body_P_sensor) : + measurement_{measurement}, k_(k), body_P_sensor_(body_P_sensor) {} + /// @} /// @name Standard Interface /// @{ @@ -61,13 +68,17 @@ class DepthCamera3 { 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 pw is a point in the world coordinate - * @param H1 is the jacobian w.r.t. [pose3 calibration] - * @param H2 is the jacobian w.r.t. inv_depth_landmark + * @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 = {}, @@ -87,12 +98,35 @@ class DepthCamera3 { Point3 observation = inv_intrinsics * observation_raw; - // Compute predicted in camera frame - Point3 predicted_cam = pose.transformTo(landmark, H1, H2); + // 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; + } - // Negate Jacobians because we're subtracting predicted_cam - if (H1) *H1 = (*H1); - if (H2) *H2 = (*H2); + // 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; } @@ -110,6 +144,7 @@ class DepthCamera3 { 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 /// @} diff --git a/gtsam_unstable/geometry/tests/testDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testDepthCamera3.cpp index 3078745d3c..3c8f2067e0 100644 --- a/gtsam_unstable/geometry/tests/testDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testDepthCamera3.cpp @@ -68,6 +68,80 @@ TEST(DepthCamera3, ProjectWithJacobians) { 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 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 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 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 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(f1, body_pose_world, 1e-6); + Matrix H2_num = numericalDerivative11(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);} /* ************************************************************************* */ diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index f94c961c2f..bd17afb5d6 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -167,6 +167,7 @@ template 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; @@ -636,6 +637,23 @@ virtual class InvDepthFactorVariant3b : gtsam::NoiseModelFactor { InvDepthFactorVariant3b(gtsam::Key poseKey1, gtsam::Key poseKey2, gtsam::Key landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model); }; +#include +template +virtual class DepthFactor3 : gtsam::NoiseModelFactor { + DepthFactor3(); + DepthFactor3(const gtsam::Point3& measured, const gtsam::noiseModel::Base* model, + gtsam::Key poseKey, gtsam::Key landmarkKey, const gtsam::Cal3_S2::shared_ptr& K); + DepthFactor3(const gtsam::Point3& measured, const gtsam::noiseModel::Base* model, + gtsam::Key poseKey, gtsam::Key landmarkKey, const gtsam::Cal3_S2::shared_ptr& K, const gtsam::Pose3& body_P_sensor); + + void print(string s = "DepthFactor3") const; + const gtsam::Point3& measurement() const; + const gtsam::Cal3_S2::shared_ptr calibration() const; + + void serializable() const; // enabling serialization functionality +}; +typedef gtsam::DepthFactor3 DepthFactor3Pose3Point3; + #include class Mechanization_bRn2 { diff --git a/gtsam_unstable/slam/DepthFactor3.h b/gtsam_unstable/slam/DepthFactor3.h index c9167ed0ea..c558d89ff6 100644 --- a/gtsam_unstable/slam/DepthFactor3.h +++ b/gtsam_unstable/slam/DepthFactor3.h @@ -10,7 +10,9 @@ #pragma once +#include #include +#include #include #include @@ -28,6 +30,7 @@ class DepthFactor3: public NoiseModelFactorN { // Keep a copy of measurement and calibration for I/O Point3 measured_; ///< 3D measurement (u, v, depth) in pixel coordinates std::shared_ptr K_; ///< shared pointer to calibration object + std::optional body_P_sensor_; ///< The pose of the sensor in the body frame public: @@ -55,10 +58,13 @@ class DepthFactor3: public NoiseModelFactorN { * @param poseKey is the index of the camera pose * @param landmarkKey is the index of the landmark * @param K shared pointer to the constant calibration + * @param body_P_sensor is the transform from body to sensor frame (default identity) */ DepthFactor3(const Point3& measured, const SharedNoiseModel& model, - const Key poseKey, Key landmarkKey, const Cal3_S2::shared_ptr& K) : - Base(model, poseKey, landmarkKey), measured_(measured), K_(K) {} + const Key poseKey, Key landmarkKey, const Cal3_S2::shared_ptr& K, + const std::optional& body_P_sensor = {}) : + Base(model, poseKey, landmarkKey), measured_(measured), K_(K), + body_P_sensor_(body_P_sensor) {} /** Virtual destructor */ ~DepthFactor3() override {} @@ -73,6 +79,9 @@ class DepthFactor3: public NoiseModelFactorN { Base::print(s, keyFormatter); std::cout << " measurement: " << measured_.transpose() << std::endl; if (K_) K_->print(" calibration: "); + if (body_P_sensor_) { + body_P_sensor_->print(" body_P_sensor: "); + } } /// equals @@ -80,14 +89,17 @@ class DepthFactor3: public NoiseModelFactorN { const This *e = dynamic_cast(&p); return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol) && - this->K_->equals(*e->K_, tol); + this->K_->equals(*e->K_, tol) && + ((!body_P_sensor_ && !e->body_P_sensor_) || + (body_P_sensor_ && e->body_P_sensor_ && + body_P_sensor_->equals(*e->body_P_sensor_, tol))); } /// Evaluate error h(x)-z and optionally derivatives Vector evaluateError(const POSE& pose, const LANDMARK& landmark, OptionalMatrixType H1, OptionalMatrixType H2) const override { try { - DepthCamera3 camera(measured_, K_); + DepthCamera3 camera(measured_, K_, body_P_sensor_); return camera.project(pose, landmark, H1, H2); } catch( CheiralityException& e) { if (H1) *H1 = Matrix::Zero(3, 6); @@ -118,6 +130,7 @@ class DepthFactor3: public NoiseModelFactorN { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); ar & BOOST_SERIALIZATION_NVP(K_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } #endif }; diff --git a/gtsam_unstable/slam/tests/testDepthFactor3.cpp b/gtsam_unstable/slam/tests/testDepthFactor3.cpp index d2d5c4f59a..6f0f1561cf 100644 --- a/gtsam_unstable/slam/tests/testDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testDepthFactor3.cpp @@ -118,6 +118,122 @@ TEST(DepthFactor3, optimize) { EXPECT(assert_equal(landmark, result_landmark, 1e-6)); } +/* ************************************************************************* */ +TEST(DepthFactor3, zeroErrorWhenConsistentWithBodyPSensor) { + // 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 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 factor with body_P_sensor + DepthFactor factor(measurement, sigma, Symbol('x',1), Symbol('l',1), K, body_P_sensor); + + // Evaluate error using body pose (not camera pose) + Vector actual_error = factor.evaluateError(body_pose_world, landmark); + + // When landmark matches the measurement, error should be approximately zero + EXPECT(assert_equal(Vector3::Zero(), actual_error, 1e-6)); +} + +/* ************************************************************************* */ +TEST(DepthFactor3, JacobiansWithBodyPSensor) { + // 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 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 factor with body_P_sensor + DepthFactor factor(measurement, sigma, Symbol('x',1), Symbol('l',1), K, body_P_sensor); + + // Compute analytic Jacobians (w.r.t. body pose) + Matrix H1, H2; + factor.evaluateError(body_pose_world, landmark, H1, H2); + + // Numerical Jacobians + auto f1 = [&](const Pose3& pose) { + return factor.evaluateError(pose, landmark); + }; + auto f2 = [&](const Point3& lm) { + return factor.evaluateError(body_pose_world, lm); + }; + + Matrix H1_num = numericalDerivative11(f1, body_pose_world, 1e-6); + Matrix H2_num = numericalDerivative11(f2, landmark, 1e-6); + + EXPECT(assert_equal(H1_num, H1, 1e-5)); + EXPECT(assert_equal(H2_num, H2, 1e-5)); +} + +/* ************************************************************************* */ +TEST(DepthFactor3, optimizeWithBodyPSensor) { + // 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 camera_with_transform(camera_pose_world, *K); + + double depth = 5.0; + // landmark 5 meters in front of camera (in camera frame: (0, 0, depth)) + Point3 landmark_cam(0, 0, depth); + // Transform to world frame + Point3 landmark = camera_pose_world.transformFrom(landmark_cam); + + // get expected projection using pinhole camera + Point2 expected_uv = camera_with_transform.project(landmark); + Point3 measurement(expected_uv.x(), expected_uv.y(), depth); + + gtsam::NonlinearFactorGraph graph; + Values initial; + + // Create factor with body_P_sensor + graph.emplace_shared(measurement, sigma, + Symbol('x',1), Symbol('l',1), K, body_P_sensor); + graph.emplace_shared(Symbol('x', 1), body_pose_world); + + // Initialize with slightly perturbed landmark + Point3 initial_landmark = landmark + Point3(0.1, 0.1, 0.1); + initial.insert(Symbol('x',1), body_pose_world); + initial.insert(Symbol('l',1), initial_landmark); + + LevenbergMarquardtParams lmParams; + Values result = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize(); + + // Verify that the landmark was optimized to the correct position + Point3 result_landmark = result.at(Symbol('l',1)); + EXPECT(assert_equal(landmark, result_landmark, 1e-6)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ From 54b2d6da2613ee4d28af6286f638d1277c454ab5 Mon Sep 17 00:00:00 2001 From: junlinp Date: Mon, 10 Nov 2025 11:43:18 +0800 Subject: [PATCH 11/15] update unit test --- .../slam/tests/testDepthFactor3.cpp | 83 +++++++++++++++++++ 1 file changed, 83 insertions(+) diff --git a/gtsam_unstable/slam/tests/testDepthFactor3.cpp b/gtsam_unstable/slam/tests/testDepthFactor3.cpp index 6f0f1561cf..093bd9cfc5 100644 --- a/gtsam_unstable/slam/tests/testDepthFactor3.cpp +++ b/gtsam_unstable/slam/tests/testDepthFactor3.cpp @@ -24,12 +24,14 @@ #include #include +#include using namespace std; using namespace gtsam; static Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); static SharedNoiseModel sigma(noiseModel::Unit::Create(3)); +static SharedNoiseModel sigma_pose(noiseModel::Isotropic::Sigma(6, 1e-6)); // camera pose at (0,0,1) looking straight along the z-axis. Pose3 level_pose = Pose3(Rot3::Ypr(0, 0, 0), gtsam::Point3(0,0,1)); @@ -234,6 +236,87 @@ TEST(DepthFactor3, optimizeWithBodyPSensor) { EXPECT(assert_equal(landmark, result_landmark, 1e-6)); } +TEST(DepthFactor3, optimizeWithBodyPSensor_ba) { + // Create a body_P_sensor transform + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.0, 0.0, 0.0)); + + // Define body poses in world frame + Pose3 ground_truth_body1_pose = Pose3(Rot3::RzRyRx(0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); + Pose3 ground_truth_body2_pose = Pose3(Rot3::RzRyRx(0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)); + std::vector ground_truth_body_poses = { + ground_truth_body1_pose, + ground_truth_body2_pose, + }; + + // Compute camera poses from body poses + std::vector ground_truth_camera_poses; + for (const auto& body_pose : ground_truth_body_poses) { + ground_truth_camera_poses.push_back(body_pose.compose(body_P_sensor)); + } + + std::vector landmarks = { + Point3(5, 0, 0), + Point3(6, 0, 1), + Point3(7, 1, 0), + Point3(8, 0, 1), + }; + + gtsam::NonlinearFactorGraph graph; + Values initial; + + // Insert body poses into initial values + for (size_t j = 0; j < ground_truth_body_poses.size(); j++) { + Key body_key = Symbol('x', j); + // Use ground truth as initial (or add very small noise if needed) + Pose3 noisy_body_pose = ground_truth_body_poses[j].compose(Pose3(Rot3::Ypr(0.001, 0.001, 0.001), Point3(0.001, 0.001, 0.001))); + initial.insert(body_key, noisy_body_pose); + if (j == 0) { + graph.emplace_shared>(body_key, ground_truth_body_poses[j], sigma_pose); + } + } + + for (size_t i = 0; i < landmarks.size(); i++) { + const auto& landmark = landmarks[i]; + Key landmark_key = Symbol('l', i); + for (size_t j = 0; j < ground_truth_camera_poses.size(); j++) { + Key body_key = Symbol('x', j); + const auto& camera_pose = ground_truth_camera_poses[j]; + + // Create camera for projection + PinholeCamera camera(camera_pose, *K); + + // Project landmark to get pixel coordinates + Point2 expected_uv = camera.project(landmark); + + // Compute depth in camera frame + Point3 landmark_cam = camera_pose.transformTo(landmark); + double depth = landmark_cam.z(); + + Point3 measurement(expected_uv.x(), expected_uv.y(), depth); + + // Create factor with body_P_sensor (using body key, not camera key) + graph.emplace_shared(measurement, sigma, body_key, landmark_key, K, body_P_sensor); + } + Point3 noised_landmark = landmark + Point3(0.001, 0.001, 0.001); + initial.insert(landmark_key, noised_landmark); + } + + LevenbergMarquardtParams lmParams; + Values result = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize(); + + // Verify that the landmark was optimized to the correct position + for (size_t i = 0; i < landmarks.size(); i++) { + Point3 result_landmark = result.at(Symbol('l', i)); + EXPECT(assert_equal(landmarks[i], result_landmark, 1e-6)); + } + + // Verify that the body poses were optimized to the correct position + for (size_t i = 0; i < ground_truth_body_poses.size(); i++) { + Key body_key = Symbol('x', i); + Pose3 result_body_pose = result.at(body_key); + EXPECT(assert_equal(ground_truth_body_poses[i], result_body_pose, 1e-6)); + } +} /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ From d97f23dee64c18dced3f43a2b7ea8968eeb51dab Mon Sep 17 00:00:00 2001 From: junlinp Date: Mon, 10 Nov 2025 13:58:17 +0800 Subject: [PATCH 12/15] move from unstable to gtsam --- .../geometry/DepthCamera3.h | 6 +--- gtsam/geometry/geometry.i | 17 ++++++++++ .../geometry/tests/testDepthCamera3.cpp | 3 +- {gtsam_unstable => gtsam}/slam/DepthFactor3.h | 2 +- gtsam/slam/slam.i | 17 ++++++++++ .../slam/tests/testDepthFactor3.cpp | 2 +- gtsam_unstable/gtsam_unstable.i | 32 ------------------- 7 files changed, 39 insertions(+), 40 deletions(-) rename {gtsam_unstable => gtsam}/geometry/DepthCamera3.h (99%) rename {gtsam_unstable => gtsam}/geometry/tests/testDepthCamera3.cpp (99%) rename {gtsam_unstable => gtsam}/slam/DepthFactor3.h (98%) rename {gtsam_unstable => gtsam}/slam/tests/testDepthFactor3.cpp (99%) diff --git a/gtsam_unstable/geometry/DepthCamera3.h b/gtsam/geometry/DepthCamera3.h similarity index 99% rename from gtsam_unstable/geometry/DepthCamera3.h rename to gtsam/geometry/DepthCamera3.h index 74d1d8a2de..7482b24c60 100644 --- a/gtsam_unstable/geometry/DepthCamera3.h +++ b/gtsam/geometry/DepthCamera3.h @@ -148,10 +148,6 @@ class DepthCamera3 { } #endif /// @} -}; // \class InvDepthCamera +}; // \class DepthCamera3 } // \namespace gtsam - - - - diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 0c30f70e5e..6baf04c7f8 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1312,6 +1312,23 @@ class PinholeCamera { typedef gtsam::PinholeCamera PinholeCameraCal3_S2; typedef gtsam::PinholeCamera PinholeCameraCal3DS2; typedef gtsam::PinholeCamera PinholeCameraCal3Unified; + +#include +template +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 DepthCamera3Cal3_S2; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; typedef gtsam::PinholeCamera PinholeCameraCal3f; typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; diff --git a/gtsam_unstable/geometry/tests/testDepthCamera3.cpp b/gtsam/geometry/tests/testDepthCamera3.cpp similarity index 99% rename from gtsam_unstable/geometry/tests/testDepthCamera3.cpp rename to gtsam/geometry/tests/testDepthCamera3.cpp index 3c8f2067e0..2b474ca2e0 100644 --- a/gtsam_unstable/geometry/tests/testDepthCamera3.cpp +++ b/gtsam/geometry/tests/testDepthCamera3.cpp @@ -13,7 +13,7 @@ #include #include -#include +#include using namespace std; using namespace gtsam; @@ -145,3 +145,4 @@ TEST(DepthCamera3, ProjectWithBodyPSensorJacobians) { /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ + diff --git a/gtsam_unstable/slam/DepthFactor3.h b/gtsam/slam/DepthFactor3.h similarity index 98% rename from gtsam_unstable/slam/DepthFactor3.h rename to gtsam/slam/DepthFactor3.h index c558d89ff6..4a11b651e9 100644 --- a/gtsam_unstable/slam/DepthFactor3.h +++ b/gtsam/slam/DepthFactor3.h @@ -13,8 +13,8 @@ #include #include #include +#include #include -#include namespace gtsam { diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 0ccff908a6..fb744b11e6 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -102,6 +102,23 @@ typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3Unified; +#include +template +virtual class DepthFactor3 : gtsam::NoiseModelFactor { + DepthFactor3(); + DepthFactor3(const gtsam::Point3& measured, const gtsam::noiseModel::Base* model, + gtsam::Key poseKey, gtsam::Key landmarkKey, const gtsam::Cal3_S2::shared_ptr& K); + DepthFactor3(const gtsam::Point3& measured, const gtsam::noiseModel::Base* model, + gtsam::Key poseKey, gtsam::Key landmarkKey, const gtsam::Cal3_S2::shared_ptr& K, const gtsam::Pose3& body_P_sensor); + + void print(string s = "DepthFactor3") const; + const gtsam::Point3& measurement() const; + const gtsam::Cal3_S2::shared_ptr calibration() const; + + void serializable() const; // enabling serialization functionality +}; +typedef gtsam::DepthFactor3 DepthFactor3Pose3Point3; + #include template virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { diff --git a/gtsam_unstable/slam/tests/testDepthFactor3.cpp b/gtsam/slam/tests/testDepthFactor3.cpp similarity index 99% rename from gtsam_unstable/slam/tests/testDepthFactor3.cpp rename to gtsam/slam/tests/testDepthFactor3.cpp index 093bd9cfc5..56668249d8 100644 --- a/gtsam_unstable/slam/tests/testDepthFactor3.cpp +++ b/gtsam/slam/tests/testDepthFactor3.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include using namespace std; diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index bd17afb5d6..98a90e0c0a 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -162,22 +162,6 @@ class BearingS2 { void serializable() const; // enabling serialization functionality }; -#include -template -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 DepthCamera3Cal3_S2; #include class SimWall2D { @@ -637,22 +621,6 @@ virtual class InvDepthFactorVariant3b : gtsam::NoiseModelFactor { InvDepthFactorVariant3b(gtsam::Key poseKey1, gtsam::Key poseKey2, gtsam::Key landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model); }; -#include -template -virtual class DepthFactor3 : gtsam::NoiseModelFactor { - DepthFactor3(); - DepthFactor3(const gtsam::Point3& measured, const gtsam::noiseModel::Base* model, - gtsam::Key poseKey, gtsam::Key landmarkKey, const gtsam::Cal3_S2::shared_ptr& K); - DepthFactor3(const gtsam::Point3& measured, const gtsam::noiseModel::Base* model, - gtsam::Key poseKey, gtsam::Key landmarkKey, const gtsam::Cal3_S2::shared_ptr& K, const gtsam::Pose3& body_P_sensor); - - void print(string s = "DepthFactor3") const; - const gtsam::Point3& measurement() const; - const gtsam::Cal3_S2::shared_ptr calibration() const; - - void serializable() const; // enabling serialization functionality -}; -typedef gtsam::DepthFactor3 DepthFactor3Pose3Point3; #include From 287e5b23b3948efa3ce0da7a97dba7104969439e Mon Sep 17 00:00:00 2001 From: junlinp Date: Mon, 10 Nov 2025 14:17:33 +0800 Subject: [PATCH 13/15] minor --- gtsam/geometry/geometry.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 6baf04c7f8..26261e1a64 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1314,7 +1314,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3DS2; typedef gtsam::PinholeCamera PinholeCameraCal3Unified; #include -template +template class DepthCamera3 { DepthCamera3(); DepthCamera3(const gtsam::Point3& measurement, const CALIBRATION::shared_ptr& k); From a4f959951a5b0202338e50b21cb3095b18e17efd Mon Sep 17 00:00:00 2001 From: junlinp Date: Mon, 10 Nov 2025 14:23:17 +0800 Subject: [PATCH 14/15] minor --- gtsam/slam/slam.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index fb744b11e6..f7f6c214c4 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -103,7 +103,7 @@ typedef gtsam::GenericProjectionFactor -template +template virtual class DepthFactor3 : gtsam::NoiseModelFactor { DepthFactor3(); DepthFactor3(const gtsam::Point3& measured, const gtsam::noiseModel::Base* model, From 1847b39fc9be9641112122bcbdd77212cffc9056 Mon Sep 17 00:00:00 2001 From: junlinp Date: Mon, 10 Nov 2025 15:13:57 +0800 Subject: [PATCH 15/15] fix cov compute. --- gtsam/slam/DepthFactor3.h | 23 +++++++- gtsam/slam/tests/testDepthFactor3.cpp | 76 +++++++++++++++++++++++++++ 2 files changed, 98 insertions(+), 1 deletion(-) diff --git a/gtsam/slam/DepthFactor3.h b/gtsam/slam/DepthFactor3.h index 4a11b651e9..b1542d38c4 100644 --- a/gtsam/slam/DepthFactor3.h +++ b/gtsam/slam/DepthFactor3.h @@ -64,7 +64,28 @@ class DepthFactor3: public NoiseModelFactorN { const Key poseKey, Key landmarkKey, const Cal3_S2::shared_ptr& K, const std::optional& body_P_sensor = {}) : Base(model, poseKey, landmarkKey), measured_(measured), K_(K), - body_P_sensor_(body_P_sensor) {} + body_P_sensor_(body_P_sensor) { + // compute the noise model for converted measurement. + // jacobian of [u, v, depth] to [u * depth, v * depth, depth] + double u = measured_.x(); + double v = measured_.y(); + double depth = measured_.z(); + Matrix33 J = (Matrix33() << depth, 0, u, + 0, depth, v, + 0, 0, 1).finished(); + // sigmas() returns standard deviations, need to square them for covariance + Vector3 sigmas = model->sigmas(); + Vector3 sigmas_sq = sigmas.array().square(); + Matrix33 original_cov = sigmas_sq.asDiagonal(); + Matrix33 new_cov = J * original_cov * J.transpose(); + Matrix33 intrinsics_inv = (Matrix33() << 1.0 / K_->fx(), 0, -K_->px() / K_->fx(), + 0, 1.0 / K_->fy(), -K_->py() / K_->fy(), + 0, 0, 1.0).finished(); + Matrix33 new_cov_intrinsics = intrinsics_inv * new_cov * intrinsics_inv.transpose(); + SharedNoiseModel new_model = noiseModel::Gaussian::Covariance(new_cov_intrinsics); + // Directly assign the new noise model (noiseModel_ is protected in NoiseModelFactor) + this->noiseModel_ = new_model; + } /** Virtual destructor */ ~DepthFactor3() override {} diff --git a/gtsam/slam/tests/testDepthFactor3.cpp b/gtsam/slam/tests/testDepthFactor3.cpp index 56668249d8..edc96a806c 100644 --- a/gtsam/slam/tests/testDepthFactor3.cpp +++ b/gtsam/slam/tests/testDepthFactor3.cpp @@ -317,6 +317,82 @@ TEST(DepthFactor3, optimizeWithBodyPSensor_ba) { EXPECT(assert_equal(ground_truth_body_poses[i], result_body_pose, 1e-6)); } } + +/* ************************************************************************* */ +TEST(DepthFactor3, NoiseModelTransformation) { + // Test that the noise model is correctly transformed + 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); + + // Create a noise model with known sigmas + Vector3 sigmas(0.5, 0.5, 0.1); // sigma_u, sigma_v, sigma_depth + SharedNoiseModel original_model = noiseModel::Diagonal::Sigmas(sigmas); + + // Create factor - this should transform the noise model + DepthFactor factor(measurement, original_model, Symbol('x',1), Symbol('l',1), K); + + // Get the transformed noise model + SharedNoiseModel transformed_model = factor.noiseModel(); + + // Verify that the noise model has been transformed (should be different from original) + // The transformed model should account for the measurement transformation + EXPECT(transformed_model != original_model); + + // Verify the transformation by manually computing expected covariance + double u = measurement.x(); + double v = measurement.y(); + Matrix33 J = (Matrix33() << depth, 0, u, + 0, depth, v, + 0, 0, 1).finished(); + // Original covariance is diagonal with sigmas^2 + Vector3 sigmas_sq = sigmas.array().square(); + Matrix33 original_cov = sigmas_sq.asDiagonal(); + Matrix33 new_cov = J * original_cov * J.transpose(); + Matrix33 intrinsics_inv = (Matrix33() << 1.0 / K->fx(), 0, -K->px() / K->fx(), + 0, 1.0 / K->fy(), -K->py() / K->fy(), + 0, 0, 1.0).finished(); + Matrix33 expected_cov = intrinsics_inv * new_cov * intrinsics_inv.transpose(); + + // Get the covariance from the transformed noise model + // For Gaussian noise models, we can get the covariance matrix + auto gaussian_model = std::dynamic_pointer_cast(transformed_model); + if (gaussian_model) { + Matrix33 actual_cov = gaussian_model->R().inverse() * gaussian_model->R().inverse().transpose(); + // Verify the covariance is approximately correct + EXPECT(assert_equal(expected_cov, actual_cov, 1e-5)); + } + + // Verify that the factor still works correctly with the transformed noise model + Vector actual_error = factor.evaluateError(level_pose, landmark); + + // The error should still be approximately zero when landmark matches measurement + // (allowing for some numerical error due to noise model transformation) + EXPECT(assert_equal(Vector3::Zero(), actual_error, 1e-5)); + + // Test with body_P_sensor as well + Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); + Pose3 body_pose_world = level_pose; + Pose3 camera_pose_world = body_pose_world.compose(body_P_sensor); + PinholeCamera camera_with_transform(camera_pose_world, *K); + + Point3 landmark_cam(5, 3, depth); + Point3 landmark2 = camera_pose_world.transformFrom(landmark_cam); + Point2 expected_uv2 = camera_with_transform.project(landmark2); + Point3 measurement2(expected_uv2.x(), expected_uv2.y(), depth); + + DepthFactor factor2(measurement2, original_model, Symbol('x',1), Symbol('l',1), K, body_P_sensor); + SharedNoiseModel transformed_model2 = factor2.noiseModel(); + + // Verify transformed model is different from original + EXPECT(transformed_model2 != original_model); + + // Verify factor works correctly + Vector actual_error2 = factor2.evaluateError(body_pose_world, landmark2); + EXPECT(assert_equal(Vector3::Zero(), actual_error2, 1e-5)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */