diff --git a/gtsam/geometry/DepthCamera3.h b/gtsam/geometry/DepthCamera3.h new file mode 100644 index 0000000000..7482b24c60 --- /dev/null +++ b/gtsam/geometry/DepthCamera3.h @@ -0,0 +1,153 @@ + +/** + * @file DepthCamera3.h + * @brief Depth Camera. + * @author junlinp + * @date Nov 9, 2025 + */ + +#pragma once + +#include +#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 + std::optional 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& 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 + /// @{ + + 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: "); + 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 + 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 + diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 0c30f70e5e..26261e1a64 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/geometry/tests/testDepthCamera3.cpp b/gtsam/geometry/tests/testDepthCamera3.cpp new file mode 100644 index 0000000000..2b474ca2e0 --- /dev/null +++ b/gtsam/geometry/tests/testDepthCamera3.cpp @@ -0,0 +1,148 @@ + +/* + * 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)); +} + +/* ************************************************************************* */ +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/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 72513d1649..bfcf274d46 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/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::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..a770c7554c 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,4 +273,119 @@ 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; + 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 pose_i previous pose key + * @param pose_j current pose 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 + /// @return void + void print(const std::string& s, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + + /// equals + /// @params other factor to compare + /// @params tol tolerance + 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 + /// @param pose_i previous pose + /// @param pose_j current pose + /// @param bias current gyroscope bias estimate + /// @param H1 jacobian of residuals repected to Ri + /// @param H2 jacobian of residuals repected to Rj + /// @param H3 jacobian of residuals repected to bias + /// @return delta rotation represented in Lie algebra + Vector evaluateError(const Pose3& pose_i, const Pose3& pose_j, 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/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, diff --git a/gtsam/slam/DepthFactor3.h b/gtsam/slam/DepthFactor3.h new file mode 100644 index 0000000000..b1542d38c4 --- /dev/null +++ b/gtsam/slam/DepthFactor3.h @@ -0,0 +1,159 @@ + +/** + * @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 +#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 + std::optional body_P_sensor_; ///< The pose of the sensor in the body frame + +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 + * @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, + const std::optional& body_P_sensor = {}) : + Base(model, poseKey, landmarkKey), measured_(measured), K_(K), + 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 {} + + /** + * 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: "); + if (body_P_sensor_) { + body_P_sensor_->print(" body_P_sensor: "); + } + } + + /// 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) && + ((!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_, body_P_sensor_); + 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_); + ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); + } +#endif +}; +} // \ namespace gtsam + diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 0ccff908a6..f7f6c214c4 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/slam/tests/testDepthFactor3.cpp b/gtsam/slam/tests/testDepthFactor3.cpp new file mode 100644 index 0000000000..edc96a806c --- /dev/null +++ b/gtsam/slam/tests/testDepthFactor3.cpp @@ -0,0 +1,399 @@ +/* ---------------------------------------------------------------------------- + * 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 +#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)); +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)); +} + +/* ************************************************************************* */ +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)); +} + +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)); + } +} + +/* ************************************************************************* */ +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);} +/* ************************************************************************* */ + diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 10b196204d..98a90e0c0a 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -622,6 +622,7 @@ virtual class InvDepthFactorVariant3b : gtsam::NoiseModelFactor { }; + #include class Mechanization_bRn2 { Mechanization_bRn2(); 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); +} +/* ************************************************************************* */