Closest

gtsam/geometry/Pose3.cpp

@@ -52,7 +52,6 @@ Pose3 Pose3::inverse() const {
 /* ************************************************************************* */
 // Calculate Adjoint map
 // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
-// Experimental - unit tests of derivatives based on it do not check out yet
 Matrix6 Pose3::AdjointMap() const {
   const Matrix3 R = R_.matrix();
   Matrix3 A = skewSymmetric(t_.x(), t_.y(), t_.z()) * R;
@@ -418,24 +417,11 @@ boost::optional<Pose3> Pose3::Align(const std::vector<Point3Pair>& abPointPairs)
   for(const Point3Pair& abPair: abPointPairs) {
     Point3 da = abPair.first - aCentroid;
     Point3 db = abPair.second - bCentroid;
-    H += db * da.transpose();
+    H += da * db.transpose();
     }
 
-  // Compute SVD
-  Eigen::JacobiSVD<Matrix> svd(H, Eigen::ComputeThinU | Eigen::ComputeThinV);
-  Matrix U = svd.matrixU();
-  Vector S = svd.singularValues();
-  Matrix V = svd.matrixV();
-
-  // Check rank
-  if (S[1] < 1e-10)
-    return boost::none;
-
-  // Recover transform with correction from Eggert97machinevisionandapplications
-  Matrix3 UVtranspose = U * V.transpose();
-  Matrix3 detWeighting = I_3x3;
-  detWeighting(2, 2) = UVtranspose.determinant();
-  Rot3 aRb(Matrix(V * detWeighting * U.transpose()));
+  // ClosestTo finds rotation matrix closest to H in Frobenius sense
+  Rot3 aRb = Rot3::ClosestTo(H);
   Point3 aTb = Point3(aCentroid) - aRb * Point3(bCentroid);
   return Pose3(aRb, aTb);
 }

gtsam/geometry/Rot3.h

@@ -228,6 +228,9 @@ namespace gtsam {
     static Rot3 AlignTwoPairs(const Unit3& a_p, const Unit3& b_p,  //
                               const Unit3& a_q, const Unit3& b_q);
 
+    /// Static, named constructor that finds Rot3 element closest to M in Frobenius norm.
+    static Rot3 ClosestTo(const Matrix3& M) { return Rot3(SO3::ClosestTo(M)); }
+
     /// @}
     /// @name Testable
     /// @{

gtsam/geometry/tests/testRot3.cpp

@@ -645,6 +645,23 @@ TEST(Rot3 , ChartDerivatives) {
   }
 }
 
+/* ************************************************************************* */
+TEST(Rot3, 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 = Rot3::ClosestTo(3*M);
+  EXPECT(assert_equal(expected, actual.matrix(), 1e-6));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam/slam/InitializePose3.cpp

@@ -75,27 +75,15 @@ Values InitializePose3::normalizeRelaxedRotations(
     const VectorValues& relaxedRot3) {
   gttic(InitializePose3_computeOrientationsChordal);
 
-  Matrix ppm = Z_3x3; // plus plus minus
-  ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
-
   Values validRot3;
   for(const auto& it: relaxedRot3) {
     Key key = it.first;
     if (key != kAnchorKey) {
-      Matrix3 R;
-      R << Eigen::Map<const Matrix3>(it.second.data()); // Recover M from vectorized
+      Matrix3 M;
+      M << Eigen::Map<const Matrix3>(it.second.data()); // Recover M from vectorized
 
       // ClosestTo finds rotation matrix closest to H in Frobenius sense
-      // Rot3 initRot = Rot3::ClosestTo(M.transpose());
-
-      Matrix U, V; Vector s;
-      svd(R.transpose(), U, s, V);
-      Matrix3 normalizedRotMat = U * V.transpose();
-
-      if(normalizedRotMat.determinant() < 0)
-        normalizedRotMat = U * ppm * V.transpose();
-
-      Rot3 initRot = Rot3(normalizedRotMat);
+      Rot3 initRot = Rot3::ClosestTo(M.transpose());
       validRot3.insert(key, initRot);
     }
   }