diff --git a/gtsam/geometry/SOt.cpp b/gtsam/geometry/SOt.cpp index bf259041d..a5e8f457d 100644 --- a/gtsam/geometry/SOt.cpp +++ b/gtsam/geometry/SOt.cpp @@ -159,16 +159,25 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, // } //****************************************************************************** -// Matrix3 SO3::Hat(const Vector3& xi) { return skewSymmetric(xi); } +template <> +Matrix3 SO3::Hat(const Vector3& xi) { + // skew symmetric matrix X = xi^ + Matrix3 Y = Z_3x3; + Y(0, 1) = -xi(2); + Y(0, 2) = +xi(1); + Y(1, 2) = -xi(0); + return Y - Y.transpose(); +} -// /* ************************************************************************* -// */ Vector3 SO3::Vee(const Matrix3& X) { -// Vector3 xi; -// xi(0) = -X(1, 2); -// xi(1) = X(0, 2); -// xi(2) = -X(0, 1); -// return xi; -// } +//****************************************************************************** +template <> +Vector3 SO3::Vee(const Matrix3& X) { + Vector3 xi; + xi(0) = -X(1, 2); + xi(1) = +X(0, 2); + xi(2) = -X(0, 1); + return xi; +} //****************************************************************************** template <> diff --git a/gtsam/geometry/SOt.h b/gtsam/geometry/SOt.h index 4b62d92e3..52d000c93 100644 --- a/gtsam/geometry/SOt.h +++ b/gtsam/geometry/SOt.h @@ -39,12 +39,15 @@ using SO3 = SO<3>; // /// Static, named constructor that finds chordal mean = argmin_R \sum // sqr(|R-R_i|_F). static SO3 ChordalMean(const std::vector& rotations); -// 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. +template <> +Matrix3 SO3::Hat(const Vector3& xi); ///< make skew symmetric matrix + +template <> +Vector3 SO3::Vee(const Matrix3& X); ///< inverse of Hat + /// Adjoint map template <> Matrix3 SO3::AdjointMap() const { diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index d6ca68ccf..f7a936a74 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -44,10 +44,27 @@ TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } //****************************************************************************** SO3 id; -Vector3 z_axis(0, 0, 1); +Vector3 z_axis(0, 0, 1), v2(1, 2, 0), v3(1, 2, 3); SO3 R1(Eigen::AngleAxisd(0.1, z_axis)); SO3 R2(Eigen::AngleAxisd(0.2, z_axis)); +//****************************************************************************** +TEST(SO3, Hat) { + // Check that Hat specialization is equal to dynamic version + EXPECT(assert_equal(SO3::Hat(z_axis), SOn::Hat(z_axis))); + EXPECT(assert_equal(SO3::Hat(v2), SOn::Hat(v2))); + EXPECT(assert_equal(SO3::Hat(v3), SOn::Hat(v3))); +} + +//****************************************************************************** +TEST(SO3, Vee) { + // Check that Hat specialization is equal to dynamic version + auto X1 = SOn::Hat(z_axis), X2 = SOn::Hat(v2), X3 = SOn::Hat(v3); + EXPECT(assert_equal(SO3::Vee(X1), SOn::Vee(X1))); + EXPECT(assert_equal(SO3::Vee(X2), SOn::Vee(X2))); + EXPECT(assert_equal(SO3::Vee(X3), SOn::Vee(X3))); +} + //****************************************************************************** TEST(SO3, Local) { Vector3 expected(0, 0, 0.1);