diff --git a/gtsam/geometry/SOn-inl.h b/gtsam/geometry/SOn-inl.h index 826efe1ee..d1c9efe29 100644 --- a/gtsam/geometry/SOn-inl.h +++ b/gtsam/geometry/SOn-inl.h @@ -115,7 +115,7 @@ typename SO::MatrixDD SO::ExpmapDerivative(const TangentVector& omega) { template typename SO::TangentVector SO::Logmap(const SO& R, ChartJacobian H) { - throw std::runtime_error("SO::Logmap only implemented for SO3 and SO4."); + throw std::runtime_error("SO::Logmap only implemented for SO3."); } template diff --git a/gtsam/geometry/SOt.cpp b/gtsam/geometry/SOt.cpp index 9249715e8..bf259041d 100644 --- a/gtsam/geometry/SOt.cpp +++ b/gtsam/geometry/SOt.cpp @@ -182,9 +182,10 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { } } -// Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { -// return sot::DexpFunctor(omega).dexp(); -// } +template <> +Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { + return sot::DexpFunctor(omega).dexp(); +} //****************************************************************************** /* Right Jacobian for Log map in SO(3) - equation (10.86) and following @@ -254,19 +255,22 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); } - if (H) *H = SO3::LogmapDerivative(omega); + if (H) *H = LogmapDerivative(omega); return omega; } //****************************************************************************** +// local vectorize static Vector9 vec3(const Matrix3& R) { return Eigen::Map(R.data()); } +// so<3> generators static const std::vector G3({SO3::Hat(Vector3::Unit(0)), SO3::Hat(Vector3::Unit(1)), SO3::Hat(Vector3::Unit(2))}); +// vectorized generators static const Matrix93 P3 = (Matrix93() << vec3(G3[0]), vec3(G3[1]), vec3(G3[2])).finished(); diff --git a/gtsam/geometry/SOt.h b/gtsam/geometry/SOt.h index 8d7632497..4b62d92e3 100644 --- a/gtsam/geometry/SOt.h +++ b/gtsam/geometry/SOt.h @@ -42,6 +42,15 @@ using SO3 = SO<3>; // static Matrix3 Hat(const Vector3 &xi); ///< make skew symmetric matrix // static Vector3 Vee(const Matrix3 &X); ///< inverse of Hat +// Below are all declarations of SO<3> specializations. +// They are *defined* in SO3.cpp. + +/// Adjoint map +template <> +Matrix3 SO3::AdjointMap() const { + return matrix_; +} + /** * Exponential map at identity - create a rotation from canonical coordinates * \f$ [R_x,R_y,R_z] \f$ using Rodrigues' formula @@ -49,13 +58,9 @@ using SO3 = SO<3>; template <> SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H); -// template<> -// Matrix3 SO3::ExpmapDerivative(const Vector3& omega) { -// return sot::DexpFunctor(omega).dexp(); -// } - /// Derivative of Expmap -// static Matrix3 ExpmapDerivative(const Vector3& omega); +template <> +Matrix3 SO3::ExpmapDerivative(const Vector3& omega); /** * Log map at identity - returns the canonical coordinates @@ -64,10 +69,9 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H); template <> Vector3 SO3::Logmap(const SO3& R, ChartJacobian H); +/// Derivative of Logmap template <> -Matrix3 SO3::AdjointMap() const { - return matrix_; -} +Matrix3 SO3::LogmapDerivative(const Vector3& omega); // Chart at origin for SO3 is *not* Cayley but actual Expmap/Logmap template <> diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index 83916bd1b..d6ca68ccf 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -157,7 +157,7 @@ TEST(SO3, ExpmapDerivative) { EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-7)); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ExpmapDerivative2) { const Vector3 theta(0.1, 0, 0.1); const Matrix Jexpected = numericalDerivative11( @@ -168,7 +168,7 @@ TEST(SO3, ExpmapDerivative2) { SO3::ExpmapDerivative(-theta))); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ExpmapDerivative3) { const Vector3 theta(10, 20, 30); const Matrix Jexpected = numericalDerivative11( @@ -179,7 +179,7 @@ TEST(SO3, ExpmapDerivative3) { SO3::ExpmapDerivative(-theta))); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ExpmapDerivative4) { // Iserles05an (Lie-group Methods) says: // scalar is easy: d exp(a(t)) / dt = exp(a(t)) a'(t) @@ -207,7 +207,7 @@ TEST(SO3, ExpmapDerivative4) { } } -/* ************************************************************************* */ +//****************************************************************************** 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); }; @@ -223,7 +223,7 @@ TEST(SO3, ExpmapDerivative5) { } } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, ExpmapDerivative6) { const Vector3 thetahat(0.1, 0, 0.1); const Matrix Jexpected = numericalDerivative11( @@ -233,7 +233,7 @@ TEST(SO3, ExpmapDerivative6) { EXPECT(assert_equal(Jexpected, Jactual)); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, LogmapDerivative) { const Vector3 thetahat(0.1, 0, 0.1); const SO3 R = SO3::Expmap(thetahat); // some rotation @@ -243,7 +243,7 @@ TEST(SO3, LogmapDerivative) { EXPECT(assert_equal(Jexpected, Jactual)); } -/* ************************************************************************* */ +//****************************************************************************** TEST(SO3, JacobianLogmap) { const Vector3 thetahat(0.1, 0, 0.1); const SO3 R = SO3::Expmap(thetahat); // some rotation