diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 4d993de8f..459f15561 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -27,8 +27,8 @@ namespace gtsam { namespace so3 { -void ExpmapFunctor::init() { - nearZero = (theta2 <= std::numeric_limits::epsilon()); +void ExpmapFunctor::init(bool nearZeroApprox) { + nearZero = nearZeroApprox || (theta2 <= std::numeric_limits::epsilon()); if (nearZero) return; theta = std::sqrt(theta2); // rotation angle sin_theta = std::sin(theta); @@ -36,10 +36,11 @@ void ExpmapFunctor::init() { one_minus_cos = 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] } -ExpmapFunctor::ExpmapFunctor(const Vector3& omega) : theta2(omega.dot(omega)) { +ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox) + : theta2(omega.dot(omega)) { 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(); + init(nearZeroApprox); if (!nearZero) { theta = std::sqrt(theta2); K = W / theta; @@ -47,12 +48,12 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega) : theta2(omega.dot(omega)) { } } -ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle) +ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox) : theta2(angle * 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; - init(); + init(nearZeroApprox); if (!nearZero) { theta = angle; KK = K * K; @@ -66,8 +67,8 @@ SO3 ExpmapFunctor::expmap() const { return I_3x3 + sin_theta * K + one_minus_cos * KK; } -DexpFunctor::DexpFunctor(const Vector3& omega) - : ExpmapFunctor(omega), omega(omega) { +DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) + : ExpmapFunctor(omega, nearZeroApprox), omega(omega) { if (nearZero) dexp_ = I_3x3 - 0.5 * W; else { @@ -79,19 +80,18 @@ DexpFunctor::DexpFunctor(const Vector3& omega) 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; - return v - 0.5 * omega.cross(v); - } if (H1) { - // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK - const Vector3 Kv = K * v; - 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() - - skewSymmetric(Kv * b / theta) + - (a * I_3x3 - b * K) * skewSymmetric(v / theta); + if (nearZero) { + *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; + 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() - + skewSymmetric(Kv * b / theta) + + (a * I_3x3 - b * K) * skewSymmetric(v / theta); + } } if (H2) *H2 = dexp_; return dexp_ * v; diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 13d6d65a7..3152fa2fb 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -143,14 +143,14 @@ class ExpmapFunctor { bool nearZero; double theta, sin_theta, one_minus_cos; // only defined if !nearZero - void init(); + void init(bool nearZeroApprox = false); public: /// Constructor with element of Lie algebra so(3) - ExpmapFunctor(const Vector3& omega); + ExpmapFunctor(const Vector3& omega, bool nearZeroApprox = false); /// Constructor with axis-angle - ExpmapFunctor(const Vector3& axis, double angle); + ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox = false); /// Rodrigues formula SO3 expmap() const; @@ -164,7 +164,7 @@ class DexpFunctor : public ExpmapFunctor { public: /// Constructor with element of Lie algebra so(3) - DexpFunctor(const Vector3& omega); + 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, diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index defa87281..68e87d5ba 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -203,49 +203,49 @@ TEST(SO3, JacobianLogmap) { EXPECT(assert_equal(Jexpected, Jactual)); } -/* ************************************************************************* */ -Vector3 apply(const Vector3& omega, const Vector3& v) { - so3::DexpFunctor local(omega); - return local.applyDexp(v); -} - /* ************************************************************************* */ TEST(SO3, ApplyDexp) { Matrix aH1, aH2; - for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), - Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { - so3::DexpFunctor local(omega); - for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), - Vector3(0.4, 0.3, 0.2)}) { - EXPECT(assert_equal(Vector3(local.dexp() * v), - local.applyDexp(v, aH1, aH2))); - EXPECT(assert_equal(numericalDerivative21(apply, omega, v), aH1)); - EXPECT(assert_equal(numericalDerivative22(apply, omega, v), aH2)); - EXPECT(assert_equal(local.dexp(), aH2)); + for (bool nearZeroApprox : {true, false}) { + boost::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZeroApprox).applyDexp(v); + }; + for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), + Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { + so3::DexpFunctor local(omega, nearZeroApprox); + for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), + Vector3(0.4, 0.3, 0.2)}) { + EXPECT(assert_equal(Vector3(local.dexp() * v), + local.applyDexp(v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(local.dexp(), aH2)); + } } } } -/* ************************************************************************* */ -Vector3 applyInv(const Vector3& omega, const Vector3& v) { - so3::DexpFunctor local(omega); - return local.applyInvDexp(v); -} - /* ************************************************************************* */ TEST(SO3, ApplyInvDexp) { Matrix aH1, aH2; - for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), - Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { - so3::DexpFunctor local(omega); - Matrix invDexp = local.dexp().inverse(); - for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), - Vector3(0.4, 0.3, 0.2)}) { - EXPECT( - assert_equal(Vector3(invDexp * v), local.applyInvDexp(v, aH1, aH2))); - EXPECT(assert_equal(numericalDerivative21(applyInv, omega, v), aH1)); - EXPECT(assert_equal(numericalDerivative22(applyInv, omega, v), aH2)); - EXPECT(assert_equal(invDexp, aH2)); + for (bool nearZeroApprox : {true, false}) { + boost::function f = + [=](const Vector3& omega, const Vector3& v) { + return so3::DexpFunctor(omega, nearZeroApprox).applyInvDexp(v); + }; + for (Vector3 omega : {Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 1, 0), + Vector3(0, 0, 1), Vector3(0.1, 0.2, 0.3)}) { + so3::DexpFunctor local(omega, nearZeroApprox); + Matrix invDexp = local.dexp().inverse(); + for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1), + Vector3(0.4, 0.3, 0.2)}) { + EXPECT(assert_equal(Vector3(invDexp * v), + local.applyInvDexp(v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(invDexp, aH2)); + } } } }