Three more constructors for SO3

gtsam/geometry/SO4.h

@@ -104,9 +104,9 @@ Matrix43 stiefel(const SO4& Q, OptionalJacobian<12, 6> H = boost::none);
  */
 
 template <>
-struct traits<SO4> : Testable<SO4>, internal::LieGroupTraits<SO4> {};
+struct traits<SO4> : public internal::LieGroup<SO4> {};
 
 template <>
-struct traits<const SO4> : Testable<SO4>, internal::LieGroupTraits<SO4> {};
+struct traits<const SO4> : public internal::LieGroup<SO4> {};
 
 }  // end namespace gtsam

gtsam/geometry/SOn.h

@@ -27,6 +27,7 @@
 
 #include <iostream>
 #include <string>
+#include <vector>
 
 namespace gtsam {
 
@@ -91,6 +92,17 @@ class SO : public LieGroup<SO<N>, internal::DimensionSO(N)> {
   template <int N_ = N, typename = IsSO3<N_>>
   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<SO>& rotations);
+
   /// Random SO(n) element (no big claims about uniformity)
   template <int N_ = N, typename = IsDynamic<N_>>
   static SO Random(boost::mt19937& rng, size_t n = 0) {

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) {
+template <>
-//   return sot::ExpmapFunctor(axis, theta).expmap();
+SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
-// }
+  return sot::ExpmapFunctor(axis, theta).expmap();
+}
 
 //******************************************************************************
-// SO3 SO3::ClosestTo(const Matrix3& M) {
+template <>
-//   Eigen::JacobiSVD<Matrix3> svd(M, Eigen::ComputeFullU |
+SO3 SO3::ClosestTo(const Matrix3& M) {
-//   Eigen::ComputeFullV); const auto& U = svd.matrixU(); const auto& V =
+  Eigen::JacobiSVD<Matrix3> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
-//   svd.matrixV(); const double det = (U * V.transpose()).determinant(); return
+  const auto& U = svd.matrixU();
-//   U * Vector3(1, 1, det).asDiagonal() * V.transpose();
+  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<SO3>& rotations) {
+template <>
-//   //  See Hartley13ijcv:
+SO3 SO3::ChordalMean(const std::vector<SO3>& rotations) {
-//   //  Cost function C(R) = \sum sqr(|R-R_i|_F)
+  // See Hartley13ijcv:
-//   // Closed form solution = ClosestTo(C_e), where C_e = \sum R_i !!!!
+  // Cost function C(R) = \sum sqr(|R-R_i|_F)
-//   Matrix3 C_e{Z_3x3};
+  // Closed form solution = ClosestTo(C_e), where C_e = \sum R_i !!!!
-//   for (const auto& R_i : rotations) {
+  Matrix3 C_e{Z_3x3};
-//     C_e += R_i;
+  for (const auto& R_i : rotations) {
-//   }
+    C_e += R_i.matrix();
-//   return ClosestTo(C_e);
+  }
-// }
+  return ClosestTo(C_e);
+}
 
 //******************************************************************************
 template <>

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<SO3>& 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<SO3>& rotations);
+
 template <>
 Matrix3 SO3::Hat(const Vector3& xi);  ///< make skew symmetric matrix
 

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<SO3> rotations = {R1, R1.inverse()};
+  EXPECT(assert_equal(SO3(), SO3::ChordalMean(rotations)));
+}
+
 //******************************************************************************
 TEST(SO3, Hat) {
   // Check that Hat specialization is equal to dynamic version