diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index e2713e9e7..7bcd32b9f 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -660,19 +660,6 @@ Matrix expm(const Matrix& A, size_t K) { return E; } -/* ************************************************************************* */ -Matrix Cayley(const Matrix& A) { - Matrix::Index n = A.cols(); - assert(A.rows() == n); - - // original -// const Matrix I = eye(n); -// return (I-A)*inverse(I+A); - - // inlined to let Eigen do more optimization - return (Matrix::Identity(n, n) - A)*(Matrix::Identity(n, n) + A).inverse(); -} - /* ************************************************************************* */ std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal) { diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 0c88886b4..20a4a6bc4 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -528,17 +528,6 @@ DLT(const Matrix& A, double rank_tol = 1e-9); */ GTSAM_EXPORT Matrix expm(const Matrix& A, size_t K=7); -/// Cayley transform -GTSAM_EXPORT Matrix Cayley(const Matrix& A); - -/// Implementation of Cayley transform using fixed size matrices to let -/// Eigen do more optimization -template -Eigen::Matrix CayleyFixed(const Eigen::Matrix& A) { - typedef Eigen::Matrix FMat; - return (FMat::Identity() - A)*(FMat::Identity() + A).inverse(); -} - std::string formatMatrixIndented(const std::string& label, const Matrix& matrix, bool makeVectorHorizontal = false); } // namespace gtsam diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index b7a9fcde4..742aeeeb5 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -16,7 +16,7 @@ **/ #include -#include +#include #define QUATERNION_TYPE Eigen::Quaternion<_Scalar,_Options> @@ -96,8 +96,7 @@ struct traits_x { _Scalar angle = omega.norm(); return Q(Eigen::AngleAxis<_Scalar>(angle, omega / angle)); } - if (H) - throw std::runtime_error("TODO: implement Jacobian"); + if (H) CONCEPT_NOT_IMPLEMENTED; } /// We use our own Logmap, as there is a slight bug in Eigen @@ -129,8 +128,7 @@ struct traits_x { return (angle / s) * q.vec(); } - if (H) - throw std::runtime_error("TODO: implement Jacobian"); + if (H) CONCEPT_NOT_IMPLEMENTED; } /// @} diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index a9209a8ca..ab44716c8 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -58,20 +58,6 @@ Rot3 Rot3::rodriguez(const Vector3& w) { return rodriguez(w/t, t); } -/* ************************************************************************* */ -Rot3 Rot3::retract(const Vector& omega, OptionalJacobian<3, 3> Hthis, - OptionalJacobian<3, 3> Hv, Rot3::CoordinatesMode mode) const { - if (Hthis || Hv) CONCEPT_NOT_IMPLEMENTED; - return retract(omega, mode); -} - -/* ************************************************************************* */ -Vector3 Rot3::localCoordinates(const Rot3& R2, OptionalJacobian<3, 3> Horigin, - OptionalJacobian<3, 3> H2, Rot3::CoordinatesMode mode) const { - if (Horigin || H2) CONCEPT_NOT_IMPLEMENTED; - return localCoordinates(R2, mode); -} - /* ************************************************************************* */ bool Rot3::equals(const Rot3 & R, double tol) const { return equal_with_abs_tol(matrix(), R.matrix(), tol); @@ -248,8 +234,8 @@ ostream &operator<<(ostream &os, const Rot3& R) { Rot3 Rot3::slerp(double t, const Rot3& other) const { // amazingly simple in GTSAM :-) assert(t>=0 && t<=1); - Vector3 omega = localCoordinates(other, Rot3::EXPMAP); - return retract(t * omega, Rot3::EXPMAP); + Vector3 omega = Logmap(between(other)); + return compose(Expmap(t * omega)); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index fd488f7b9..544990ce8 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -52,7 +52,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ - class GTSAM_EXPORT Rot3 { + class GTSAM_EXPORT Rot3 : public LieGroup { private: @@ -64,8 +64,6 @@ namespace gtsam { #endif public: - /// The intrinsic dimension of this manifold. - enum { dimension = 3 }; /// @name Constructors and named constructors /// @{ @@ -210,16 +208,13 @@ namespace gtsam { return Rot3(); } - /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity - Rot3 inverse(OptionalJacobian<3,3> H1=boost::none) const; - - /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const; - /** compose two rotations */ Rot3 operator*(const Rot3& R2) const; + Rot3 inverse() const { + return Rot3(Matrix3(transpose())); + } + /** * Conjugation: given a rotation acting in frame B, compute rotation c1Rc2 acting in a frame C * @param cRb rotation from B frame to C frame @@ -230,23 +225,10 @@ namespace gtsam { return cRb * (*this) * cRb.inverse(); } - /** - * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' - */ - Rot3 between(const Rot3& R2, - OptionalJacobian<3,3> H1=boost::none, - OptionalJacobian<3,3> H2=boost::none) const; - /// @} /// @name Manifold /// @{ - /// dimension of the variable - used to autodetect sizes - static size_t Dim() { return 3; } - - /// return dimensionality of tangent space, DOF = 3 - size_t dim() const { return 3; } - /** * The method retract() is used to map from the tangent space back to the manifold. * Its inverse, is localCoordinates(). For Lie groups, an obvious retraction is the @@ -260,21 +242,29 @@ namespace gtsam { EXPMAP, ///< Use the Lie group exponential map to retract #ifndef GTSAM_USE_QUATERNIONS CAYLEY, ///< Retract and localCoordinates using the Cayley transform. - SLOW_CAYLEY ///< Slow matrix implementation of Cayley transform (for tests only). #endif }; #ifndef GTSAM_USE_QUATERNIONS + + // Cayley chart around origin, no derivatives + struct CayleyChart { + static Rot3 Retract(const Vector3& v); + static Vector3 Local(const Rot3& r); + }; + /// Retraction from R^3 to Rot3 manifold using the Cayley transform - Rot3 retractCayley(const Vector& omega) const; + Rot3 retractCayley(const Vector& omega) const { + return compose(CayleyChart::Retract(omega)); + } + + /// Inverse of retractCayley + Vector3 localCayley(const Rot3& other) const { + return CayleyChart::Local(between(other)); + } + #endif - /// Retraction from R^3 \f$ [R_x,R_y,R_z] \f$ to Rot3 manifold neighborhood around current rotation - Rot3 retract(const Vector& omega, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const; - - /// Returns local retract coordinates \f$ [R_x,R_y,R_z] \f$ in neighborhood around current rotation - Vector3 localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const; - /// @} /// @name Lie Group /// @{ @@ -300,27 +290,16 @@ namespace gtsam { /// Derivative of Logmap static Matrix3 LogmapDerivative(const Vector3& x); - Rot3 retract(const Vector& omega, OptionalJacobian<3, 3> Hthis, - OptionalJacobian<3, 3> Hv = boost::none, Rot3::CoordinatesMode mode = - ROT3_DEFAULT_COORDINATES_MODE) const; - - Vector3 localCoordinates(const Rot3& R2, OptionalJacobian<3, 3> Horigin, - OptionalJacobian<3, 3> H2 = boost::none, Rot3::CoordinatesMode mode = - ROT3_DEFAULT_COORDINATES_MODE) const; - /** Calculate Adjoint map */ Matrix3 AdjointMap() const { return matrix(); } - /** - * Right Jacobian for Exponential map in SO(3) - equation (10.86) and following equations in - * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - */ - static Matrix3 rightJacobianExpMapSO3(const Vector3& x); + // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE + struct ChartAtOrigin { + static Rot3 Retract(const Vector3& v, ChartJacobian H = boost::none); + static Vector3 Local(const Rot3& r, ChartJacobian H = boost::none); + }; - /** Right Jacobian for Log map in SO(3) - equation (10.86) and following equations in - * G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. - */ - static Matrix3 rightJacobianExpMapSO3inverse(const Vector3& x); + using LieGroup::inverse; // version with derivative /// @} /// @name Group Action on Point3 diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index f31e80ce8..5c1ac377c 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -139,13 +139,6 @@ Rot3 Rot3::rodriguez(const Vector3& w, double theta) { -swy + C02, swx + C12, c + C22); } -/* ************************************************************************* */ -Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I_3x3; - return *this * R2; -} - /* ************************************************************************* */ Rot3 Rot3::operator*(const Rot3& R2) const { return Rot3(Matrix3(rot_*R2.rot_)); @@ -157,21 +150,6 @@ Matrix3 Rot3::transpose() const { return rot_.transpose(); } -/* ************************************************************************* */ -Rot3 Rot3::inverse(OptionalJacobian<3,3> H1) const { - if (H1) *H1 = -rot_; - return Rot3(Matrix3(transpose())); -} - -/* ************************************************************************* */ -Rot3 Rot3::between (const Rot3& R2, - OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { - if (H1) *H1 = -(R2.transpose()*rot_); - if (H2) *H2 = I_3x3; - Matrix3 R12 = transpose()*R2.rot_; - return Rot3(R12); -} - /* ************************************************************************* */ Point3 Rot3::rotate(const Point3& p, OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const { @@ -228,26 +206,41 @@ Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3,3> H) { } /* ************************************************************************* */ -Rot3 Rot3::retractCayley(const Vector& omega) const { +Rot3 Rot3::CayleyChart::Retract(const Vector3& omega) { const double x = omega(0), y = omega(1), z = omega(2); const double x2 = x * x, y2 = y * y, z2 = z * z; const double xy = x * y, xz = x * z, yz = y * z; const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f; - return (*this) - * Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, + return Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f, (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f); } /* ************************************************************************* */ -Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { - if(mode == Rot3::EXPMAP) { - return (*this)*Expmap(omega); - } else if(mode == Rot3::CAYLEY) { - return retractCayley(omega); - } else if(mode == Rot3::SLOW_CAYLEY) { - Matrix3 Omega = skewSymmetric(omega); - return (*this)*CayleyFixed<3>(-Omega/2); +Vector3 Rot3::CayleyChart::Local(const Rot3& R) { + // Create a fixed-size matrix + Matrix3 A = R.matrix(); + // Mathematica closed form optimization (procrastination?) gone wild: + const double a = A(0, 0), b = A(0, 1), c = A(0, 2); + const double d = A(1, 0), e = A(1, 1), f = A(1, 2); + const double g = A(2, 0), h = A(2, 1), i = A(2, 2); + const double di = d * i, ce = c * e, cd = c * d, fg = f * g; + const double M = 1 + e - f * h + i + e * i; + const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg)); + const double x = a * f - cd + f; + const double y = b * f - ce - c; + const double z = fg - di - d; + return K * Vector3(x, y, z); +} + +/* ************************************************************************* */ +Rot3 Rot3::ChartAtOrigin::Retract(const Vector3& omega, ChartJacobian H) { + static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE; + if (mode == Rot3::EXPMAP) return Expmap(omega, H); + + if (H) CONCEPT_NOT_IMPLEMENTED; + if(mode == Rot3::CAYLEY) { + return CayleyChart::Retract(omega); } else { assert(false); exit(1); @@ -255,29 +248,13 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { } /* ************************************************************************* */ -Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { - if(mode == Rot3::EXPMAP) { - return Logmap(between(T)); - } else if(mode == Rot3::CAYLEY) { - // Create a fixed-size matrix - Matrix3 A = rot_.transpose() * T.matrix(); - // Mathematica closed form optimization (procrastination?) gone wild: - const double a=A(0,0),b=A(0,1),c=A(0,2); - const double d=A(1,0),e=A(1,1),f=A(1,2); - const double g=A(2,0),h=A(2,1),i=A(2,2); - const double di = d*i, ce = c*e, cd = c*d, fg=f*g; - const double M = 1 + e - f*h + i + e*i; - const double K = - 4.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); - const double x = a * f - cd + f; - const double y = b * f - ce - c; - const double z = fg - di - d; - return K * Vector3(x, y, z); - } else if(mode == Rot3::SLOW_CAYLEY) { - // Create a fixed-size matrix - Matrix3 A(between(T).matrix()); - // using templated version of Cayley - Matrix3 Omega = CayleyFixed<3>(A); - return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); +Vector3 Rot3::ChartAtOrigin::Local(const Rot3& R, ChartJacobian H) { + static const CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE; + if (mode == Rot3::EXPMAP) return Logmap(R,H); + + if (H) CONCEPT_NOT_IMPLEMENTED; + if(mode == Rot3::CAYLEY) { + return CayleyChart::Local(R); } else { assert(false); exit(1); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 44e751f7d..b232aa92c 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -294,12 +294,10 @@ TEST(Rot3, manifold_expmap) Rot3 origin; // log behaves correctly - Vector d12 = gR1.localCoordinates(gR2, Rot3::EXPMAP); - CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::EXPMAP))); - Vector d21 = gR2.localCoordinates(gR1, Rot3::EXPMAP); - CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::EXPMAP))); + Vector d12 = Rot3::Logmap(gR1.between(gR2)); + Vector d21 = Rot3::Logmap(gR2.between(gR1)); - // Check that it is expmap + // Check expmap CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12))); CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21))); @@ -596,6 +594,12 @@ TEST(Rot3, quaternion) { } /* ************************************************************************* */ +Matrix Cayley(const Matrix& A) { + Matrix::Index n = A.cols(); + const Matrix I = eye(n); + return (I-A)*inverse(I+A); +} + TEST( Rot3, Cayley ) { Matrix A = skewSymmetric(1,2,-3); Matrix Q = Cayley(A); diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index a25db07f6..12fb94bbc 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -10,21 +10,14 @@ * -------------------------------------------------------------------------- */ /** - * @file testRot3.cpp - * @brief Unit tests for Rot3 class + * @file testRot3M.cpp + * @brief Unit tests for Rot3 class, matrix version * @author Alireza Fathi * @author Frank Dellaert */ -#include #include - #include -#include -#include - -#include - #include #ifndef GTSAM_USE_QUATERNIONS @@ -46,38 +39,10 @@ TEST(Rot3, manifold_cayley) Rot3 origin; // log behaves correctly - Vector d12 = gR1.localCoordinates(gR2, Rot3::CAYLEY); - CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::CAYLEY))); - Vector d21 = gR2.localCoordinates(gR1, Rot3::CAYLEY); - CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::CAYLEY))); - - // Check that log(t1,t2)=-log(t2,t1) - CHECK(assert_equal(d12,-d21)); - - // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = Vector3(0.1, 0.2, 0.3); - // exp(-d)=inverse(exp(d)) - CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); - // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) - Rot3 R2 = Rot3::Expmap (2 * d); - Rot3 R3 = Rot3::Expmap (3 * d); - Rot3 R5 = Rot3::Expmap (5 * d); - CHECK(assert_equal(R5,R2*R3)); - CHECK(assert_equal(R5,R3*R2)); -} - -/* ************************************************************************* */ -TEST(Rot3, manifold_slow_cayley) -{ - Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); - Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); - Rot3 origin; - - // log behaves correctly - Vector d12 = gR1.localCoordinates(gR2, Rot3::SLOW_CAYLEY); - CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::SLOW_CAYLEY))); - Vector d21 = gR2.localCoordinates(gR1, Rot3::SLOW_CAYLEY); - CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::SLOW_CAYLEY))); + Vector d12 = gR1.localCayley(gR2); + CHECK(assert_equal(gR2, gR1.retractCayley(d12))); + Vector d21 = gR2.localCayley(gR1); + CHECK(assert_equal(gR1, gR2.retractCayley(d21))); // Check that log(t1,t2)=-log(t2,t1) CHECK(assert_equal(d12,-d21)); diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 195490e87..ca8a63634 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -140,8 +140,7 @@ Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias, boost::optional H) const { const Vector3 biasOmegaIncr = bias - biasHat_; Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr; - const Rot3 deltaRij_biascorrected = deltaRij_.retract( - delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP); + const Rot3 deltaRij_biascorrected = deltaRij_ * Rot3::Expmap(delRdelBiasOmega_biasOmegaIncr); const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); if (H) { const Matrix3 Jrinv_theta_bc = // diff --git a/gtsam/navigation/CombinedImuFactor.cpp b/gtsam/navigation/CombinedImuFactor.cpp index 10f12ec66..ff1e1e278 100644 --- a/gtsam/navigation/CombinedImuFactor.cpp +++ b/gtsam/navigation/CombinedImuFactor.cpp @@ -280,7 +280,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_ // We compute factor's Jacobians, according to [3] /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); + const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_ * Rot3::Expmap(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); @@ -480,7 +480,7 @@ PoseVelocityBias CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity } - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); + const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_ * Rot3::Expmap(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr); // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected - diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index cdebffa63..85e7411ee 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -262,7 +262,7 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i, const // We compute factor's Jacobians /* ---------------------------------------------------------------------------------------------------- */ - const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_.retract(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); + const Rot3 deltaRij_biascorrected = preintegratedMeasurements_.deltaRij_ * Rot3::Expmap(preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr); // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); @@ -422,7 +422,7 @@ PoseVelocity ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i, vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity } - const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_.retract(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr, Rot3::EXPMAP); + const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij_ * Rot3::Expmap(preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr); // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected); Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected -