From 29416436eb0728bc87056422cfeaf5edc9ce95aa Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 31 Jan 2016 23:29:51 -0800 Subject: [PATCH 1/4] Modernized all of Expmap, moved tests from SO3, added ApplyExpmapDerivative --- gtsam/geometry/SO3.cpp | 178 +++++++++++------------ gtsam/geometry/SO3.h | 13 +- gtsam/geometry/tests/testRot3.cpp | 123 ---------------- gtsam/geometry/tests/testSO3.cpp | 233 ++++++++++++++++++++++++++---- 4 files changed, 298 insertions(+), 249 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index ca80167f4..233ca3339 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -11,8 +11,10 @@ /** * @file SO3.cpp - * @brief 3*3 matrix representation o SO(3) + * @brief 3*3 matrix representation of SO(3) * @author Frank Dellaert + * @author Luca Carlone + * @author Duy Nguyen Ta * @date December 2014 */ @@ -24,65 +26,94 @@ namespace gtsam { /* ************************************************************************* */ -// Near zero, we just have I + skew(omega) -static SO3 FirstOrder(const Vector3& omega) { - Matrix3 R; - R(0, 0) = 1.; - R(1, 0) = omega.z(); - R(2, 0) = -omega.y(); - R(0, 1) = -omega.z(); - R(1, 1) = 1.; - R(2, 1) = omega.x(); - R(0, 2) = omega.y(); - R(1, 2) = -omega.x(); - R(2, 2) = 1.; - return R; -} +// Functor that helps implement Exponential map and its derivatives +struct ExpmapImpl { + const Vector3 omega; + const double theta2; + Matrix3 W; + bool nearZero; + double theta, s1, s2, c_1; + + // omega: element of Lie algebra so(3): W = omega^, normalized by normx + ExpmapImpl(const Vector3& omega) : omega(omega), 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; // Skew[omega] + nearZero = (theta2 <= std::numeric_limits::epsilon()); + if (!nearZero) { + theta = std::sqrt(theta2); // rotation angle + s1 = std::sin(theta) / theta; + s2 = std::sin(theta / 2.0); + c_1 = 2.0 * s2 * s2; // numerically better behaved than [1 - cos(theta)] + } + } + + SO3 operator()() const { + if (nearZero) + return I_3x3 + W; + else + return I_3x3 + s1 * W + c_1 * W * W / theta2; + } + + // NOTE(luca): 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. + // 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) */ + SO3 dexp() const { + if (nearZero) { + return I_3x3 - 0.5 * W; + } else { + const double a = c_1 / theta2; + const double b = (1.0 - s1) / theta2; + return I_3x3 - a * W + b * W * W; + } + } + + // Just multiplies with dexp() + Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const { + if (nearZero) { + if (H1) *H1 = 0.5 * skewSymmetric(v); + if (H2) *H2 = I_3x3; + return v; + } else { + const double a = c_1 / theta2; + const double b = (1.0 - s1) / theta2; + Matrix3 dexp = I_3x3 - a * W + b * W * W; + if (H1) { + const Vector3 Wv = omega.cross(v); + const Vector3 WWv = omega.cross(Wv); + const Matrix3 T = skewSymmetric(v); + const double Da = (s1 - 2.0 * a) / theta2; + const double Db = (3.0 * s1 - std::cos(theta) - 2.0) / theta2 / theta2; + *H1 = (-Da * Wv + Db * WWv) * omega.transpose() + a * T - + b * skewSymmetric(Wv) - b * W * T; + } + if (H2) *H2 = dexp; + return dexp * v; + } + } +}; SO3 SO3::AxisAngle(const Vector3& axis, double theta) { - if (theta*theta > std::numeric_limits::epsilon()) { - using std::cos; - using std::sin; - - // get components of axis \omega, where is a unit vector - const double& wx = axis.x(), wy = axis.y(), wz = axis.z(); - - const double costheta = cos(theta), sintheta = sin(theta), s2 = sin(theta/2.0), c_1 = 2.0*s2*s2; - const double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta, - wz_sintheta = wz * sintheta; - - const double C00 = c_1 * wx * wx, C01 = c_1 * wx * wy, C02 = c_1 * wx * wz; - const double C11 = c_1 * wy * wy, C12 = c_1 * wy * wz; - const double C22 = c_1 * wz * wz; - - Matrix3 R; - R(0, 0) = costheta + C00; - R(1, 0) = wz_sintheta + C01; - R(2, 0) = -wy_sintheta + C02; - R(0, 1) = -wz_sintheta + C01; - R(1, 1) = costheta + C11; - R(2, 1) = wx_sintheta + C12; - R(0, 2) = wy_sintheta + C02; - R(1, 2) = -wx_sintheta + C12; - R(2, 2) = costheta + C22; - return R; - } else { - return FirstOrder(axis*theta); - } - + return ExpmapImpl(axis*theta)(); } -/// simply convert omega to axis/angle representation SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { - if (H) *H = ExpmapDerivative(omega); + ExpmapImpl impl(omega); + if (H) *H = impl.dexp(); + return impl(); +} - double theta2 = omega.dot(omega); - if (theta2 > std::numeric_limits::epsilon()) { - double theta = std::sqrt(theta2); - return AxisAngle(omega / theta, theta); - } else { - return FirstOrder(omega); - } +Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { + return ExpmapImpl(omega).dexp(); +} + +Vector3 SO3::ApplyExpmapDerivative(const Vector3& omega, const Vector3& v, + OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) { + return ExpmapImpl(omega).applyDexp(v, H1, H2); } /* ************************************************************************* */ @@ -129,44 +160,7 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { } /* ************************************************************************* */ -Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { - using std::sin; - - const double theta2 = omega.dot(omega); - if (theta2 <= std::numeric_limits::epsilon()) return I_3x3; - const double theta = std::sqrt(theta2); // rotation angle -#ifdef DUY_VERSION - /// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version) - Matrix3 X = skewSymmetric(omega); - Matrix3 X2 = X*X; - double vi = theta/2.0; - double s1 = sin(vi)/vi; - double s2 = (theta - sin(theta))/(theta*theta*theta); - return I_3x3 - 0.5*s1*s1*X + s2*X2; -#else // Luca's version - /** - * 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. - * expmap(thetahat + omega) \approx expmap(thetahat) * expmap(Jr * omega) - * where Jr = ExpmapDerivative(thetahat); - * This maps a perturbation in the tangent space (omega) to - * a perturbation on the manifold (expmap(Jr * omega)) - */ - // element of Lie algebra so(3): X = omega^, normalized by normx - const double wx = omega.x(), wy = omega.y(), wz = omega.z(); - Matrix3 Y; - Y << 0.0, -wz, +wy, - +wz, 0.0, -wx, - -wy, +wx, 0.0; - const double s2 = sin(theta / 2.0); - const double a = (2.0 * s2 * s2 / theta2); - const double b = (1.0 - sin(theta) / theta) / theta2; - return I_3x3 - a * Y + b * Y * Y; -#endif -} - -/* ************************************************************************* */ -Matrix3 SO3::LogmapDerivative(const Vector3& omega) { +Matrix3 SO3::LogmapDerivative(const Vector3& omega) { using std::cos; using std::sin; diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 580287eac..92c290d02 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -13,6 +13,8 @@ * @file SO3.h * @brief 3*3 matrix representation of SO(3) * @author Frank Dellaert + * @author Luca Carlone + * @author Duy Nguyen Ta * @date December 2014 */ @@ -97,15 +99,20 @@ public: */ static SO3 Expmap(const Vector3& omega, ChartJacobian H = boost::none); + /// Derivative of Expmap + static Matrix3 ExpmapDerivative(const Vector3& omega); + + /// Implement ExpmapDerivative(omega) * v, with derivatives + static Vector3 ApplyExpmapDerivative(const Vector3& omega, const Vector3& v, + OptionalJacobian<3, 3> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none); + /** * Log map at identity - returns the canonical coordinates * \f$ [R_x,R_y,R_z] \f$ of this rotation */ static Vector3 Logmap(const SO3& R, ChartJacobian H = boost::none); - /// Derivative of Expmap - static Matrix3 ExpmapDerivative(const Vector3& omega); - /// Derivative of Logmap static Matrix3 LogmapDerivative(const Vector3& omega); diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 25ddd16ce..4b3c84e01 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -243,129 +243,6 @@ TEST(Rot3, retract_localCoordinates2) Vector d21 = t2.localCoordinates(t1); EXPECT(assert_equal(t1, t2.retract(d21))); } -/* ************************************************************************* */ -namespace exmap_derivative { -static const Vector3 w(0.1, 0.27, -0.2); -} -// Left trivialized Derivative of exp(w) wrpt w: -// How does exp(w) change when w changes? -// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 -// => y = log (exp(-w) * exp(w+dw)) -Vector3 testDexpL(const Vector3& dw) { - using exmap_derivative::w; - return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); -} - -TEST( Rot3, ExpmapDerivative) { - using exmap_derivative::w; - const Matrix actualDexpL = Rot3::ExpmapDerivative(w); - const Matrix expectedDexpL = numericalDerivative11(testDexpL, - Vector3::Zero(), 1e-2); - EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7)); - - const Matrix actualDexpInvL = Rot3::LogmapDerivative(w); - EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7)); - } - -/* ************************************************************************* */ -TEST( Rot3, ExpmapDerivative2) -{ - const Vector3 theta(0.1, 0, 0.1); - const Matrix Jexpected = numericalDerivative11( - boost::bind(&Rot3::Expmap, _1, boost::none), theta); - - CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta))); - CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta))); -} - -/* ************************************************************************* */ -TEST( Rot3, ExpmapDerivative3) -{ - const Vector3 theta(10, 20, 30); - const Matrix Jexpected = numericalDerivative11( - boost::bind(&Rot3::Expmap, _1, boost::none), theta); - - CHECK(assert_equal(Jexpected, Rot3::ExpmapDerivative(theta))); - CHECK(assert_equal(Matrix3(Jexpected.transpose()), Rot3::ExpmapDerivative(-theta))); -} - -/* ************************************************************************* */ -TEST(Rot3, ExpmapDerivative4) { - // Iserles05an (Lie-group Methods) says: - // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) - // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) - // where A(t): R -> so(3) is a trajectory in the tangent space of SO(3) - // and dexp[A] is a linear map from 3*3 to 3*3 derivatives of se(3) - // Hence, the above matrix equation is typed: 3*3 = SO(3) * linear_map(3*3) - - // In GTSAM, we don't work with the skew-symmetric matrices A directly, but with 3-vectors. - // omega is easy: d Expmap(w(t)) / dt = ExmapDerivative[w(t)] * w'(t) - - // Let's verify the above formula. - - auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; - auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; - - // We define a function R - auto R = [w](double t) { return Rot3::Expmap(w(t)); }; - - for (double t = -2.0; t < 2.0; t += 0.3) { - const Matrix expected = numericalDerivative11(R, t); - const Matrix actual = Rot3::ExpmapDerivative(w(t)) * w_dot(t); - CHECK(assert_equal(expected, actual, 1e-7)); - } -} - -/* ************************************************************************* */ -TEST(Rot3, ExpmapDerivative5) { - auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; - auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; - - // Same as above, but define R as mapping local coordinates to neighborhood aroud R0. - const Rot3 R0 = Rot3::Rodrigues(0.1, 0.4, 0.2); - auto R = [R0, w](double t) { return R0.expmap(w(t)); }; - - for (double t = -2.0; t < 2.0; t += 0.3) { - const Matrix expected = numericalDerivative11(R, t); - const Matrix actual = Rot3::ExpmapDerivative(w(t)) * w_dot(t); - CHECK(assert_equal(expected, actual, 1e-7)); - } -} - -/* ************************************************************************* */ -TEST( Rot3, jacobianExpmap ) -{ - const Vector3 thetahat(0.1, 0, 0.1); - const Matrix Jexpected = numericalDerivative11(boost::bind( - &Rot3::Expmap, _1, boost::none), thetahat); - Matrix3 Jactual; - const Rot3 R = Rot3::Expmap(thetahat, Jactual); - EXPECT(assert_equal(Jexpected, Jactual)); -} - -/* ************************************************************************* */ -TEST( Rot3, LogmapDerivative ) -{ - const Vector3 thetahat(0.1, 0, 0.1); - const Rot3 R = Rot3::Expmap(thetahat); // some rotation - const Matrix Jexpected = numericalDerivative11(boost::bind( - &Rot3::Logmap, _1, boost::none), R); - const Matrix3 Jactual = Rot3::LogmapDerivative(thetahat); - EXPECT(assert_equal(Jexpected, Jactual)); -} - -/* ************************************************************************* */ -TEST( Rot3, JacobianLogmap ) -{ - const Vector3 thetahat(0.1, 0, 0.1); - const Rot3 R = Rot3::Expmap(thetahat); // some rotation - const Matrix Jexpected = numericalDerivative11(boost::bind( - &Rot3::Logmap, _1, boost::none), R); - Matrix3 Jactual; - Rot3::Logmap(R, Jactual); - EXPECT(assert_equal(Jexpected, Jactual)); -} - /* ************************************************************************* */ TEST(Rot3, manifold_expmap) { diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index bc32e0df0..0075a76e8 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -1,6 +1,6 @@ /* ---------------------------------------------------------------------------- - * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * 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) @@ -24,16 +24,14 @@ using namespace std; using namespace gtsam; //****************************************************************************** -TEST(SO3 , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); - BOOST_CONCEPT_ASSERT((IsManifold)); - BOOST_CONCEPT_ASSERT((IsLieGroup)); +TEST(SO3, Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsLieGroup)); } //****************************************************************************** -TEST(SO3 , Constructor) { - SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); -} +TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } //****************************************************************************** SO3 id; @@ -42,46 +40,220 @@ SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); //****************************************************************************** -TEST(SO3 , Local) { +TEST(SO3, Local) { Vector3 expected(0, 0, 0.1); Vector3 actual = traits::Local(R1, R2); - EXPECT(assert_equal((Vector)expected,actual)); + EXPECT(assert_equal((Vector)expected, actual)); } //****************************************************************************** -TEST(SO3 , Retract) { +TEST(SO3, Retract) { Vector3 v(0, 0, 0.1); SO3 actual = traits::Retract(R1, v); EXPECT(actual.isApprox(R2)); } //****************************************************************************** -TEST(SO3 , Invariants) { - EXPECT(check_group_invariants(id,id)); - EXPECT(check_group_invariants(id,R1)); - EXPECT(check_group_invariants(R2,id)); - EXPECT(check_group_invariants(R2,R1)); +TEST(SO3, Invariants) { + EXPECT(check_group_invariants(id, id)); + EXPECT(check_group_invariants(id, R1)); + EXPECT(check_group_invariants(R2, id)); + EXPECT(check_group_invariants(R2, R1)); - EXPECT(check_manifold_invariants(id,id)); - EXPECT(check_manifold_invariants(id,R1)); - EXPECT(check_manifold_invariants(R2,id)); - EXPECT(check_manifold_invariants(R2,R1)); + EXPECT(check_manifold_invariants(id, id)); + EXPECT(check_manifold_invariants(id, R1)); + EXPECT(check_manifold_invariants(R2, id)); + EXPECT(check_manifold_invariants(R2, R1)); } //****************************************************************************** -TEST(SO3 , LieGroupDerivatives) { - CHECK_LIE_GROUP_DERIVATIVES(id,id); - CHECK_LIE_GROUP_DERIVATIVES(id,R2); - CHECK_LIE_GROUP_DERIVATIVES(R2,id); - CHECK_LIE_GROUP_DERIVATIVES(R2,R1); +TEST(SO3, LieGroupDerivatives) { + CHECK_LIE_GROUP_DERIVATIVES(id, id); + CHECK_LIE_GROUP_DERIVATIVES(id, R2); + CHECK_LIE_GROUP_DERIVATIVES(R2, id); + CHECK_LIE_GROUP_DERIVATIVES(R2, R1); } //****************************************************************************** -TEST(SO3 , ChartDerivatives) { - CHECK_CHART_DERIVATIVES(id,id); - CHECK_CHART_DERIVATIVES(id,R2); - CHECK_CHART_DERIVATIVES(R2,id); - CHECK_CHART_DERIVATIVES(R2,R1); +TEST(SO3, ChartDerivatives) { + CHECK_CHART_DERIVATIVES(id, id); + CHECK_CHART_DERIVATIVES(id, R2); + CHECK_CHART_DERIVATIVES(R2, id); + CHECK_CHART_DERIVATIVES(R2, R1); +} + +/* ************************************************************************* */ +namespace exmap_derivative { +static const Vector3 w(0.1, 0.27, -0.2); +} +// Left trivialized Derivative of exp(w) wrpt w: +// How does exp(w) change when w changes? +// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 +// => y = log (exp(-w) * exp(w+dw)) +Vector3 testDexpL(const Vector3& dw) { + using exmap_derivative::w; + return SO3::Logmap(SO3::Expmap(-w) * SO3::Expmap(w + dw)); +} + +TEST(SO3, ExpmapDerivative) { + using exmap_derivative::w; + const Matrix actualDexpL = SO3::ExpmapDerivative(w); + const Matrix expectedDexpL = + numericalDerivative11(testDexpL, Vector3::Zero(), 1e-2); + EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-7)); + + const Matrix actualDexpInvL = SO3::LogmapDerivative(w); + EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-7)); +} + +/* ************************************************************************* */ +TEST(SO3, ExpmapDerivative2) { + const Vector3 theta(0.1, 0, 0.1); + const Matrix Jexpected = numericalDerivative11( + boost::bind(&SO3::Expmap, _1, boost::none), theta); + + CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); + CHECK(assert_equal(Matrix3(Jexpected.transpose()), + SO3::ExpmapDerivative(-theta))); +} + +/* ************************************************************************* */ +TEST(SO3, ExpmapDerivative3) { + const Vector3 theta(10, 20, 30); + const Matrix Jexpected = numericalDerivative11( + boost::bind(&SO3::Expmap, _1, boost::none), theta); + + CHECK(assert_equal(Jexpected, SO3::ExpmapDerivative(theta))); + CHECK(assert_equal(Matrix3(Jexpected.transpose()), + SO3::ExpmapDerivative(-theta))); +} + +/* ************************************************************************* */ +TEST(SO3, ExpmapDerivative4) { + // Iserles05an (Lie-group Methods) says: + // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) + // matrix is hard: d exp(A(t)) / dt = exp(A(t)) dexp[-A(t)] A'(t) + // where A(t): R -> so(3) is a trajectory in the tangent space of SO(3) + // and dexp[A] is a linear map from 3*3 to 3*3 derivatives of se(3) + // Hence, the above matrix equation is typed: 3*3 = SO(3) * linear_map(3*3) + + // In GTSAM, we don't work with the skew-symmetric matrices A directly, but + // with 3-vectors. + // omega is easy: d Expmap(w(t)) / dt = ExmapDerivative[w(t)] * w'(t) + + // Let's verify the above formula. + + auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; + auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; + + // We define a function R + auto R = [w](double t) { return SO3::Expmap(w(t)); }; + + for (double t = -2.0; t < 2.0; t += 0.3) { + const Matrix expected = numericalDerivative11(R, t); + const Matrix actual = SO3::ExpmapDerivative(w(t)) * w_dot(t); + CHECK(assert_equal(expected, actual, 1e-7)); + } +} + +/* ************************************************************************* */ +TEST(SO3, ExpmapDerivative5) { + auto w = [](double t) { return Vector3(2 * t, sin(t), 4 * t * t); }; + auto w_dot = [](double t) { return Vector3(2, cos(t), 8 * t); }; + + // Now define R as mapping local coordinates to neighborhood around R0. + const SO3 R0 = SO3::Expmap(Vector3(0.1, 0.4, 0.2)); + auto R = [R0, w](double t) { return R0.expmap(w(t)); }; + + for (double t = -2.0; t < 2.0; t += 0.3) { + const Matrix expected = numericalDerivative11(R, t); + const Matrix actual = SO3::ExpmapDerivative(w(t)) * w_dot(t); + CHECK(assert_equal(expected, actual, 1e-7)); + } +} + +/* ************************************************************************* */ +TEST(SO3, ExpmapDerivative6) { + const Vector3 thetahat(0.1, 0, 0.1); + const Matrix Jexpected = numericalDerivative11( + boost::bind(&SO3::Expmap, _1, boost::none), thetahat); + Matrix3 Jactual; + SO3::Expmap(thetahat, Jactual); + EXPECT(assert_equal(Jexpected, Jactual)); +} + +/* ************************************************************************* */ +TEST(SO3, LogmapDerivative) { + const Vector3 thetahat(0.1, 0, 0.1); + const SO3 R = SO3::Expmap(thetahat); // some rotation + const Matrix Jexpected = numericalDerivative11( + boost::bind(&SO3::Logmap, _1, boost::none), R); + const Matrix3 Jactual = SO3::LogmapDerivative(thetahat); + EXPECT(assert_equal(Jexpected, Jactual)); +} + +/* ************************************************************************* */ +TEST(SO3, JacobianLogmap) { + const Vector3 thetahat(0.1, 0, 0.1); + const SO3 R = SO3::Expmap(thetahat); // some rotation + const Matrix Jexpected = numericalDerivative11( + boost::bind(&SO3::Logmap, _1, boost::none), R); + Matrix3 Jactual; + SO3::Logmap(R, Jactual); + EXPECT(assert_equal(Jexpected, Jactual)); +} + +/* ************************************************************************* */ +TEST(SO3, ApplyExpmapDerivative1) { + Matrix aH1, aH2; + boost::function f = + boost::bind(SO3::ApplyExpmapDerivative, _1, _2, boost::none, boost::none); + for (Vector3 omega : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) { + for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) { + Matrix3 H = SO3::ExpmapDerivative(omega); + Vector3 expected = H * v; + EXPECT(assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v))); + EXPECT(assert_equal(expected, + SO3::ApplyExpmapDerivative(omega, v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(H, aH2)); + } + } +} + +/* ************************************************************************* */ +TEST(SO3, ApplyExpmapDerivative2) { + Matrix aH1, aH2; + boost::function f = + boost::bind(SO3::ApplyExpmapDerivative, _1, _2, boost::none, boost::none); + const Vector3 omega(0, 0, 0); + for (Vector3 v : {Vector3(1, 0, 0), Vector3(0, 1, 0), Vector3(0, 0, 1)}) { + Matrix3 H = SO3::ExpmapDerivative(omega); + Vector3 expected = H * v; + EXPECT(assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v))); + EXPECT( + assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(H, aH2)); + } +} + +/* ************************************************************************* */ +TEST(SO3, ApplyExpmapDerivative3) { + Matrix aH1, aH2; + boost::function f = + boost::bind(SO3::ApplyExpmapDerivative, _1, _2, boost::none, boost::none); + const Vector3 omega(0.1, 0.2, 0.3), v(0.4, 0.3, 0.2); + Matrix3 H = SO3::ExpmapDerivative(omega); + Vector3 expected = H * v; + EXPECT(assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v))); + EXPECT( + assert_equal(expected, SO3::ApplyExpmapDerivative(omega, v, aH1, aH2))); + EXPECT(assert_equal(numericalDerivative21(f, omega, v), aH1)); + EXPECT(assert_equal(numericalDerivative22(f, omega, v), aH2)); + EXPECT(assert_equal(H, aH2)); } //****************************************************************************** @@ -90,4 +262,3 @@ int main() { return TestRegistry::runAllTests(tr); } //****************************************************************************** - From f054a004577d2cfd356adb9b1b7fda2e1f6a7e86 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 31 Jan 2016 23:39:42 -0800 Subject: [PATCH 2/4] Initialize --- gtsam/geometry/SO3.cpp | 62 ++++++++++++++++++++++-------------------- 1 file changed, 33 insertions(+), 29 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 233ca3339..76cae09d9 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -32,26 +32,38 @@ struct ExpmapImpl { const double theta2; Matrix3 W; bool nearZero; - double theta, s1, s2, c_1; + double theta, sin_over_theta, one_minus_cos; - // omega: element of Lie algebra so(3): W = omega^, normalized by normx - ExpmapImpl(const Vector3& omega) : omega(omega), theta2(omega.dot(omega)) { + void Initialize() { const double wx = omega.x(), wy = omega.y(), wz = omega.z(); W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; // Skew[omega] nearZero = (theta2 <= std::numeric_limits::epsilon()); if (!nearZero) { theta = std::sqrt(theta2); // rotation angle - s1 = std::sin(theta) / theta; - s2 = std::sin(theta / 2.0); - c_1 = 2.0 * s2 * s2; // numerically better behaved than [1 - cos(theta)] + sin_over_theta = std::sin(theta) / theta; + const double s2 = std::sin(theta / 2.0); + one_minus_cos = + 2.0 * s2 * s2; // numerically better behaved than [1 - cos(theta)] } } + // Constructor with element of Lie algebra so(3): W = omega^, normalized by + // normx + ExpmapImpl(const Vector3& omega) : omega(omega), theta2(omega.dot(omega)) { + Initialize(); + } + + // Constructor with axis-angle + ExpmapImpl(const Vector3& axis, double theta) + : omega(axis * theta), theta2(theta * theta) { + Initialize(); + } + SO3 operator()() const { if (nearZero) return I_3x3 + W; else - return I_3x3 + s1 * W + c_1 * W * W / theta2; + return I_3x3 + sin_over_theta * W + one_minus_cos * W * W / theta2; } // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation @@ -64,8 +76,8 @@ struct ExpmapImpl { if (nearZero) { return I_3x3 - 0.5 * W; } else { - const double a = c_1 / theta2; - const double b = (1.0 - s1) / theta2; + const double a = one_minus_cos / theta2; + const double b = (1.0 - sin_over_theta) / theta2; return I_3x3 - a * W + b * W * W; } } @@ -78,15 +90,16 @@ struct ExpmapImpl { if (H2) *H2 = I_3x3; return v; } else { - const double a = c_1 / theta2; - const double b = (1.0 - s1) / theta2; + const double a = one_minus_cos / theta2; + const double b = (1.0 - sin_over_theta) / theta2; Matrix3 dexp = I_3x3 - a * W + b * W * W; if (H1) { const Vector3 Wv = omega.cross(v); const Vector3 WWv = omega.cross(Wv); const Matrix3 T = skewSymmetric(v); - const double Da = (s1 - 2.0 * a) / theta2; - const double Db = (3.0 * s1 - std::cos(theta) - 2.0) / theta2 / theta2; + const double Da = (sin_over_theta - 2.0 * a) / theta2; + const double Db = + (3.0 * sin_over_theta - std::cos(theta) - 2.0) / theta2 / theta2; *H1 = (-Da * Wv + Db * WWv) * omega.transpose() + a * T - b * skewSymmetric(Wv) - b * W * T; } @@ -97,7 +110,7 @@ struct ExpmapImpl { }; SO3 SO3::AxisAngle(const Vector3& axis, double theta) { - return ExpmapImpl(axis*theta)(); + return ExpmapImpl(axis, theta)(); } SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { @@ -127,7 +140,7 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { const double& R31 = R(2, 0), R32 = R(2, 1), R33 = R(2, 2); // Get trace(R) - double tr = R.trace(); + const double tr = R.trace(); Vector3 omega; @@ -143,7 +156,7 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) { omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); } else { double magnitude; - double tr_3 = tr - 3.0; // always negative + const double tr_3 = tr - 3.0; // always negative if (tr_3 < -1e-7) { double theta = acos((tr - 1.0) / 2.0); magnitude = theta / (2.0 * sin(theta)); @@ -167,14 +180,6 @@ Matrix3 SO3::LogmapDerivative(const Vector3& omega) { double theta2 = omega.dot(omega); if (theta2 <= std::numeric_limits::epsilon()) return I_3x3; double theta = std::sqrt(theta2); // rotation angle -#ifdef DUY_VERSION - /// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version) - Matrix3 X = skewSymmetric(omega); - Matrix3 X2 = X*X; - double vi = theta/2.0; - double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta); - return I_3x3 + 0.5*X - s2*X2; -#else // Luca's version /** 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. * logmap( Rhat * expmap(omega) ) \approx logmap( Rhat ) + Jrinv * omega @@ -182,11 +187,10 @@ Matrix3 SO3::LogmapDerivative(const Vector3& omega) { * This maps a perturbation on the manifold (expmap(omega)) * to a perturbation in the tangent space (Jrinv * omega) */ - const Matrix3 X = skewSymmetric(omega); // element of Lie algebra so(3): X = omega^ - return I_3x3 + 0.5 * X - + (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * X - * X; -#endif + const Matrix3 W = skewSymmetric(omega); // element of Lie algebra so(3): W = omega^ + return I_3x3 + 0.5 * W + + (1 / (theta * theta) - (1 + cos(theta)) / (2 * theta * sin(theta))) * + W * W; } /* ************************************************************************* */ From c838d7c1336116485c0ab72b679d5171be4a4997 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 1 Feb 2016 00:48:47 -0800 Subject: [PATCH 3/4] Switch to Skew[axis] = Skew[omega/angle] for simpler forms --- gtsam/geometry/SO3.cpp | 80 +++++++++++++++++++++++------------------- 1 file changed, 44 insertions(+), 36 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 76cae09d9..35b2c15b5 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -30,40 +30,55 @@ namespace gtsam { struct ExpmapImpl { const Vector3 omega; const double theta2; - Matrix3 W; + Matrix3 W, K, KK; bool nearZero; - double theta, sin_over_theta, one_minus_cos; + double theta, sin_theta, sin_over_theta, one_minus_cos, a, b; - void Initialize() { - const double wx = omega.x(), wy = omega.y(), wz = omega.z(); - W << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0; // Skew[omega] + void init() { nearZero = (theta2 <= std::numeric_limits::epsilon()); if (!nearZero) { theta = std::sqrt(theta2); // rotation angle - sin_over_theta = std::sin(theta) / theta; + sin_theta = std::sin(theta); + sin_over_theta = sin_theta / theta; const double s2 = std::sin(theta / 2.0); one_minus_cos = - 2.0 * s2 * s2; // numerically better behaved than [1 - cos(theta)] + 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] + a = one_minus_cos / theta; + b = 1.0 - sin_over_theta; } } // Constructor with element of Lie algebra so(3): W = omega^, normalized by // normx ExpmapImpl(const Vector3& omega) : omega(omega), theta2(omega.dot(omega)) { - Initialize(); + 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(); + if (!nearZero) { + theta = std::sqrt(theta2); + K = W / theta; + KK = K * K; + } } // Constructor with axis-angle - ExpmapImpl(const Vector3& axis, double theta) - : omega(axis * theta), theta2(theta * theta) { - Initialize(); + ExpmapImpl(const Vector3& axis, double angle) + : omega(axis * angle), 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(); + if (!nearZero) { + theta = angle; + KK = K * K; + } } SO3 operator()() const { if (nearZero) return I_3x3 + W; else - return I_3x3 + sin_over_theta * W + one_minus_cos * W * W / theta2; + return I_3x3 + sin_theta * K + one_minus_cos * K * K; } // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation @@ -73,39 +88,32 @@ struct ExpmapImpl { // This maps a perturbation v in the tangent space to // a perturbation on the manifold Expmap(dexp * v) */ SO3 dexp() const { - if (nearZero) { + if (nearZero) return I_3x3 - 0.5 * W; - } else { - const double a = one_minus_cos / theta2; - const double b = (1.0 - sin_over_theta) / theta2; - return I_3x3 - a * W + b * W * W; - } + else + return I_3x3 - a * K + b * KK; } // Just multiplies with dexp() - Vector3 applyDexp(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none, - OptionalJacobian<3, 3> H2 = boost::none) const { + Vector3 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; - } else { - const double a = one_minus_cos / theta2; - const double b = (1.0 - sin_over_theta) / theta2; - Matrix3 dexp = I_3x3 - a * W + b * W * W; - if (H1) { - const Vector3 Wv = omega.cross(v); - const Vector3 WWv = omega.cross(Wv); - const Matrix3 T = skewSymmetric(v); - const double Da = (sin_over_theta - 2.0 * a) / theta2; - const double Db = - (3.0 * sin_over_theta - std::cos(theta) - 2.0) / theta2 / theta2; - *H1 = (-Da * Wv + Db * WWv) * omega.transpose() + a * T - - b * skewSymmetric(Wv) - b * W * T; - } - if (H2) *H2 = dexp; - return dexp * v; } + Matrix3 dexp = I_3x3 - a * K + b * KK; + if (H1) { + const Vector3 Kv = omega.cross(v / theta); + const Vector3 KKv = omega.cross(Kv / theta); + const Matrix3 T = skewSymmetric(v / theta); + const double Da = (sin_over_theta - 2.0 * a / theta) / theta; + const double Db = (3.0 * sin_over_theta - std::cos(theta) - 2.0) / theta2; + *H1 = (-Da * Kv + Db * KKv) * omega.transpose() + a * T - + skewSymmetric(Kv * b / theta) - b * K * T; + } + if (H2) *H2 = dexp; + return dexp * v; } }; From 85fbde030bbf05b8dbb1ecd2876ec7711d1308d3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 1 Feb 2016 08:38:19 -0800 Subject: [PATCH 4/4] Made expmap only path faster by splitting functor. --- gtsam/geometry/SO3.cpp | 84 ++++++++++++++++++++++++------------------ 1 file changed, 48 insertions(+), 36 deletions(-) diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 35b2c15b5..b269e3021 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -26,31 +26,24 @@ namespace gtsam { /* ************************************************************************* */ -// Functor that helps implement Exponential map and its derivatives +// Functor implementing Exponential map struct ExpmapImpl { - const Vector3 omega; const double theta2; Matrix3 W, K, KK; bool nearZero; - double theta, sin_theta, sin_over_theta, one_minus_cos, a, b; + double theta, sin_theta, one_minus_cos; // only defined if !nearZero void init() { nearZero = (theta2 <= std::numeric_limits::epsilon()); - if (!nearZero) { - theta = std::sqrt(theta2); // rotation angle - sin_theta = std::sin(theta); - sin_over_theta = sin_theta / theta; - const double s2 = std::sin(theta / 2.0); - one_minus_cos = - 2.0 * s2 * s2; // numerically better than [1 - cos(theta)] - a = one_minus_cos / theta; - b = 1.0 - sin_over_theta; - } + if (nearZero) return; + theta = std::sqrt(theta2); // rotation angle + 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)] } - // Constructor with element of Lie algebra so(3): W = omega^, normalized by - // normx - ExpmapImpl(const Vector3& omega) : omega(omega), theta2(omega.dot(omega)) { + // Constructor with element of Lie algebra so(3) + ExpmapImpl(const Vector3& omega) : 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(); @@ -62,8 +55,7 @@ struct ExpmapImpl { } // Constructor with axis-angle - ExpmapImpl(const Vector3& axis, double angle) - : omega(axis * angle), theta2(angle * angle) { + ExpmapImpl(const Vector3& axis, double angle) : 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; @@ -74,12 +66,32 @@ struct ExpmapImpl { } } - SO3 operator()() const { + // Rodrgues formula + SO3 expmap() const { if (nearZero) return I_3x3 + W; else return I_3x3 + sin_theta * K + one_minus_cos * K * K; } +}; + +/* ************************************************************************* */ +SO3 SO3::AxisAngle(const Vector3& axis, double theta) { + return ExpmapImpl(axis, theta).expmap(); +} + +/* ************************************************************************* */ +// Functor that implements Exponential map *and* its derivatives +struct DexpImpl : ExpmapImpl { + const Vector3 omega; + double a, b; // constants used in dexp and applyDexp + + // Constructor with element of Lie algebra so(3) + DexpImpl(const Vector3& omega) : ExpmapImpl(omega), omega(omega) { + if (nearZero) return; + a = one_minus_cos / theta; + b = 1.0 - sin_theta / theta; + } // NOTE(luca): Right Jacobian for Exponential map in SO(3) - equation // (10.86) and following equations in G.S. Chirikjian, "Stochastic Models, @@ -100,41 +112,41 @@ struct ExpmapImpl { if (nearZero) { if (H1) *H1 = 0.5 * skewSymmetric(v); if (H2) *H2 = I_3x3; - return v; + return v - 0.5 * omega.cross(v); } - Matrix3 dexp = I_3x3 - a * K + b * KK; + const Vector3 Kv = omega.cross(v / theta); + const Vector3 KKv = omega.cross(Kv / theta); if (H1) { - const Vector3 Kv = omega.cross(v / theta); - const Vector3 KKv = omega.cross(Kv / theta); + // TODO(frank): Iserles hints that there should be a form I + c*K + d*KK const Matrix3 T = skewSymmetric(v / theta); - const double Da = (sin_over_theta - 2.0 * a / theta) / theta; - const double Db = (3.0 * sin_over_theta - std::cos(theta) - 2.0) / theta2; + const double Da = (sin_theta - 2.0 * a) / theta2; + const double Db = (one_minus_cos - 3.0 * b) / theta2; *H1 = (-Da * Kv + Db * KKv) * omega.transpose() + a * T - skewSymmetric(Kv * b / theta) - b * K * T; } - if (H2) *H2 = dexp; - return dexp * v; + if (H2) *H2 = dexp(); + return v - a * Kv + b * KKv; } }; -SO3 SO3::AxisAngle(const Vector3& axis, double theta) { - return ExpmapImpl(axis, theta)(); -} - +/* ************************************************************************* */ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { - ExpmapImpl impl(omega); - if (H) *H = impl.dexp(); - return impl(); + if (H) { + DexpImpl impl(omega); + *H = impl.dexp(); + return impl.expmap(); + } else + return ExpmapImpl(omega).expmap(); } Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { - return ExpmapImpl(omega).dexp(); + return DexpImpl(omega).dexp(); } Vector3 SO3::ApplyExpmapDerivative(const Vector3& omega, const Vector3& v, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) { - return ExpmapImpl(omega).applyDexp(v, H1, H2); + return DexpImpl(omega).applyDexp(v, H1, H2); } /* ************************************************************************* */