diff --git a/cmake/HandleGeneralOptions.cmake b/cmake/HandleGeneralOptions.cmake index e5e2e194ac..f73fe6e2e5 100644 --- a/cmake/HandleGeneralOptions.cmake +++ b/cmake/HandleGeneralOptions.cmake @@ -32,6 +32,7 @@ option(GTSAM_FORCE_STATIC_LIB "Force gtsam to be a static library, option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON) +option(GTSAM_EXTENDED_POSE_RETRACT "Enable/Disable using ExtendedPose3::Retract as the default for NavState" OFF) option(GTSAM_DT_MERGING "Enable/Disable merging of equal leaf nodes in DecisionTrees. This leads to significant speed up and memory savings." ON) option(GTSAM_ENABLE_TIMING "Enable the timing tools (gttic/gttoc)" OFF) option(GTSAM_HYBRID_TIMING "Enable the timing of hybrid factor graph machinery" OFF) @@ -46,6 +47,7 @@ option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matl option(GTSAM_ALLOW_DEPRECATED_SINCE_V43 "Allow use of methods/functions deprecated in GTSAM 4.3" ON) option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) +option(GTSAM_LIEGROUP_PREINTEGRATION "Use IMU pre-integration based on Lie groups" OFF) option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF) option(GTSAM_SLOW_BUT_CORRECT_EXPMAP "Use slower but correct expmap for Pose2" OFF) diff --git a/cmake/HandlePrintConfiguration.cmake b/cmake/HandlePrintConfiguration.cmake index ac68be20fe..09740ffcd2 100644 --- a/cmake/HandlePrintConfiguration.cmake +++ b/cmake/HandlePrintConfiguration.cmake @@ -90,11 +90,13 @@ print_enabled_config(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency c print_enabled_config(${GTSAM_ENABLE_MEMORY_SANITIZER} "Build with Memory Sanitizer ") print_enabled_config(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ") print_enabled_config(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ") +print_enabled_config(${GTSAM_EXTENDED_POSE_RETRACT} "NavState uses ExtendedPose3 retract") print_enabled_config(${GTSAM_DT_MERGING} "Enable branch merging in DecisionTree") print_enabled_config(${GTSAM_ENABLE_TIMING} "Enable timing machinery") print_enabled_config(${GTSAM_ALLOW_DEPRECATED_SINCE_V43} "Allow features deprecated in GTSAM 4.3") print_enabled_config(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ") print_enabled_config(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration") +print_enabled_config(${GTSAM_LIEGROUP_PREINTEGRATION} "Use Lie group based preintegration") message(STATUS "MATLAB toolbox flags") print_enabled_config(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install MATLAB toolbox ") diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 58b93ee1ce..c105545253 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -39,6 +39,9 @@ #cmakedefine GTSAM_ROT3_EXPMAP #endif +// Whether GTSAM is compiled to use ExtendedPose3::Retract as the default for NavState +#cmakedefine GTSAM_EXTENDED_POSE_RETRACT + // Whether to enable merging of equal leaf nodes in the Discrete Decision Tree. #cmakedefine GTSAM_DT_MERGING @@ -81,9 +84,12 @@ // Support Metis-based nested dissection #cmakedefine GTSAM_SUPPORT_NESTED_DISSECTION -// Support Metis-based nested dissection +// Whether to use IMU pre-integration on tangent space #cmakedefine GTSAM_TANGENT_PREINTEGRATION +// Whether to use IMU pre-integration based on Lie groups +#cmakedefine GTSAM_LIEGROUP_PREINTEGRATION + // Whether to use the system installed Metis instead of the provided one #cmakedefine GTSAM_USE_SYSTEM_METIS diff --git a/gtsam/geometry/ExtendedPose3.cpp b/gtsam/geometry/ExtendedPose3.cpp new file mode 100644 index 0000000000..7b44221269 --- /dev/null +++ b/gtsam/geometry/ExtendedPose3.cpp @@ -0,0 +1,35 @@ +/* ---------------------------------------------------------------------------- + + * 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 ExtendedPose3.cpp + * @brief 3D ExtendedPose3 + */ + +#include +#include +#include + +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +/** instantiate concept checks */ +// GTSAM_CONCEPT_POSE_INST(ExtendedPose3); + + + +} // namespace gtsam diff --git a/gtsam/geometry/ExtendedPose3.h b/gtsam/geometry/ExtendedPose3.h new file mode 100644 index 0000000000..41fd153dd3 --- /dev/null +++ b/gtsam/geometry/ExtendedPose3.h @@ -0,0 +1,720 @@ +/* ---------------------------------------------------------------------------- + + * 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 ExtendedPose3.h + *@brief 3D Extended Pose + */ + +// \callgraph +#pragma once + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gtsam { + +/** + * A 3D extended pose (R,v,p) : (Rot3,Point3,Point3) + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT ExtendedPose3: public LieGroup { +public: + + /** Pose Concept requirements */ + typedef Rot3 Rotation; + typedef Point3 Velocity; + typedef Point3 Position; + typedef Position Translation; + +private: + + Rot3 R_; ///< Rotation gRp, between global and pose frame + Point3 v_; ///< Velocity gVp, from global origin to pose frame origin + Point3 p_; ///< Position gPp, from global origin to pose frame origin + +public: + + /// @name Standard Constructors + /// @{ + + /** Default constructor is origin */ + ExtendedPose3() : R_(traits::Identity()), v_(traits::Identity()), p_(traits::Identity()) {} + + /** Copy constructor */ + ExtendedPose3(const ExtendedPose3& pose) : + R_(pose.R_), v_(pose.v_), p_(pose.p_) { + } + + /** Construct from R,v,p */ + ExtendedPose3(const Rot3& R, const Point3& v, const Point3& p) : + R_(R), v_(v), p_(p) { + } + + //explicit Pose3(const Pose2& pose2); + + /** Constructor from 5*5 matrix */ + ExtendedPose3(const Matrix &T) : + R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), T(2, 1), + T(2, 2)), v_(T(0, 3), T(1, 3), T(2, 3)), p_(T(0, 4), T(1, 4), T(2, 4)) { + } + + /// Named constructor with derivatives + static ExtendedPose3 Create(const Rot3& R, const Point3& v, const Point3& p, + OptionalJacobian<9, 3> HR = {}, + OptionalJacobian<9, 3> Hv = {}, + OptionalJacobian<9, 3> Hp = {}) { + if (HR) *HR << I_3x3, Z_3x3, Z_3x3; + if (Hv) *Hv << Z_3x3, R.transpose(), Z_3x3; + if (Hp) *Hp << Z_3x3, Z_3x3, R.transpose(); + return ExtendedPose3(R, v, p); + } + + /** + * Create Pose3 by aligning two point pairs + * A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point + * Note this allows for noise on the points but in that case the mapping will not be exact. + */ + static std::optional Align(const std::vector& abPointPairs) { + const size_t n = abPointPairs.size(); + if (n < 3) + return {}; // we need at least three pairs + + // calculate centroids + Point3 aCentroid(0,0,0), bCentroid(0,0,0); + for(const Point3Pair& abPair: abPointPairs) { + aCentroid += abPair.first; + bCentroid += abPair.second; + } + double f = 1.0 / n; + aCentroid *= f; + bCentroid *= f; + + // Add to form H matrix + Matrix3 H = Z_3x3; + for(const Point3Pair& abPair: abPointPairs) { + Point3 da = abPair.first - aCentroid; + Point3 db = abPair.second - bCentroid; + H += da * db.transpose(); + } + + // ClosestTo finds rotation matrix closest to H in Frobenius sense + Rot3 aRb = Rot3::ClosestTo(H); + Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid); + Point3 v; + return ExtendedPose3(aRb, v, aTb); + } + + /// @} + /// @name Testable + /// @{ + + /// print with optional string + void print(const std::string& s = "") const { + std::cout << s; + R_.print("R:\n"); + std::cout << "v:" << v_ << ";" << std::endl; + std::cout << "p:" << p_ << ";" << std::endl; + } + + /// assert equality up to a tolerance + bool equals(const ExtendedPose3& pose, double tol = 1e-9) const { + return R_.equals(pose.R_, tol) && traits::Equals(v_, pose.v_, tol) && traits::Equals(p_, pose.p_, tol); + } + + /// @} + /// @name Group + /// @{ + + /// identity for group operation + static ExtendedPose3 Identity() { + return ExtendedPose3(); + } + + /// inverse transformation without derivative + ExtendedPose3 inverse() const { + Rot3 Rt = R_.inverse(); + return ExtendedPose3(Rt, Rt * (-v_), Rt * (-p_)); + } + + using LieGroup::inverse; // version with derivative + + /// compose syntactic sugar + ExtendedPose3 operator*(const ExtendedPose3& T) const { + return ExtendedPose3(R_ * T.R_, v_ + R_ * T.v_, p_ + R_ * T.p_); + } + + /// @} + /// @name Lie Group + /// @{ + + /// Exponential map at identity - create a rotation from canonical coordinates \f$ [R_x,R_y,R_z,V_x,V_y,V_z,P_x,P_y,P_z] \f$ + static ExtendedPose3 Expmap(const Vector9& xi, OptionalJacobian<9, 9> Hxi = {}) { + if (Hxi) *Hxi = ExpmapDerivative(xi); + + Vector3 phi(xi(0), xi(1), xi(2)), nu(xi(3), xi(4), xi(5)), rho(xi(6), xi(7), xi(8)); + + Rot3 R = Rot3::Expmap(phi); + + double phi_norm = phi.norm(); + if (phi_norm < 1e-8) + return ExtendedPose3(Rot3(), Point3(nu), Point3(rho)); + else { + Matrix W = skewSymmetric(phi/phi_norm); + Matrix A = I_3x3 + ((1 - cos(phi_norm)) / phi_norm) * W + ((phi_norm - sin(phi_norm)) / phi_norm) * (W * W); + return ExtendedPose3(Rot3::Expmap(phi), Point3(A * nu), Point3(A * rho)); + } + } + + /// Log map at identity - return the canonical coordinates \f$ [R_x,R_y,R_z,V_x,V_y,V_z,P_x,P_y,P_z] \f$ of this rotation + static Vector9 Logmap(const ExtendedPose3& pose, OptionalJacobian<9, 9> Hpose = {}) { + if (Hpose) *Hpose = LogmapDerivative(pose); + const Vector3 phi = Rot3::Logmap(pose.rotation()); + const Vector3 v = pose.velocity(); + const Vector3 p = pose.position(); + const double t = phi.norm(); + if (t < 1e-8) { + Vector9 log; + log << phi, v, p; + return log; + } else { + const Matrix3 W = skewSymmetric(phi / t); + + const double Tan = tan(0.5 * t); + const Vector3 Wp = W * p; + const Vector3 Wv = W * v; + const Vector3 nu = v - (0.5 * t) * Wv + (1 - t / (2. * Tan)) * (W * Wv); + const Vector3 rho = p - (0.5 * t) * Wp + (1 - t / (2. * Tan)) * (W * Wp); + Vector9 log; + log << phi, nu, rho; + return log; + } + } + + /** + * Calculate Adjoint map, transforming a twist in the this pose's (i.e, body) frame to the world spatial frame + * Ad_pose is 9*9 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,V_x,V_y,V_z,P_x,P_y,P_z] \f$, returns Ad_pose(xi) + */ + Matrix9 AdjointMap() const { + const Matrix3 R = R_.matrix(); + Matrix3 A = skewSymmetric(v_.x(), v_.y(), v_.z()) * R; + Matrix3 B = skewSymmetric(p_.x(), p_.y(), p_.z()) * R; + Matrix9 adj; + adj << R, Z_3x3, Z_3x3, + A, R, Z_3x3, + B, Z_3x3, R; + return adj; + } + + /** + * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame + * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ + */ + Vector9 Adjoint(const Vector9& xi_b) const { + return AdjointMap() * xi_b; + } + + /** + * Compute the ad operator + */ + static Matrix9 adjointMap(const Vector9 &xi) { + Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); + Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); + Matrix3 a_hat = skewSymmetric(xi(6), xi(7), xi(8)); + Matrix9 adj; + adj << w_hat, Z_3x3, Z_3x3, + v_hat, w_hat, Z_3x3, + a_hat, Z_3x3, w_hat; + return adj; + } + + /** + * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives + */ + static Vector9 adjoint(const Vector9 &xi, const Vector9 &y, + OptionalJacobian<9, 9> Hxi = {}) { + if (Hxi) { + Hxi->setZero(); + for (int i = 0; i < 9; ++i) { + Vector9 dxi; + dxi.setZero(); + dxi(i) = 1.0; + Matrix9 Gi = adjointMap(dxi); + Hxi->col(i) = Gi * y; + } + } + return adjointMap(xi) * y; + } + + /** + * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. + */ + static Vector9 adjointTranspose(const Vector9& xi, const Vector9& y, + OptionalJacobian<9, 9> Hxi = {}) { + if (Hxi) { + Hxi->setZero(); + for (int i = 0; i < 9; ++i) { + Vector9 dxi; + dxi.setZero(); + dxi(i) = 1.0; + Matrix9 GTi = adjointMap(dxi).transpose(); + Hxi->col(i) = GTi * y; + } + } + return adjointMap(xi).transpose() * y; + } + + /// Derivative of Expmap + static Matrix9 ExpmapDerivative(const Vector9& xi) { + const Vector3 w = xi.head<3>(); + const Matrix3 Jw = Rot3::ExpmapDerivative(w); + const Matrix63 Q = computeQforExpmapDerivative(xi); + const Matrix3 Qv = Q.topRows<3>(); + const Matrix3 Qp = Q.bottomRows<3>(); + Matrix9 J; + J << Jw, Z_3x3, Z_3x3, + Qv, Jw, Z_3x3, + Qp, Z_3x3, Jw; + return J; + } + + /// Derivative of Logmap + static Matrix9 LogmapDerivative(const ExtendedPose3& pose) { + const Vector9 xi = Logmap(pose); + const Vector3 w = xi.head<3>(); + const Matrix3 Jw = Rot3::LogmapDerivative(w); + const Matrix63 Q = computeQforExpmapDerivative(xi); + const Matrix3 Qv = Q.topRows<3>(); + const Matrix3 Qp = Q.bottomRows<3>(); + const Matrix3 Qv2 = -Jw*Qv*Jw; + const Matrix3 Qp2 = -Jw*Qp*Jw; + Matrix9 J; + + J << Jw, Z_3x3, Z_3x3, + Qv2, Jw, Z_3x3, + Qp2, Z_3x3, Jw; + return J; + } + + // Chart at origin, depends on compile-time flag GTSAM_POSE3_EXPMAP + struct ChartAtOrigin { + static ExtendedPose3 Retract(const Vector9& xi, ChartJacobian Hxi = {}) { +#ifdef GTSAM_POSE3_EXPMAP + return Expmap(xi, Hxi); +#else + +#endif + } + static Vector9 Local(const ExtendedPose3& pose, ChartJacobian Hpose = {}) { +#ifdef GTSAM_POSE3_EXPMAP + return Logmap(pose, Hpose); +#else + +#endif + } + }; + + Vector9 boxminus(const ExtendedPose3& g) const { + // Matrix3 D_dR_R, D_dt_R, D_dv_R; + // const Rot3 dR = R_.between(g.R_, H1 ? &D_dR_R : 0); + // const Point3 dt = R_.unrotate(g.p_ - p_, H1 ? &D_dt_R : 0); + // const Vector dv = R_.unrotate(g.v_ - v_, H1 ? &D_dv_R : 0); + + // Vector9 xi; + // Matrix3 D_xi_R; + // xi << Rot3::Logmap(dR, (H1 || H2) ? &D_xi_R : 0), dt, dv; + // if (H1) { + // *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3, // + // D_dt_R, -I_3x3, Z_3x3, // + // D_dv_R, Z_3x3, -I_3x3; + // } + // if (H2) { + // *H2 << D_xi_R, Z_3x3, Z_3x3, // + // Z_3x3, dR.matrix(), Z_3x3, // + // Z_3x3, Z_3x3, dR.matrix(); + // } + Matrix3 D_dR_R, D_dt_R, D_dv_R; + const Rot3 dR = g.R_; + const Point3 dt = g.p_; + const Vector dv = g.v_; + + Vector9 xi; + xi << Rot3::Logmap(dR), dt, dv; + return xi; + } + + /** + * wedge for ExtendedPose3: + * @param xi 9-dim twist (omega,nu,rho) + * @return 5*5 element of Lie algebra + */ + static Matrix wedge(double phix, double phiy, double phiz, double nux, double nuy, double nuz, double rhox, double rhoy, double rhoz) { + return (Matrix(5, 5) << 0., -phiz, phiy, nux, rhox, phiz, 0., -phix, nuy, rhoy, -phiy, phix, 0., nuz, rhoz, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.).finished(); + } + + /** + * Compute the 6x3 bottom-left block Qs of the SE_2(3) Expmap derivative matrix + */ + static Matrix63 computeQforExpmapDerivative(const Vector9& xi) { + const auto omega = xi.head<3>(); + const auto nu = xi.segment<3>(3); + const auto rho = xi.tail<3>(); + const Matrix3 V = skewSymmetric(nu); + const Matrix3 P = skewSymmetric(rho); + const Matrix3 W = skewSymmetric(omega); + + Matrix3 Qv, Qp; + Matrix63 Q; + +#ifdef NUMERICAL_EXPMAP_DERIV + +#else + // The closed-form formula in Barfoot14tro eq. (102) + double phi = omega.norm(); + if (std::abs(phi)>1e-5) { + const double sinPhi = sin(phi), cosPhi = cos(phi); + const double phi2 = phi * phi, phi3 = phi2 * phi, phi4 = phi3 * phi, phi5 = phi4 * phi; + // Invert the sign of odd-order terms to have the right Jacobian + Qv = -0.5*V + (phi-sinPhi)/phi3*(W*V + V*W - W*V*W) + + (1-phi2/2-cosPhi)/phi4*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*V*W*W + W*W*V*W); + Qp = -0.5*P + (phi-sinPhi)/phi3*(W*P + P*W - W*P*W) + + (1-phi2/2-cosPhi)/phi4*(W*W*P + P*W*W - 3*W*P*W) + - 0.5*((1-phi2/2-cosPhi)/phi4 - 3*(phi-sinPhi-phi3/6.)/phi5)*(W*P*W*W + W*W*P*W); + } + else { + Qv = -0.5*V + 1./6.*(W*V + V*W - W*V*W) + + 1./24.*(W*W*V + V*W*W - 3*W*V*W) + - 0.5*(1./24. + 3./120.)*(W*V*W*W + W*W*V*W); + Qp = -0.5*P + 1./6.*(W*P + P*W - W*P*W) + + 1./24.*(W*W*P + P*W*W - 3*W*P*W) + - 0.5*(1./24. + 3./120.)*(W*P*W*W + W*W*P*W); + } +#endif + Q << Qv, Qp; + return Q; + } + + /// @} + /// @name Group Action on Point3 + /// @{ + + /** + * @brief takes point in Pose coordinates and transforms it to world coordinates + * @param point point in Pose coordinates + * @param Hself optional 3*6 Jacobian wrpt this pose + * @param Hpoint optional 3*3 Jacobian wrpt point + * @return point in world coordinates + */ + Point3 transformFrom(const Point3& point, + OptionalJacobian<3, 9> Hself = {}, + OptionalJacobian<3, 3> Hpoint = {}) const { + // Only get matrix once, to avoid multiple allocations, + // as well as multiple conversions in the Quaternion case + const Matrix3 R = R_.matrix(); + if (Hself) { + Hself->leftCols<3>() = + R * skewSymmetric(-point.x(), -point.y(), -point.z()); + Hself->rightCols<3>() = R; + } + if (Hpoint) { + *Hpoint = R; + } + return R_ * point + p_; + } + + /** syntactic sugar for transformFrom */ + inline Point3 operator*(const Point3& point) const { + return transformFrom(point); + } + + /** + * @brief takes point in world coordinates and transforms it to Pose coordinates + * @param point point in world coordinates + * @param Hself optional 3*6 Jacobian wrpt this pose + * @param Hpoint optional 3*3 Jacobian wrpt point + * @return point in Pose coordinates + */ + Point3 transformTo(const Point3& point, + OptionalJacobian<3, 9> Hself = {}, + OptionalJacobian<3, 3> Hpoint = {}) const { + // Only get transpose once, to avoid multiple allocations, + // as well as multiple conversions in the Quaternion case + const Matrix3 Rt = R_.transpose(); + const Point3 q(Rt * (point - p_)); + if (Hself) { + const double wx = q.x(), wy = q.y(), wz = q.z(); + (*Hself) << 0.0, -wz, +wy, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, +wz, 0.0, -wx, + 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, -wy, +wx, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + -1.0; + } + if (Hpoint) { + *Hpoint = Rt; + } + return q; + } + + /// @} + /// @name Standard Interface + /// @{ + + /// get rotation + const Rot3& rotation(OptionalJacobian<3, 9> Hself = {}) const { + if (Hself) { + *Hself << I_3x3, Z_3x3, Z_3x3; + } + return R_; + } + + /// get translation + const Point3& velocity(OptionalJacobian<3, 9> Hself = {}) const { + if (Hself) { + *Hself << Z_3x3, rotation().matrix(), Z_3x3; + } + return v_; + } + + /// get position + const Point3& position(OptionalJacobian<3, 9> Hself = {}) const { + if (Hself) *Hself << Z_3x3, Z_3x3, rotation().matrix(); + return p_; + } + + const Point3& translation(OptionalJacobian<3, 9> Hself = {}) const { + if (Hself) *Hself << Z_3x3, Z_3x3, rotation().matrix(); + return p_; + } + + /// get x + double x() const { + return p_.x(); + } + + /// get y + double y() const { + return p_.y(); + } + + /// get z + double z() const { + return p_.z(); + } + + /** convert to 5*5 matrix */ + Matrix5 matrix() const { + Matrix5 mat; + mat << R_.matrix(), v_, p_, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 1.0; + return mat; + } + + /** + * Assuming self == wTa, takes a pose aTb in local coordinates + * and transforms it to world coordinates wTb = wTa * aTb. + * This is identical to compose. + */ + ExtendedPose3 transformPoseFrom( + const ExtendedPose3& aTb, OptionalJacobian<9, 9> Hself = {}, + OptionalJacobian<9, 9> HaTb = {}) const { + const ExtendedPose3& wTa = *this; + return wTa.compose(aTb, Hself, HaTb); + } + + /** + * Assuming self == wTa, takes a pose wTb in world coordinates + * and transforms it to local coordinates aTb = inv(wTa) * wTb + */ + ExtendedPose3 transformPoseTo( + const ExtendedPose3& wTb, OptionalJacobian<9, 9> Hself = {}, + OptionalJacobian<9, 9> HwTb = {}) const { + if (Hself) *Hself = -wTb.inverse().AdjointMap() * AdjointMap(); + if (HwTb) *HwTb = I_9x9; + const ExtendedPose3& wTa = *this; + return wTa.inverse() * wTb; + } + + /** + * Calculate range to a landmark + * @param point 3D location of landmark + * @return range (double) + */ + double range(const Point3& point, OptionalJacobian<1, 9> Hself = {}, + OptionalJacobian<1, 3> Hpoint = {}) const { + Matrix39 D_local_pose; + Matrix3 D_local_point; + Point3 local = transformTo(point, Hself ? &D_local_pose : 0, + Hpoint ? &D_local_point : 0); + if (!Hself && !Hpoint) { + return local.norm(); + } else { + Matrix13 D_r_local; + const double r = norm3(local, D_r_local); + if (Hself) *Hself = D_r_local * D_local_pose; + if (Hpoint) *Hpoint = D_r_local * D_local_point; + return r; + } + } + + /** + * Calculate range to another pose + * @param pose Other SO(3) pose + * @return range (double) + */ + double range(const ExtendedPose3& pose, + OptionalJacobian<1, 9> Hself = {}, + OptionalJacobian<1, 9> Hpose = {}) const { + Matrix13 D_local_point; + double r = range(pose.translation(), Hself, Hpose ? &D_local_point : 0); + if (Hpose) + *Hpose << Matrix13::Zero(), D_local_point * pose.rotation().matrix(); + return r; + } + + /** + * Calculate bearing to a landmark + * @param point 3D location of landmark + * @return bearing (Unit3) + */ + Unit3 bearing(const Point3& point, OptionalJacobian<2, 9> Hself = {}, + OptionalJacobian<2, 3> Hpoint = {}) const { + Matrix39 D_local_pose; + Matrix3 D_local_point; + Point3 local = transformTo(point, Hself ? &D_local_pose : 0, + Hpoint ? &D_local_point : 0); + if (!Hself && !Hpoint) { + return Unit3(local); + } else { + Matrix23 D_b_local; + Unit3 b = Unit3::FromPoint3(local, D_b_local); + if (Hself) *Hself = D_b_local * D_local_pose; + if (Hpoint) *Hpoint = D_b_local * D_local_point; + return b; + } + } + + /** + * Calculate bearing to another pose + * @param other 3D location and orientation of other body. The orientation + * information is ignored. + * @return bearing (Unit3) + */ + Unit3 bearing(const ExtendedPose3& pose, + OptionalJacobian<2, 9> Hself = {}, + OptionalJacobian<2, 9> Hpose = {}) const { + if (Hpose) { + Hpose->setZero(); + return bearing(pose.translation(), Hself, Hpose.cols<3>(3)); + } + return bearing(pose.translation(), Hself, {}); + } + + /// @} + /// @name Advanced Interface + /// @{ + + /** + * Return the start and end indices (inclusive) of the translation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + inline static std::pair velocityInterval() { + return std::make_pair(3, 5); + } + + /** + * Return the start and end indices (inclusive) of the translation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + inline static std::pair positionInterval() { + return std::make_pair(6, 8); + } + inline static std::pair translationInterval() { + return std::make_pair(6, 8); + } + + /** + * Return the start and end indices (inclusive) of the rotation component of the + * exponential map parameterization + * @return a pair of [start, end] indices into the tangent space vector + */ + static std::pair rotationInterval() { + return std::make_pair(0, 2); + } + + /// Output stream operator + GTSAM_EXPORT + friend std::ostream& operator<<(std::ostream& os, const ExtendedPose3& pose) { + os << pose.rotation(); + const Point3& v = pose.velocity(); + const Point3& p = pose.position(); + os << "v:[" << v.x() << ", " << v.y() << ", " << v.z() << "];\n"; + os << "p:[" << p.x() << ", " << p.y() << ", " << p.z() << "];\n"; + return os; + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(R_); + ar & BOOST_SERIALIZATION_NVP(v_); + ar & BOOST_SERIALIZATION_NVP(p_); + } + /// @} + +#ifdef GTSAM_USE_QUATERNIONS + // Align if we are using Quaternions + public: + GTSAM_MAKE_ALIGNED_OPERATOR_NEW +#endif +}; +// ExtendedPose3 class + +/** + * wedge for ExtendedPose3: + * @param xi 9-dim twist (omega,nu,rho) + * @return 5*5 element of Lie algebra + */ +template<> +inline Matrix wedge(const Vector& xi) { + return ExtendedPose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5), xi(6), xi(7), xi(8)); +} + + + +template <> +struct traits : public internal::LieGroup {}; + +template <> +struct traits : public internal::LieGroup {}; + +// bearing and range traits, used in RangeFactor +template <> +struct Bearing : HasBearing {}; + +template<> +struct Bearing : HasBearing {}; + +template +struct Range : HasRange {}; +} // namespace gtsam diff --git a/gtsam/geometry/tests/testExtendedPose3.cpp b/gtsam/geometry/tests/testExtendedPose3.cpp new file mode 100644 index 0000000000..bd1e779749 --- /dev/null +++ b/gtsam/geometry/tests/testExtendedPose3.cpp @@ -0,0 +1,529 @@ +/* ---------------------------------------------------------------------------- + + * 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 testExtendedPose3.cpp + * @brief Unit tests for ExtendedPose3 class + */ + +#include +#include +#include + +#include +#include + +using namespace std; +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(ExtendedPose3) +GTSAM_CONCEPT_LIE_INST(ExtendedPose3) + +static const Point3 V(3,0.4,-2.2); +static const Point3 P(0.2,0.7,-2); +static const Rot3 R = Rot3::Rodrigues(0.3,0,0); +static const Point3 V2(-6.5,3.5,6.2); +static const Point3 P2(3.5,-8.2,4.2); +static const ExtendedPose3 T(R,V2,P2); +static const ExtendedPose3 T2(Rot3::Rodrigues(0.3,0.2,0.1),V2,P2); +static const ExtendedPose3 T3(Rot3::Rodrigues(-90, 0, 0), Point3(5,6,7), Point3(1, 2, 3)); + +/* ************************************************************************* */ +TEST( ExtendedPose3, equals) +{ + ExtendedPose3 pose2 = T3; + EXPECT(T3.equals(pose2)); + ExtendedPose3 origin; + EXPECT(!T3.equals(origin)); +} + +/* ************************************************************************* */ +#ifndef GTSAM_POSE3_EXPMAP +TEST( ExtendedPose3, retract_first_order) +{ + ExtendedPose3 id; + Vector xi = Z_9x1; + xi(0) = 0.3; + EXPECT(assert_equal(ExtendedPose3(R, Vector3(0,0,0), Point3(0,0,0)), id.retract(xi),1e-2)); + xi(3)=3;xi(4)=0.4;xi(5)=-2.2; + xi(6)=0.2;xi(7)=0.7;xi(8)=-2; + EXPECT(assert_equal(ExtendedPose3(R, V, P),id.retract(v),1e-2)); +} +#endif +/* ************************************************************************* */ +TEST( ExtendedPose3, retract_expmap) +{ + Vector xi = Z_9x1; xi(0) = 0.3; + ExtendedPose3 pose = ExtendedPose3::Expmap(xi); + EXPECT(assert_equal(ExtendedPose3(R, Point3(0,0,0), Point3(0,0,0)), pose, 1e-2)); + EXPECT(assert_equal(xi,ExtendedPose3::Logmap(pose),1e-2)); +} + +/* ************************************************************************* */ +TEST( ExtendedPose3, expmap_a_full) +{ + ExtendedPose3 id; + Vector xi = Z_9x1; + xi(0) = 0.3; + EXPECT(assert_equal(expmap_default(id, xi), ExtendedPose3(R, Vector3(0,0,0), Point3(0,0,0)))); + xi(3)=-0.2;xi(4)=-0.394742;xi(5)=2.08998; + xi(6)=0.2;xi(7)=0.394742;xi(8)=-2.08998; + EXPECT(assert_equal(ExtendedPose3(R, -P, P),expmap_default(id, xi),1e-5)); +} + +/* ************************************************************************* */ +TEST( ExtendedPose3, expmap_a_full2) +{ + ExtendedPose3 id; + Vector xi = Z_9x1; + xi(0) = 0.3; + EXPECT(assert_equal(expmap_default(id, xi), ExtendedPose3(R, Point3(0,0,0), Point3(0,0,0)))); + xi(3)=-0.2;xi(4)=-0.394742;xi(5)=2.08998; + xi(6)=0.2;xi(7)=0.394742;xi(8)=-2.08998; + EXPECT(assert_equal(ExtendedPose3(R, -P, P),expmap_default(id, xi),1e-5)); +} + +/* ************************************************************************* */ +TEST(ExtendedPose3, expmap_b) +{ + ExtendedPose3 p1(Rot3(), Vector3(-100, 0, 0), Point3(100, 0, 0)); + ExtendedPose3 p2 = p1.retract((Vector(9) << 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished()); + ExtendedPose3 expected(Rot3::Rodrigues(0.0, 0.0, 0.1), Point3(-100.0, 0.0, 0.0), Point3(100.0, 0.0, 0.0)); + EXPECT(assert_equal(expected, p2,1e-2)); +} + +/* ************************************************************************* */ +// test case for screw motion in the plane +namespace screwExtendedPose3 { + double a=0.3, c=cos(a), s=sin(a), w=0.3; + Vector xi = (Vector(9) << 0.0, 0.0, w, w, 0.0, 1.0, w, 0.0, 1.0).finished(); + Rot3 expectedR(c, -s, 0, s, c, 0, 0, 0, 1); + Point3 expectedV(0.29552, 0.0446635, 1); + Point3 expectedP(0.29552, 0.0446635, 1); + ExtendedPose3 expected(expectedR, expectedV, expectedP); +} + +/* ************************************************************************* */ +// Checks correct exponential map (Expmap) with brute force matrix exponential +TEST(ExtendedPose3, expmap_c_full) +{ + EXPECT(assert_equal(screwExtendedPose3::expected, expm(screwExtendedPose3::xi),1e-6)); + EXPECT(assert_equal(screwExtendedPose3::expected, ExtendedPose3::Expmap(screwExtendedPose3::xi),1e-6)); +} + +/* ************************************************************************* */ +// assert that T*exp(xi)*T^-1 is equal to exp(Ad_T(xi)) +TEST(ExtendedPose3, Adjoint_full) +{ + ExtendedPose3 expected = T * ExtendedPose3::Expmap( screwExtendedPose3::xi) * T.inverse(); + Vector xiprime = T.Adjoint( screwExtendedPose3::xi); + EXPECT(assert_equal(expected, ExtendedPose3::Expmap(xiprime), 1e-6)); + + ExtendedPose3 expected2 = T2 * ExtendedPose3::Expmap( screwExtendedPose3::xi) * T2.inverse(); + Vector xiprime2 = T2.Adjoint( screwExtendedPose3::xi); + EXPECT(assert_equal(expected2, ExtendedPose3::Expmap(xiprime2), 1e-6)); + + ExtendedPose3 expected3 = T3 * ExtendedPose3::Expmap( screwExtendedPose3::xi) * T3.inverse(); + Vector xiprime3 = T3.Adjoint( screwExtendedPose3::xi); + EXPECT(assert_equal(expected3, ExtendedPose3::Expmap(xiprime3), 1e-6)); +} + +/* ************************************************************************* */ +// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi)) +TEST(ExtendedPose3, Adjoint_hat) +{ + auto hat = [](const Vector& xi) { return ::wedge(xi); }; + Matrix5 expected = T.matrix() * hat( screwExtendedPose3::xi) * T.matrix().inverse(); + Matrix5 xiprime = hat(T.Adjoint( screwExtendedPose3::xi)); + + EXPECT(assert_equal(expected, xiprime, 1e-6)); + + Matrix5 expected2 = T2.matrix() * hat( screwExtendedPose3::xi) * T2.matrix().inverse(); + Matrix5 xiprime2 = hat(T2.Adjoint( screwExtendedPose3::xi)); + EXPECT(assert_equal(expected2, xiprime2, 1e-6)); + + Matrix5 expected3 = T3.matrix() * hat( screwExtendedPose3::xi) * T3.matrix().inverse(); + + Matrix5 xiprime3 = hat(T3.Adjoint( screwExtendedPose3::xi)); + EXPECT(assert_equal(expected3, xiprime3, 1e-6)); +} + +/* ************************************************************************* */ +TEST(ExtendedPose3, expmaps_galore_full) +{ + Vector xi; ExtendedPose3 actual; + xi = (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished(); + actual = ExtendedPose3::Expmap(xi); + EXPECT(assert_equal(expm(xi), actual,1e-6)); + EXPECT(assert_equal(xi, ExtendedPose3::Logmap(actual),1e-6)); + + xi = (Vector(9) << 0.1, -0.2, 0.3, -0.4, 0.5, -0.6, -0.7, -0.8, -0.9).finished(); + for (double theta=1.0;0.3*theta<=M_PI;theta*=2) { + Vector txi = xi*theta; + actual = ExtendedPose3::Expmap(txi); + EXPECT(assert_equal(expm(txi,30), actual,1e-6)); + Vector log = ExtendedPose3::Logmap(actual); + EXPECT(assert_equal(actual, ExtendedPose3::Expmap(log),1e-6)); + EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps + } + + // Works with large v as well, but expm needs 10 iterations! + xi = (Vector(9) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0, 12, 14, 45).finished(); + actual = ExtendedPose3::Expmap(xi); + EXPECT(assert_equal(expm(xi,10), actual,1e-5)); + EXPECT(assert_equal(xi, ExtendedPose3::Logmap(actual),1e-9)); +} + +/* ************************************************************************* */ +// Check position and its pushforward + +TEST(ExtendedPose3, position) { + Matrix actualH; + EXPECT(assert_equal(Point3(3.5, -8.2, 4.2), T.position(actualH), 1e-8)); + Matrix numericalH = numericalDerivative11( + std::bind(&ExtendedPose3::position, std::placeholders::_1, nullptr), T); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + +/* ************************************************************************* */ +// Check rotation and its pushforward +TEST(ExtendedPose3, rotation) { + Matrix actualH; + EXPECT(assert_equal(R, T.rotation(actualH), 1e-8)); + + Matrix numericalH = numericalDerivative11( + std::bind(&ExtendedPose3::rotation, std::placeholders::_1, nullptr), T); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + +/* ************************************************************************* */ +// Check velocity and its pushforward +TEST(ExtendedPose3, velocity) { + Matrix actualH; + EXPECT(assert_equal(Point3(-6.5,3.5,6.2), T.velocity(actualH), 1e-8)); + Matrix numericalH = numericalDerivative11( + std::bind(&ExtendedPose3::velocity, std::placeholders::_1, nullptr), T); + EXPECT(assert_equal(numericalH, actualH, 1e-6)); +} + +/* ************************************************************************* */ +TEST(ExtendedPose3, Adjoint_compose_full) +{ + // To debug derivatives of compose, assert that + // T1*T2*exp(Adjoint(inv(T2),x) = T1*exp(x)*T2 + const ExtendedPose3& T1 = T; + Vector x = (Vector(9) << 0.1, 0.1, 0.1, 0.4, 0.2, 0.8, 0.4, 0.2, 0.8).finished(); + ExtendedPose3 expected = T1 * ExtendedPose3::Expmap(x) * T2; + Vector y = T2.inverse().Adjoint(x); + ExtendedPose3 actual = T1 * T2 * ExtendedPose3::Expmap(y); + EXPECT(assert_equal(expected, actual, 1e-6)); +} + +/* ************************************************************************* */ +// Check compose and its pushforward +// NOTE: testing::compose(t1,t2) = t1.compose(t2) (see lieProxies.h) +TEST( ExtendedPose3, compose ) +{ + Matrix actual = (T2*T2).matrix(); + + Matrix expected = T2.matrix()*T2.matrix(); + EXPECT(assert_equal(actual,expected,1e-8)); + + Matrix actualDcompose1, actualDcompose2; + T2.compose(T2, actualDcompose1, actualDcompose2); + + Matrix numericalH1 = numericalDerivative21(testing::compose, T2, T2); + + EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(),actualDcompose1,5e-3)); + + Matrix numericalH2 = numericalDerivative22(testing::compose, T2, T2); + EXPECT(assert_equal(numericalH2,actualDcompose2,1e-4)); +} + +/* ************************************************************************* */ +// Check compose and its pushforward, another case +TEST( ExtendedPose3, compose2 ) +{ + const ExtendedPose3& T1 = T; + Matrix actual = (T1*T2).matrix(); + Matrix expected = T1.matrix()*T2.matrix(); + EXPECT(assert_equal(actual,expected,1e-8)); + + Matrix actualDcompose1, actualDcompose2; + T1.compose(T2, actualDcompose1, actualDcompose2); + + Matrix numericalH1 = numericalDerivative21(testing::compose, T1, T2); + EXPECT(assert_equal(numericalH1,actualDcompose1,5e-3)); + EXPECT(assert_equal(T2.inverse().AdjointMap(),actualDcompose1,5e-3)); + + Matrix numericalH2 = numericalDerivative22(testing::compose, T1, T2); + EXPECT(assert_equal(numericalH2,actualDcompose2,1e-5)); +} + +/* ************************************************************************* */ +TEST( ExtendedPose3, inverse) +{ + Matrix actualDinverse; + Matrix actual = T.inverse(actualDinverse).matrix(); + Matrix expected = T.matrix().inverse(); + EXPECT(assert_equal(actual,expected,1e-8)); + + Matrix numericalH = numericalDerivative11(testing::inverse, T); + EXPECT(assert_equal(numericalH,actualDinverse,5e-3)); + EXPECT(assert_equal(-T.AdjointMap(),actualDinverse,5e-3)); +} + +/* ************************************************************************* */ +TEST( ExtendedPose3, inverseDerivatives2) +{ + Rot3 R = Rot3::Rodrigues(0.3,0.4,-0.5); + Vector3 v(3.5,-8.2,4.2); + Point3 p(3.5,-8.2,4.2); + ExtendedPose3 T(R,v,p); + + Matrix numericalH = numericalDerivative11(testing::inverse, T); + Matrix actualDinverse; + T.inverse(actualDinverse); + EXPECT(assert_equal(numericalH,actualDinverse,5e-3)); + EXPECT(assert_equal(-T.AdjointMap(),actualDinverse,5e-3)); +} + +/* ************************************************************************* */ +TEST( ExtendedPose3, compose_inverse) +{ + Matrix actual = (T*T.inverse()).matrix(); + Matrix expected = I_5x5; + EXPECT(assert_equal(actual,expected,1e-8)); +} + +/* ************************************************************************* */ +TEST(ExtendedPose3, Retract_LocalCoordinates) +{ + Vector9 d; + d << 1,2,3,4,5,6,7,8,9; d/=10; + const Rot3 R = Rot3::Retract(d.head<3>()); + ExtendedPose3 t = ExtendedPose3::Retract(d); + EXPECT(assert_equal(d, ExtendedPose3::LocalCoordinates(t))); +} +/* ************************************************************************* */ +TEST(ExtendedPose3, retract_localCoordinates) +{ + Vector9 d12; + d12 << 1,2,3,4,5,6,7,8,9; d12/=10; + ExtendedPose3 t1 = T, t2 = t1.retract(d12); + EXPECT(assert_equal(d12, t1.localCoordinates(t2))); +} +/* ************************************************************************* */ +TEST(ExtendedPose3, expmap_logmap) +{ + Vector d12 = Vector9::Constant(0.1); + ExtendedPose3 t1 = T, t2 = t1.expmap(d12); + EXPECT(assert_equal(d12, t1.logmap(t2))); +} + +/* ************************************************************************* */ +TEST(ExtendedPose3, retract_localCoordinates2) +{ + ExtendedPose3 t1 = T; + ExtendedPose3 t2 = T3; + ExtendedPose3 origin; + Vector d12 = t1.localCoordinates(t2); + EXPECT(assert_equal(t2, t1.retract(d12))); + Vector d21 = t2.localCoordinates(t1); + EXPECT(assert_equal(t1, t2.retract(d21))); + EXPECT(assert_equal(d12, -d21)); +} +/* ************************************************************************* */ +TEST(ExtendedPose3, manifold_expmap) +{ + ExtendedPose3 t1 = T; + ExtendedPose3 t2 = T3; + ExtendedPose3 origin; + Vector d12 = t1.logmap(t2); + EXPECT(assert_equal(t2, t1.expmap(d12))); + Vector d21 = t2.logmap(t1); + EXPECT(assert_equal(t1, t2.expmap(d21))); + + // Check that log(t1,t2)=-log(t2,t1) + EXPECT(assert_equal(d12,-d21)); +} + +/* ************************************************************************* */ +TEST(ExtendedPose3, subgroups) +{ + // Frank - Below only works for correct "Agrawal06iros style expmap + // lines in canonical coordinates correspond to Abelian subgroups in SE(3) + Vector d = (Vector(9) << 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9).finished(); + // exp(-d)=inverse(exp(d)) + EXPECT(assert_equal(ExtendedPose3::Expmap(-d),ExtendedPose3::Expmap(d).inverse())); + // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) + ExtendedPose3 T2 = ExtendedPose3::Expmap(2*d); + ExtendedPose3 T3 = ExtendedPose3::Expmap(3*d); + ExtendedPose3 T5 = ExtendedPose3::Expmap(5*d); + EXPECT(assert_equal(T5,T2*T3)); + EXPECT(assert_equal(T5,T3*T2)); +} + +/* ************************************************************************* */ +TEST( ExtendedPose3, between ) +{ + ExtendedPose3 expected = T2.inverse() * T3; + Matrix actualDBetween1,actualDBetween2; + ExtendedPose3 actual = T2.between(T3, actualDBetween1,actualDBetween2); + EXPECT(assert_equal(expected,actual)); + + Matrix numericalH1 = numericalDerivative21(testing::between , T2, T3); + EXPECT(assert_equal(numericalH1,actualDBetween1,5e-3)); + + Matrix numericalH2 = numericalDerivative22(testing::between , T2, T3); + EXPECT(assert_equal(numericalH2,actualDBetween2,1e-5)); +} + + +/* ************************************************************************* */ +TEST( ExtendedPose3, adjointMap) { + Matrix res = ExtendedPose3::adjointMap( screwExtendedPose3::xi); + Matrix wh = skewSymmetric( screwExtendedPose3::xi(0), screwExtendedPose3::xi(1), screwExtendedPose3::xi(2)); + Matrix vh = skewSymmetric( screwExtendedPose3::xi(3), screwExtendedPose3::xi(4), screwExtendedPose3::xi(5)); + Matrix rh = skewSymmetric( screwExtendedPose3::xi(6), screwExtendedPose3::xi(7), screwExtendedPose3::xi(8)); + Matrix9 expected; + expected << wh, Z_3x3, Z_3x3, vh, wh, Z_3x3, rh, Z_3x3, wh; + EXPECT(assert_equal(expected,res,1e-5)); +} + +/* ************************************************************************* */ + +TEST( ExtendedPose3, ExpmapDerivative1) { + Matrix9 actualH; + Vector9 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0; + ExtendedPose3::Expmap(w,actualH); + Matrix expectedH = numericalDerivative21 >(&ExtendedPose3::Expmap, w, nullptr); + EXPECT(assert_equal(expectedH, actualH)); +} + +/* ************************************************************************* */ +TEST( ExtendedPose3, LogmapDerivative) { + Matrix9 actualH; + Vector9 w; w << 0.1, 0.2, 0.3, 4.0, 5.0, 6.0,7.0,8.0,9.0; + ExtendedPose3 p = ExtendedPose3::Expmap(w); + EXPECT(assert_equal(w, ExtendedPose3::Logmap(p,actualH), 1e-5)); + Matrix expectedH = numericalDerivative21 >(&ExtendedPose3::Logmap, p, nullptr); + EXPECT(assert_equal(expectedH, actualH)); +} + +/* ************************************************************************* */ +TEST( ExtendedPose3, stream) +{ + ExtendedPose3 T; + std::ostringstream os; + os << T; + EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\nv:[0, 0, 0];\np:[0, 0, 0];\n"); +} + +//****************************************************************************** +TEST(ExtendedPose3 , Invariants) { + ExtendedPose3 id; + + EXPECT(check_group_invariants(id,id)); + EXPECT(check_group_invariants(id,T3)); + EXPECT(check_group_invariants(T2,id)); + EXPECT(check_group_invariants(T2,T3)); + + EXPECT(check_manifold_invariants(id,id)); + EXPECT(check_manifold_invariants(id,T3)); + EXPECT(check_manifold_invariants(T2,id)); + EXPECT(check_manifold_invariants(T2,T3)); +} + +//****************************************************************************** +TEST(ExtendedPose3 , LieGroupDerivatives) { + ExtendedPose3 id; + + CHECK_LIE_GROUP_DERIVATIVES(id,id); + CHECK_LIE_GROUP_DERIVATIVES(id,T2); + CHECK_LIE_GROUP_DERIVATIVES(T2,id); + CHECK_LIE_GROUP_DERIVATIVES(T2,T3); +} + +//****************************************************************************** +TEST(ExtendedPose3 , ChartDerivatives) { + ExtendedPose3 id; + if (ROT3_DEFAULT_COORDINATES_MODE == Rot3::EXPMAP) { + CHECK_CHART_DERIVATIVES(id,id); +// CHECK_CHART_DERIVATIVES(id,T2); +// CHECK_CHART_DERIVATIVES(T2,id); +// CHECK_CHART_DERIVATIVES(T2,T3); + } +} + + +/* ************************************************************************* */ +TEST(ExtendedPose3, interpolate) { + EXPECT(assert_equal(T2, interpolate(T2,T3, 0.0))); + EXPECT(assert_equal(T3, interpolate(T2,T3, 1.0))); +} + +/* ************************************************************************* */ + +TEST(ExtendedPose3, Create) { + Matrix93 actualH1, actualH2, actualH3; + ExtendedPose3 actual = ExtendedPose3::Create(R, V2, P2, actualH1, actualH2, actualH3); + EXPECT(assert_equal(T, actual)); + std::function create = std::bind( + ExtendedPose3::Create, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, nullptr, nullptr, nullptr); + EXPECT(assert_equal(numericalDerivative31(create, R, V2, P2), actualH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative32(create, R, V2, P2), actualH2, 1e-9)); + EXPECT(assert_equal(numericalDerivative33(create, R, V2, P2), actualH3, 1e-9)); +} + +/* ************************************************************************* */ +TEST(ExtendedPose3, print) { + std::stringstream redirectStream; + std::streambuf* ssbuf = redirectStream.rdbuf(); + std::streambuf* oldbuf = std::cout.rdbuf(); + // redirect cout to redirectStream + std::cout.rdbuf(ssbuf); + + ExtendedPose3 pose(Rot3(), Vector3(1, 2, 3), Point3(1, 2, 3)); + // output is captured to redirectStream + pose.print(); + + // Generate the expected output + std::stringstream expected; + Vector3 velocity(1, 2, 3); + Point3 position(1, 2, 3); + +#ifdef GTSAM_TYPEDEF_POINTS_TO_VECTORS + expected << "1\n" + "2\n" + "3;\n"; +#else + expected << "v:[" << velocity.x() << ", " << velocity.y() << ", " << velocity.z() << "]';\np:[" << position.x() << ", " << position.y() << ", " << position.z() << "]';\n"; +#endif + + // reset cout to the original stream + std::cout.rdbuf(oldbuf); + + // Get substring corresponding to position part + std::string actual = redirectStream.str().substr(38); + CHECK(expected.str() == actual); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 87444e624f..0bde371f8d 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -24,14 +24,19 @@ /* GTSAM includes */ #include +#include namespace gtsam { #ifdef GTSAM_TANGENT_PREINTEGRATION typedef TangentPreintegration PreintegrationType; #else +#ifdef GTSAM_LIEGROUP_PREINTEGRATION +typedef LieGroupPreintegration PreintegrationType; +#else typedef ManifoldPreintegration PreintegrationType; #endif +#endif /* * If you are using the factor, please cite: @@ -53,6 +58,8 @@ typedef ManifoldPreintegration PreintegrationType; * Robotics: Science and Systems (RSS), 2015. */ + + /** * PreintegratedCombinedMeasurements integrates the IMU measurements * (rotation rates and accelerations) and the corresponding covariance matrix. diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index 2e6dff0dc8..b0982cfe1f 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -24,6 +24,7 @@ /* GTSAM includes */ #include #include +#include #include #include @@ -32,8 +33,12 @@ namespace gtsam { #ifdef GTSAM_TANGENT_PREINTEGRATION typedef TangentPreintegration PreintegrationType; #else +#ifdef GTSAM_LIEGROUP_PREINTEGRATION +typedef LieGroupPreintegration PreintegrationType; +#else typedef ManifoldPreintegration PreintegrationType; #endif +#endif /* * If you are using the factor, please cite: diff --git a/gtsam/navigation/LieGroupPreintegration.cpp b/gtsam/navigation/LieGroupPreintegration.cpp new file mode 100644 index 0000000000..6e6c4b1cd1 --- /dev/null +++ b/gtsam/navigation/LieGroupPreintegration.cpp @@ -0,0 +1,200 @@ +/* ---------------------------------------------------------------------------- + + * 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 LieGroupPreintegration.cpp + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + * @author Martin Brossard + **/ + +#include "LieGroupPreintegration.h" + +using namespace std; + +namespace gtsam { + +//------------------------------------------------------------------------------ +LieGroupPreintegration::LieGroupPreintegration( + const std::shared_ptr& p, const Bias& biasHat) : + PreintegrationBase(p, biasHat) { + resetIntegration(); +} + +//------------------------------------------------------------------------------ +void LieGroupPreintegration::resetIntegration() { + deltaTij_ = 0.0; + deltaXij_ = NavState(); + delRdelBiasOmega_.setZero(); + delPdelBiasAcc_.setZero(); + delPdelBiasOmega_.setZero(); + delVdelBiasAcc_.setZero(); + delVdelBiasOmega_.setZero(); +} + +//------------------------------------------------------------------------------ +bool LieGroupPreintegration::equals(const LieGroupPreintegration& other, + double tol) const { + return p_->equals(*other.p_, tol) && std::abs(deltaTij_ - other.deltaTij_) < tol + && biasHat_.equals(other.biasHat_, tol) + && deltaXij_.equals(other.deltaXij_, tol) + && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol) + && equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol) + && equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol) + && equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol) + && equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol); +} + +//------------------------------------------------------------------------------ +void LieGroupPreintegration::update(const Vector3& measuredAcc, + const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B, + Matrix93* C) { + + // Correct for bias in the sensor frame + Vector3 acc = biasHat_.correctAccelerometer(measuredAcc); + Vector3 omega = biasHat_.correctGyroscope(measuredOmega); + + // Possibly correct for sensor pose + Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega; + if (p().body_P_sensor) + std::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega, + D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega); + + // Save current rotation for updating Jacobians + const Rot3 oldRij = deltaXij_.attitude(); + + // Do update + deltaTij_ += dt; + updateFactor(acc, omega, dt, A, B, C); // functional + + if (p().body_P_sensor) { + //NOT checked + // More complicated derivatives in case of non-trivial sensor pose + *C *= D_correctedOmega_omega; + if (!p().body_P_sensor->translation().isZero()) + *C += *B * D_correctedAcc_omega; + *B *= D_correctedAcc_acc; // NOTE(frank): needs to be last + } + + // Update Jacobians + // TODO(frank): Try same simplification as in new approach + Matrix3 D_acc_R; + oldRij.rotate(acc, D_acc_R); + const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_; + + const Vector3 integratedOmega = omega * dt; + Matrix3 D_incrR_integratedOmega; + const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! + const Matrix3 incrRt = incrR.transpose(); + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt; + + double dt22 = 0.5 * dt * dt; + const Matrix3 dRij = oldRij.matrix(); // expensive + delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij; + delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega; + delVdelBiasAcc_ += -dRij * dt; + delVdelBiasOmega_ += D_acc_biasOmega * dt; +} + +//------------------------------------------------------------------------------ +void LieGroupPreintegration::updateFactor(const Vector3& b_acceleration, const Vector3& b_omega, const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, + OptionalJacobian<9, 3> G2) { + + Vector9 xi; + Matrix39 D_xiP_state; + Vector3 b_v = deltaXij_.bodyVelocity(F ? &D_xiP_state : 0); + double dt22 = 0.5 * dt * dt; + + // Integrate on tangent space. TODO(frank): coriolis? + deltaXij_.dR(xi) << dt * b_omega; + deltaXij_.dP(xi) << dt * b_v + dt22 * b_acceleration; + deltaXij_.dV(xi) << dt * b_acceleration; + + // Bring back to manifold + Matrix9 D_newState_xi; + deltaXij_ = deltaXij_.retract(xi, F, G1 || G2 ? &D_newState_xi : 0); + + // Derivative wrt state is computed by retract directly + // However, as dP(xi) also depends on state, we need to add that contribution + if (F) { + F->middleRows<3>(3) += dt * F->block<3,3>(3,3) * D_xiP_state; + } + // derivative wrt acceleration + if (G1) { + // D_newState_dPxi = D_newState_xi.middleCols<3>(3) + // D_dPxi_acc = dt22 * I_3x3 + // D_newState_dVxi = D_newState_xi.rightCols<3>() + // D_dVxi_acc = dt * I_3x3 + // *G2 = D_newState_acc = D_newState_dPxi * D_dPxi_acc + D_newState_dVxi * D_dVxi_acc + *G1 = D_newState_xi.middleCols<3>(3) * dt22 + + D_newState_xi.rightCols<3>() * dt; + } + // derivative wrt omega + if (G2) { + // D_newState_dRxi = D_newState_xi.leftCols<3>() + // D_dRxi_omega = dt * I_3x3 + // *G1 = D_newState_omega = D_newState_dRxi * D_dRxi_omega + *G2 = D_newState_xi.leftCols<3>() * dt; + } + +} + +//------------------------------------------------------------------------------ +Vector9 LieGroupPreintegration::biasCorrectedDelta( + const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const { + // Correct deltaRij, derivative is delRdelBiasOmega_ + const imuBias::ConstantBias biasIncr = bias_i - biasHat_; + Matrix3 D_correctedRij_bias, D_dR_correctedRij; + const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope(); + const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, {}, + H ? &D_correctedRij_bias : 0); + if (H) + D_correctedRij_bias *= delRdelBiasOmega_; + + Rot3 R = Rot3::Expmap(biasInducedOmega); + ExtendedPose3 T0(deltaRij(),deltaPij(),deltaVij()); + Vector9 xi_corr; + Matrix9 H2, H3; + xi_corr << delRdelBiasOmega_ * biasIncr.gyroscope(), + delPdelBiasAcc_ * biasIncr.accelerometer() + delPdelBiasOmega_ * biasIncr.gyroscope(), + delVdelBiasAcc_ * biasIncr.accelerometer() + + delVdelBiasOmega_ * biasIncr.gyroscope(); + + ExtendedPose3 Tplus = ExtendedPose3::Expmap(xi_corr, H2); + Vector9 xi; + ExtendedPose3 T(correctedRij, + T0.position() + Tplus.position(), + T0.velocity() + Tplus.velocity()); + xi = T.boxminus(T); + Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0); + + if (H) { + Matrix36 D_dR_bias, D_dP_bias, D_dV_bias; + Matrix3 Qv = H2.block<3,3>(6,0); + Matrix3 Qt = H2.block<3,3>(3,0); + Matrix3 Jw = H2.block<3,3>(0,0); + + D_dR_bias << Z_3x3, D_dR_correctedRij*D_correctedRij_bias; + D_dP_bias << Jw*delPdelBiasAcc_, Jw*delPdelBiasOmega_ + Qt*delRdelBiasOmega_; + D_dV_bias << Jw*delVdelBiasAcc_, Jw*delVdelBiasOmega_ + Qv*delRdelBiasOmega_; + (*H) << D_dR_bias, R.matrix()*D_dP_bias, R.matrix()*D_dV_bias; + (*H) = (*H); + } + return xi; +} + +//------------------------------------------------------------------------------ + +}// namespace gtsam diff --git a/gtsam/navigation/LieGroupPreintegration.h b/gtsam/navigation/LieGroupPreintegration.h new file mode 100644 index 0000000000..ddf40e6aa7 --- /dev/null +++ b/gtsam/navigation/LieGroupPreintegration.h @@ -0,0 +1,137 @@ +/* ---------------------------------------------------------------------------- + + * 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 LieGroupPreintegration.h + * @author Luca Carlone + * @author Stephen Williams + * @author Richard Roberts + * @author Vadim Indelman + * @author David Jensen + * @author Frank Dellaert + * @author Martin Brossard + **/ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * IMU pre-integration on SE_2(3) Lie group. + */ +class GTSAM_EXPORT LieGroupPreintegration : public PreintegrationBase { + protected: + + /** + * Pre-integrated navigation state, from frame i to frame j + * Note: relative position does not take into account velocity at time i, see deltap+, in [2] + * Note: velocity is now also in frame i, as opposed to deltaVij in [2] + */ + NavState deltaXij_; + Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias + Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias + Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias + Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias + Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias + + /// Default constructor for serialization + LieGroupPreintegration() { + resetIntegration(); + } + +public: + /// @name Constructors + /// @{ + + /** + * Constructor, initializes the variables in the base class + * @param p Parameters, typically fixed in a single application + * @param bias Current estimate of acceleration and rotation rate biases + */ + LieGroupPreintegration(const std::shared_ptr& p, + const imuBias::ConstantBias& biasHat = imuBias::ConstantBias()); + + /// @} + + /// @name Basic utilities + /// @{ + /// Re-initialize PreintegratedMeasurements + void resetIntegration() override; + + /// @} + + /// @name Instance variables access + /// @{ + NavState deltaXij() const override { return deltaXij_; } + Rot3 deltaRij() const override { return deltaXij_.attitude(); } + Vector3 deltaPij() const override { return deltaXij_.position(); } + Vector3 deltaVij() const override { return deltaXij_.velocity(); } + + Matrix3 delRdelBiasOmega() const { return delRdelBiasOmega_; } + Matrix3 delPdelBiasAcc() const { return delPdelBiasAcc_; } + Matrix3 delPdelBiasOmega() const { return delPdelBiasOmega_; } + Matrix3 delVdelBiasAcc() const { return delVdelBiasAcc_; } + Matrix3 delVdelBiasOmega() const { return delVdelBiasOmega_; } + + /// @name Testable + /// @{ + bool equals(const LieGroupPreintegration& other, double tol) const; + /// @} + + /// @name Main functionality + /// @{ + + /// Update preintegrated measurements and get derivatives + /// It takes measured quantities in the j frame + /// Modifies preintegrated quantities in place after correcting for bias and possibly sensor pose + void update(const Vector3& measuredAcc, const Vector3& measuredOmega, const double dt, + Matrix9* A, Matrix93* B, Matrix93* C) override; + + // Integrate forward in time given angular velocity and acceleration in body frame + /// Uses second order integration for position, returns derivatives except dt. + void updateFactor(const Vector3& b_acceleration, const Vector3& b_omega, + const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1, + OptionalJacobian<9, 3> G2); + + /// Given the estimate of the bias, return a NavState tangent vector + /// summarizing the preintegrated IMU measurements so far + Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i, + OptionalJacobian<9, 6> H = {}) const override; + + /** Dummy clone for MATLAB */ + virtual std::shared_ptr clone() const { + return std::shared_ptr(); + } + + /// @} + +private: + /** Serialization function */ +#if GTSAM_ENABLE_BOOST_SERIALIZATION + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + namespace bs = ::boost::serialization; + ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase); + ar & BOOST_SERIALIZATION_NVP(deltaXij_); + ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_); + ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_); + } +#endif +}; + +} /// namespace gtsam diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index bd482132e4..606eec3089 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -371,43 +371,39 @@ NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega, //------------------------------------------------------------------------------ Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder, OptionalJacobian<9, 9> H) const { - Rot3 nRb = R_; - Point3 n_t = t_, n_v = v_; + const Rot3& nRb = R_; + const Velocity3& n_v = v_; + const Point3& n_t = t_; const double dt2 = dt * dt; const Vector3 omega_cross_vel = omega.cross(n_v); - // Get perturbations in nav frame - Vector9 n_xi, xi; - Matrix3 D_dR_R, D_dP_R, D_dV_R, D_body_nav; - dR(n_xi) << ((-dt) * omega); - dP(n_xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper - dV(n_xi) << ((-2.0 * dt) * omega_cross_vel); + Vector9 xi; + Matrix3 D_dP_R, D_dP_dR, D_dV_dR; + dR(xi) << nRb.unrotate((-dt) * omega, H ? &D_dP_R : 0); + dP(xi) << nRb.unrotate((-dt2) * omega_cross_vel, H ? &D_dP_dR : 0); + dV(xi) << nRb.unrotate((-2.0 * dt) * omega_cross_vel, H ? &D_dV_dR : 0); + if (secondOrder) { const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t)); - dP(n_xi) -= (0.5 * dt2) * omega_cross2_t; - dV(n_xi) -= dt * omega_cross2_t; + dP(xi) -= (0.5 * dt2) * omega_cross2_t; + dV(xi) -= dt * omega_cross2_t; } - - // Transform n_xi into the body frame - xi << nRb.unrotate(dR(n_xi), H ? &D_dR_R : 0, H ? &D_body_nav : 0), - nRb.unrotate(dP(n_xi), H ? &D_dP_R : 0), - nRb.unrotate(dV(n_xi), H ? &D_dV_R : 0); - if (H) { H->setZero(); const Matrix3 Omega = skewSymmetric(omega); const Matrix3 D_cross_state = Omega * R(); H->setZero(); - D_R_R(H) << D_dR_R; - D_t_v(H) << D_body_nav * (-dt2) * D_cross_state; - D_t_R(H) << D_dP_R; - D_v_v(H) << D_body_nav * (-2.0 * dt) * D_cross_state; - D_v_R(H) << D_dV_R; + D_R_R(H) << D_dP_R; + D_t_v(H) << nRb.transpose() * (-dt2) * D_cross_state; + D_v_v(H) << nRb.transpose() * (-2.0 * dt) * D_cross_state; + D_t_R(H) << D_dP_dR; + D_v_R(H) << D_dV_dR; + if (secondOrder) { const Matrix3 D_cross2_state = Omega * D_cross_state; - D_t_t(H) -= D_body_nav * (0.5 * dt2) * D_cross2_state; - D_v_t(H) -= D_body_nav * dt * D_cross2_state; + D_t_t(H) -= (0.5 * dt2) * D_cross2_state; + D_v_t(H) -= dt * D_cross2_state; } } return xi; @@ -420,29 +416,58 @@ Vector9 NavState::correctPIM(const Vector9& pim, double dt, OptionalJacobian<9, 9> H2) const { const Rot3& nRb = R_; const Velocity3& n_v = v_; // derivative is Ri ! + const Point3& n_t = t_; // 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; - 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); + Matrix3 D_dP_Ri, D_dV_Ri, D_dP_Gx, D_dV_Gv; + Matrix3 D_Gx_nt = Matrix::Zero(3, 3); + Matrix3 D_Gv_nt = Matrix::Zero(3, 3); + Matrix3 D_Gx_nv = dt * Matrix::Identity(3, 3); + Vector3 Gamma_v, Gamma_p; if (omegaCoriolis) { - xi += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1); + Vector6 omega_n_gravity; + omega_n_gravity << -(*omegaCoriolis)*dt, n_gravity*dt; + Pose3 pose = Pose3::Expmap(omega_n_gravity); + Rot3 Gamma_R = pose.rotation(); + Matrix3 GOmGt = skewSymmetric(Gamma_R.rotate(*omegaCoriolis)); + Vector3 n_v_prime = n_v + GOmGt * n_t; + + // add initial state contribution + Gamma_v = pose.translation() + GOmGt * n_t; + // add velocity and initial position contribution + Matrix3 Omega = skewSymmetric(*omegaCoriolis); + double phi = (*omegaCoriolis).norm(); + double c = phi * dt; + double phi3 = phi * phi * phi; + double phi4 = phi3 * phi; + double a = (c * cos(c) - sin(c)) /(phi3); + double b = (c*c/2 - cos(c) - c*sin(c) + 1) / (phi4); + Matrix3 mat = dt22 * Matrix::Identity(3, 3); + mat += a * Omega + b * Omega * Omega; + Gamma_p = dt * n_v_prime + mat * n_gravity; + + D_Gv_nt = GOmGt; + D_Gx_nt = dt * GOmGt; + } else { + Gamma_v = dt * n_gravity; + Gamma_p = dt * n_v + dt22 * n_gravity; } + dR(xi) = dR(pim); + dP(xi) = dP(pim)+nRb.unrotate(Gamma_p, H1 ? &D_dP_Ri : 0, H1 ? &D_dP_Gx : 0); + dV(xi) = dV(pim)+nRb.unrotate(Gamma_v, H1 ? &D_dV_Ri : 0, H1 ? &D_dV_Gv : 0); if (H1 || H2) { Matrix3 Ri = nRb.matrix(); if (H1) { - 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; + H1->setZero(); // if coriolis H1 is already initialized + D_t_R(H1) += D_dP_Ri; + D_t_v(H1) += D_dP_Gx * D_Gx_nv * Ri; + D_v_R(H1) += D_dV_Ri; + D_t_t(H1) += D_dP_Gx * D_Gx_nt * Ri; + D_v_t(H1) += D_dV_Gv * D_Gv_nt * Ri; } if (H2) { H2->setIdentity(); diff --git a/gtsam/navigation/NavState.h b/gtsam/navigation/NavState.h index e246cbe271..0a6bca6a32 100644 --- a/gtsam/navigation/NavState.h +++ b/gtsam/navigation/NavState.h @@ -18,6 +18,7 @@ #pragma once +#include #include #include #include diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index dd86b829c5..e5f089fd8d 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -119,23 +119,44 @@ NavState PreintegrationBase::predict(const NavState& state_i, OptionalJacobian<9, 6> H2) const { Matrix96 D_biasCorrected_bias; Vector9 biasCorrected = biasCorrectedDelta(bias_i, - H2 ? &D_biasCorrected_bias : nullptr); + H2 ? &D_biasCorrected_bias : nullptr); + + // Correct for Earth rate + NavState state_ii = state_i; + if (p().omegaCoriolis){ + const Vector3 omega = *(p().omegaCoriolis); + const Rot3 Gamma_R = Rot3::Expmap(-omega*deltaTij_); + + state_ii = NavState(Gamma_R * state_i.attitude(), + Gamma_R * state_i.position(), + Gamma_R * state_i.velocity()); + // Jacobian is identity + } // Correct for initial velocity and gravity Matrix9 D_delta_state, D_delta_biasCorrected; - Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity, - p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : nullptr, - H2 ? &D_delta_biasCorrected : nullptr); + Vector9 xi = state_ii.correctPIM(biasCorrected, deltaTij_, p().n_gravity, + p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : nullptr, + H2 ? &D_delta_biasCorrected : 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); + + Matrix99 H = Matrix99::Identity(9, 9); + if (p().omegaCoriolis){ + // apply v_t = v'_t - Omega p_t + const Rot3 nRb = state_j.attitude(); + const Matrix3 Omega = skewSymmetric(*(p().omegaCoriolis)); + state_j = NavState(nRb, state_j.position(), + state_j.velocity() - Omega * state_j.position()); + } if (H1) - *H1 = D_predict_state + D_predict_delta * D_delta_state; + *H1 = H * (D_predict_state + D_predict_delta * D_delta_state); if (H2) - *H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias; + *H2 = H * (D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias); return state_j; } diff --git a/gtsam/navigation/tests/testLieGroupPreintegration.cpp b/gtsam/navigation/tests/testLieGroupPreintegration.cpp new file mode 100644 index 0000000000..db26985134 --- /dev/null +++ b/gtsam/navigation/tests/testLieGroupPreintegration.cpp @@ -0,0 +1,107 @@ +/* ---------------------------------------------------------------------------- + + * 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 testLieGroupPreintegration.cpp + * @brief Unit test for the LieGroupPreintegration + * @author Luca Carlone + */ + +#include +#include +#include +#include +#include +#include + +#include "imuFactorTesting.h" + +namespace testing { +// Create default parameters with Z-down and above noise parameters +static std::shared_ptr Params() { + auto p = PreintegrationParams::MakeSharedD(kGravity); + p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3; + p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3; + p->integrationCovariance = 0.0001 * I_3x3; + return p; +} +} // namespace testing + +/* ************************************************************************* */ +TEST(LieGroupPreintegration, BiasCorrectionJacobians) { + testing::SomeMeasurements measurements; + + std::function deltaRij = + [=](const Vector3& a, const Vector3& w) { + LieGroupPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaRij(); + }; + + std::function deltaPij = + [=](const Vector3& a, const Vector3& w) { + LieGroupPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaPij(); + }; + + std::function deltaVij = + [=](const Vector3& a, const Vector3& w) { + LieGroupPreintegration pim(testing::Params(), Bias(a, w)); + testing::integrateMeasurements(measurements, &pim); + return pim.deltaVij(); + }; + + // Actual pre-integrated values + LieGroupPreintegration pim(testing::Params()); + testing::integrateMeasurements(measurements, &pim); + + EXPECT(assert_equal(numericalDerivative21(deltaRij, kZero, kZero), + Matrix3(Z_3x3))); + EXPECT(assert_equal(numericalDerivative22(deltaRij, kZero, kZero), + pim.delRdelBiasOmega(), 1e-3)); + + EXPECT(assert_equal(numericalDerivative21(deltaPij, kZero, kZero), + pim.delPdelBiasAcc())); + EXPECT(assert_equal(numericalDerivative22(deltaPij, kZero, kZero), + pim.delPdelBiasOmega(), 1e-3)); + + EXPECT(assert_equal(numericalDerivative21(deltaVij, kZero, kZero), + pim.delVdelBiasAcc())); + EXPECT(assert_equal(numericalDerivative22(deltaVij, kZero, kZero), + pim.delVdelBiasOmega(), 1e-3)); +} + +/* ************************************************************************* */ +TEST(LieGroupPreintegration, computeError) { + using namespace std::placeholders; + LieGroupPreintegration pim(testing::Params()); + NavState x1, x2; + imuBias::ConstantBias bias; + Matrix9 aH1, aH2; + Matrix96 aH3; + pim.computeError(x1, x2, bias, aH1, aH2, aH3); + std::function + f = std::bind(&LieGroupPreintegration::computeError, pim, _1, _2, _3, + nullptr, nullptr, nullptr); + // NOTE(frank): tolerance of 1e-3 on H1 because approximate away from 0 + EXPECT(assert_equal(numericalDerivative31(f, x1, x2, bias), aH1, 1e-9)); + EXPECT(assert_equal(numericalDerivative32(f, x1, x2, bias), aH2, 1e-9)); + EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/navigation/tests/testNavState.cpp b/gtsam/navigation/tests/testNavState.cpp index 4275314154..81479354fd 100644 --- a/gtsam/navigation/tests/testNavState.cpp +++ b/gtsam/navigation/tests/testNavState.cpp @@ -57,7 +57,7 @@ TEST(NavState, Constructor) { std::function create = std::bind(&NavState::Create, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, nullptr, nullptr, nullptr); - Matrix aH1, aH2, aH3; + Matrix93 aH1, aH2, aH3; EXPECT( assert_equal(kState1, NavState::Create(kAttitude, kPosition, kVelocity, aH1, aH2, aH3))); @@ -68,8 +68,8 @@ assert_equal( assert_equal( numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); EXPECT( -assert_equal( - numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2)); + assert_equal( + numericalDerivative33(create, kAttitude, kPosition, kVelocity), aH3)); } /* ************************************************************************* */