From cd46bfa36311c25219251aab0a8e3370c670981f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 30 Sep 2024 13:44:09 -0700 Subject: [PATCH 1/4] Formatting and comments --- gtsam/navigation/AttitudeFactor.h | 152 +++++++++++++++--------------- 1 file changed, 74 insertions(+), 78 deletions(-) diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index d61db2166..676e0a46d 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -17,59 +17,61 @@ **/ #pragma once -#include #include #include +#include namespace gtsam { /** - * Base class for prior on attitude - * Example: - * - measurement is direction of gravity in body frame bF - * - reference is direction of gravity in navigation frame nG - * This factor will give zero error if nRb * bF == nG + * @class AttitudeFactor + * + * @brief Base class for an attitude factor that constrains the rotation between + * body and navigation frames. + * + * This factor enforces that when a known reference direction in the body frame + * (e.g., accelerometer axis) is rotated into the navigation frame using the + * rotation variable, it should align with a measured direction in the + * navigation frame (e.g., gravity vector). + * + * Mathematically, the error is zero when: + * nRb * bRef == nZ + * + * This is useful for incorporating absolute orientation measurements into the + * factor graph. + * * @ingroup navigation */ class AttitudeFactor { + protected: + Unit3 nZ_, bRef_; ///< Position measurement in -protected: - - Unit3 nZ_, bRef_; ///< Position measurement in - -public: - + public: /** default constructor - only use for serialization */ - AttitudeFactor() { - } + AttitudeFactor() {} /** * @brief Constructor - * @param nZ measured direction in navigation frame - * @param bRef reference direction in body frame (default Z-axis in NED frame, i.e., [0; 0; 1]) + * @param nZ Measured direction in the navigation frame (e.g., gravity). + * @param bRef Reference direction in the body frame (e.g., accelerometer + * axis). Default is Unit3(0, 0, 1). */ - AttitudeFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) : - nZ_(nZ), bRef_(bRef) { - } + AttitudeFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) + : nZ_(nZ), bRef_(bRef) {} /** vector of errors */ - Vector attitudeError(const Rot3& p, - OptionalJacobian<2,3> H = {}) const; + Vector attitudeError(const Rot3& p, OptionalJacobian<2, 3> H = {}) const; - const Unit3& nZ() const { - return nZ_; - } - const Unit3& bRef() const { - return bRef_; - } + const Unit3& nZ() const { return nZ_; } + const Unit3& bRef() const { return bRef_; } #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - ar & boost::serialization::make_nvp("nZ_", nZ_); - ar & boost::serialization::make_nvp("bRef_", bRef_); + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp("nZ_", nZ_); + ar& boost::serialization::make_nvp("bRef_", bRef_); } #endif }; @@ -78,12 +80,11 @@ public: * Version of AttitudeFactor for Rot3 * @ingroup navigation */ -class GTSAM_EXPORT Rot3AttitudeFactor: public NoiseModelFactorN, public AttitudeFactor { - +class GTSAM_EXPORT Rot3AttitudeFactor : public NoiseModelFactorN, + public AttitudeFactor { typedef NoiseModelFactorN Base; -public: - + public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; @@ -94,11 +95,9 @@ public: typedef Rot3AttitudeFactor This; /** default constructor - only use for serialization */ - Rot3AttitudeFactor() { - } + Rot3AttitudeFactor() {} - ~Rot3AttitudeFactor() override { - } + ~Rot3AttitudeFactor() override {} /** * @brief Constructor @@ -108,9 +107,8 @@ public: * @param bRef reference direction in body frame (default Z-axis) */ Rot3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model, - const Unit3& bRef = Unit3(0, 0, 1)) : - Base(model, key), AttitudeFactor(nZ, bRef) { - } + const Unit3& bRef = Unit3(0, 0, 1)) + : Base(model, key), AttitudeFactor(nZ, bRef) {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -123,46 +121,46 @@ public: DefaultKeyFormatter) const override; /** equals */ - bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; + bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const override; /** vector of errors */ Vector evaluateError(const Rot3& nRb, OptionalMatrixType H) const override { return attitudeError(nRb, H); } -private: - + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility - ar & boost::serialization::make_nvp("NoiseModelFactor1", - boost::serialization::base_object(*this)); - ar & boost::serialization::make_nvp("AttitudeFactor", + ar& boost::serialization::make_nvp( + "NoiseModelFactor1", boost::serialization::base_object(*this)); + ar& boost::serialization::make_nvp( + "AttitudeFactor", boost::serialization::base_object(*this)); } #endif -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /// traits -template<> struct traits : public Testable {}; +template <> +struct traits : public Testable {}; /** * Version of AttitudeFactor for Pose3 * @ingroup navigation */ -class GTSAM_EXPORT Pose3AttitudeFactor: public NoiseModelFactorN, - public AttitudeFactor { - +class GTSAM_EXPORT Pose3AttitudeFactor : public NoiseModelFactorN, + public AttitudeFactor { typedef NoiseModelFactorN Base; -public: - + public: // Provide access to the Matrix& version of evaluateError: using Base::evaluateError; @@ -173,11 +171,9 @@ public: typedef Pose3AttitudeFactor This; /** default constructor - only use for serialization */ - Pose3AttitudeFactor() { - } + Pose3AttitudeFactor() {} - ~Pose3AttitudeFactor() override { - } + ~Pose3AttitudeFactor() override {} /** * @brief Constructor @@ -187,9 +183,8 @@ public: * @param bRef reference direction in body frame (default Z-axis) */ Pose3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model, - const Unit3& bRef = Unit3(0, 0, 1)) : - Base(model, key), AttitudeFactor(nZ, bRef) { - } + const Unit3& bRef = Unit3(0, 0, 1)) + : Base(model, key), AttitudeFactor(nZ, bRef) {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -202,40 +197,41 @@ public: DefaultKeyFormatter) const override; /** equals */ - bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; + bool equals(const NonlinearFactor& expected, + double tol = 1e-9) const override; /** vector of errors */ Vector evaluateError(const Pose3& nTb, OptionalMatrixType H) const override { Vector e = attitudeError(nTb.rotation(), H); if (H) { Matrix H23 = *H; - *H = Matrix::Zero(2,6); - H->block<2,3>(0,0) = H23; + *H = Matrix::Zero(2, 6); + H->block<2, 3>(0, 0) = H23; } return e; } -private: - + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { // NoiseModelFactor1 instead of NoiseModelFactorN for backward compatibility - ar & boost::serialization::make_nvp("NoiseModelFactor1", - boost::serialization::base_object(*this)); - ar & boost::serialization::make_nvp("AttitudeFactor", + ar& boost::serialization::make_nvp( + "NoiseModelFactor1", boost::serialization::base_object(*this)); + ar& boost::serialization::make_nvp( + "AttitudeFactor", boost::serialization::base_object(*this)); } #endif -public: + public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW }; /// traits -template<> struct traits : public Testable {}; - -} /// namespace gtsam +template <> +struct traits : public Testable {}; +} // namespace gtsam From 81a4062edd24ee874f077d86ce3dcff9f5622e60 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 30 Sep 2024 13:44:19 -0700 Subject: [PATCH 2/4] Added documentation --- gtsam/navigation/AttitudeFactor.md | 142 +++++++++++++++++++++++++++++ 1 file changed, 142 insertions(+) create mode 100644 gtsam/navigation/AttitudeFactor.md diff --git a/gtsam/navigation/AttitudeFactor.md b/gtsam/navigation/AttitudeFactor.md new file mode 100644 index 000000000..beb52b0b6 --- /dev/null +++ b/gtsam/navigation/AttitudeFactor.md @@ -0,0 +1,142 @@ +# AttitudeFactor in GTSAM + +[Cautionary note: this was generated from the source using ChatGPT but edited by Frank] + +## Introduction + +The `AttitudeFactor` in GTSAM is a factor that constrains the orientation (attitude) of a robot or sensor platform based on directional measurements. This is particularly useful in GPS-denied navigation contexts where orientation must be estimated from inertial sensors like accelerometers or magnetometers. + +This document explains the mathematical foundation of the `AttitudeFactor` and guides users on how to use this factor effectively in GTSAM. + +## Mathematical Foundation + +### Concept + +The `AttitudeFactor` constrains the rotation $\mathbf{R}_{nb}$ (from body frame $b$ to navigation frame $n$) such that a known reference direction in the body frame aligns with a measured direction in the navigation frame when rotated. The factor enforces that: + +$$ +\mathbf{R}_{nb} \cdot \mathbf{b}_{\text{Ref}} \approx \mathbf{nZ} +$$ + +where: + +- $\mathbf{R}_{nb}$ is the rotation matrix representing the orientation from body to navigation frame. +- $\mathbf{b}_{\text{Ref}}$ is a known reference direction in the body frame (e.g., the accelerometer's sensitive axis). +- $\mathbf{nZ}$ is the measured direction in the navigation frame (e.g., the gravity vector measured by an IMU). + +### Error Function + +The error function computes the angular difference between the rotated reference direction and the measured direction: + +$$ +\mathbf{e} = \text{error}(\mathbf{nZ}, \mathbf{R}_{nb} \cdot \mathbf{b}_{\text{Ref}}) +$$ + +This error is minimal (zero) when the rotated body reference direction aligns perfectly with the measured navigation direction. + +The error is computed internally using the `attitudeError` function: + +```cpp +Vector AttitudeFactor::attitudeError(const Rot3& nRb) const { + Unit3 nRef = nRb * bRef_; + return nZ_.error(nRef); +} +``` + +#### Explanation: +- The function computes the rotated reference direction `nRef` and then the error between `nRef` and the measured direction `nZ`. +- `nZ_.error(nRef)` is a 2D vector-valued error between two directions, defined in [Unit3.h](../geometry/Unit3.h). + +### Jacobians + +For optimization, the $2 \times 3$ Jacobian of the error function with respect to the rotation parameters is required. The Jacobian is computed using chain rule differentiation, involving the derivative of the rotated vector with respect to the rotation parameters and the derivative of the error with respect to the rotated vector. + +## Usage in GTSAM + +### Including the Header + +Include the `AttitudeFactor.h` header in your code: + +```cpp +#include +``` + +### Creating an Attitude Factor + +You can create an attitude factor for either a `Rot3` (rotation only) or a `Pose3` (position and rotation) variable. + +#### For `Rot3` Variables + +```cpp +// Define keys +gtsam::Key rotKey = ...; + +// Measured direction in navigation frame (e.g., gravity) +gtsam::Unit3 nZ(0, 0, -1); // Assuming gravity points down in navigation frame + +// Reference direction in body frame (e.g., accelerometer axis) +gtsam::Unit3 bRef(0, 0, 1); // Default is the Z-axis + +// Noise model +auto noiseModel = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); // 2D error, sigma = 0.1 + +// Add to factor graph +gtsam::NonlinearFactorGraph graph; +graph.emplace_shared(rotKey, nZ, noiseModel, bRef); +``` + +#### For `Pose3` Variables + +There is also a `Pose3AttitudeFactor` that automatically extracts the rotation from the pose, taking into account the chain rule for this operation so the Jacobians with respect to pose are correct. + +```cpp +// Define keys +gtsam::Key poseKey = ...; + +// Measured direction in navigation frame +gtsam::Unit3 nZ(0, 0, -1); + +// Reference direction in body frame +gtsam::Unit3 bRef(0, 0, 1); + +// Noise model +auto noiseModel = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); + +// Add to factor graph +gtsam::NonlinearFactorGraph graph; +graph.emplace_shared(poseKey, nZ, noiseModel, bRef); +``` + +### Explanation of Parameters + +- **Key**: The variable key in the factor graph corresponding to the `Rot3` or `Pose3` variable you are constraining. +- **nZ**: The measured direction in the navigation frame. This is typically obtained from sensors like accelerometers or magnetometers. +- **bRef**: The known reference direction in the body frame. By default, this is set to the Z-axis `[0; 0; 1]`, but it should match the direction your sensor measures. +- **noiseModel**: The noise model representing the uncertainty in the measurement. + +## Practical Tips + +- **Gravity Measurement**: When using an IMU, the accelerometer readings (after removing the dynamic acceleration) can provide the gravity direction in the body frame. +- **Magnetic North Measurement**: A magnetometer can provide the direction of the Earth's magnetic field, which can be used similarly. +- **Reference Direction**: Ensure that `bRef` correctly represents the sensor's measurement axis in the body frame. +- **Noise Model**: The choice of noise model affects the weight of this factor in the optimization. Adjust the standard deviation based on the confidence in your measurements. + +## Example in GPS-Denied Navigation + +In GPS-denied environments, orientation estimation relies heavily on inertial measurements. By incorporating the `AttitudeFactor`, you can: + +- Constrain the roll and pitch angles using gravity measurements from an accelerometer. +- Constrain the yaw angle using magnetic field measurements from a magnetometer (with caution due to magnetic disturbances). + +This factor helps maintain an accurate orientation estimate over time, which is crucial for applications like drone flight, underwater vehicles, or indoor robotics. + +## Conclusion + +The `AttitudeFactor` is a powerful tool in GTSAM for incorporating orientation measurements into your factor graph. By aligning a known reference direction in the body frame with a measured direction in the navigation frame, it provides a constraint that improves the estimation of the rotation variable. Correct understanding and usage of this factor enhance the performance of navigation algorithms, especially in challenging environments where GPS is unavailable. + +Remember to verify the alignment of your reference and measured directions and to adjust the noise model to reflect the reliability of your sensors. + +# References + +- [GTSAM Documentation](https://gtsam.org/) +- [Unit3 Class Reference](https://gtsam.org/doxygen/) \ No newline at end of file From 95742baccce83679b78f75b733501b33b5b4e95b Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 30 Sep 2024 14:14:46 -0700 Subject: [PATCH 3/4] Added README --- gtsam/navigation/ImuFactor.h | 14 +++++------- gtsam/navigation/README.md | 42 ++++++++++++++++++++++++++++++++++++ 2 files changed, 47 insertions(+), 9 deletions(-) create mode 100644 gtsam/navigation/README.md diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 40bc15e11..7254838fd 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -37,13 +37,9 @@ typedef ManifoldPreintegration PreintegrationType; /* * If you are using the factor, please cite: - * L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating - * conditionally independent sets in factor graphs: a unifying perspective based - * on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014. - * - * C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on - * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", - * Robotics: Science and Systems (RSS), 2015. + * Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, + * "On-Manifold Preintegration for Real-Time Visual-Inertial Odometry", IEEE + * Transactions on Robotics, 2017. * * REFERENCES: * [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", @@ -54,8 +50,8 @@ typedef ManifoldPreintegration PreintegrationType; * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: * Computation of the Jacobian Matrices", Tech. Report, 2013. * Available in this repo as "PreintegratedIMUJacobians.pdf". - * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on - * Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", + * [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration + * on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", * Robotics: Science and Systems (RSS), 2015. */ diff --git a/gtsam/navigation/README.md b/gtsam/navigation/README.md new file mode 100644 index 000000000..fe99d8f73 --- /dev/null +++ b/gtsam/navigation/README.md @@ -0,0 +1,42 @@ +# Navigation Factors + +This directory contains factors related to navigation, including various IMU factors. + +## IMU Factor: + +![IMU Factor Diagram](https://www.mathworks.com/help/examples/shared_positioning/win64/FactorGraphPedestrianIMUGPSLocalizationExample_02.png) + +The `ImuFactor` is a 5-ways factor involving previous state (pose and velocity of +the vehicle at previous time step), current state (pose and velocity at +current time step), and the bias estimate. +Following the preintegration +scheme proposed in [2], the `ImuFactor` includes many IMU measurements, which +are "summarized" using the PreintegratedIMUMeasurements class. +The figure above, courtesy of [Mathworks' navigation toolbox](https://www.mathworks.com/help/nav/index.html), which are also using our work, shows the factor graph fragment for two time slices. + +Note that this factor does not model "temporal consistency" of the biases +(which are usually slowly varying quantities), which is up to the caller. +See also `CombinedImuFactor` for a class that does this for you. + +If you are using the factor, please cite: +> Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "On-Manifold Preintegration for Real-Time Visual-Inertial Odometry", IEEE Transactions on Robotics, 2017. + +## REFERENCES: +1. G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", + Volume 2, 2008. +2. T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for + High-Dynamic Motion in Built Environments Without Initial Conditions", + TRO, 28(1):61-76, 2012. +3. L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: + Computation of the Jacobian Matrices", Tech. Report, 2013. + Available in this repo as "PreintegratedIMUJacobians.pdf". +4. C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on + Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", + Robotics: Science and Systems (RSS), 2015. + +## The Attitude Factor + +The `AttitudeFactor` in GTSAM is a factor that constrains the orientation (attitude) of a robot or sensor platform based on directional measurements. Both `Rot3` and `Pose3` versions are available. + +Written up in detail with the help of ChatGPT [here](AttitudeFactor.md). + From 38ffcc3e5cf3994e782913dbcd84c94fd352cb4e Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 30 Oct 2024 12:04:04 -0700 Subject: [PATCH 4/4] Switch measured/reference language. --- gtsam/navigation/AttitudeFactor.cpp | 30 +++++----- gtsam/navigation/AttitudeFactor.h | 56 ++++++++++-------- gtsam/navigation/AttitudeFactor.md | 58 +++++++++---------- gtsam/navigation/navigation.i | 28 ++++----- gtsam/navigation/tests/testAttitudeFactor.cpp | 39 ++++++------- 5 files changed, 104 insertions(+), 107 deletions(-) diff --git a/gtsam/navigation/AttitudeFactor.cpp b/gtsam/navigation/AttitudeFactor.cpp index 8c8eb5772..1ca161ebb 100644 --- a/gtsam/navigation/AttitudeFactor.cpp +++ b/gtsam/navigation/AttitudeFactor.cpp @@ -26,16 +26,16 @@ namespace gtsam { Vector AttitudeFactor::attitudeError(const Rot3& nRb, OptionalJacobian<2, 3> H) const { if (H) { - Matrix23 D_nRef_R; - Matrix22 D_e_nRef; - Unit3 nRef = nRb.rotate(bRef_, D_nRef_R); - Vector e = nZ_.error(nRef, D_e_nRef); + Matrix23 D_nRotated_R; + Matrix22 D_e_nRotated; + Unit3 nRotated = nRb.rotate(bMeasured_, D_nRotated_R); + Vector e = nRef_.error(nRotated, D_e_nRotated); - (*H) = D_e_nRef * D_nRef_R; + (*H) = D_e_nRotated * D_nRotated_R; return e; } else { - Unit3 nRef = nRb * bRef_; - return nZ_.error(nRef); + Unit3 nRotated = nRb * bMeasured_; + return nRef_.error(nRotated); } } @@ -44,8 +44,8 @@ void Rot3AttitudeFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << (s.empty() ? "" : s + " ") << "Rot3AttitudeFactor on " << keyFormatter(this->key()) << "\n"; - nZ_.print(" measured direction in nav frame: "); - bRef_.print(" reference direction in body frame: "); + nRef_.print(" reference direction in nav frame: "); + bMeasured_.print(" measured direction in body frame: "); this->noiseModel_->print(" noise model: "); } @@ -53,16 +53,16 @@ void Rot3AttitudeFactor::print(const string& s, bool Rot3AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) - && this->bRef_.equals(e->bRef_, tol); + return e != nullptr && Base::equals(*e, tol) && this->nRef_.equals(e->nRef_, tol) + && this->bMeasured_.equals(e->bMeasured_, tol); } //*************************************************************************** void Pose3AttitudeFactor::print(const string& s, const KeyFormatter& keyFormatter) const { cout << s << "Pose3AttitudeFactor on " << keyFormatter(this->key()) << "\n"; - nZ_.print(" measured direction in nav frame: "); - bRef_.print(" reference direction in body frame: "); + nRef_.print(" reference direction in nav frame: "); + bMeasured_.print(" measured direction in body frame: "); this->noiseModel_->print(" noise model: "); } @@ -70,8 +70,8 @@ void Pose3AttitudeFactor::print(const string& s, bool Pose3AttitudeFactor::equals(const NonlinearFactor& expected, double tol) const { const This* e = dynamic_cast(&expected); - return e != nullptr && Base::equals(*e, tol) && this->nZ_.equals(e->nZ_, tol) - && this->bRef_.equals(e->bRef_, tol); + return e != nullptr && Base::equals(*e, tol) && this->nRef_.equals(e->nRef_, tol) + && this->bMeasured_.equals(e->bMeasured_, tol); } //*************************************************************************** diff --git a/gtsam/navigation/AttitudeFactor.h b/gtsam/navigation/AttitudeFactor.h index 676e0a46d..2fada724d 100644 --- a/gtsam/navigation/AttitudeFactor.h +++ b/gtsam/navigation/AttitudeFactor.h @@ -29,13 +29,13 @@ namespace gtsam { * @brief Base class for an attitude factor that constrains the rotation between * body and navigation frames. * - * This factor enforces that when a known reference direction in the body frame - * (e.g., accelerometer axis) is rotated into the navigation frame using the - * rotation variable, it should align with a measured direction in the + * This factor enforces that when the measured direction in the body frame + * (e.g., from an IMU accelerometer) is rotated into the navigation frame using the + * rotation variable, it should align with a known reference direction in the * navigation frame (e.g., gravity vector). * * Mathematically, the error is zero when: - * nRb * bRef == nZ + * nRb * bMeasured == nRef * * This is useful for incorporating absolute orientation measurements into the * factor graph. @@ -44,7 +44,7 @@ namespace gtsam { */ class AttitudeFactor { protected: - Unit3 nZ_, bRef_; ///< Position measurement in + Unit3 nRef_, bMeasured_; ///< Position measurement in public: /** default constructor - only use for serialization */ @@ -52,26 +52,34 @@ class AttitudeFactor { /** * @brief Constructor - * @param nZ Measured direction in the navigation frame (e.g., gravity). - * @param bRef Reference direction in the body frame (e.g., accelerometer - * axis). Default is Unit3(0, 0, 1). + * @param nRef Reference direction in the navigation frame (e.g., gravity). + * @param bMeasured Measured direction in the body frame (e.g., from IMU + * accelerometer). Default is Unit3(0, 0, 1). */ - AttitudeFactor(const Unit3& nZ, const Unit3& bRef = Unit3(0, 0, 1)) - : nZ_(nZ), bRef_(bRef) {} + AttitudeFactor(const Unit3& nRef, const Unit3& bMeasured = Unit3(0, 0, 1)) + : nRef_(nRef), bMeasured_(bMeasured) {} /** vector of errors */ Vector attitudeError(const Rot3& p, OptionalJacobian<2, 3> H = {}) const; - const Unit3& nZ() const { return nZ_; } - const Unit3& bRef() const { return bRef_; } + const Unit3& nRef() const { return nRef_; } + const Unit3& bMeasured() const { return bMeasured_; } + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 + [[deprecated("Use nRef() instead")]] + const Unit3& nZ() const { return nRef_; } + + [[deprecated("Use bMeasured() instead")]] + const Unit3& bRef() const { return bMeasured_; } +#endif #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ friend class boost::serialization::access; template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - ar& boost::serialization::make_nvp("nZ_", nZ_); - ar& boost::serialization::make_nvp("bRef_", bRef_); + ar& boost::serialization::make_nvp("nRef_", nRef_); + ar& boost::serialization::make_nvp("bMeasured_", bMeasured_); } #endif }; @@ -102,13 +110,13 @@ class GTSAM_EXPORT Rot3AttitudeFactor : public NoiseModelFactorN, /** * @brief Constructor * @param key of the Rot3 variable that will be constrained - * @param nZ measured direction in navigation frame + * @param nRef reference direction in navigation frame * @param model Gaussian noise model - * @param bRef reference direction in body frame (default Z-axis) + * @param bMeasured measured direction in body frame (default Z-axis) */ - Rot3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model, - const Unit3& bRef = Unit3(0, 0, 1)) - : Base(model, key), AttitudeFactor(nZ, bRef) {} + Rot3AttitudeFactor(Key key, const Unit3& nRef, const SharedNoiseModel& model, + const Unit3& bMeasured = Unit3(0, 0, 1)) + : Base(model, key), AttitudeFactor(nRef, bMeasured) {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { @@ -178,13 +186,13 @@ class GTSAM_EXPORT Pose3AttitudeFactor : public NoiseModelFactorN, /** * @brief Constructor * @param key of the Pose3 variable that will be constrained - * @param nZ measured direction in navigation frame + * @param nRef reference direction in navigation frame * @param model Gaussian noise model - * @param bRef reference direction in body frame (default Z-axis) + * @param bMeasured measured direction in body frame (default Z-axis) */ - Pose3AttitudeFactor(Key key, const Unit3& nZ, const SharedNoiseModel& model, - const Unit3& bRef = Unit3(0, 0, 1)) - : Base(model, key), AttitudeFactor(nZ, bRef) {} + Pose3AttitudeFactor(Key key, const Unit3& nRef, const SharedNoiseModel& model, + const Unit3& bMeasured = Unit3(0, 0, 1)) + : Base(model, key), AttitudeFactor(nRef, bMeasured) {} /// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { diff --git a/gtsam/navigation/AttitudeFactor.md b/gtsam/navigation/AttitudeFactor.md index beb52b0b6..a4d45b783 100644 --- a/gtsam/navigation/AttitudeFactor.md +++ b/gtsam/navigation/AttitudeFactor.md @@ -12,24 +12,24 @@ This document explains the mathematical foundation of the `AttitudeFactor` and g ### Concept -The `AttitudeFactor` constrains the rotation $\mathbf{R}_{nb}$ (from body frame $b$ to navigation frame $n$) such that a known reference direction in the body frame aligns with a measured direction in the navigation frame when rotated. The factor enforces that: +The `AttitudeFactor` constrains the rotation $\mathbf{R}_{nb}$ (from body frame $b$ to navigation frame $n$) such that a known reference direction in the navigation frame aligns with a measured direction in the body frame, when rotated. The factor enforces that: $$ -\mathbf{R}_{nb} \cdot \mathbf{b}_{\text{Ref}} \approx \mathbf{nZ} +\text{nRef} \approx \mathbf{R}_{nb} \cdot \text{bMeasured} $$ where: - $\mathbf{R}_{nb}$ is the rotation matrix representing the orientation from body to navigation frame. -- $\mathbf{b}_{\text{Ref}}$ is a known reference direction in the body frame (e.g., the accelerometer's sensitive axis). -- $\mathbf{nZ}$ is the measured direction in the navigation frame (e.g., the gravity vector measured by an IMU). +- $\text{bMeasured}$ is the measured direction in the body frame (e.g., the accelerometer reading). +- $\text{nRef}$ is the known reference direction in the navigation frame (e.g., the gravity vector in nav). ### Error Function The error function computes the angular difference between the rotated reference direction and the measured direction: $$ -\mathbf{e} = \text{error}(\mathbf{nZ}, \mathbf{R}_{nb} \cdot \mathbf{b}_{\text{Ref}}) +\mathbf{e} = \text{error}(\text{nRef}, \mathbf{R}_{nb} \cdot \text{bMeasured}) $$ This error is minimal (zero) when the rotated body reference direction aligns perfectly with the measured navigation direction. @@ -38,19 +38,21 @@ The error is computed internally using the `attitudeError` function: ```cpp Vector AttitudeFactor::attitudeError(const Rot3& nRb) const { - Unit3 nRef = nRb * bRef_; - return nZ_.error(nRef); + Unit3 nRotated = nRb * bMeasured_; + return nRef_.error(nRotated); } ``` #### Explanation: -- The function computes the rotated reference direction `nRef` and then the error between `nRef` and the measured direction `nZ`. -- `nZ_.error(nRef)` is a 2D vector-valued error between two directions, defined in [Unit3.h](../geometry/Unit3.h). +- The function computes the rotated measurement `nRotated` and then the error between `nRef` and `nRotated`. +- `nRef_.error(nRotated)` is a 2D vector-valued error between two directions, defined in [Unit3.h](../geometry/Unit3.h). ### Jacobians For optimization, the $2 \times 3$ Jacobian of the error function with respect to the rotation parameters is required. The Jacobian is computed using chain rule differentiation, involving the derivative of the rotated vector with respect to the rotation parameters and the derivative of the error with respect to the rotated vector. +Note the Jacobian for this specific error function vanishes 180 degrees away from the true direction, and the factor is only expected to behave well when the `nRb` value is initialized in the correct hemisphere. + ## Usage in GTSAM ### Including the Header @@ -71,55 +73,49 @@ You can create an attitude factor for either a `Rot3` (rotation only) or a `Pose // Define keys gtsam::Key rotKey = ...; -// Measured direction in navigation frame (e.g., gravity) -gtsam::Unit3 nZ(0, 0, -1); // Assuming gravity points down in navigation frame +// Known reference direction in navigation frame (e.g., gravity) +gtsam::Unit3 nRef(0, 0, -1); // Assuming gravity points down in navigation frame -// Reference direction in body frame (e.g., accelerometer axis) -gtsam::Unit3 bRef(0, 0, 1); // Default is the Z-axis +// Measured direction in body frame (e.g., accelerometer direction) +gtsam::Unit3 bMeasured(0, 0, 9.8); // will be normalized automatically // Noise model auto noiseModel = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); // 2D error, sigma = 0.1 // Add to factor graph gtsam::NonlinearFactorGraph graph; -graph.emplace_shared(rotKey, nZ, noiseModel, bRef); +graph.emplace_shared(rotKey, nRef, noiseModel, bMeasured); ``` #### For `Pose3` Variables There is also a `Pose3AttitudeFactor` that automatically extracts the rotation from the pose, taking into account the chain rule for this operation so the Jacobians with respect to pose are correct. +The same caveat about vanishing Jacobian holds. ```cpp // Define keys gtsam::Key poseKey = ...; -// Measured direction in navigation frame -gtsam::Unit3 nZ(0, 0, -1); +// Known reference direction in navigation frame (e.g., gravity) +gtsam::Unit3 nRef(0, 0, -1); // Assuming gravity points down in navigation frame -// Reference direction in body frame -gtsam::Unit3 bRef(0, 0, 1); +// Measured direction in body frame (e.g., accelerometer direction) +gtsam::Unit3 bMeasured(0, 0, 9.8); // will be normalized automatically // Noise model auto noiseModel = gtsam::noiseModel::Isotropic::Sigma(2, 0.1); // Add to factor graph gtsam::NonlinearFactorGraph graph; -graph.emplace_shared(poseKey, nZ, noiseModel, bRef); +graph.emplace_shared(poseKey, nRef, noiseModel, bMeasured); ``` ### Explanation of Parameters - **Key**: The variable key in the factor graph corresponding to the `Rot3` or `Pose3` variable you are constraining. -- **nZ**: The measured direction in the navigation frame. This is typically obtained from sensors like accelerometers or magnetometers. -- **bRef**: The known reference direction in the body frame. By default, this is set to the Z-axis `[0; 0; 1]`, but it should match the direction your sensor measures. -- **noiseModel**: The noise model representing the uncertainty in the measurement. - -## Practical Tips - -- **Gravity Measurement**: When using an IMU, the accelerometer readings (after removing the dynamic acceleration) can provide the gravity direction in the body frame. -- **Magnetic North Measurement**: A magnetometer can provide the direction of the Earth's magnetic field, which can be used similarly. -- **Reference Direction**: Ensure that `bRef` correctly represents the sensor's measurement axis in the body frame. -- **Noise Model**: The choice of noise model affects the weight of this factor in the optimization. Adjust the standard deviation based on the confidence in your measurements. +- **nRef**: The known direction in the navigation frame. This is typically obtained from sensors like accelerometers or magnetometers. +- **bMeasured**: The measured direction in the body frame. By default, this is set to the Z-axis `[0; 0; 1]`, but it should match the direction your sensor measures. When constructing a `Unit3`, will be automatically normalized. +- **noiseModel**: The noise model representing the uncertainty in the measurement. Adjust the standard deviation based on the confidence in your measurements. ## Example in GPS-Denied Navigation @@ -132,9 +128,7 @@ This factor helps maintain an accurate orientation estimate over time, which is ## Conclusion -The `AttitudeFactor` is a powerful tool in GTSAM for incorporating orientation measurements into your factor graph. By aligning a known reference direction in the body frame with a measured direction in the navigation frame, it provides a constraint that improves the estimation of the rotation variable. Correct understanding and usage of this factor enhance the performance of navigation algorithms, especially in challenging environments where GPS is unavailable. - -Remember to verify the alignment of your reference and measured directions and to adjust the noise model to reflect the reliability of your sensors. +The `AttitudeFactor` is a useful tool in GTSAM for incorporating orientation measurements into your factor graph. By aligning a measured direction in the body frame with a known reference direction in the navigation frame, it provides a constraint that improves the estimation of the rotation variable. Correct understanding and usage of this factor enhance the performance of navigation algorithms, especially in challenging environments where GPS is unavailable. # References diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index db47baa75..831788073 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -256,34 +256,30 @@ virtual class AHRSFactor : gtsam::NonlinearFactor { }; #include -//virtual class AttitudeFactor : gtsam::NonlinearFactor { -// AttitudeFactor(const Unit3& nZ, const Unit3& bRef); -// AttitudeFactor(); -//}; -virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ - Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); +virtual class Rot3AttitudeFactor : gtsam::NoiseModelFactor { + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nRef, const gtsam::noiseModel::Diagonal* model, + const gtsam::Unit3& bMeasured); + Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nRef, const gtsam::noiseModel::Diagonal* model); Rot3AttitudeFactor(); void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; + gtsam::Unit3 nRef() const; + gtsam::Unit3 bMeasured() const; }; -virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, +virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor { + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nRef, const gtsam::noiseModel::Diagonal* model, - const gtsam::Unit3& bRef); - Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, + const gtsam::Unit3& bMeasured); + Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nRef, const gtsam::noiseModel::Diagonal* model); Pose3AttitudeFactor(); void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; bool equals(const gtsam::NonlinearFactor& expected, double tol) const; - gtsam::Unit3 nZ() const; - gtsam::Unit3 bRef() const; + gtsam::Unit3 nRef() const; + gtsam::Unit3 bMeasured() const; }; #include diff --git a/gtsam/navigation/tests/testAttitudeFactor.cpp b/gtsam/navigation/tests/testAttitudeFactor.cpp index f1fde1dca..e01706df9 100644 --- a/gtsam/navigation/tests/testAttitudeFactor.cpp +++ b/gtsam/navigation/tests/testAttitudeFactor.cpp @@ -11,44 +11,44 @@ /** * @file testAttitudeFactor.cpp - * @brief Unit test for Rot3AttitudeFactor + * @brief Unit test for AttitudeFactors (rot3 and Pose3 versions) * @author Frank Dellaert * @date January 22, 2014 */ -#include +#include #include #include +#include #include + #include "gtsam/base/Matrix.h" -#include using namespace std::placeholders; using namespace std; using namespace gtsam; // ************************************************************************* -TEST( Rot3AttitudeFactor, Constructor ) { - +TEST(Rot3AttitudeFactor, Constructor) { // Example: pitch and roll of aircraft in an ENU Cartesian frame. // If pitch and roll are zero for an aerospace frame, // that means Z is pointing down, i.e., direction of Z = (0,0,-1) - Unit3 bZ(0, 0, 1); // reference direction is body Z axis - Unit3 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement" + Unit3 bMeasured(0, 0, 1); // measured direction is body Z axis + Unit3 nDown(0, 0, -1); // down, in ENU navigation frame, is "reference" // Factor Key key(1); SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25); Rot3AttitudeFactor factor0(key, nDown, model); - Rot3AttitudeFactor factor(key, nDown, model, bZ); - EXPECT(assert_equal(factor0,factor,1e-5)); + Rot3AttitudeFactor factor(key, nDown, model, bMeasured); + EXPECT(assert_equal(factor0, factor, 1e-5)); // Create a linearization point at the zero-error point Rot3 nRb; - EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(nRb),1e-5)); + EXPECT(assert_equal((Vector)Z_2x1, factor.evaluateError(nRb), 1e-5)); - auto err_fn = [&factor](const Rot3& r){ + auto err_fn = [&factor](const Rot3& r) { return factor.evaluateError(r, OptionalNone); }; // Calculate numerical derivatives @@ -80,32 +80,31 @@ TEST(Rot3AttitudeFactor, CopyAndMove) { } // ************************************************************************* -TEST( Pose3AttitudeFactor, Constructor ) { - +TEST(Pose3AttitudeFactor, Constructor) { // Example: pitch and roll of aircraft in an ENU Cartesian frame. // If pitch and roll are zero for an aerospace frame, // that means Z is pointing down, i.e., direction of Z = (0,0,-1) - Unit3 bZ(0, 0, 1); // reference direction is body Z axis - Unit3 nDown(0, 0, -1); // down, in ENU navigation frame, is "measurement" + Unit3 bMeasured(0, 0, 1); // measured direction is body Z axis + Unit3 nDown(0, 0, -1); // down, in ENU navigation frame, is "reference" // Factor Key key(1); SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25); Pose3AttitudeFactor factor0(key, nDown, model); - Pose3AttitudeFactor factor(key, nDown, model, bZ); - EXPECT(assert_equal(factor0,factor,1e-5)); + Pose3AttitudeFactor factor(key, nDown, model, bMeasured); + EXPECT(assert_equal(factor0, factor, 1e-5)); // Create a linearization point at the zero-error point Pose3 T(Rot3(), Point3(-5.0, 8.0, -11.0)); - EXPECT(assert_equal((Vector) Z_2x1,factor.evaluateError(T),1e-5)); + EXPECT(assert_equal((Vector)Z_2x1, factor.evaluateError(T), 1e-5)); Matrix actualH1; - auto err_fn = [&factor](const Pose3& p){ + auto err_fn = [&factor](const Pose3& p) { return factor.evaluateError(p, OptionalNone); }; // Calculate numerical derivatives - Matrix expectedH = numericalDerivative11(err_fn, T); + Matrix expectedH = numericalDerivative11(err_fn, T); // Use the factor to calculate the derivative Matrix actualH;