Finish Sim3 align and transformFrom functions.

gtsam/geometry/Pose3.h

@@ -356,6 +356,9 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
   return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5));
 }
 
+// Convenience typedef
+typedef std::pair<Pose3, Pose3> Pose3Pair;
+
 // For MATLAB wrapper
 typedef std::vector<Pose3> Pose3Vector;
 

gtsam_unstable/geometry/Similarity3.cpp

@@ -85,10 +85,105 @@ Point3 Similarity3::transformFrom(const Point3& p, //
   return s_ * q;
 }
 
+Pose3 Similarity3::transformFrom(const Pose3& T) const {
+  Rot3 R = R_.compose(T.rotation());
+  Point3 t = Point3(s_ * (R_.matrix() * T.translation().vector() + t_.vector()));
+  return Pose3(R, t);
+}
+
 Point3 Similarity3::operator*(const Point3& p) const {
   return transformFrom(p);
 }
 
+Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
+  const size_t n = abPointPairs.size();
+  if (n < 3) throw std::runtime_error("less than 3 pairs");  // we need at least three pairs
+
+  // calculate centroids
+  Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0);
+  for (const Point3Pair& abPair : abPointPairs) {
+    aCentroid += abPair.first;
+    bCentroid += abPair.second;
+  }
+  double f = 1.0 / n;
+  aCentroid *= f;
+  bCentroid *= f;
+
+  // Add to form H matrix
+  Matrix3 H = Z_3x3;
+  for (const Point3Pair& abPair : abPointPairs) {
+    Point3 da = abPair.first - aCentroid;
+    Point3 db = abPair.second - bCentroid;
+    H += da * db.transpose();
+  }
+
+  // ClosestTo finds rotation matrix closest to H in Frobenius sense
+  Rot3 aRb = Rot3::ClosestTo(H);
+
+  // Calculate scale
+  double x = 0;
+  double y = 0;
+  for (const Point3Pair& abPair : abPointPairs) {
+    Point3 da = abPair.first - aCentroid;
+    Point3 db = abPair.second - bCentroid;
+    Vector3 Rdb = aRb * db;
+    y += da.transpose() * Rdb;
+    x += Rdb.transpose() * Rdb;
+  }
+  double s = y / x;
+
+  Point3 aTb = (Point3(aCentroid) - s * (aRb * Point3(bCentroid))) / s;
+  return Similarity3(aRb, aTb, s);
+}
+
+Rot3 Similarity3::rotationAveraging(const std::vector<Rot3>& rotations, double error) {
+  Rot3 R = rotations[0];
+  const size_t n = rotations.size();
+  Vector3 r;
+  r << 0, 0, 0;
+  while (1) {
+    for (const Rot3 R_i : rotations) {
+      r += Rot3::Logmap(R.inverse().compose(R_i));
+    }
+    r /= n;
+    if (r.norm() < error) return R;
+    R = R.compose(Rot3::Expmap(r));
+  }
+}
+
+Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) {
+  const size_t n = abPosePairs.size();
+  if (n < 2) throw std::runtime_error("less than 2 pairs");  // we need at least two pairs
+
+  // calculate centroids
+  Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0);
+  vector<Rot3> rotationList;
+  for (const Pose3Pair& abPair : abPosePairs) {
+    aCentroid += abPair.first.translation();
+    bCentroid += abPair.second.translation();
+    rotationList.push_back(abPair.first.rotation().compose(abPair.second.rotation().inverse()));
+  }
+  double f = 1.0 / n;
+  aCentroid *= f;
+  bCentroid *= f;
+  Rot3 aRb = Similarity3::rotationAveraging(rotationList);
+
+  // Calculate scale
+  double x = 0;
+  double y = 0;
+  for (const Pose3Pair& abPair : abPosePairs) {
+    Point3 da = abPair.first.translation() - aCentroid;
+    Point3 db = abPair.second.translation() - bCentroid;
+    Vector3 Rdb = aRb * db;
+    y += da.transpose() * Rdb;
+    x += Rdb.transpose() * Rdb;
+  }
+  double s = y / x;
+
+  Point3 aTb = (Point3(aCentroid) - s * (aRb * Point3(bCentroid))) / s;
+  return Similarity3(aRb, aTb, s);
+}
+
 Matrix4 Similarity3::wedge(const Vector7& xi) {
   // http://www.ethaneade.org/latex2html/lie/node29.html
   const auto w = xi.head<3>();

gtsam_unstable/geometry/Similarity3.h

@@ -19,6 +19,7 @@
 
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/Lie.h>
 #include <gtsam/base/Manifold.h>
 #include <gtsam_unstable/dllexport.h>
@@ -101,9 +102,27 @@ public:
       OptionalJacobian<3, 7> H1 = boost::none, //
       OptionalJacobian<3, 3> H2 = boost::none) const;
 
+  /// Action on a pose T
+  GTSAM_UNSTABLE_EXPORT Pose3 transformFrom(const Pose3& T) const;
+
   /** syntactic sugar for transformFrom */
   GTSAM_UNSTABLE_EXPORT Point3 operator*(const Point3& p) const;
 
+  /**
+   *  Create Similarity3 by aligning two point pairs
+   */
+  GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
+
+  /**
+   *  Calculate the average rotation from a list of rotations
+   */
+  GTSAM_UNSTABLE_EXPORT static Rot3 rotationAveraging(const std::vector<Rot3>& rotations, double error = 1e-10);
+
+  /**
+   *  Create Similarity3 by aligning two pose pairs
+   */
+  GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
+
   /// @}
   /// @name Lie Group
   /// @{

gtsam_unstable/geometry/tests/testSimilarity3.cpp

@@ -51,6 +51,8 @@ static const Similarity3 T4(R, P, s);
 static const Similarity3 T5(R, P, 10);
 static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform
 
+const double degree = M_PI / 180;
+
 //******************************************************************************
 TEST(Similarity3, Concepts) {
   BOOST_CONCEPT_ASSERT((IsGroup<Similarity3 >));
@@ -255,6 +257,107 @@ TEST(Similarity3, GroupAction) {
   }
 }
 
+//******************************************************************************
+// Group action on Pose3
+TEST(Similarity3, GroupActionPose3) {
+  Similarity3 bTa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
+
+  // Create source poses
+  Pose3 Ta1 = Pose3(Rot3(), Point3(0, 0, 0));
+  Pose3 Ta2 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0));
+
+  // Create destination poses
+  Pose3 expectedTb1 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
+  Pose3 expectedTb2 = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10));
+
+  EXPECT(assert_equal(expectedTb1, bTa.transformFrom(Ta1)));
+  EXPECT(assert_equal(expectedTb2, bTa.transformFrom(Ta2)));
+}
+
+//******************************************************************************
+// Align with Point3 Pairs
+TEST(Similarity3, AlignPoint3_1) {
+  Similarity3 expected(Rot3::Rz(-90 * degree), Point3(3, 4, 5), 2.0);
+
+  Point3 p1 = Point3(0, 0, 0);
+  Point3 p2 = Point3(3, 0, 0);
+  Point3 p3 = Point3(3, 0, 4);
+
+  Point3Pair ab1(make_pair(expected.transformFrom(p1), p1));
+  Point3Pair ab2(make_pair(expected.transformFrom(p2), p2));
+  Point3Pair ab3(make_pair(expected.transformFrom(p3), p3));
+
+  vector<Point3Pair> correspondences{ab1, ab2, ab3};
+
+  Similarity3 actual = Similarity3::Align(correspondences);
+  EXPECT(assert_equal(expected, actual));
+}
+
+TEST(Similarity3, AlignPoint3_2) {
+  Similarity3 expected(Rot3(), Point3(10, 10, 0), 1.0);
+
+  Point3 p1 = Point3(0, 0, 0);
+  Point3 p2 = Point3(20, 10, 0);
+  Point3 p3 = Point3(10, 20, 0);
+
+  Point3Pair ab1(make_pair(expected.transformFrom(p1), p1));
+  Point3Pair ab2(make_pair(expected.transformFrom(p2), p2));
+  Point3Pair ab3(make_pair(expected.transformFrom(p3), p3));
+
+  vector<Point3Pair> correspondences{ab1, ab2, ab3};
+
+  Similarity3 actual = Similarity3::Align(correspondences);
+  EXPECT(assert_equal(expected, actual));
+}
+
+TEST(Similarity3, AlignPoint3_3) {
+  Similarity3 expected(Rot3::RzRyRx(0.3, 0.2, 0.1), Point3(20, 10, 5), 1.0);
+
+  Point3 p1 = Point3(0, 0, 1);
+  Point3 p2 = Point3(10, 0, 2);
+  Point3 p3 = Point3(20, -10, 30);
+
+  Point3Pair ab1(make_pair(expected.transformFrom(p1), p1));
+  Point3Pair ab2(make_pair(expected.transformFrom(p2), p2));
+  Point3Pair ab3(make_pair(expected.transformFrom(p3), p3));
+
+  vector<Point3Pair> correspondences{ab1, ab2, ab3};
+
+  Similarity3 actual = Similarity3::Align(correspondences);
+  EXPECT(assert_equal(expected, actual));
+}
+
+//******************************************************************************
+// Rotation Averaging
+TEST(Similarity3, RotationAveraging) {
+  Rot3 expected = Rot3::Rx(90 * degree);
+  vector<Rot3> rotations{Rot3(), Rot3::Rx(90 * degree), Rot3::Rx(180 * degree)};
+  Rot3 actual = Similarity3::rotationAveraging(rotations);
+  EXPECT(assert_equal(expected, actual));
+}
+
+//******************************************************************************
+// Align with Pose3 Pairs
+TEST(Similarity3, AlignPose3) {
+  Similarity3 expected(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
+
+  // Create source poses
+  Pose3 Ta1 = Pose3(Rot3(), Point3(0, 0, 0));
+  Pose3 Ta2 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0));
+
+  // Create destination poses
+  Pose3 Tb1 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
+  Pose3 Tb2 = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10));
+
+  Pose3Pair bTa1(make_pair(Tb1, Ta1));
+  Pose3Pair bTa2(make_pair(Tb2, Ta2));
+
+  vector<Pose3Pair> correspondences{bTa1, bTa2};
+
+  Similarity3 actual = Similarity3::Align(correspondences);
+  EXPECT(assert_equal(expected, actual));
+}
+
 //******************************************************************************
 // Test very simple prior optimization example
 TEST(Similarity3, Optimization) {