diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 2585c37be..d4c30119e 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -48,12 +48,15 @@ GTSAM_EXPORT Matrix3 compose(const Matrix3& M, const SO3& R, OptionalJacobian<9, } void ExpmapFunctor::init(bool nearZeroApprox) { + WW = W * W; nearZero = nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); if (!nearZero) { sin_theta = std::sin(theta); const double s2 = std::sin(theta / 2.0); one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] + A = sin_theta / theta; + B = one_minus_cos / theta2; } } @@ -62,39 +65,39 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) const double wx = omega.x(), wy = omega.y(), wz = omega.z(); W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; init(nearZeroApprox); - if (!nearZero) { - K = W / theta; - KK = K * K; - } } ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox) : theta2(angle * angle), theta(angle) { const double ax = axis.x(), ay = axis.y(), az = axis.z(); - K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; - W = K * angle; + W << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; + W *= angle; init(nearZeroApprox); - if (!nearZero) { - KK = K * K; - } } SO3 ExpmapFunctor::expmap() const { if (nearZero) - return SO3(I_3x3 + W); + return SO3(I_3x3 + W + 0.5 * WW); else - return SO3(I_3x3 + sin_theta * K + one_minus_cos * KK); + return SO3(I_3x3 + A * W + B * WW); +} + +Matrix3 DexpFunctor::leftJacobian() const { + if (nearZero) { + return I_3x3 + 0.5 * W + one_sixth * WW; + } else { + return I_3x3 + B * W + C * WW; + } } DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { if (nearZero) { - dexp_ = I_3x3 - 0.5 * W; + rightJacobian_ = I_3x3 - 0.5 * W + one_sixth * WW; } else { - a = one_minus_cos / theta; - b = 1.0 - sin_theta / theta; - dexp_ = I_3x3 - a * K + b * KK; + C = (1 - A) / theta2; + rightJacobian_ = I_3x3 - B * W + C * WW; } } @@ -105,21 +108,23 @@ Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, *H1 = 0.5 * skewSymmetric(v); } else { // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK - const Vector3 Kv = K * v; + double a = B * theta; + double b = C * theta2; + const Vector3 Kv = W * v / theta; const double Da = (sin_theta - 2.0 * a) / theta2; const double Db = (one_minus_cos - 3.0 * b) / theta2; - *H1 = (Db * K - Da * I_3x3) * Kv * omega.transpose() - + *H1 = (Db * W / theta - Da * I_3x3) * Kv * omega.transpose() - skewSymmetric(Kv * b / theta) + - (a * I_3x3 - b * K) * skewSymmetric(v / theta); + (a * I_3x3 - b * W / theta) * skewSymmetric(v / theta); } } - if (H2) *H2 = dexp_; - return dexp_ * v; + if (H2) *H2 = rightJacobian_; + return rightJacobian_ * v; } Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - const Matrix3 invDexp = dexp_.inverse(); + const Matrix3 invDexp = rightJacobian_.inverse(); const Vector3 c = invDexp * v; if (H1) { Matrix3 D_dexpv_omega; diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index cd67bfb8c..8d4401c31 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -136,7 +136,8 @@ GTSAM_EXPORT Matrix99 Dcompose(const SO3& R); class GTSAM_EXPORT ExpmapFunctor { protected: const double theta2; - Matrix3 W, K, KK; + Matrix3 W, WW; + double A, B; // Ethan Eade's constants bool nearZero; double theta, sin_theta, one_minus_cos; // only defined if !nearZero @@ -155,13 +156,16 @@ class GTSAM_EXPORT ExpmapFunctor { /// Functor that implements Exponential map *and* its derivatives class DexpFunctor : public ExpmapFunctor { + protected: + static constexpr double one_sixth = 1.0 / 6.0; const Vector3 omega; - double a, b; - Matrix3 dexp_; + double C; // Ethan Eade's C constant + Matrix3 rightJacobian_; public: /// Constructor with element of Lie algebra so(3) - GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, bool nearZeroApprox = false); + GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, + bool nearZeroApprox = false); // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, @@ -169,16 +173,21 @@ class DexpFunctor : public ExpmapFunctor { // expmap(omega + v) \approx expmap(omega) * expmap(dexp * v) // This maps a perturbation v in the tangent space to // a perturbation on the manifold Expmap(dexp * v) */ - const Matrix3& dexp() const { return dexp_; } + const Matrix3& dexp() const { return rightJacobian_; } /// Multiplies with dexp(), with optional derivatives - GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}) const; + GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; /// Multiplies with dexp().inverse(), with optional derivatives GTSAM_EXPORT Vector3 applyInvDexp(const Vector3& v, - OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}) const; + OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; + + // Compute the left Jacobian for Exponential map in SO(3) + // Note precomputed, as not used as much + Matrix3 leftJacobian() const; }; } // namespace so3