diff --git a/gtsam/geometry/SO4.h b/gtsam/geometry/SO4.h index a92cdfc5c..49b8b5183 100644 --- a/gtsam/geometry/SO4.h +++ b/gtsam/geometry/SO4.h @@ -104,9 +104,9 @@ Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H = boost::none); */ template <> -struct traits : Testable, internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; template <> -struct traits : Testable, internal::LieGroupTraits {}; +struct traits : public internal::LieGroup {}; } // end namespace gtsam diff --git a/gtsam/geometry/SOn.h b/gtsam/geometry/SOn.h index 56cdeb3fe..f5696d765 100644 --- a/gtsam/geometry/SOn.h +++ b/gtsam/geometry/SOn.h @@ -27,6 +27,7 @@ #include #include +#include namespace gtsam { @@ -91,6 +92,17 @@ class SO : public LieGroup, internal::DimensionSO(N)> { template > SO(const Eigen::AngleAxisd& angleAxis) : matrix_(angleAxis) {} + /// Constructor from axis and angle. Only defined for SO3 + static SO AxisAngle(const Vector3& axis, double theta); + + /// Named constructor that finds SO(n) matrix closest to M in Frobenius norm, + /// currently only defined for SO3. + static SO ClosestTo(const MatrixNN& M); + + /// Named constructor that finds chordal mean = argmin_R \sum sqr(|R-R_i|_F), + /// currently only defined for SO3. + static SO ChordalMean(const std::vector& rotations); + /// Random SO(n) element (no big claims about uniformity) template > static SO Random(boost::mt19937& rng, size_t n = 0) { diff --git a/gtsam/geometry/SOt.cpp b/gtsam/geometry/SOt.cpp index a5e8f457d..9fc3aac42 100644 --- a/gtsam/geometry/SOt.cpp +++ b/gtsam/geometry/SOt.cpp @@ -134,29 +134,33 @@ Vector3 DexpFunctor::applyInvDexp(const Vector3& v, OptionalJacobian<3, 3> H1, } // namespace sot //****************************************************************************** -// SO3 SO3::AxisAngle(const Vector3& axis, double theta) { -// return sot::ExpmapFunctor(axis, theta).expmap(); -// } +template <> +SO3 SO3::AxisAngle(const Vector3& axis, double theta) { + return sot::ExpmapFunctor(axis, theta).expmap(); +} //****************************************************************************** -// SO3 SO3::ClosestTo(const Matrix3& M) { -// Eigen::JacobiSVD svd(M, Eigen::ComputeFullU | -// Eigen::ComputeFullV); const auto& U = svd.matrixU(); const auto& V = -// svd.matrixV(); const double det = (U * V.transpose()).determinant(); return -// U * Vector3(1, 1, det).asDiagonal() * V.transpose(); -// } +template <> +SO3 SO3::ClosestTo(const Matrix3& M) { + Eigen::JacobiSVD svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV); + const auto& U = svd.matrixU(); + const auto& V = svd.matrixV(); + const double det = (U * V.transpose()).determinant(); + return SO3(U * Vector3(1, 1, det).asDiagonal() * V.transpose()); +} //****************************************************************************** -// SO3 SO3::ChordalMean(const std::vector& rotations) { -// // See Hartley13ijcv: -// // Cost function C(R) = \sum sqr(|R-R_i|_F) -// // Closed form solution = ClosestTo(C_e), where C_e = \sum R_i !!!! -// Matrix3 C_e{Z_3x3}; -// for (const auto& R_i : rotations) { -// C_e += R_i; -// } -// return ClosestTo(C_e); -// } +template <> +SO3 SO3::ChordalMean(const std::vector& rotations) { + // See Hartley13ijcv: + // Cost function C(R) = \sum sqr(|R-R_i|_F) + // Closed form solution = ClosestTo(C_e), where C_e = \sum R_i !!!! + Matrix3 C_e{Z_3x3}; + for (const auto& R_i : rotations) { + C_e += R_i.matrix(); + } + return ClosestTo(C_e); +} //****************************************************************************** template <> diff --git a/gtsam/geometry/SOt.h b/gtsam/geometry/SOt.h index 52d000c93..f94d5cc98 100644 --- a/gtsam/geometry/SOt.h +++ b/gtsam/geometry/SOt.h @@ -33,15 +33,18 @@ namespace gtsam { using SO3 = SO<3>; -// /// Static, named constructor that finds SO(3) matrix closest to M in -// Frobenius norm. static SO3 ClosestTo(const Matrix3& M); - -// /// Static, named constructor that finds chordal mean = argmin_R \sum -// sqr(|R-R_i|_F). static SO3 ChordalMean(const std::vector& rotations); - // Below are all declarations of SO<3> specializations. // They are *defined* in SO3.cpp. +template <> +SO3 SO3::AxisAngle(const Vector3& axis, double theta); + +template <> +SO3 SO3::ClosestTo(const Matrix3& M); + +template <> +SO3 SO3::ChordalMean(const std::vector& rotations); + template <> Matrix3 SO3::Hat(const Vector3& xi); ///< make skew symmetric matrix diff --git a/gtsam/geometry/tests/testSO3.cpp b/gtsam/geometry/tests/testSO3.cpp index f7a936a74..3d68fda7f 100644 --- a/gtsam/geometry/tests/testSO3.cpp +++ b/gtsam/geometry/tests/testSO3.cpp @@ -40,7 +40,30 @@ TEST(SO3, Concept) { } //****************************************************************************** -TEST(SO3, Constructor) { SO3 q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } +TEST(SO3, Constructors) { + const Vector3 axis(0, 0, 1); + const double angle = 2.5; + SO3 Q(Eigen::AngleAxisd(angle, axis)); + SO3 R = SO3::AxisAngle(axis, angle); + EXPECT(assert_equal(Q, R)); +} + +/* ************************************************************************* */ +TEST(SO3, ClosestTo) { + // Example top-left of SO(4) matrix not quite on SO(3) manifold + Matrix3 M; + M << 0.79067393, 0.6051136, -0.0930814, // + 0.4155925, -0.64214347, -0.64324489, // + -0.44948549, 0.47046326, -0.75917576; + + Matrix expected(3, 3); + expected << 0.790687, 0.605096, -0.0931312, // + 0.415746, -0.642355, -0.643844, // + -0.449411, 0.47036, -0.759468; + + auto actual = SO3::ClosestTo(3 * M); + EXPECT(assert_equal(expected, actual.matrix(), 1e-6)); +} //****************************************************************************** SO3 id; @@ -48,6 +71,12 @@ 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, ChordalMean) { + std::vector rotations = {R1, R1.inverse()}; + EXPECT(assert_equal(SO3(), SO3::ChordalMean(rotations))); +} + //****************************************************************************** TEST(SO3, Hat) { // Check that Hat specialization is equal to dynamic version