diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index e341911b93..ef1fa374b4 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -93,15 +93,7 @@ DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) E = nearZero ? _one_sixtieth : (B - 3.0 * C) / theta2; } -Matrix3 DexpFunctor::rightJacobian() const { - if (nearZero) { - return I_3x3 - B * W; // + C * WW; - } else { - return I_3x3 - B * W + C * WW; - } -} - -Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const { +Vector3 DexpFunctor::crossB(const Vector3& v, OptionalJacobian<3, 3> H) const { // Wv = omega x * v const Vector3 Wv = gtsam::cross(omega, v); if (H) { @@ -113,8 +105,8 @@ Vector3 DexpFunctor::cross(const Vector3& v, OptionalJacobian<3, 3> H) const { return B * Wv; } -Vector3 DexpFunctor::doubleCross(const Vector3& v, - OptionalJacobian<3, 3> H) const { +Vector3 DexpFunctor::doubleCrossC(const Vector3& v, + OptionalJacobian<3, 3> H) const { // WWv = omega x (omega x * v) Matrix3 doubleCrossJacobian; const Vector3 WWv = @@ -131,18 +123,12 @@ Vector3 DexpFunctor::doubleCross(const Vector3& v, // Multiplies v with left Jacobian through vector operations only. Vector3 DexpFunctor::applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - if (nearZero) { - if (H1) *H1 = 0.5 * skewSymmetric(v); - if (H2) *H2 = I_3x3 - 0.5 * W; - return v - 0.5 * gtsam::cross(omega, v); - } else { Matrix3 D_BWv_omega, D_CWWv_omega; - const Vector3 BWv = cross(v, D_BWv_omega); - const Vector3 CWWv = doubleCross(v, D_CWWv_omega); - if (H1) *H1 = - D_BWv_omega + D_CWWv_omega; + const Vector3 BWv = crossB(v, D_BWv_omega); + const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega); + if (H1) *H1 = -D_BWv_omega + D_CWWv_omega; if (H2) *H2 = rightJacobian(); return v - BWv + CWWv; - } } Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, @@ -158,29 +144,15 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, return c; } -Matrix3 DexpFunctor::leftJacobian() const { - if (nearZero) { - return I_3x3 + 0.5 * W; // + one_sixth * WW; - } else { - return I_3x3 + B * W + C * WW; - } -} - Vector3 DexpFunctor::applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const { - if (nearZero) { - if (H1) *H1 = - 0.5 * skewSymmetric(v); - if (H2) *H2 = I_3x3 + 0.5 * W; - return v + 0.5 * gtsam::cross(omega, v); - } else { - Matrix3 D_BWv_omega, D_CWWv_omega; - const Vector3 BWv = cross(v, D_BWv_omega); - const Vector3 CWWv = doubleCross(v, D_CWWv_omega); - if (H1) *H1 = D_BWv_omega + D_CWWv_omega; - if (H2) *H2 = leftJacobian(); - return v + BWv + CWWv; - } + Matrix3 D_BWv_omega, D_CWWv_omega; + const Vector3 BWv = crossB(v, D_BWv_omega); + const Vector3 CWWv = doubleCrossC(v, D_CWWv_omega); + if (H1) *H1 = D_BWv_omega + D_CWWv_omega; + if (H2) *H2 = leftJacobian(); + return v + BWv + CWWv; } } // namespace so3 diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 6403c3ae0d..efb947e730 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -155,7 +155,7 @@ class GTSAM_EXPORT ExpmapFunctor { }; /// Functor that implements Exponential map *and* its derivatives -class DexpFunctor : public ExpmapFunctor { +class GTSAM_EXPORT DexpFunctor : public ExpmapFunctor { protected: const Vector3 omega; double C; // Ethan's C constant: (1 - A) / theta^2 or 1/6 for theta->0 @@ -164,15 +164,8 @@ class DexpFunctor : public ExpmapFunctor { double E; // (B - 3.0 * C) / theta2 or -1/60 for theta->0 public: - /// Computes B * (omega x v). - Vector3 cross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; - - /// Computes C * (omega x (omega x v)). - Vector3 doubleCross(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; - /// Constructor with element of Lie algebra so(3) - GTSAM_EXPORT explicit DexpFunctor(const Vector3& omega, - bool nearZeroApprox = false); + 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, @@ -180,28 +173,31 @@ 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) - GTSAM_EXPORT Matrix3 rightJacobian() const; + Matrix3 rightJacobian() const { return I_3x3 - B * W + C * WW; } + + // Compute the left Jacobian for Exponential map in SO(3) + Matrix3 leftJacobian() const { return I_3x3 + B * W + C * WW; } /// differential of expmap == right Jacobian - GTSAM_EXPORT Matrix3 dexp() const { return rightJacobian(); } + inline Matrix3 dexp() const { return rightJacobian(); } + + /// Computes B * (omega x v). + Vector3 crossB(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; + + /// Computes C * (omega x (omega x v)). + Vector3 doubleCrossC(const Vector3& v, OptionalJacobian<3, 3> H = {}) const; /// Multiplies with dexp(), with optional derivatives - GTSAM_EXPORT Vector3 applyDexp(const Vector3& v, - OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}) const; + 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; - - // Compute the left Jacobian for Exponential map in SO(3) - GTSAM_EXPORT Matrix3 leftJacobian() const; + Vector3 applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; /// Multiplies with leftJacobian(), with optional derivatives - GTSAM_EXPORT Vector3 applyLeftJacobian(const Vector3& v, - OptionalJacobian<3, 3> H1 = {}, - OptionalJacobian<3, 3> H2 = {}) const; + Vector3 applyLeftJacobian(const Vector3& v, OptionalJacobian<3, 3> H1 = {}, + OptionalJacobian<3, 3> H2 = {}) const; protected: static constexpr double one_sixth = 1.0 / 6.0;