diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index ca80167f4..b269e3021 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,127 @@ 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 implementing Exponential map +struct ExpmapImpl { + const double theta2; + Matrix3 W, K, KK; + bool nearZero; + double theta, sin_theta, one_minus_cos; // only defined if !nearZero + void init() { + nearZero = (theta2 <= std::numeric_limits::epsilon()); + 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) + 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(); + if (!nearZero) { + theta = std::sqrt(theta2); + K = W / theta; + KK = K * K; + } + } + + // Constructor with axis-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; + init(); + if (!nearZero) { + theta = angle; + KK = K * K; + } + } + + // 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) { - 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).expmap(); } -/// simply convert omega to axis/angle representation -SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { - if (H) *H = ExpmapDerivative(omega); +/* ************************************************************************* */ +// Functor that implements Exponential map *and* its derivatives +struct DexpImpl : ExpmapImpl { + const Vector3 omega; + double a, b; // constants used in dexp and applyDexp - 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); + // 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, + // 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 + return I_3x3 - a * K + b * KK; + } + + // Just multiplies with dexp() + 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 - 0.5 * omega.cross(v); + } + const Vector3 Kv = omega.cross(v / theta); + const Vector3 KKv = omega.cross(Kv / theta); + if (H1) { + // 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_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 v - a * Kv + b * KKv; + } +}; + +/* ************************************************************************* */ +SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { + if (H) { + DexpImpl impl(omega); + *H = impl.dexp(); + return impl.expmap(); + } else + return ExpmapImpl(omega).expmap(); +} + +Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { + return DexpImpl(omega).dexp(); +} + +Vector3 SO3::ApplyExpmapDerivative(const Vector3& omega, const Vector3& v, + OptionalJacobian<3, 3> H1, + OptionalJacobian<3, 3> H2) { + return DexpImpl(omega).applyDexp(v, H1, H2); } /* ************************************************************************* */ @@ -96,7 +160,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; @@ -112,7 +176,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)); @@ -129,58 +193,13 @@ 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; 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 @@ -188,11 +207,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; } /* ************************************************************************* */ 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); } //****************************************************************************** -