diff --git a/doc/ImuFactor.lyx b/doc/ImuFactor.lyx index a286295d40..5c0b7ce197 100644 --- a/doc/ImuFactor.lyx +++ b/doc/ImuFactor.lyx @@ -324,6 +324,200 @@ Since the Combined IMU Factor has a larger state variable due to the inclusion , similarly defined on the tangent space of the NavState manifold. \end_layout +\begin_layout Subsubsection* +Gravity as an Optimized Variable +\end_layout + +\begin_layout Standard +Both factors above treat the gravity vector +\begin_inset Formula $g\in\Rthree$ +\end_inset + +, expressed in the navigation frame, as a known constant taken from the + preintegration parameters. + Because both preintegration variants accumulate the IMU measurements +\emph on +without +\emph default + gravity +\begin_inset space ~ +\end_inset + +\begin_inset CommandInset citation +LatexCommand cite +key "Lupton12tro" +literal "false" + +\end_inset + +, gravity enters only when the preintegrated measurements are corrected + to predict +\begin_inset Formula $\widehat{X_{j}}$ +\end_inset + +, and can therefore be changed, or estimated, at correction time without + re-integrating. + The ImuFactorWithGravity and CombinedImuFactorWithGravity families exploit + this to make gravity an additional optimized variable, for navigation frames + that are not gravity-aligned (e.g. + LiDAR-odometry map frames), for initialization-free operation, or for online + gravity refinement. +\end_layout + +\begin_layout Standard +With +\begin_inset Formula $R_{i}$ +\end_inset + + the attitude at +\begin_inset Formula $t_{i}$ +\end_inset + + and +\begin_inset Formula $\Delta t_{ij}$ +\end_inset + + the preintegration interval, the gravity-dependent part of the correction + adds +\begin_inset Formula $\frac{1}{2}\Delta t_{ij}^{2}R_{i}^{T}g$ +\end_inset + + to the position rows and +\begin_inset Formula $\Delta t_{ij}R_{i}^{T}g$ +\end_inset + + to the velocity rows of the corrected tangent vector +\begin_inset Formula $\xi\in\Rnine$ +\end_inset + +, so the Jacobian of +\begin_inset Formula $\xi$ +\end_inset + + with respect to the gravity vector is simply +\begin_inset Formula +\begin{equation} +\frac{\partial\xi}{\partial g}=\begin{bmatrix}0_{3\times3}\\ +\frac{1}{2}\Delta t_{ij}^{2}R_{i}^{T}\\ +\Delta t_{ij}R_{i}^{T} +\end{bmatrix}\in\Rninethree\label{eq:gravity-jacobian} +\end{equation} + +\end_inset + +where the three block rows correspond to the rotation, position, and velocity + components of the tangent vector. + Two parametrizations of the gravity variable chain onto this matrix: +\end_layout + +\begin_layout Enumerate +ImuFactorWithGravityDirection optimizes a direction +\begin_inset Formula $d\in S^{2}$ +\end_inset + + (a Unit3, 2 +\begin_inset space ~ +\end_inset + +DOF) with a fixed magnitude +\begin_inset Formula $m$ +\end_inset + + given at construction, i.e. + +\begin_inset Formula $g=m\,d$ +\end_inset + + and +\begin_inset Formula $\partial g/\partial\delta d=m\,B$ +\end_inset + + with +\begin_inset Formula $B\in\mathbb{R}^{3\times2}$ +\end_inset + + the tangent basis of +\begin_inset Formula $d$ +\end_inset + +. + On Earth, standard gravity is accurate to about +\begin_inset Formula $0.3\%$ +\end_inset + + everywhere +\begin_inset space ~ +\end_inset + +\begin_inset CommandInset citation +LatexCommand cite +key "Nemiroff23arxiv" +literal "false" + +\end_inset + +, so fixing the magnitude removes a weakly observable degree of freedom + by construction. +\end_layout + +\begin_layout Enumerate +ImuFactorWithGravityVector optimizes the free vector +\begin_inset Formula $g\in\Rthree$ +\end_inset + +, following +\begin_inset CommandInset citation +LatexCommand cite +key "Lupton12tro" +literal "false" + +\end_inset + +. + When the magnitude is approximately known it should be paired with a norm + pseudo-observation +\begin_inset Formula $e=\left\Vert g\right\Vert -g_{0}$ +\end_inset + +, +\begin_inset Formula $\partial e/\partial g=g^{T}/\left\Vert g\right\Vert $ +\end_inset + + (a VectorNormFactor), added +\emph on +once +\emph default + per gravity variable (never per IMU factor, which would count the same + prior information multiple times). +\end_layout + +\begin_layout Standard +Since only the combination +\begin_inset Formula $R_{i}^{T}g$ +\end_inset + + is observed by the accelerometer, gravity and initial attitude are entangled: + a graph should anchor exactly one of the two, either prior knowledge of + roll and pitch or a prior on the gravity variable, but not both tightly. + Gravity magnitude and the accelerometer bias component along gravity are + only jointly observable under rotation excitation whose axis changes over + time +\begin_inset space ~ +\end_inset + +\begin_inset CommandInset citation +LatexCommand cite +key "Nemiroff23arxiv" +literal "false" + +\end_inset + +. + In the CombinedImuFactorWithGravity variants only the nine preintegration + rows of the error depend on gravity; the bias random-walk rows have a zero + gravity Jacobian. +\end_layout + \begin_layout Subsubsection* Covariance Matrices \end_layout diff --git a/doc/ImuFactor.pdf b/doc/ImuFactor.pdf index cf5d109224..6494d4b02b 100644 Binary files a/doc/ImuFactor.pdf and b/doc/ImuFactor.pdf differ diff --git a/doc/refs.bib b/doc/refs.bib index b97d4123f9..fac7b18127 100644 --- a/doc/refs.bib +++ b/doc/refs.bib @@ -70,3 +70,9 @@ @inproceedings{Trawny05report_IndirectKF author={Nikolas Trawny and Stergios I. Roumeliotis}, year={2005} } + +@misc{Nemiroff23arxiv, + author = {Nemiroff, Ryan and Chen, Kenny and Lopez, Brett T.}, + title = {Joint On-Manifold Gravity and Accelerometer Intrinsics Estimation for Inertially Aligned Mapping}, + howpublished = {arXiv:2303.03505}, + year = {2023}} diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index c957f9c76d..02e7fe9e69 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -154,6 +154,16 @@ Vector3 Unit3::unitVector(OptionalJacobian<3, 2> H) const { return p_; } +/* ************************************************************************* */ +Vector3 Unit3::scaled(double magnitude, OptionalJacobian<3, 2> H_this, + OptionalJacobian<3, 1> H_magnitude) const { + if (H_this) + *H_this = magnitude * basis(); + if (H_magnitude) + *H_magnitude = p_; + return magnitude * p_; +} + /* ************************************************************************* */ std::ostream& operator<<(std::ostream& os, const Unit3& pair) { os << pair.p_ << endl; diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 75ec07ce05..335272f4c7 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -136,7 +136,20 @@ class GTSAM_EXPORT Unit3 { /// Return unit-norm Vector Vector3 unitVector(OptionalJacobian<3, 2> H = {}) const; - /// Return scaled direction as Point3 + /** + * Return this direction scaled by a magnitude, i.e. magnitude * unitVector(). + * Useful to reconstruct a physical vector from a direction and a known (or + * separately estimated) magnitude, e.g. a gravity or magnetic field vector. + * @param magnitude scale applied to the unit vector + * @param H_this optional 3x2 Jacobian wrt this direction + * @param H_magnitude optional 3x1 Jacobian wrt the magnitude + * @sa operator*(double, const Unit3&), which returns the same value + * without Jacobians + */ + Vector3 scaled(double magnitude, OptionalJacobian<3, 2> H_this = {}, + OptionalJacobian<3, 1> H_magnitude = {}) const; + + /// Return scaled direction as Point3 (no Jacobians; see scaled()) friend Point3 operator*(double s, const Unit3& d) { return Point3(s * d.p_); } diff --git a/gtsam/geometry/tests/testUnit3.cpp b/gtsam/geometry/tests/testUnit3.cpp index a9f6cf7af5..b872219d92 100644 --- a/gtsam/geometry/tests/testUnit3.cpp +++ b/gtsam/geometry/tests/testUnit3.cpp @@ -56,6 +56,26 @@ TEST(Unit3, point3) { } } +//******************************************************************************* +static Vector3 scaled_(const Unit3& p, const double& magnitude) { + return p.scaled(magnitude); +} + +TEST(Unit3, scaled) { + const Unit3 d(1, 2, 3); + const double magnitude = 9.81; + Matrix32 actualH_d; + Matrix31 actualH_m; + const Vector3 actual = d.scaled(magnitude, actualH_d, actualH_m); + EXPECT(assert_equal(Vector3(magnitude * d.unitVector()), actual, 1e-9)); + const Matrix expectedH_d = + numericalDerivative21(scaled_, d, magnitude); + const Matrix expectedH_m = + numericalDerivative22(scaled_, d, magnitude); + EXPECT(assert_equal(expectedH_d, Matrix(actualH_d), 1e-5)); + EXPECT(assert_equal(expectedH_m, Matrix(actualH_m), 1e-5)); +} + //******************************************************************************* static Unit3 rotate_(const Rot3& R, const Unit3& p) { return R * p; } diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 8061c81cce..c04bf44623 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -184,14 +184,19 @@ bool CombinedImuFactorT::equals(const NonlinearFactor& other, double tol) c } //------------------------------------------------------------------------------ +namespace internal { +/// Shared 15-dof error and block-Jacobian assembly for CombinedImuFactorT and +/// CombinedImuFactorWithGravityT: rows 0-8 are the preintegration error for +/// the given gravity vector, rows 9-14 the bias random walk. If D_r_gvec is +/// given, it receives the 9x3 Jacobian of the preintegration rows wrt the +/// gravity vector (the bias rows have a zero gravity Jacobian). template -Vector CombinedImuFactorT::evaluateError(const Pose3& pose_i, +static Vector combinedImuError(const PIM& pim, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, - OptionalMatrixType H1, OptionalMatrixType H2, - OptionalMatrixType H3, OptionalMatrixType H4, - OptionalMatrixType H5, OptionalMatrixType H6) const { - + const Vector3& n_gravity, OptionalMatrixType H1, OptionalMatrixType H2, + OptionalMatrixType H3, OptionalMatrixType H4, OptionalMatrixType H5, + OptionalMatrixType H6, Matrix93* D_r_gvec) { // error wrt bias evolution model (random walk) Matrix6 Hbias_i, Hbias_j; Vector6 fbias = traits::Between(bias_j, bias_i, @@ -201,9 +206,10 @@ Vector CombinedImuFactorT::evaluateError(const Pose3& pose_i, Matrix93 D_r_vel_i, D_r_vel_j; // error wrt preintegrated measurements - Vector9 r_Rpv = pim_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, - bias_i, H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0, - H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0); + Vector9 r_Rpv = pim.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, + bias_i, n_gravity, H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, + H3 ? &D_r_pose_j : 0, H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0, + D_r_gvec); // if we need the jacobians if (H1) { @@ -245,9 +251,23 @@ Vector CombinedImuFactorT::evaluateError(const Pose3& pose_i, // overall error Vector r(15); - r << r_Rpv, fbias; // vector of size 15 + r << r_Rpv, fbias; // vector of size 15 return r; } +} // namespace internal + +//------------------------------------------------------------------------------ +template +Vector CombinedImuFactorT::evaluateError(const Pose3& pose_i, + const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, + OptionalMatrixType H1, OptionalMatrixType H2, + OptionalMatrixType H3, OptionalMatrixType H4, + OptionalMatrixType H5, OptionalMatrixType H6) const { + return internal::combinedImuError(pim_, pose_i, vel_i, pose_j, vel_j, + bias_i, bias_j, pim_.params()->n_gravity, + H1, H2, H3, H4, H5, H6, nullptr); +} //------------------------------------------------------------------------------ template @@ -272,5 +292,86 @@ template GTSAM_EXPORT std::ostream& operator<<>( std::ostream& os, const CombinedImuFactorT>& f); +//------------------------------------------------------------------------------ +// CombinedImuFactorWithGravityT methods +//------------------------------------------------------------------------------ +template +std::ostream& operator<<(std::ostream& os, + const CombinedImuFactorWithGravityT& f) { + f.preintegratedMeasurements().print("combined preintegrated measurements:\n"); + if (internal::GravityParametrization::usesMagnitude) + os << " gravity magnitude: " << f.gravityMagnitude() << "\n"; + os << " noise model sigmas: " << f.noiseModel()->sigmas().transpose(); + return os; +} + +//------------------------------------------------------------------------------ +template +void CombinedImuFactorWithGravityT::print( + const std::string& s, const KeyFormatter& keyFormatter) const { + std::cout << (s.empty() ? s : s + "\n") << "CombinedImuFactorWithGravity(" + << keyFormatter(this->template key<1>()) << "," << keyFormatter(this->template key<2>()) + << "," << keyFormatter(this->template key<3>()) << "," << keyFormatter(this->template key<4>()) + << "," << keyFormatter(this->template key<5>()) << "," << keyFormatter(this->template key<6>()) + << "," << keyFormatter(this->template key<7>()) << ")\n"; + std::cout << *this << std::endl; +} -} // namespace gtsam \ No newline at end of file +//------------------------------------------------------------------------------ +template +bool CombinedImuFactorWithGravityT::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) && + (!internal::GravityParametrization::usesMagnitude || + std::abs(gravityMagnitude_ - e->gravityMagnitude_) < tol); +} + +//------------------------------------------------------------------------------ +template +Vector CombinedImuFactorWithGravityT::evaluateError( + const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, + const Vector3& vel_j, const imuBias::ConstantBias& bias_i, + const imuBias::ConstantBias& bias_j, const GRAVITY& gravity, + OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, + OptionalMatrixType H4, OptionalMatrixType H5, OptionalMatrixType H6, + OptionalMatrixType H7) const { + typedef internal::GravityParametrization Parametrization; + + Eigen::Matrix D_gvec_gravity; + const Vector3 n_gravity = Parametrization::vector( + gravity, gravityMagnitude_, H7 ? &D_gvec_gravity : nullptr); + + Matrix93 D_r_gvec; + const Vector r = internal::combinedImuError( + pim_, pose_i, vel_i, pose_j, vel_j, bias_i, bias_j, n_gravity, + H1, H2, H3, H4, H5, H6, H7 ? &D_r_gvec : nullptr); + + if (H7) { + // Only the preintegration rows depend on gravity: + H7->resize(15, Parametrization::dimension); + H7->template block<9, Parametrization::dimension>(0, 0) = + D_r_gvec * D_gvec_gravity; + H7->template block<6, Parametrization::dimension>(9, 0).setZero(); + } + return r; +} + +//------------------------------------------------------------------------------ +// CombinedImuFactorWithGravityT instantiations +template class GTSAM_EXPORT CombinedImuFactorWithGravityT, Unit3>; +template class GTSAM_EXPORT CombinedImuFactorWithGravityT, Unit3>; +template class GTSAM_EXPORT CombinedImuFactorWithGravityT, Point3>; +template class GTSAM_EXPORT CombinedImuFactorWithGravityT, Point3>; + +template GTSAM_EXPORT std::ostream& operator<<( + std::ostream& os, const CombinedImuFactorWithGravityT, Unit3>& f); +template GTSAM_EXPORT std::ostream& operator<<( + std::ostream& os, const CombinedImuFactorWithGravityT, Unit3>& f); +template GTSAM_EXPORT std::ostream& operator<<( + std::ostream& os, const CombinedImuFactorWithGravityT, Point3>& f); +template GTSAM_EXPORT std::ostream& operator<<( + std::ostream& os, const CombinedImuFactorWithGravityT, Point3>& f); + + +} // namespace gtsam diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 5764d87131..eb1e607b92 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -308,6 +308,139 @@ using CombinedImuFactor = CombinedImuFactorT<>; template GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const CombinedImuFactorT& f); +/** + * CombinedImuFactorWithGravityT is CombinedImuFactorT with an additional + * GRAVITY variable, so that gravity can be optimized instead of being fixed + * by the preintegration parameters. The 15-dimensional residual is identical + * to CombinedImuFactorT (9 preintegration rows and 6 bias random walk rows); + * only the preintegration rows depend on gravity. Jointly estimating gravity + * and the (evolving) accelerometer bias requires sufficient rotation + * excitation, see Nemiroff, Chen and Lopez, "Joint On-Manifold Gravity and + * Accelerometer Intrinsics Estimation for Inertially Aligned Mapping", 2023. + * + * See ImuFactorWithGravityDirection / ImuFactorWithGravityVector for the two + * gravity parametrizations and their usage guidance. + * + * @ingroup navigation + */ +template +class GTSAM_EXPORT CombinedImuFactorWithGravityT + : public NoiseModelFactorN { + private: + typedef CombinedImuFactorWithGravityT This; + typedef NoiseModelFactorN + Base; + + PIM pim_; + double gravityMagnitude_; ///< used by the Unit3 parametrization only + + public: + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + /** Shorthand for a smart pointer to a factor */ + typedef std::shared_ptr shared_ptr; + + /** Default constructor - only use for serialization */ + CombinedImuFactorWithGravityT() : gravityMagnitude_(0.0) {} + + /** + * Constructor + * @param pose_i Previous pose key + * @param vel_i Previous velocity key + * @param pose_j Current pose key + * @param vel_j Current velocity key + * @param bias_i Previous bias key + * @param bias_j Current bias key + * @param gravity Gravity key + * @param preintegratedMeasurements Combined IMU measurements + * @param gravityMagnitude The known gravity magnitude for the Unit3 + * parametrization; defaults to the norm of the gravity vector in the + * preintegration params. Unused by the Point3 parametrization. + */ + CombinedImuFactorWithGravityT( + Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, + Key gravity, const PIM& preintegratedMeasurements, + std::optional gravityMagnitude = {}) + : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov()), + pose_i, vel_i, pose_j, vel_j, bias_i, bias_j, gravity), + pim_(preintegratedMeasurements), + gravityMagnitude_(gravityMagnitude + ? *gravityMagnitude + : preintegratedMeasurements.params()->n_gravity.norm()) { + if (internal::GravityParametrization::usesMagnitude && + !(gravityMagnitude_ > 0.0)) + throw std::invalid_argument( + "CombinedImuFactorWithGravityT: gravityMagnitude must be positive"); + } + + ~CombinedImuFactorWithGravityT() override {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::make_shared(*this); + } + + /// @name Testable + /// @{ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; + /// @} + + /** Access the preintegrated measurements. */ + const PIM& preintegratedMeasurements() const { return pim_; } + + /** The gravity magnitude used by the Unit3 parametrization. */ + double gravityMagnitude() const { return gravityMagnitude_; } + + /** implement functions needed to derive from Factor */ + + /// vector of errors + Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, + const imuBias::ConstantBias& bias_j, + const GRAVITY& gravity, OptionalMatrixType H1, + OptionalMatrixType H2, OptionalMatrixType H3, + OptionalMatrixType H4, OptionalMatrixType H5, + OptionalMatrixType H6, OptionalMatrixType H7) const override; + + private: +#if GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + // Archive name for the base follows the sibling factors' convention: + ar& boost::serialization::make_nvp( + "NoiseModelFactor7", boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(pim_); + ar& BOOST_SERIALIZATION_NVP(gravityMagnitude_); + } +#endif +}; +// class CombinedImuFactorWithGravityT + +/// CombinedImuFactor variant optimizing the gravity direction (Unit3) with a +/// fixed, known magnitude; see ImuFactorWithGravityDirection. +using CombinedImuFactorWithGravityDirection = + CombinedImuFactorWithGravityT; + +/// CombinedImuFactor variant optimizing the free gravity vector (Point3); +/// see ImuFactorWithGravityVector. +using CombinedImuFactorWithGravityVector = + CombinedImuFactorWithGravityT; + +// operator<< for CombinedImuFactorWithGravityT +template +GTSAM_EXPORT std::ostream& operator<<( + std::ostream& os, const CombinedImuFactorWithGravityT& f); + template <> struct traits : public Testable {}; @@ -319,4 +452,8 @@ struct traits> template struct traits> : public Testable> {}; +template +struct traits> + : public Testable> {}; + } // namespace gtsam diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index cee2f5cb3d..bedea0c3c1 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -210,5 +210,74 @@ template GTSAM_EXPORT std::ostream& operator<<>( std::ostream& os, const ImuFactor2T>& f); +//------------------------------------------------------------------------------ +// ImuFactorWithGravityT methods +//------------------------------------------------------------------------------ +template +std::ostream& operator<<(std::ostream& os, const ImuFactorWithGravityT& f) { + f.preintegratedMeasurements().print("preintegrated measurements:\n"); + if (internal::GravityParametrization::usesMagnitude) + os << " gravity magnitude: " << f.gravityMagnitude() << "\n"; + os << " noise model sigmas: " << f.noiseModel()->sigmas().transpose(); + return os; +} + +//------------------------------------------------------------------------------ +template +void ImuFactorWithGravityT::print(const string& s, + const KeyFormatter& keyFormatter) const { + cout << (s.empty() ? s : s + "\n") << "ImuFactorWithGravity(" + << keyFormatter(this->template key<1>()) << "," << keyFormatter(this->template key<2>()) + << "," << keyFormatter(this->template key<3>()) << "," << keyFormatter(this->template key<4>()) + << "," << keyFormatter(this->template key<5>()) << "," << keyFormatter(this->template key<6>()) + << ")\n"; + cout << *this << endl; +} + +//------------------------------------------------------------------------------ +template +bool ImuFactorWithGravityT::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) && + (!internal::GravityParametrization::usesMagnitude || + std::abs(gravityMagnitude_ - e->gravityMagnitude_) < tol); +} + +//------------------------------------------------------------------------------ +template +Vector ImuFactorWithGravityT::evaluateError(const Pose3& pose_i, + const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, const GRAVITY& gravity, + OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3, + OptionalMatrixType H4, OptionalMatrixType H5, OptionalMatrixType H6) const { + typedef internal::GravityParametrization Parametrization; + Eigen::Matrix D_gvec_gravity; + const Vector3 n_gravity = Parametrization::vector( + gravity, gravityMagnitude_, H6 ? &D_gvec_gravity : nullptr); + Matrix93 D_r_gvec; + const Vector r = pim_.computeErrorAndJacobians( + pose_i, vel_i, pose_j, vel_j, bias_i, n_gravity, H1, H2, H3, H4, H5, + H6 ? &D_r_gvec : nullptr); + if (H6) *H6 = D_r_gvec * D_gvec_gravity; + return r; +} + +//------------------------------------------------------------------------------ +// ImuFactorWithGravityT instantiations +template class GTSAM_EXPORT ImuFactorWithGravityT, Unit3>; +template class GTSAM_EXPORT ImuFactorWithGravityT, Unit3>; +template class GTSAM_EXPORT ImuFactorWithGravityT, Point3>; +template class GTSAM_EXPORT ImuFactorWithGravityT, Point3>; + +template GTSAM_EXPORT std::ostream& operator<<( + std::ostream& os, const ImuFactorWithGravityT, Unit3>& f); +template GTSAM_EXPORT std::ostream& operator<<( + std::ostream& os, const ImuFactorWithGravityT, Unit3>& f); +template GTSAM_EXPORT std::ostream& operator<<( + std::ostream& os, const ImuFactorWithGravityT, Point3>& f); +template GTSAM_EXPORT std::ostream& operator<<( + std::ostream& os, const ImuFactorWithGravityT, Point3>& f); + } // namespace gtsam diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index eb90b477b5..f6c89fac39 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -436,6 +436,197 @@ using ImuFactor2 = ImuFactor2T<>; template GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const ImuFactor2T& f); +/** + * ImuFactorWithGravityT is a 6-ways factor: in addition to the previous and + * current states (pose and velocity) and the bias estimate of ImuFactorT, it + * involves a GRAVITY variable so that gravity can be optimized instead of + * being fixed by the preintegration parameters. Two parametrizations are + * provided, see ImuFactorWithGravityDirection and ImuFactorWithGravityVector. + * + * As gravity in the nav frame is entangled with the initial attitude (only + * their combination is observed by the accelerometer), a graph should anchor + * one of the two: either prior knowledge of attitude (roll/pitch), or a prior + * on the gravity variable - not both tightly. + * + * Like ImuFactorT, this factor does not model temporal consistency of the + * biases, which is up to the caller; see CombinedImuFactorWithGravityT for a + * variant that does. + * + * @ingroup navigation + */ +template +class GTSAM_EXPORT ImuFactorWithGravityT + : public NoiseModelFactorN { +private: + + typedef ImuFactorWithGravityT This; + typedef NoiseModelFactorN Base; + + PIM pim_; + double gravityMagnitude_; ///< used by the Unit3 parametrization only + +public: + + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + /** Shorthand for a smart pointer to a factor */ + typedef std::shared_ptr shared_ptr; + + /** Default constructor - only use for serialization */ + ImuFactorWithGravityT() : gravityMagnitude_(0.0) {} + + /** + * Constructor + * @param pose_i Previous pose key + * @param vel_i Previous velocity key + * @param pose_j Current pose key + * @param vel_j Current velocity key + * @param bias Previous bias key + * @param gravity Gravity key + * @param preintegratedMeasurements The preintegrated measurements since the + * last pose + * @param gravityMagnitude The known gravity magnitude for the Unit3 + * parametrization; defaults to the norm of the gravity vector in the + * preintegration params. Unused by the Point3 parametrization. + */ + ImuFactorWithGravityT(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, + Key gravity, const PIM& preintegratedMeasurements, + std::optional gravityMagnitude = {}) + : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.preintMeasCov()), + pose_i, vel_i, pose_j, vel_j, bias, gravity), + pim_(preintegratedMeasurements), + gravityMagnitude_(gravityMagnitude + ? *gravityMagnitude + : preintegratedMeasurements.params()->n_gravity.norm()) { + if (internal::GravityParametrization::usesMagnitude && + !(gravityMagnitude_ > 0.0)) + throw std::invalid_argument( + "ImuFactorWithGravityT: gravityMagnitude must be positive"); + } + + ~ImuFactorWithGravityT() override { + } + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::make_shared(*this); + } + + /// @name Testable + /// @{ + void print(const std::string& s = "", const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const override; + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override; + /// @} + + /** Access the preintegrated measurements. */ + const PIM& preintegratedMeasurements() const { + return pim_; + } + + /** The gravity magnitude used by the Unit3 parametrization. */ + double gravityMagnitude() const { + return gravityMagnitude_; + } + + /** implement functions needed to derive from Factor */ + + /// vector of errors + Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, + const GRAVITY& gravity, OptionalMatrixType H1, + OptionalMatrixType H2, OptionalMatrixType H3, + OptionalMatrixType H4, OptionalMatrixType H5, + OptionalMatrixType H6) const override; + + /// Merge two factors sharing bias and gravity keys, with consecutive states + template >::value + >::type + > + static typename ImuFactorWithGravityT::shared_ptr Merge( + const typename ImuFactorWithGravityT::shared_ptr& f01, + const typename ImuFactorWithGravityT::shared_ptr& f12 + ) { + if (f01->template key<5>() != f12->template key<5>()) + throw std::domain_error("ImuFactorWithGravityT::Merge: IMU bias keys must be the same"); + + if (f01->template key<6>() != f12->template key<6>()) + throw std::domain_error("ImuFactorWithGravityT::Merge: gravity keys must be the same"); + + if (internal::GravityParametrization::usesMagnitude && + std::abs(f01->gravityMagnitude() - f12->gravityMagnitude()) > 1e-9) + throw std::domain_error( + "ImuFactorWithGravityT::Merge: gravity magnitudes must be the same"); + + if (f01->template key<3>() != f12->template key<1>() || f01->template key<4>() != f12->template key<2>()) + throw std::domain_error( + "ImuFactorWithGravityT::Merge: intermediate pose, velocity keys need to match up"); + + auto pim02 = ImuFactorT::Merge(f01->preintegratedMeasurements(), + f12->preintegratedMeasurements()); + + return std::make_shared( + f01->template key<1>(), // P0 + f01->template key<2>(), // V0 + f12->template key<3>(), // P2 + f12->template key<4>(), // V2 + f01->template key<5>(), // B + f01->template key<6>(), // G + pim02, f01->gravityMagnitude()); + } + + private: +#if GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + // Archive name for the base follows the sibling factors' convention: + ar & boost::serialization::make_nvp("NoiseModelFactor6", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(pim_); + ar & BOOST_SERIALIZATION_NVP(gravityMagnitude_); + } +#endif +}; +// class ImuFactorWithGravityT + +/** + * ImuFactor variant with the gravity direction as an optimized Unit3 variable + * and a fixed, known magnitude (given at construction, defaulting to the norm + * of the gravity vector in the preintegration params). This is the preferred + * parametrization when the magnitude is known, eg. on Earth where standard + * gravity is accurate to ~0.3% everywhere; it removes the magnitude degree of + * freedom by construction. See eg. Nemiroff, Chen and Lopez, "Joint + * On-Manifold Gravity and Accelerometer Intrinsics Estimation for Inertially + * Aligned Mapping", 2023. + */ +using ImuFactorWithGravityDirection = + ImuFactorWithGravityT; + +/** + * ImuFactor variant with the gravity vector as a free Point3 variable, ie. + * direction and magnitude both optimized, following Lupton and Sukkarieh, + * "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built + * Environments Without Initial Conditions", TRO 2012. If the magnitude is + * approximately known, add a VectorNormFactor<3> on the gravity variable once + * (Lupton's magnitude pseudo-observation); if it is known exactly, prefer + * ImuFactorWithGravityDirection. + */ +using ImuFactorWithGravityVector = + ImuFactorWithGravityT; + +// operator<< for ImuFactorWithGravityT +template +GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, + const ImuFactorWithGravityT& f); + template struct traits> : public Testable> {}; @@ -445,4 +636,8 @@ struct traits> : public Testable> {}; template struct traits> : public Testable> {}; +template +struct traits> + : public Testable> {}; + } /// namespace gtsam diff --git a/gtsam/navigation/MagFactor.h b/gtsam/navigation/MagFactor.h index 4f497d503e..eaf5d3cec9 100644 --- a/gtsam/navigation/MagFactor.h +++ b/gtsam/navigation/MagFactor.h @@ -209,16 +209,16 @@ class MagFactor3: public NoiseModelFactorN { const Point3& bias, OptionalMatrixType H1, OptionalMatrixType H2, OptionalMatrixType H3) const override { // measured bM = nRb� * nM + b, where b is unknown bias - Unit3 rotated = bRn_.rotate(direction, OptionalNone, H2); - Point3 hx = scale * rotated.point3() + bias; + Matrix22 D_rotated_direction; + Unit3 rotated = bRn_.rotate(direction, {}, H2 ? &D_rotated_direction : 0); + Matrix32 D_hx_rotated; + Matrix31 D_hx_scale; + Point3 hx = rotated.scaled(scale, H2 ? &D_hx_rotated : 0, + H1 ? &D_hx_scale : 0) + bias; if (H1) - *H1 = rotated.point3(); - if (H2) // H2 is 2*2, but we need 3*2 - { - Matrix H; - rotated.point3(H); - *H2 = scale * H * (*H2); - } + *H1 = D_hx_scale; + if (H2) + *H2 = D_hx_rotated * D_rotated_direction; if (H3) *H3 = I_3x3; return (hx - measured_); diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 54ab9e678f..6ee530ed6a 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -294,36 +294,46 @@ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, Vector9 NavState::correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity, const std::optional& omegaCoriolis, bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 9> H2) const { + OptionalJacobian<9, 9> H2, OptionalJacobian<9, 3> H3) const { const Rot3& nRb = R_; const Velocity3 n_v = t_.col(1); // derivative is Ri ! const double dt22 = 0.5 * dt * dt; Vector9 xi; - Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri; + Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv; + // The gravity contributions to the position and velocity rows share both + // the unrotated vector and the Jacobians wrt rotation and wrt gravity. + Matrix3 D_bGravity_nGravity; + const Vector3 b_gravity = nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0, + H3 ? &D_bGravity_nGravity : 0); dR(xi) = dR(pim); dP(xi) = dP(pim) + dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0) - + dt22 * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0); - dV(xi) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0); + + dt22 * b_gravity; + dV(xi) = dV(pim) + dt * b_gravity; if (omegaCoriolis) { xi += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1); } - if (H1 || H2) { - Matrix3 Ri = nRb.matrix(); - + if (H1 || H2 || H3) { if (H1) { + const Matrix3 Ri = nRb.matrix(); if (!omegaCoriolis) H1->setZero(); // if coriolis H1 is already initialized D_t_R(H1) += dt * D_dP_Ri1 + dt22 * D_dP_Ri2; D_t_v(H1) += dt * D_dP_nv * Ri; - D_v_R(H1) += dt * D_dV_Ri; + D_v_R(H1) += dt * D_dP_Ri2; } if (H2) { H2->setIdentity(); } + if (H3) { + // The rotation rows do not depend on gravity: + H3->block<3, 3>(0, 0).setZero(); + H3->block<3, 3>(3, 0) = dt22 * D_bGravity_nGravity; + H3->block<3, 3>(6, 0) = dt * D_bGravity_nGravity; + } } return xi; diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index 62e76bef58..63a7ebe9ce 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -241,7 +241,8 @@ class GTSAM_EXPORT NavState : public ExtendedPose3<2, NavState> { Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity, const std::optional& omegaCoriolis, bool use2ndOrderCoriolis = false, OptionalJacobian<9, 9> H1 = {}, - OptionalJacobian<9, 9> H2 = {}) const; + OptionalJacobian<9, 9> H2 = {}, + OptionalJacobian<9, 3> H3 = {}) const; /// @} diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index a4427f8e3c..3fb80c4a7f 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -115,62 +115,91 @@ void PreintegrationBase::integrateMeasurement(const Vector3& measuredAcc, //------------------------------------------------------------------------------ NavState PreintegrationBase::predict(const NavState& state_i, - const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, - OptionalJacobian<9, 6> H2) const { + const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, + OptionalJacobian<9, 9> H1, OptionalJacobian<9, 6> H2, + OptionalJacobian<9, 3> H3) const { Matrix96 D_biasCorrected_bias; Vector9 biasCorrected = biasCorrectedDelta(bias_i, H2 ? &D_biasCorrected_bias : nullptr); // Correct for initial velocity and gravity Matrix9 D_delta_state, D_delta_biasCorrected; - Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, + Matrix93 D_delta_gravity; + Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, n_gravity, p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : nullptr, - H2 ? &D_delta_biasCorrected : nullptr); + H2 ? &D_delta_biasCorrected : nullptr, + H3 ? &D_delta_gravity : nullptr); // Use retract to get back to NavState manifold Matrix9 D_predict_state, D_predict_delta; NavState state_j = state_i.retract(xi, H1 ? &D_predict_state : nullptr, - H1 || H2 ? &D_predict_delta : nullptr); + H1 || H2 || H3 ? &D_predict_delta : nullptr); if (H1) *H1 = D_predict_state + D_predict_delta * D_delta_state; if (H2) *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias; + if (H3) + *H3 = D_predict_delta * D_delta_gravity; return state_j; } +//------------------------------------------------------------------------------ +NavState PreintegrationBase::predict(const NavState& state_i, + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 6> H2) const { + return predict(state_i, bias_i, p().n_gravity, H1, H2, nullptr); +} + //------------------------------------------------------------------------------ Vector9 PreintegrationBase::computeError(const NavState& state_i, const NavState& state_j, const imuBias::ConstantBias& bias_i, + const Vector3& n_gravity, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, - OptionalJacobian<9, 6> H3) const { + OptionalJacobian<9, 6> H3, + OptionalJacobian<9, 3> H4) const { // Predict state at time j Matrix9 D_predict_state_i; Matrix96 D_predict_bias_i; + Matrix93 D_predict_gravity; NavState predictedState_j = predict( - state_i, bias_i, H1 ? &D_predict_state_i : 0, H3 ? &D_predict_bias_i : 0); + state_i, bias_i, n_gravity, H1 ? &D_predict_state_i : 0, + H3 ? &D_predict_bias_i : 0, H4 ? &D_predict_gravity : 0); // Calculate error Matrix9 D_error_state_j, D_error_predict; Vector9 error = state_j.localCoordinates(predictedState_j, H2 ? &D_error_state_j : 0, - H1 || H3 ? &D_error_predict : 0); + H1 || H3 || H4 ? &D_error_predict : 0); if (H1) *H1 << D_error_predict * D_predict_state_i; if (H2) *H2 << D_error_state_j; if (H3) *H3 << D_error_predict * D_predict_bias_i; + if (H4) *H4 << D_error_predict * D_predict_gravity; return error; } +//------------------------------------------------------------------------------ +Vector9 PreintegrationBase::computeError(const NavState& state_i, + const NavState& state_j, + const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 9> H1, + OptionalJacobian<9, 9> H2, + OptionalJacobian<9, 6> H3) const { + return computeError(state_i, state_j, bias_i, p().n_gravity, H1, H2, H3, + nullptr); +} + //------------------------------------------------------------------------------ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1, - OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3, - OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const { + const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, + OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2, + OptionalJacobian<9, 6> H3, OptionalJacobian<9, 3> H4, + OptionalJacobian<9, 6> H5, OptionalJacobian<9, 3> H6) const { // Note that derivative of constructors below is not identity for velocity, but // a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose() @@ -179,8 +208,8 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, // Predict state at time j Matrix9 D_error_state_i, D_error_state_j; - Vector9 error = computeError(state_i, state_j, bias_i, - H1 || H2 ? &D_error_state_i : 0, H3 || H4 ? &D_error_state_j : 0, H5); + Vector9 error = computeError(state_i, state_j, bias_i, n_gravity, + H1 || H2 ? &D_error_state_i : 0, H3 || H4 ? &D_error_state_j : 0, H5, H6); // Separate out derivatives in terms of 5 arguments // Note that doing so requires special treatment of velocities, as when treated as @@ -195,6 +224,16 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, return error; } +//------------------------------------------------------------------------------ +Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, + const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1, + OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3, + OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const { + return computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i, + p().n_gravity, H1, H2, H3, H4, H5, nullptr); +} + //------------------------------------------------------------------------------ } // namespace gtsam diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index b902c94210..79c5fcf193 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -150,26 +151,57 @@ class GTSAM_EXPORT PreintegrationBase { virtual Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H = {}) const = 0; - /// Predict state at time j + /** + * Predict state at time j, for a given gravity vector in the nav frame. + * This overload allows gravity to differ from params (eg. when gravity is + * an optimized variable, see ImuFactorWithGravityDirection and + * ImuFactorWithGravityVector); H3 is the Jacobian wrt that vector. + */ + NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, + const Vector3& n_gravity, + OptionalJacobian<9, 9> H1 = {}, + OptionalJacobian<9, 6> H2 = {}, + OptionalJacobian<9, 3> H3 = {}) const; + + /// Predict state at time j, using the gravity vector from params NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1 = {}, OptionalJacobian<9, 6> H2 = {}) const; - /// Calculate error given navStates + /// Calculate error given navStates and a gravity vector in the nav frame + Vector9 computeError(const NavState& state_i, const NavState& state_j, + const imuBias::ConstantBias& bias_i, + const Vector3& n_gravity, + OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, + OptionalJacobian<9, 6> H3, + OptionalJacobian<9, 3> H4) const; + + /// Calculate error given navStates, using the gravity vector from params Vector9 computeError(const NavState& state_i, const NavState& state_j, const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2, OptionalJacobian<9, 6> H3) const; + /** + * Compute errors w.r.t. preintegrated measurements and jacobians + * wrt pose_i, vel_i, bias_i, pose_j, bias_j and the given gravity vector + */ + Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, + const Pose3& pose_j, const Vector3& vel_j, + const imuBias::ConstantBias& bias_i, const Vector3& n_gravity, + OptionalJacobian<9, 6> H1 = {}, OptionalJacobian<9, 3> H2 = {}, + OptionalJacobian<9, 6> H3 = {}, OptionalJacobian<9, 3> H4 = {}, + OptionalJacobian<9, 6> H5 = {}, OptionalJacobian<9, 3> H6 = {}) const; + /** * Compute errors w.r.t. preintegrated measurements and jacobians * wrt pose_i, vel_i, bias_i, pose_j, bias_j */ Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, - const imuBias::ConstantBias& bias_i, + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1 = {}, OptionalJacobian<9, 3> H2 = {}, - OptionalJacobian<9, 6> H3 = {}, OptionalJacobian<9, 3> H4 = {}, + OptionalJacobian<9, 6> H3 = {}, OptionalJacobian<9, 3> H4 = {}, OptionalJacobian<9, 6> H5 = {}) const; private: @@ -185,4 +217,40 @@ class GTSAM_EXPORT PreintegrationBase { #endif }; +namespace internal { + +/** + * Adapter mapping a gravity parametrization GRAVITY to the nav-frame gravity + * vector expected by PreintegrationBase, with the chain-rule Jacobian block. + * Two parametrizations are provided: + * - Unit3: an optimized direction scaled by a fixed, known magnitude + * - Point3: a free vector entangling direction and magnitude + * See ImuFactorWithGravityDirection and ImuFactorWithGravityVector. + */ +template +struct GravityParametrization; + +template <> +struct GravityParametrization { + constexpr static int dimension = 2; + constexpr static bool usesMagnitude = true; + static Vector3 vector(const Unit3& gravity, double magnitude, + OptionalJacobian<3, 2> H = {}) { + return gravity.scaled(magnitude, H); + } +}; + +template <> +struct GravityParametrization { + constexpr static int dimension = 3; + constexpr static bool usesMagnitude = false; + static Vector3 vector(const Point3& gravity, double /*magnitude*/, + OptionalJacobian<3, 3> H = {}) { + if (H) H->setIdentity(); + return gravity; + } +}; + +} // namespace internal + } /// namespace gtsam diff --git a/gtsam/navigation/doc/CombinedImuFactor.ipynb b/gtsam/navigation/doc/CombinedImuFactor.ipynb index 76cf4e738c..4d46be3963 100644 --- a/gtsam/navigation/doc/CombinedImuFactor.ipynb +++ b/gtsam/navigation/doc/CombinedImuFactor.ipynb @@ -15,7 +15,9 @@ "2. **6-Way Factor:** Consequently, it connects *six* variables: Pose_i, Vel_i, Bias_i, Pose_j, Vel_j, and Bias_j.\n", "3. **Combined Preintegration:** It uses `PreintegratedCombinedMeasurements` which propagates a full 15x15 covariance matrix, accounting for correlations between the preintegrated state and the biases, as well as the bias random walk noise.\n", "\n", - "This factor is generally preferred when bias stability is a concern or when modeling the time-varying nature of biases is important for accuracy. It eliminates the need for separate `BetweenFactor`s on bias variables." + "This factor is generally preferred when bias stability is a concern or when modeling the time-varying nature of biases is important for accuracy. It eliminates the need for separate `BetweenFactor`s on bias variables.\n", + "\n", + "To additionally estimate gravity as an optimized variable (eg. in a nav frame that is not gravity-aligned), see [CombinedImuFactorWithGravityDirection / ...Vector](ImuFactorWithGravity.ipynb)." ] }, { diff --git a/gtsam/navigation/doc/ImuFactor.ipynb b/gtsam/navigation/doc/ImuFactor.ipynb index b68e353486..0f01f95f2a 100644 --- a/gtsam/navigation/doc/ImuFactor.ipynb +++ b/gtsam/navigation/doc/ImuFactor.ipynb @@ -36,7 +36,9 @@ "\n", "Both factors assume that the bias is *constant* between time $i$ and $j$ for the purpose of evaluating its error. That is typically a very good assumption, as bias evolves slowly over time.\n", "\n", - "The factors do *not* model the evolution of bias over time; if bias is expected to change, separate `BetweenFactor`s on bias variables are typically needed, or the `CombinedImuFactor` should be used instead." + "The factors do *not* model the evolution of bias over time; if bias is expected to change, separate `BetweenFactor`s on bias variables are typically needed, or the `CombinedImuFactor` should be used instead.\n", + "\n", + "If the navigation frame is not gravity-aligned, or gravity is uncertain, gravity can be estimated as an optimized variable instead of being fixed by the preintegration parameters: see the [ImuFactorWithGravity family](ImuFactorWithGravity.ipynb)." ] }, { diff --git a/gtsam/navigation/doc/ImuFactorWithGravity.ipynb b/gtsam/navigation/doc/ImuFactorWithGravity.ipynb new file mode 100644 index 0000000000..2b0763e183 --- /dev/null +++ b/gtsam/navigation/doc/ImuFactorWithGravity.ipynb @@ -0,0 +1,334 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "81b99136", + "metadata": {}, + "source": [ + "# ImuFactorWithGravity Family\n", + "\n", + "\"Open\n", + "\n", + "## Overview\n", + "\n", + "The standard [ImuFactor](ImuFactor.ipynb) treats the nav-frame gravity vector as a known\n", + "constant from `PreintegrationParams`. The `ImuFactorWithGravity` family instead makes\n", + "gravity an **optimized variable**, for workflows where the navigation frame is not\n", + "gravity-aligned (eg. LiDAR-odometry map frames), for initialization-free operation, or\n", + "for online gravity refinement.\n", + "\n", + "Gravity knowledge falls on a spectrum, handled by three factor choices:\n", + "\n", + "1. **Known exactly**: use plain `ImuFactor` (gravity from params). Zero cost.\n", + "2. **Known magnitude, unknown direction**: `ImuFactorWithGravityDirection` optimizes a\n", + " `Unit3` direction (2 DOF on $S^2$) scaled by a fixed magnitude given at construction.\n", + " On Earth, standard gravity is accurate to ~0.3% everywhere, so this is usually the\n", + " right choice; it removes the magnitude degree of freedom by construction.\n", + "3. **Unknown magnitude and direction**: `ImuFactorWithGravityVector` optimizes the free\n", + " vector $g \\in \\mathbb{R}^3$ (as a `Point3`), following Lupton and Sukkarieh (2012).\n", + " When the magnitude is *approximately* known, pair it with a single\n", + " [VectorNormFactor](../../nonlinear/doc/VectorNormFactor.ipynb) on the gravity variable.\n", + "\n", + "`CombinedImuFactorWithGravityDirection` / `...Vector` are the corresponding variants of\n", + "[CombinedImuFactor](CombinedImuFactor.ipynb), adding gravity as a 7th variable while\n", + "keeping the bias random walk rows (which have a zero gravity Jacobian).\n", + "\n", + "**Observability**: only the combination $R_i^T g$ is observed by the accelerometer, so\n", + "nav-frame gravity and initial attitude are entangled: anchor exactly one of them (a\n", + "roll/pitch prior *or* a gravity prior, not both tightly). Gravity magnitude and the\n", + "accelerometer bias along gravity are only jointly observable under rotation excitation\n", + "whose axis changes over time (Nemiroff, Chen and Lopez, 2023)." + ] + }, + { + "cell_type": "markdown", + "id": "9efc39bf", + "metadata": {}, + "source": [ + "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n", + "Atlanta, Georgia 30332-0415\n", + "All Rights Reserved\n", + "\n", + "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", + "\n", + "See LICENSE for the license information" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "id": "b9b26d2f", + "metadata": { + "execution": { + "iopub.execute_input": "2026-08-01T10:20:52.382090Z", + "iopub.status.busy": "2026-08-01T10:20:52.381908Z", + "iopub.status.idle": "2026-08-01T10:20:52.944753Z", + "shell.execute_reply": "2026-08-01T10:20:52.944192Z" + } + }, + "outputs": [], + "source": [ + "# Install gtsam-develop if not installed\n", + "try:\n", + " import gtsam\n", + "except ImportError:\n", + " %pip install --quiet gtsam-develop" + ] + }, + { + "cell_type": "markdown", + "id": "3a646b2d", + "metadata": {}, + "source": [ + "## Mathematical Formulation\n", + "\n", + "Both tangent- and manifold-preintegration accumulate the IMU measurements *without*\n", + "gravity; gravity enters only in the correction step `NavState::correctPIM` (the key\n", + "insight of Lupton and Sukkarieh: gravity can be changed, or estimated, at correction\n", + "time without re-integrating). With $R_i$ the attitude of state $i$ and $\\Delta t$ the\n", + "preintegration interval, the corrected tangent vector is\n", + "\n", + "$$\n", + "\\begin{aligned}\n", + "\\xi_{\\Delta R} &= \\tilde\\xi_{\\Delta R} \\\\\n", + "\\xi_{\\Delta p} &= \\tilde\\xi_{\\Delta p} + \\Delta t\\, R_i^T v_i + \\tfrac{1}{2}\\Delta t^2\\, R_i^T g \\\\\n", + "\\xi_{\\Delta v} &= \\tilde\\xi_{\\Delta v} + \\Delta t\\, R_i^T g\n", + "\\end{aligned}\n", + "$$\n", + "\n", + "so the Jacobian with respect to the gravity *vector* is simply\n", + "\n", + "$$\n", + "\\frac{\\partial \\xi}{\\partial g} =\n", + "\\begin{bmatrix} 0_{3\\times3} \\\\ \\tfrac{1}{2}\\Delta t^2 R_i^T \\\\ \\Delta t\\, R_i^T \\end{bmatrix}\n", + "\\in \\mathbb{R}^{9\\times3}.\n", + "$$\n", + "\n", + "The two parametrizations chain onto this block:\n", + "- **Direction**: $g = m\\,d$ with $d \\in S^2$ a `Unit3` and $m$ fixed, so\n", + " $\\partial g / \\partial \\delta d = m B$ where $B \\in \\mathbb{R}^{3\\times2}$ is the\n", + " tangent basis of $d$ (`Unit3::scaled`) — the same scale-times-direction\n", + " decomposition as [MagFactor3](MagFactor.ipynb).\n", + "- **Vector**: $g$ free, $\\partial g / \\partial g = I_3$; the optional norm\n", + " pseudo-observation is $e = \\lVert g \\rVert - 9.81$ with\n", + " $\\partial e/\\partial g = g^T/\\lVert g \\rVert$, added **once per gravity variable**\n", + " (never per IMU factor, or the same prior information is counted multiple times).\n", + "\n", + "See the \"Gravity as an Optimized Variable\" section of [doc/ImuFactor.pdf](https://github.com/borglab/gtsam/blob/develop/doc/ImuFactor.pdf) for the full derivation." + ] + }, + { + "cell_type": "markdown", + "id": "395d84cc", + "metadata": {}, + "source": [ + "## Usage Example\n", + "\n", + "A stationary IMU in a nav frame whose true gravity is tilted away from the params'\n", + "$-z$ direction: the accelerometer measures $-R^T g_{true}$, so optimizing the gravity\n", + "variable must recover the true tilted gravity." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "id": "ec87c70f", + "metadata": { + "execution": { + "iopub.execute_input": "2026-08-01T10:20:52.949842Z", + "iopub.status.busy": "2026-08-01T10:20:52.947469Z", + "iopub.status.idle": "2026-08-01T10:20:52.963619Z", + "shell.execute_reply": "2026-08-01T10:20:52.962902Z" + } + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "true gravity: [ 0.2941 0.4902 -9.7933]\n" + ] + } + ], + "source": [ + "import numpy as np\n", + "\n", + "import gtsam\n", + "from gtsam.symbol_shorthand import B, G, V, X\n", + "\n", + "# True gravity: tilted ~3.3 degrees away from straight down\n", + "true_gravity = gtsam.Rot3.Rodrigues(0.05, -0.03, 0.0).rotate(\n", + " gtsam.Point3(0, 0, -9.81))\n", + "print(f\"true gravity: {np.round(true_gravity, 4)}\")\n", + "\n", + "# Nominal params believe gravity is straight down (MakeSharedU: z-up nav frame)\n", + "params = gtsam.PreintegrationParams.MakeSharedU(9.81)\n", + "params.setAccelerometerCovariance(1e-4 * np.eye(3))\n", + "params.setGyroscopeCovariance(1e-6 * np.eye(3))\n", + "params.setIntegrationCovariance(1e-8 * np.eye(3))\n", + "\n", + "# Stationary body at identity: the accelerometer measures -g_true\n", + "pim = gtsam.PreintegratedImuMeasurements(params)\n", + "for _ in range(10):\n", + " pim.integrateMeasurement(-true_gravity, np.zeros(3), 0.1)" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "c64ee81a", + "metadata": { + "execution": { + "iopub.execute_input": "2026-08-01T10:20:52.969144Z", + "iopub.status.busy": "2026-08-01T10:20:52.968927Z", + "iopub.status.idle": "2026-08-01T10:20:52.978487Z", + "shell.execute_reply": "2026-08-01T10:20:52.977203Z" + } + }, + "outputs": [], + "source": [ + "# Common graph: tight priors anchor states and bias, gravity is free\n", + "def make_graph_and_values():\n", + " graph = gtsam.NonlinearFactorGraph()\n", + " tight_pose = gtsam.noiseModel.Isotropic.Sigma(6, 1e-6)\n", + " tight_vec = gtsam.noiseModel.Isotropic.Sigma(3, 1e-6)\n", + " tight_bias = gtsam.noiseModel.Isotropic.Sigma(6, 1e-6)\n", + " graph.addPriorPose3(X(1), gtsam.Pose3(), tight_pose)\n", + " graph.addPriorPose3(X(2), gtsam.Pose3(), tight_pose)\n", + " graph.addPriorVector(V(1), np.zeros(3), tight_vec)\n", + " graph.addPriorVector(V(2), np.zeros(3), tight_vec)\n", + " graph.addPriorConstantBias(B(1), gtsam.imuBias.ConstantBias(), tight_bias)\n", + " values = gtsam.Values()\n", + " values.insert(X(1), gtsam.Pose3())\n", + " values.insert(X(2), gtsam.Pose3())\n", + " values.insert(V(1), np.zeros(3))\n", + " values.insert(V(2), np.zeros(3))\n", + " values.insert(B(1), gtsam.imuBias.ConstantBias())\n", + " return graph, values" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "id": "3aec4ecb", + "metadata": { + "execution": { + "iopub.execute_input": "2026-08-01T10:20:52.980345Z", + "iopub.status.busy": "2026-08-01T10:20:52.980152Z", + "iopub.status.idle": "2026-08-01T10:20:52.999390Z", + "shell.execute_reply": "2026-08-01T10:20:52.998560Z" + } + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "recovered gravity (direction mode): [ 0.2941 0.4902 -9.7933]\n", + "direction error: 1.78e-15\n" + ] + } + ], + "source": [ + "# Mode 2: direction on the sphere, magnitude fixed to 9.81 (from params)\n", + "graph, values = make_graph_and_values()\n", + "graph.add(gtsam.ImuFactorWithGravityDirection(\n", + " X(1), V(1), X(2), V(2), B(1), G(0), pim))\n", + "values.insert(G(0), gtsam.Unit3(np.array([0.0, 0.0, -1.0]))) # initial guess: down\n", + "\n", + "result = gtsam.LevenbergMarquardtOptimizer(graph, values).optimize()\n", + "recovered = result.atUnit3(G(0)).unitVector() * 9.81\n", + "print(f\"recovered gravity (direction mode): {np.round(recovered, 4)}\")\n", + "print(f\"direction error: {np.linalg.norm(recovered - true_gravity):.2e}\")" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "id": "923871a3", + "metadata": { + "execution": { + "iopub.execute_input": "2026-08-01T10:20:53.005787Z", + "iopub.status.busy": "2026-08-01T10:20:53.003620Z", + "iopub.status.idle": "2026-08-01T10:20:53.016775Z", + "shell.execute_reply": "2026-08-01T10:20:53.015619Z" + } + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "recovered gravity (vector mode): [ 0.2941 0.4902 -9.7933]\n", + "vector error: 7.85e-17\n" + ] + } + ], + "source": [ + "# Mode 3: free vector, with Lupton's magnitude pseudo-observation\n", + "graph, values = make_graph_and_values()\n", + "graph.add(gtsam.ImuFactorWithGravityVector(\n", + " X(1), V(1), X(2), V(2), B(1), G(0), pim))\n", + "graph.add(gtsam.VectorNormFactor3(\n", + " G(0), 9.81, gtsam.noiseModel.Isotropic.Sigma(1, 0.03)))\n", + "values.insert(G(0), gtsam.Point3(0, 0, -9.0)) # never initialize at zero!\n", + "\n", + "result = gtsam.LevenbergMarquardtOptimizer(graph, values).optimize()\n", + "recovered = result.atPoint3(G(0))\n", + "print(f\"recovered gravity (vector mode): {np.round(recovered, 4)}\")\n", + "print(f\"vector error: {np.linalg.norm(recovered - true_gravity):.2e}\")" + ] + }, + { + "cell_type": "markdown", + "id": "f165b393", + "metadata": {}, + "source": [ + "## Key Functionality / API\n", + "\n", + "- **Constructors**: same keys as `ImuFactor` plus a gravity key; the Direction variants\n", + " accept an optional `gravityMagnitude` (defaulting to the norm of the params' gravity\n", + " vector). `gravityMagnitude()` returns it.\n", + "- **`evaluateError(pose_i, vel_i, pose_j, vel_j, bias, gravity)`** with `gravity` a\n", + " `Unit3` (Direction) or `Point3` (Vector); optional Jacobians follow the usual\n", + " convention (9×2 or 9×3 for the gravity argument).\n", + "- **`Merge`** (tangent preintegration only) merges consecutive factors sharing bias and\n", + " gravity keys, and rejects mismatched gravity keys or magnitudes.\n", + "- The Combined variants add the second bias key: `CombinedImuFactorWithGravity*` with\n", + " keys `(pose_i, vel_i, pose_j, vel_j, bias_i, bias_j, gravity)` and a 15-dimensional\n", + " error whose bias rows have a zero gravity Jacobian.\n", + "\n", + "## Source\n", + "\n", + "- [ImuFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/ImuFactor.h),\n", + " [CombinedImuFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/navigation/CombinedImuFactor.h)\n", + "- Derivation: `doc/ImuFactor.pdf`, section \"Gravity as an Optimized Variable\"\n", + "- T. Lupton and S. Sukkarieh, \"Visual-Inertial-Aided Navigation for High-Dynamic Motion\n", + " in Built Environments Without Initial Conditions\", IEEE T-RO 28(1), 2012.\n", + "- R. Nemiroff, K. Chen and B. T. Lopez, \"Joint On-Manifold Gravity and Accelerometer\n", + " Intrinsics Estimation for Inertially Aligned Mapping\", arXiv:2303.03505, 2023." + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py311", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.11.15" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/navigation/doc/MagFactor.ipynb b/gtsam/navigation/doc/MagFactor.ipynb index 70bdd80e56..1256f95a4a 100644 --- a/gtsam/navigation/doc/MagFactor.ipynb +++ b/gtsam/navigation/doc/MagFactor.ipynb @@ -202,7 +202,9 @@ "## Important Notes\n", "- **Coordinate Frames**: Be very careful with navigation frame (NED vs ENU) and body frame conventions. Ensure the `direction` vector and the `Rot3`/`Pose3` variables use the same navigation frame. The `measured` and `bias` are typically in the body frame.\n", "- **Units**: Ensure consistency between the `scale`, `bias`, `measured` values and the noise model sigma (e.g., all in nanoTesla (nT)).\n", - "- **Calibration**: Accurate knowledge of `scale`, `direction`, and `bias` is crucial for `MagFactor1` and `MagPoseFactor`. If these are unknown, consider using `MagFactor2`/`MagFactor3` or online calibration techniques." + "- **Calibration**: Accurate knowledge of `scale`, `direction`, and `bias` is crucial for `MagFactor1` and `MagPoseFactor`. If these are unknown, consider using `MagFactor2`/`MagFactor3` or online calibration techniques.\n", + "\n", + "When using `MagFactor2` (unknown field vector), the field *magnitude* is often known from the [NOAA tables](http://www.ngdc.noaa.gov/geomag-web/#igrfwmm) even when its direction in the nav frame is not: add a single [VectorNormFactor](../../nonlinear/doc/VectorNormFactor.ipynb) on the field variable as a magnitude pseudo-observation." ] }, { diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 248905c8f3..72194292eb 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -241,6 +241,41 @@ virtual class ImuFactor2: gtsam::NonlinearFactor { void serialize() const; }; +virtual class ImuFactorWithGravityDirection: gtsam::NonlinearFactor { + ImuFactorWithGravityDirection(gtsam::Key pose_i, gtsam::Key vel_i, + gtsam::Key pose_j, gtsam::Key vel_j, gtsam::Key bias, gtsam::Key gravity, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); + ImuFactorWithGravityDirection(gtsam::Key pose_i, gtsam::Key vel_i, + gtsam::Key pose_j, gtsam::Key vel_j, gtsam::Key bias, gtsam::Key gravity, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, + double gravityMagnitude); + + // Standard Interface + gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; + double gravityMagnitude() const; + gtsam::Vector evaluateError(const gtsam::Pose3& pose_i, gtsam::Vector vel_i, + const gtsam::Pose3& pose_j, gtsam::Vector vel_j, + const gtsam::imuBias::ConstantBias& bias_i, const gtsam::Unit3& gravity); + + // enable serialization functionality + void serialize() const; +}; + +virtual class ImuFactorWithGravityVector: gtsam::NonlinearFactor { + ImuFactorWithGravityVector(gtsam::Key pose_i, gtsam::Key vel_i, + gtsam::Key pose_j, gtsam::Key vel_j, gtsam::Key bias, gtsam::Key gravity, + const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); + + // Standard Interface + gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; + gtsam::Vector evaluateError(const gtsam::Pose3& pose_i, gtsam::Vector vel_i, + const gtsam::Pose3& pose_j, gtsam::Vector vel_j, + const gtsam::imuBias::ConstantBias& bias_i, const gtsam::Point3& gravity); + + // enable serialization functionality + void serialize() const; +}; + #include virtual class PreintegrationCombinedParams : gtsam::PreintegrationParams { PreintegrationCombinedParams(gtsam::Vector n_gravity); @@ -312,6 +347,46 @@ virtual class CombinedImuFactor: gtsam::NoiseModelFactor { void serialize() const; }; +virtual class CombinedImuFactorWithGravityDirection: gtsam::NoiseModelFactor { + CombinedImuFactorWithGravityDirection(gtsam::Key pose_i, gtsam::Key vel_i, + gtsam::Key pose_j, gtsam::Key vel_j, gtsam::Key bias_i, gtsam::Key bias_j, + gtsam::Key gravity, + const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); + CombinedImuFactorWithGravityDirection(gtsam::Key pose_i, gtsam::Key vel_i, + gtsam::Key pose_j, gtsam::Key vel_j, gtsam::Key bias_i, gtsam::Key bias_j, + gtsam::Key gravity, + const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements, + double gravityMagnitude); + + // Standard Interface + gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; + double gravityMagnitude() const; + gtsam::Vector evaluateError(const gtsam::Pose3& pose_i, gtsam::Vector vel_i, + const gtsam::Pose3& pose_j, gtsam::Vector vel_j, + const gtsam::imuBias::ConstantBias& bias_i, + const gtsam::imuBias::ConstantBias& bias_j, const gtsam::Unit3& gravity); + + // enable serialization functionality + void serialize() const; +}; + +virtual class CombinedImuFactorWithGravityVector: gtsam::NoiseModelFactor { + CombinedImuFactorWithGravityVector(gtsam::Key pose_i, gtsam::Key vel_i, + gtsam::Key pose_j, gtsam::Key vel_j, gtsam::Key bias_i, gtsam::Key bias_j, + gtsam::Key gravity, + const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); + + // Standard Interface + gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; + gtsam::Vector evaluateError(const gtsam::Pose3& pose_i, gtsam::Vector vel_i, + const gtsam::Pose3& pose_j, gtsam::Vector vel_j, + const gtsam::imuBias::ConstantBias& bias_i, + const gtsam::imuBias::ConstantBias& bias_j, const gtsam::Point3& gravity); + + // enable serialization functionality + void serialize() const; +}; + #include class PreintegratedAhrsMeasurements { // Standard Constructor diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index f5e46660c3..2a3966293e 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -31,6 +31,7 @@ #include #include #include +#include #include @@ -315,6 +316,100 @@ TEST(CombinedImuFactor, Accelerating) { EXPECT(assert_equal(estimatedCov, expected, 0.1)); } +/* ************************************************************************* */ +/* ************************************************************************* */ +TEST_PIM(CombinedImuFactorWithGravity, Jacobians) { + using symbol_shorthand::G; + Bias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); + Bias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); + Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)), Point3(5.0, 1.0, -50.0)); + Vector3 v1(0.5, 0.0, 0.0); + Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)), + Point3(5.5, 1.0, -50.0)); + Vector3 v2(0.5, 0.0, 0.0); + + auto p = combined::Params(); + CombinedPIM pim(p, Bias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))); + const Vector3 measuredAcc = + x1.rotation().unrotate(-p->n_gravity) + Vector3(0.2, 0.0, 0.0); + const Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3); + pim.integrateMeasurement(measuredAcc, measuredOmega, 1.0); + + Values values; + values.insert(X(1), x1); + values.insert(V(1), v1); + values.insert(X(2), x2); + values.insert(V(2), v2); + values.insert(B(1), bias); + values.insert(B(2), bias2); + + { + CombinedImuFactorWithGravityT factor( + X(1), V(1), X(2), V(2), B(1), B(2), G(0), pim); + Values v = values; + v.insert(G(0), Unit3(0.1, -0.2, -1.0)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, v, 1e-7, 1e-3); + } + { + CombinedImuFactorWithGravityT factor( + X(1), V(1), X(2), V(2), B(1), B(2), G(0), pim); + Values v = values; + v.insert(G(0), Point3(0.4, -0.6, -9.5)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, v, 1e-7, 1e-3); + } +} + +/* ************************************************************************* */ +TEST(CombinedImuFactorWithGravity, Equals) { + using symbol_shorthand::G; + auto p = combined::Params(); + PreintegratedCombinedMeasurements pim(p); + + CombinedImuFactorWithGravityDirection d1(X(1), V(1), X(2), V(2), B(1), B(2), + G(0), pim); + CombinedImuFactorWithGravityDirection d2(X(1), V(1), X(2), V(2), B(1), B(2), + G(0), pim); + CombinedImuFactorWithGravityDirection d3(X(1), V(1), X(2), V(2), B(1), B(2), + G(0), pim, 1.62); + EXPECT(d1.equals(d2)); + EXPECT(!d1.equals(d3)); + + // The Point3 parametrization does not use the magnitude, so factors that + // differ only in the (inert) stored magnitude must compare equal: + CombinedImuFactorWithGravityVector v1(X(1), V(1), X(2), V(2), B(1), B(2), + G(0), pim); + CombinedImuFactorWithGravityVector v2(X(1), V(1), X(2), V(2), B(1), B(2), + G(0), pim); + EXPECT(v1.equals(v2)); + + // Comparing against a different factor type must not crash: + EXPECT(!d1.equals(v1)); + EXPECT(!v1.equals(d1)); +} + +/* ************************************************************************* */ +TEST_PIM(CombinedImuFactorWithGravity, ConsistentWithCombinedImuFactor) { + using symbol_shorthand::G; + auto p = combined::Params(); + CombinedPIM pim(p); + pim.integrateMeasurement(Vector3(0.1, 0.2, -9.81), Vector3(0.1, 0, 0), 0.5); + + const Pose3 x1; + const Vector3 v1(0.1, 0, 0); + const Pose3 x2; + const Vector3 v2(0.1, 0, 0); + const Bias bias1, bias2; + + CombinedImuFactorT plain(X(1), V(1), X(2), V(2), B(1), B(2), pim); + CombinedImuFactorWithGravityT withGravity( + X(1), V(1), X(2), V(2), B(1), B(2), G(0), pim); + // With the params' gravity direction, the errors must agree: + EXPECT(assert_equal( + plain.evaluateError(x1, v1, x2, v2, bias1, bias2), + withGravity.evaluateError(x1, v1, x2, v2, bias1, bias2, + Unit3(p->n_gravity)))); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 203d90a270..1745b10e70 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -29,7 +29,10 @@ #include #include #include +#include +#include #include +#include #include #include @@ -123,7 +126,12 @@ TEST_PIM(ImuFactor, PreintegratedMeasurements) { Matrix9 aH1, aH2; Matrix96 aH3; actual.computeError(x1, x2, bias, aH1, aH2, aH3); - auto f = std::bind(&PreintegrationBase::computeError, actual, + // Select the overload without the gravity parameter: + using ComputeErrorNoGravity = Vector9 (PreintegrationBase::*)( + const NavState&, const NavState&, const imuBias::ConstantBias&, + OptionalJacobian<9, 9>, OptionalJacobian<9, 9>, + OptionalJacobian<9, 6>) const; + auto f = std::bind(static_cast(&PreintegrationBase::computeError), actual, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, nullptr, nullptr, nullptr); EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); @@ -167,6 +175,10 @@ static const NavState state2(x2, v2); /* ************************************************************************* */ TEST_PIM(ImuFactor, PreintegrationBaseMethods) { + // Select the overload without the gravity parameter: + using PredictNoGravity = NavState (PreintegrationBase::*)( + const NavState&, const imuBias::ConstantBias&, OptionalJacobian<9, 9>, + OptionalJacobian<9, 6>) const; using namespace common; auto p = testing::Params(); p->omegaCoriolis = Vector3(0.02, 0.03, 0.04); @@ -188,15 +200,63 @@ TEST_PIM(ImuFactor, PreintegrationBaseMethods) { Matrix96 aH2; NavState predictedState = pim.predict(state1, kZeroBias, aH1, aH2); Matrix eH1 = numericalDerivative11( - std::bind(&PreintegrationBase::predict, pim, std::placeholders::_1, + std::bind(static_cast(&PreintegrationBase::predict), pim, std::placeholders::_1, kZeroBias, nullptr, nullptr), state1); EXPECT(assert_equal(eH1, aH1)); Matrix eH2 = numericalDerivative11( - std::bind(&PreintegrationBase::predict, pim, state1, + std::bind(static_cast(&PreintegrationBase::predict), pim, state1, std::placeholders::_1, nullptr, nullptr), kZeroBias); EXPECT(assert_equal(eH2, aH2)); } +/* ************************************************************************* */ +TEST_PIM(ImuFactor, PredictWithGravityVector) { + using namespace common; + PIM pim(testing::Params(), kZeroBiasHat); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + // A gravity vector tilted away from the params' vector, with a different + // magnitude: + const Vector3 tilted_gravity = Vector3(0.5, -0.3, 9.7); + + // The gravity overload with the params' gravity must match the legacy one: + EXPECT(assert_equal(pim.predict(state1, kZeroBias), + pim.predict(state1, kZeroBias, testing::Params()->n_gravity))); + + // Check all three Jacobians of the gravity overload: + Matrix9 aH1; + Matrix96 aH2; + Matrix93 aH3; + pim.predict(state1, kZeroBias, tilted_gravity, aH1, aH2, aH3); + EXPECT(assert_equal( + numericalDerivative11( + [&](const NavState& s) { return pim.predict(s, kZeroBias, tilted_gravity); }, + state1), + Matrix(aH1))); + EXPECT(assert_equal( + numericalDerivative11( + [&](const Bias& b) { return pim.predict(state1, b, tilted_gravity); }, + kZeroBias), + Matrix(aH2))); + EXPECT(assert_equal( + numericalDerivative11( + [&](const Vector3& g) { return pim.predict(state1, kZeroBias, g); }, + tilted_gravity), + Matrix(aH3))); + + // And the gravity Jacobian of computeError: + Matrix93 aH4; + pim.computeError(state1, state2, kZeroBias, tilted_gravity, {}, {}, {}, aH4); + EXPECT(assert_equal( + numericalDerivative11( + [&](const Vector3& g) { + return pim.computeError(state1, state2, kZeroBias, g, {}, {}, {}, {}); + }, + tilted_gravity), + Matrix(aH4))); +} + /* ************************************************************************* */ TEST_PIM(ImuFactor, MultipleMeasurements) { using namespace common; @@ -878,6 +938,187 @@ TEST_PIM(ImuFactor, CheckCovariance) { EXPECT(assert_equal(expected, actual.preintMeasCov())); } +/* ************************************************************************* */ +/* ************************************************************************* */ +TEST_PIM(ImuFactorWithGravity, DirectionJacobians) { + using namespace common; + using symbol_shorthand::G; + PIM pim(testing::Params()); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + ImuFactorWithGravityT factor(X(1), V(1), X(2), V(2), B(1), G(0), + pim); + // The default magnitude comes from the params' gravity vector: + DOUBLES_EQUAL(testing::Params()->n_gravity.norm(), factor.gravityMagnitude(), + 1e-9); + + // With the params' gravity direction, the error must match plain ImuFactor: + ImuFactorT plain(X(1), V(1), X(2), V(2), B(1), pim); + EXPECT(assert_equal( + plain.evaluateError(x1, v1, x2, v2, kZeroBias), + factor.evaluateError(x1, v1, x2, v2, kZeroBias, + Unit3(testing::Params()->n_gravity)))); + + Values values; + values.insert(X(1), x1); + values.insert(V(1), v1); + values.insert(X(2), x2); + values.insert(V(2), v2); + values.insert(B(1), kZeroBias); + values.insert(G(0), Unit3(0.1, -0.2, -1.0)); // tilted away from params + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-3); + + // The magnitude scales the gravity Jacobian block; check it at a + // deliberately non-default value: + ImuFactorWithGravityT lunar(X(1), V(1), X(2), V(2), B(1), G(0), + pim, 1.62); + EXPECT_CORRECT_FACTOR_JACOBIANS(lunar, values, 1e-7, 1e-3); +} + +/* ************************************************************************* */ +TEST_PIM(ImuFactorWithGravity, VectorJacobians) { + using namespace common; + using symbol_shorthand::G; + PIM pim(testing::Params()); + pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); + + ImuFactorWithGravityT factor(X(1), V(1), X(2), V(2), B(1), G(0), + pim); + + Values values; + values.insert(X(1), x1); + values.insert(V(1), v1); + values.insert(X(2), x2); + values.insert(V(2), v2); + values.insert(B(1), kZeroBias); + // Tilted direction and non-standard magnitude: + values.insert(G(0), Point3(0.4, -0.6, -9.5)); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-3); +} + +/* ************************************************************************* */ +TEST(ImuFactorWithGravity, Equals) { + using symbol_shorthand::G; + auto pim = PreintegratedImuMeasurements(testing::Params()); + ImuFactorWithGravityDirection factor1(X(1), V(1), X(2), V(2), B(1), G(0), pim); + ImuFactorWithGravityDirection factor2(X(1), V(1), X(2), V(2), B(1), G(0), pim); + ImuFactorWithGravityDirection factor3(X(1), V(1), X(2), V(2), B(1), G(0), pim, + 1.62); // lunar gravity + EXPECT(factor1.equals(factor2)); + EXPECT(!factor1.equals(factor3)); + // Comparing against a different factor type must not crash: + ImuFactorWithGravityVector vectorFactor(X(1), V(1), X(2), V(2), B(1), G(0), pim); + EXPECT(!factor1.equals(vectorFactor)); + EXPECT(!vectorFactor.equals(factor1)); +} + +/* ************************************************************************* */ +// A stationary IMU in a nav frame whose gravity is tilted away from the +// params' -z direction: the accelerometer measures -g in the body frame, so +// optimizing the gravity variable must recover the true tilted gravity. +namespace tilted { +static const Vector3 trueGravity = + Rot3::Rodrigues(0.05, -0.03, 0.0) * Vector3(0, 0, -9.81); +static PreintegratedImuMeasurements integrateStationary() { + auto p = testing::Params(); // note: n_gravity = (0, 0, kGravity) = (0, 0, 10) + PreintegratedImuMeasurements pim(p); + for (int i = 0; i < 10; ++i) + pim.integrateMeasurement(-trueGravity, Vector3::Zero(), 0.1); + return pim; +} +static void addStationaryPriors(NonlinearFactorGraph* graph) { + using symbol_shorthand::G; + auto tightPose = noiseModel::Isotropic::Sigma(6, 1e-6); + auto tightVector = noiseModel::Isotropic::Sigma(3, 1e-6); + auto tightBias = noiseModel::Isotropic::Sigma(6, 1e-6); + graph->addPrior(X(1), Pose3::Identity(), tightPose); + graph->addPrior(X(2), Pose3::Identity(), tightPose); + graph->addPrior(V(1), Vector3(Vector3::Zero()), tightVector); + graph->addPrior(V(2), Vector3(Vector3::Zero()), tightVector); + graph->addPrior(B(1), kZeroBias, tightBias); +} +static Values stationaryInitial() { + Values initial; + initial.insert(X(1), Pose3::Identity()); + initial.insert(X(2), Pose3::Identity()); + initial.insert(V(1), Vector3(Vector3::Zero())); + initial.insert(V(2), Vector3(Vector3::Zero())); + initial.insert(B(1), kZeroBias); + return initial; +} +} // namespace tilted + +TEST(ImuFactorWithGravity, RecoverGravityDirection) { + using symbol_shorthand::G; + NonlinearFactorGraph graph; + // The magnitude is given explicitly since the params' norm is kGravity = 10, + // while the data was generated with |g| = 9.81: + graph.emplace_shared( + X(1), V(1), X(2), V(2), B(1), G(0), tilted::integrateStationary(), 9.81); + tilted::addStationaryPriors(&graph); + + Values initial = tilted::stationaryInitial(); + initial.insert(G(0), Unit3(0, 0, -1)); + + const Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + EXPECT(assert_equal(Unit3(tilted::trueGravity), result.at(G(0)), 1e-5)); +} + +/* ************************************************************************* */ +TEST(ImuFactorWithGravity, RecoverGravityVector) { + using symbol_shorthand::G; + NonlinearFactorGraph graph; + graph.emplace_shared( + X(1), V(1), X(2), V(2), B(1), G(0), tilted::integrateStationary()); + // Lupton-style magnitude pseudo-observation (consistent with trueGravity): + graph.emplace_shared>( + G(0), 9.81, noiseModel::Isotropic::Sigma(1, 0.03)); + tilted::addStationaryPriors(&graph); + + Values initial = tilted::stationaryInitial(); + initial.insert(G(0), Point3(0, 0, -9.0)); + + const Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + EXPECT(assert_equal(Point3(tilted::trueGravity), result.at(G(0)), 1e-4)); +} + +/* ************************************************************************* */ +#ifdef GTSAM_TANGENT_PREINTEGRATION +TEST(ImuFactorWithGravity, Merge) { + using symbol_shorthand::G; + auto p = testing::Params(); + PreintegratedImuMeasurements pim01(p), pim12(p); + const Vector3 acc(0.1, 0.2, -9.81), omega(0.1, 0.02, 0.03); + for (int i = 0; i < 10; ++i) { + pim01.integrateMeasurement(acc, omega, 0.1); + pim12.integrateMeasurement(acc, omega, 0.1); + } + auto f01 = std::make_shared( + X(1), V(1), X(2), V(2), B(1), G(0), pim01); + auto f12 = std::make_shared( + X(2), V(2), X(3), V(3), B(1), G(0), pim12); + + auto f02 = ImuFactorWithGravityDirection::Merge(f01, f12); + DOUBLES_EQUAL(2.0, f02->preintegratedMeasurements().deltaTij(), 1e-9); + DOUBLES_EQUAL(f01->gravityMagnitude(), f02->gravityMagnitude(), 1e-9); + EXPECT(f02->key<1>() == X(1)); + EXPECT(f02->key<3>() == X(3)); + EXPECT(f02->key<6>() == G(0)); + + // Mismatched gravity magnitudes must be rejected: + auto f12_moon = std::make_shared( + X(2), V(2), X(3), V(3), B(1), G(0), pim12, 1.62); + CHECK_EXCEPTION(ImuFactorWithGravityDirection::Merge(f01, f12_moon), + std::domain_error); + + // Mismatched gravity keys must be rejected: + auto f12_key = std::make_shared( + X(2), V(2), X(3), V(3), B(1), G(1), pim12); + CHECK_EXCEPTION(ImuFactorWithGravityDirection::Merge(f01, f12_key), + std::domain_error); +} +#endif + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index 797bcae219..b67704ef32 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -84,7 +84,12 @@ TEST(ManifoldPreintegration, computeError) { Matrix9 aH1, aH2; Matrix96 aH3; pim.computeError(x1, x2, bias, aH1, aH2, aH3); - auto f = std::bind(&ManifoldPreintegration::computeError, pim, + // Select the overload without the gravity parameter: + using ComputeErrorNoGravity = Vector9 (PreintegrationBase::*)( + const NavState&, const NavState&, const imuBias::ConstantBias&, + OptionalJacobian<9, 9>, OptionalJacobian<9, 9>, + OptionalJacobian<9, 6>) const; + auto f = std::bind(static_cast(&ManifoldPreintegration::computeError), pim, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, nullptr, nullptr, nullptr); diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 0312a18f30..0cb93563cd 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -446,12 +446,31 @@ TEST(NavState, CorrectPIM) { xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3; double dt = 0.5; Matrix9 aH1, aH2; + Matrix93 aH3; auto correctPIM = std::bind(&NavState::correctPIM, std::placeholders::_1, std::placeholders::_2, dt, kGravity, kOmegaCoriolis, false, - nullptr, nullptr); - kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2); + nullptr, nullptr, nullptr); + kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2, aH3); EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1)); EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2)); + + // Check the Jacobian wrt the gravity vector: + auto correctPIMGravity = [&](const Vector3& gravity) { + return kState1.correctPIM(xi, dt, gravity, kOmegaCoriolis, false); + }; + EXPECT(assert_equal( + numericalDerivative11(correctPIMGravity, kGravity), + Matrix(aH3))); + + // The gravity Jacobian must also be correct with coriolis disabled: + Matrix93 aH3NoCoriolis; + kState1.correctPIM(xi, dt, kGravity, {}, false, {}, {}, aH3NoCoriolis); + auto correctPIMNoCoriolis = [&](const Vector3& gravity) { + return kState1.correctPIM(xi, dt, gravity, {}, false); + }; + EXPECT(assert_equal( + numericalDerivative11(correctPIMNoCoriolis, kGravity), + Matrix(aH3NoCoriolis))); } /* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testSerializationNavigation.cpp b/gtsam/navigation/tests/testSerializationNavigation.cpp index bd2928c471..eb418beda9 100644 --- a/gtsam/navigation/tests/testSerializationNavigation.cpp +++ b/gtsam/navigation/tests/testSerializationNavigation.cpp @@ -103,6 +103,21 @@ TEST(ImuFactor2, serialization) { EXPECT(equalsBinary(factor)); } +/* ************************************************************************* */ +TEST(ImuFactorWithGravity, serialization) { + auto pim = getPreintegratedMeasurements(); + + ImuFactorWithGravityDirection direction(1, 2, 3, 4, 5, 6, pim, 9.81); + EXPECT(equalsObj(direction)); + EXPECT(equalsXML(direction)); + EXPECT(equalsBinary(direction)); + + ImuFactorWithGravityVector vector(1, 2, 3, 4, 5, 6, pim); + EXPECT(equalsObj(vector)); + EXPECT(equalsXML(vector)); + EXPECT(equalsBinary(vector)); +} + /* ************************************************************************* */ TEST(CombinedImuFactor, Serialization) { auto pim = getPreintegratedMeasurements(); @@ -118,6 +133,21 @@ TEST(CombinedImuFactor, Serialization) { EXPECT(equalsBinary(factor)); } +/* ************************************************************************* */ +TEST(CombinedImuFactorWithGravity, Serialization) { + auto pim = getPreintegratedMeasurements(); + + CombinedImuFactorWithGravityDirection direction(1, 2, 3, 4, 5, 6, 7, pim, 9.81); + EXPECT(equalsObj(direction)); + EXPECT(equalsXML(direction)); + EXPECT(equalsBinary(direction)); + + CombinedImuFactorWithGravityVector vector(1, 2, 3, 4, 5, 6, 7, pim); + EXPECT(equalsObj(vector)); + EXPECT(equalsXML(vector)); + EXPECT(equalsBinary(vector)); +} + /* ************************************************************************* */ TEST(AttitudeFactorRot3, Serialization) { Unit3 nDown(0, 0, -1); diff --git a/gtsam/navigation/tests/testTangentPreintegration.cpp b/gtsam/navigation/tests/testTangentPreintegration.cpp index 2b7369d772..de260bc9d2 100644 --- a/gtsam/navigation/tests/testTangentPreintegration.cpp +++ b/gtsam/navigation/tests/testTangentPreintegration.cpp @@ -92,7 +92,12 @@ TEST(TangentPreintegration, computeError) { Matrix9 aH1, aH2; Matrix96 aH3; pim.computeError(x1, x2, bias, aH1, aH2, aH3); - auto f = std::bind(&TangentPreintegration::computeError, pim, + // Select the overload without the gravity parameter: + using ComputeErrorNoGravity = Vector9 (PreintegrationBase::*)( + const NavState&, const NavState&, const imuBias::ConstantBias&, + OptionalJacobian<9, 9>, OptionalJacobian<9, 9>, + OptionalJacobian<9, 6>) const; + auto f = std::bind(static_cast(&TangentPreintegration::computeError), pim, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, nullptr, nullptr, nullptr); diff --git a/gtsam/nonlinear/VectorNormFactor.h b/gtsam/nonlinear/VectorNormFactor.h new file mode 100644 index 0000000000..5c603eeb43 --- /dev/null +++ b/gtsam/nonlinear/VectorNormFactor.h @@ -0,0 +1,151 @@ +/* ---------------------------------------------------------------------------- + + * 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 VectorNormFactor.h + * @brief Factor constraining the norm of a vector-space variable + * @date July 2026 + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * Unary factor constraining the norm of an N-dimensional vector-space variable + * to a given value, with error = ||v|| - norm. + * + * A typical use is a magnitude pseudo-observation on a physical vector whose + * direction and magnitude are entangled in a single variable, e.g. an + * estimated gravity vector (see ImuFactorWithGravityVector and + * [Lupton and Sukkarieh, TRO 2012]) or a local magnetic field vector + * (see MagFactor2). Add it once per variable: adding it per measurement + * factor would count the same prior knowledge multiple times. + * + * @warning The error is not differentiable at ||v|| = 0; below kMinNorm the + * Jacobian is set to zero, so the factor exerts no pull there. In particular, + * an optimizer whose initial estimate for the variable is exactly zero will + * receive no gradient from this factor and the variable will not move. + * Always initialize the variable with a non-zero guess. + * + * This is a hand-written factor rather than an ExpressionFactor over norm3 + * for discoverability and to avoid the expression-tree evaluation overhead at + * every linearization; see gtsam/slam/expressions.h for the expression + * building blocks if composition with other expressions is needed. + * + * With a noiseModel::Constrained model this acts as a hard constraint under + * QR-based elimination (not recommended with Cholesky factorization). If the + * norm is known exactly, prefer a parametrization that fixes it instead, + * e.g. Unit3 and a fixed magnitude (see ImuFactorWithGravityDirection). + * + * @ingroup nonlinear + */ +template +class VectorNormFactor : public NoiseModelFactorN> { + static_assert(N > 0, "VectorNormFactor requires a fixed positive dimension"); + + public: + typedef Eigen::Matrix VectorN; + + private: + typedef VectorNormFactor This; + typedef NoiseModelFactorN Base; + + double norm_; ///< desired norm of the vector + + public: + /// Below this norm the error is treated as non-differentiable and the + /// Jacobian is zero (see class warning). + static constexpr double kMinNorm = 1e-10; + + // Provide access to the Matrix& version of evaluateError: + using Base::evaluateError; + + /** Shorthand for a smart pointer to a factor */ + typedef std::shared_ptr shared_ptr; + + /** Default constructor - only use for serialization */ + VectorNormFactor() : norm_(0.0) {} + + /** + * Constructor + * @param key of the vector-space variable + * @param norm the desired norm of the vector + * @param model of the (1-dimensional) noise on the norm error + */ + VectorNormFactor(Key key, double norm, const SharedNoiseModel& model) + : Base(model, key), norm_(norm) {} + + ~VectorNormFactor() override {} + + /// @return a deep copy of this factor + NonlinearFactor::shared_ptr clone() const override { + return std::make_shared(*this); + } + + /// @name Testable + /// @{ + void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + std::cout << (s.empty() ? s : s + " ") << "VectorNormFactor on " + << keyFormatter(this->template key<1>()) << ", norm = " << norm_ + << std::endl; + this->noiseModel_->print(" noise model: "); + } + + bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override { + const This* e = dynamic_cast(&expected); + return e != nullptr && Base::equals(*e, tol) && + std::abs(norm_ - e->norm_) < tol; + } + /// @} + + /// The desired norm + double norm() const { return norm_; } + + /// vector of errors: ||v|| - norm + Vector evaluateError(const VectorN& v, OptionalMatrixType H) const override { + const double vnorm = v.norm(); + if (H) { + if (vnorm > kMinNorm) { + *H = v.transpose() / vnorm; + } else { + // The norm is not differentiable at the origin. Return a zero + // Jacobian: the factor exerts no pull on a (near-)zero vector. + H->setZero(1, N); + } + } + return Vector1(vnorm - norm_); + } + + 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::make_nvp( + "NoiseModelFactor1", boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(norm_); + } +#endif +}; + +/// The most common instantiation, eg. for gravity or magnetic field vectors +using VectorNormFactor3 = VectorNormFactor<3>; + +/// traits +template +struct traits> : public Testable> {}; + +} // namespace gtsam diff --git a/gtsam/nonlinear/doc/VectorNormFactor.ipynb b/gtsam/nonlinear/doc/VectorNormFactor.ipynb new file mode 100644 index 0000000000..fe7e246dae --- /dev/null +++ b/gtsam/nonlinear/doc/VectorNormFactor.ipynb @@ -0,0 +1,171 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "ed487bc4", + "metadata": {}, + "source": [ + "# VectorNormFactor\n", + "\n", + "\"Open\n", + "\n", + "## Overview\n", + "\n", + "`VectorNormFactor` is a unary factor constraining the norm of an N-dimensional\n", + "vector-space variable to a given value, with error\n", + "\n", + "$$ e = \\lVert v \\rVert - \\text{norm}, \\qquad\n", + "\\frac{\\partial e}{\\partial v} = \\frac{v^T}{\\lVert v \\rVert}. $$\n", + "\n", + "The typical use is a **magnitude pseudo-observation** on a physical vector whose\n", + "direction and magnitude are entangled in a single variable:\n", + "\n", + "- an estimated gravity vector, paired with\n", + " [ImuFactorWithGravityVector](../../navigation/doc/ImuFactorWithGravity.ipynb)\n", + " (Lupton and Sukkarieh, 2012), or\n", + "- a local magnetic field vector estimated with\n", + " [MagFactor2](../../navigation/doc/MagFactor.ipynb), whose magnitude is often known\n", + " from the [NOAA tables](http://www.ngdc.noaa.gov/geomag-web/#igrfwmm) even when its\n", + " direction in the nav frame is not.\n", + "\n", + "Add it **once per variable**: adding it per measurement factor would count the same\n", + "prior knowledge multiple times. The N=3 instantiation is available in Python as\n", + "`VectorNormFactor3`.\n", + "\n", + "**Warning**: the error is not differentiable at $\\lVert v \\rVert = 0$; the Jacobian\n", + "is zero there, so a variable initialized at exactly zero receives no gradient from this\n", + "factor and will not move. Always initialize with a non-zero guess." + ] + }, + { + "cell_type": "markdown", + "id": "4fa7ea59", + "metadata": {}, + "source": [ + "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n", + "Atlanta, Georgia 30332-0415\n", + "All Rights Reserved\n", + "\n", + "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", + "\n", + "See LICENSE for the license information" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "id": "d796072b", + "metadata": { + "execution": { + "iopub.execute_input": "2026-08-01T10:20:56.326366Z", + "iopub.status.busy": "2026-08-01T10:20:56.326254Z", + "iopub.status.idle": "2026-08-01T10:20:56.526270Z", + "shell.execute_reply": "2026-08-01T10:20:56.525643Z" + } + }, + "outputs": [], + "source": [ + "# Install gtsam-develop if not installed\n", + "try:\n", + " import gtsam\n", + "except ImportError:\n", + " %pip install --quiet gtsam-develop" + ] + }, + { + "cell_type": "markdown", + "id": "f355f1ac", + "metadata": {}, + "source": [ + "## Usage Example\n", + "\n", + "A loose vector prior fixes the direction; the norm factor pulls the magnitude to the\n", + "target along that direction." + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "id": "39d3bf51", + "metadata": { + "execution": { + "iopub.execute_input": "2026-08-01T10:20:56.538172Z", + "iopub.status.busy": "2026-08-01T10:20:56.537704Z", + "iopub.status.idle": "2026-08-01T10:20:56.542997Z", + "shell.execute_reply": "2026-08-01T10:20:56.542570Z" + } + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "optimized vector: [5.6638 5.6638 5.6638]\n", + "norm: 9.81000 (target 9.81)\n" + ] + } + ], + "source": [ + "import numpy as np\n", + "\n", + "import gtsam\n", + "from gtsam.symbol_shorthand import G\n", + "\n", + "graph = gtsam.NonlinearFactorGraph()\n", + "direction = np.array([1.0, 1.0, 1.0]) / np.sqrt(3.0)\n", + "graph.addPriorPoint3(G(0), gtsam.Point3(8.0 * direction),\n", + " gtsam.noiseModel.Isotropic.Sigma(3, 10.0))\n", + "graph.add(gtsam.VectorNormFactor3(\n", + " G(0), 9.81, gtsam.noiseModel.Isotropic.Sigma(1, 1e-3)))\n", + "\n", + "values = gtsam.Values()\n", + "values.insert(G(0), gtsam.Point3(8.0 * direction)) # non-zero initial guess\n", + "\n", + "result = gtsam.LevenbergMarquardtOptimizer(graph, values).optimize()\n", + "optimized = result.atPoint3(G(0))\n", + "print(f\"optimized vector: {np.round(optimized, 4)}\")\n", + "print(f\"norm: {np.linalg.norm(optimized):.5f} (target 9.81)\")" + ] + }, + { + "cell_type": "markdown", + "id": "574a4abb", + "metadata": {}, + "source": [ + "## Key Functionality / API\n", + "\n", + "- `VectorNormFactor3(key, norm, noiseModel)`: constrain a `Point3`/`Vector3` variable.\n", + "- `norm()`: the target norm.\n", + "- With a `noiseModel::Constrained` model it acts as a hard constraint under QR-based\n", + " elimination (not recommended with Cholesky). If the norm is known *exactly*, prefer a\n", + " parametrization that fixes it instead, eg. `Unit3` with a fixed magnitude\n", + " ([ImuFactorWithGravityDirection](../../navigation/doc/ImuFactorWithGravity.ipynb)).\n", + "\n", + "## Source\n", + "\n", + "- [VectorNormFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/VectorNormFactor.h)" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "py311", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.11.15" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 6d24a716f4..ba4549b27c 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -876,6 +876,19 @@ virtual class ConcentratedGaussian : gtsam::ExtendedPriorFactor { This operator*(const This& other) const; }; +#include +virtual class VectorNormFactor3 : gtsam::NoiseModelFactor { + VectorNormFactor3(gtsam::Key key, double norm, + const gtsam::noiseModel::Base* noiseModel); + + // Standard Interface + double norm() const; + gtsam::Vector evaluateError(const gtsam::Point3& v); + + // enabling serialization functionality + void serialize() const; +}; + #include template +#include +#include +#include +#include +#include +#include +#include + +#if GTSAM_ENABLE_BOOST_SERIALIZATION +#include +#endif + +using namespace std; +using namespace gtsam; +using symbol_shorthand::G; + +static const SharedNoiseModel kModel = noiseModel::Isotropic::Sigma(1, 0.1); + +/* ************************************************************************* */ +TEST(VectorNormFactor, Error) { + VectorNormFactor<3> factor(G(0), 5.0, kModel); + const Vector3 v(3.0, 0.0, 4.0); // norm 5 + EXPECT(assert_equal(Vector1(0.0), factor.evaluateError(v))); + EXPECT(assert_equal(Vector1(1.0), factor.evaluateError(Vector3(0, 0, 6)))); +} + +/* ************************************************************************* */ +TEST(VectorNormFactor, Jacobian) { + VectorNormFactor<3> factor(G(0), 9.81, kModel); + const Vector3 v(0.3, -0.2, -9.7); + Matrix actualH; + factor.evaluateError(v, actualH); + const Matrix expectedH = numericalDerivative11( + [&factor](const Vector3& x) { return factor.evaluateError(x); }, v); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); +} + +/* ************************************************************************* */ +TEST(VectorNormFactor, JacobianDimension2) { + VectorNormFactor<2> factor(G(0), 2.0, kModel); + const Vector2 v(1.0, -1.5); + Matrix actualH; + factor.evaluateError(v, actualH); + const Matrix expectedH = numericalDerivative11( + [&factor](const Vector2& x) { return factor.evaluateError(x); }, v); + EXPECT(assert_equal(expectedH, actualH, 1e-5)); +} + +/* ************************************************************************* */ +TEST(VectorNormFactor, ZeroVector) { + // The norm is not differentiable at the origin: the error must still be + // finite and the Jacobian zero (no NaNs). + VectorNormFactor<3> factor(G(0), 9.81, kModel); + Matrix actualH; + const Vector error = factor.evaluateError(Vector3::Zero(), actualH); + EXPECT(assert_equal(Vector1(-9.81), error)); + EXPECT(assert_equal(Matrix(Matrix::Zero(1, 3)), actualH)); +} + +/* ************************************************************************* */ +TEST(VectorNormFactor, Optimization) { + // A loose vector prior fixes the direction; the norm factor should pull the + // magnitude to the target along that direction. + NonlinearFactorGraph graph; + const Vector3 direction = Vector3(1.0, 1.0, 1.0).normalized(); + graph.emplace_shared>( + G(0), Vector3(8.0 * direction), noiseModel::Isotropic::Sigma(3, 10.0)); + graph.emplace_shared>( + G(0), 9.81, noiseModel::Isotropic::Sigma(1, 1e-3)); + + Values initial; + initial.insert(G(0), Vector3(8.0 * direction)); + const Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + + const Vector3 optimized = result.at(G(0)); + DOUBLES_EQUAL(9.81, optimized.norm(), 1e-5); + EXPECT(assert_equal(direction, Vector3(optimized.normalized()), 1e-3)); +} + +/* ************************************************************************* */ +TEST(VectorNormFactor, Equals) { + VectorNormFactor<3> factor1(G(0), 9.81, kModel); + VectorNormFactor<3> factor2(G(0), 9.81, kModel); + VectorNormFactor<3> factor3(G(0), 1.0, kModel); + EXPECT(factor1.equals(factor2)); + EXPECT(!factor1.equals(factor3)); + // Comparing against a different factor type must not crash: + PriorFactor prior(G(0), Vector3::Zero(), + noiseModel::Isotropic::Sigma(3, 1.0)); + EXPECT(!factor1.equals(prior)); +} + +/* ************************************************************************* */ +#if GTSAM_ENABLE_BOOST_SERIALIZATION +BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic") + +TEST(VectorNormFactor, Serialization) { + using namespace gtsam::serializationTestHelpers; + VectorNormFactor<3> factor(G(0), 9.81, kModel); + EXPECT(equalsObj>(factor)); + EXPECT(equalsXML>(factor)); + EXPECT(equalsBinary>(factor)); +} +#endif + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/python/gtsam/examples/ImuFactorWithGravityExample.py b/python/gtsam/examples/ImuFactorWithGravityExample.py new file mode 100644 index 0000000000..63c4ef9575 --- /dev/null +++ b/python/gtsam/examples/ImuFactorWithGravityExample.py @@ -0,0 +1,103 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +A script demonstrating gravity estimation with the ImuFactorWithGravity family: +a stationary IMU in a navigation frame whose true gravity is tilted away from +the direction assumed by the preintegration parameters. The accelerometer +measures -R^T g_true, so optimizing the gravity variable recovers the tilt. + +Two parametrizations are demonstrated: +- ImuFactorWithGravityDirection: Unit3 direction with a fixed, known magnitude. +- ImuFactorWithGravityVector: free Point3 vector, paired with a single + VectorNormFactor3 as the magnitude pseudo-observation of + [Lupton and Sukkarieh, TRO 2012]. + +Author: Nikhil Khedekar +""" + +# pylint: disable=no-name-in-module,import-error + +import numpy as np + +import gtsam +from gtsam.symbol_shorthand import B, G, V, X + + +def stationary_pim(true_gravity: np.ndarray) -> gtsam.PreintegratedImuMeasurements: + """Integrate one second of stationary IMU data under the given gravity.""" + # Nominal params believe gravity is straight down (z-up navigation frame); + # they also provide the default magnitude for the Direction factor. + params = gtsam.PreintegrationParams.MakeSharedU(9.81) + params.setAccelerometerCovariance(1e-4 * np.eye(3)) + params.setGyroscopeCovariance(1e-6 * np.eye(3)) + params.setIntegrationCovariance(1e-8 * np.eye(3)) + + pim = gtsam.PreintegratedImuMeasurements(params) + for _ in range(10): + # A stationary accelerometer measures the specific force -R^T g_true: + pim.integrateMeasurement(-true_gravity, np.zeros(3), 0.1) + return pim + + +def make_graph_and_values(): + """Tight priors anchor both states and the bias; gravity is left free.""" + graph = gtsam.NonlinearFactorGraph() + tight_pose = gtsam.noiseModel.Isotropic.Sigma(6, 1e-6) + tight_vec = gtsam.noiseModel.Isotropic.Sigma(3, 1e-6) + tight_bias = gtsam.noiseModel.Isotropic.Sigma(6, 1e-6) + graph.addPriorPose3(X(1), gtsam.Pose3(), tight_pose) + graph.addPriorPose3(X(2), gtsam.Pose3(), tight_pose) + graph.addPriorVector(V(1), np.zeros(3), tight_vec) + graph.addPriorVector(V(2), np.zeros(3), tight_vec) + graph.addPriorConstantBias(B(1), gtsam.imuBias.ConstantBias(), tight_bias) + + values = gtsam.Values() + values.insert(X(1), gtsam.Pose3()) + values.insert(X(2), gtsam.Pose3()) + values.insert(V(1), np.zeros(3)) + values.insert(V(2), np.zeros(3)) + values.insert(B(1), gtsam.imuBias.ConstantBias()) + return graph, values + + +def main(): + # True gravity: tilted ~3.3 degrees away from straight down. + true_gravity = gtsam.Rot3.Rodrigues(0.05, -0.03, 0.0).rotate( + gtsam.Point3(0, 0, -9.81)) + print(f"true gravity: {np.round(true_gravity, 4)}") + + pim = stationary_pim(true_gravity) + + # Mode 1 (for reference): plain ImuFactor with gravity fixed by the params + # would leave a large residual here, since the params assume (0, 0, -9.81). + + # Mode 2: direction on the sphere, magnitude fixed to 9.81 (from params). + graph, values = make_graph_and_values() + graph.add(gtsam.ImuFactorWithGravityDirection( + X(1), V(1), X(2), V(2), B(1), G(0), pim)) + values.insert(G(0), gtsam.Unit3(np.array([0.0, 0.0, -1.0]))) + result = gtsam.LevenbergMarquardtOptimizer(graph, values).optimize() + recovered = result.atUnit3(G(0)).unitVector() * 9.81 + print(f"recovered (direction, |g| fixed): {np.round(recovered, 4)}" + f" error {np.linalg.norm(recovered - true_gravity):.2e}") + + # Mode 3: free vector with Lupton's magnitude pseudo-observation, added + # once on the gravity variable (never per IMU factor). + graph, values = make_graph_and_values() + graph.add(gtsam.ImuFactorWithGravityVector( + X(1), V(1), X(2), V(2), B(1), G(0), pim)) + graph.add(gtsam.VectorNormFactor3( + G(0), 9.81, gtsam.noiseModel.Isotropic.Sigma(1, 0.03))) + values.insert(G(0), gtsam.Point3(0, 0, -9.0)) # never initialize at zero! + result = gtsam.LevenbergMarquardtOptimizer(graph, values).optimize() + recovered = result.atPoint3(G(0)) + print(f"recovered (free vector + norm): {np.round(recovered, 4)}" + f" error {np.linalg.norm(recovered - true_gravity):.2e}") + + +if __name__ == "__main__": + main() diff --git a/python/gtsam/tests/test_ImuFactorWithGravity.py b/python/gtsam/tests/test_ImuFactorWithGravity.py new file mode 100644 index 0000000000..9551f93722 --- /dev/null +++ b/python/gtsam/tests/test_ImuFactorWithGravity.py @@ -0,0 +1,108 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Wrapper unit tests for the IMU factors with an optimized gravity variable. +""" + +import unittest + +import numpy as np + +import gtsam +from gtsam import ImuFactorWithGravityDirection, ImuFactorWithGravityVector +from gtsam.symbol_shorthand import B, G, V, X +from gtsam.utils.test_case import GtsamTestCase + + +def stationary_pim(gravity=np.array([0.0, 0.0, -9.81])): + """Integrate a stationary IMU: the accelerometer measures -gravity. + + Note: the params' own gravity (MakeSharedD: z-DOWN, (0, 0, +9.81)) points + opposite to the gravity used to generate the data. Only the params' NORM is + used by the factors (as the default magnitude), so this deliberately + catches any leakage of the params' gravity direction into the error. + """ + params = gtsam.PreintegrationParams.MakeSharedD(9.81) + params.setAccelerometerCovariance(1e-4 * np.eye(3)) + params.setGyroscopeCovariance(1e-4 * np.eye(3)) + params.setIntegrationCovariance(1e-7 * np.eye(3)) + pim = gtsam.PreintegratedImuMeasurements(params) + for _ in range(10): + pim.integrateMeasurement(-gravity, np.zeros(3), 0.1) + return pim + + +class TestImuFactorWithGravity(GtsamTestCase): + """Smoke tests: construction and error evaluation through the wrapper.""" + + def test_direction_factor(self): + pim = stationary_pim() + factor = ImuFactorWithGravityDirection( + X(1), V(1), X(2), V(2), B(1), G(0), pim) + self.assertAlmostEqual(factor.gravityMagnitude(), 9.81) + + error = factor.evaluateError( + gtsam.Pose3(), np.zeros(3), gtsam.Pose3(), np.zeros(3), + gtsam.imuBias.ConstantBias(), gtsam.Unit3(np.array([0.0, 0.0, -1.0]))) + np.testing.assert_allclose(error, np.zeros(9), atol=1e-9) + + def test_direction_factor_explicit_magnitude(self): + pim = stationary_pim() + factor = ImuFactorWithGravityDirection( + X(1), V(1), X(2), V(2), B(1), G(0), pim, 1.62) + self.assertAlmostEqual(factor.gravityMagnitude(), 1.62) + + def test_vector_factor(self): + pim = stationary_pim() + factor = ImuFactorWithGravityVector( + X(1), V(1), X(2), V(2), B(1), G(0), pim) + error = factor.evaluateError( + gtsam.Pose3(), np.zeros(3), gtsam.Pose3(), np.zeros(3), + gtsam.imuBias.ConstantBias(), np.array([0.0, 0.0, -9.81])) + np.testing.assert_allclose(error, np.zeros(9), atol=1e-9) + + def test_combined_direction_factor(self): + params = gtsam.PreintegrationCombinedParams.MakeSharedD(9.81) + params.setAccelerometerCovariance(1e-4 * np.eye(3)) + params.setGyroscopeCovariance(1e-6 * np.eye(3)) + params.setIntegrationCovariance(1e-8 * np.eye(3)) + params.setBiasAccCovariance(1e-6 * np.eye(3)) + params.setBiasOmegaCovariance(1e-8 * np.eye(3)) + pim = gtsam.PreintegratedCombinedMeasurements(params) + for _ in range(10): + pim.integrateMeasurement(np.array([0.0, 0.0, 9.81]), np.zeros(3), 0.1) + + zb = gtsam.imuBias.ConstantBias() + factor = gtsam.CombinedImuFactorWithGravityDirection( + X(1), V(1), X(2), V(2), B(1), B(2), G(0), pim) + self.assertAlmostEqual(factor.gravityMagnitude(), 9.81) + error = factor.evaluateError( + gtsam.Pose3(), np.zeros(3), gtsam.Pose3(), np.zeros(3), zb, zb, + gtsam.Unit3(np.array([0.0, 0.0, -1.0]))) + np.testing.assert_allclose(error, np.zeros(15), atol=1e-9) + + explicit = gtsam.CombinedImuFactorWithGravityDirection( + X(1), V(1), X(2), V(2), B(1), B(2), G(0), pim, 1.62) + self.assertAlmostEqual(explicit.gravityMagnitude(), 1.62) + + vector_factor = gtsam.CombinedImuFactorWithGravityVector( + X(1), V(1), X(2), V(2), B(1), B(2), G(0), pim) + error = vector_factor.evaluateError( + gtsam.Pose3(), np.zeros(3), gtsam.Pose3(), np.zeros(3), zb, zb, + np.array([0.0, 0.0, -9.81])) + np.testing.assert_allclose(error, np.zeros(15), atol=1e-9) + + def test_vector_norm_factor(self): + model = gtsam.noiseModel.Isotropic.Sigma(1, 0.03) + factor = gtsam.VectorNormFactor3(G(0), 9.81, model) + self.assertAlmostEqual(factor.norm(), 9.81) + error = factor.evaluateError(np.array([0.0, 0.0, -9.81])) + np.testing.assert_allclose(error, np.zeros(1), atol=1e-9) + + +if __name__ == "__main__": + unittest.main()