borglab · dellaert · Sep 28, 2020 · May 20, 2019 · Sep 19, 2019 · Sep 19, 2019
diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp
@@ -75,6 +75,19 @@ double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
   return p.x() * q.x() + p.y() * q.y() + p.z() * q.z();
 }
 
+Point3Pair mean(const std::vector<Point3Pair> &abPointPairs) {
+  const size_t n = abPointPairs.size();
+  Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0);
+  for (const Point3Pair &abPair : abPointPairs) {
+    aCentroid += abPair.first;
+    bCentroid += abPair.second;
+  }
+  const double f = 1.0 / n;
+  aCentroid *= f;
+  bCentroid *= f;
+  return make_pair(aCentroid, bCentroid);
+}
+
 /* ************************************************************************* */
 ostream &operator<<(ostream &os, const gtsam::Point3Pair &p) {
   os << p.first << " <-> " << p.second;

diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
@@ -26,6 +26,7 @@
 #include <gtsam/base/Vector.h>
 #include <gtsam/dllexport.h>
 #include <boost/serialization/nvp.hpp>
+#include <numeric>
 
 namespace gtsam {
 
@@ -58,6 +59,17 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
                         OptionalJacobian<1, 3> H_p = boost::none,
                         OptionalJacobian<1, 3> H_q = boost::none);
 
+/// mean
+template <class CONTAINER>
+GTSAM_EXPORT Point3 mean(const CONTAINER& points) {
+  Point3 sum(0, 0, 0);
+  sum = std::accumulate(points.begin(), points.end(), sum);
+  return sum / points.size();
+}
+
+/// mean of Point3 pair 
+GTSAM_EXPORT Point3Pair mean(const std::vector<Point3Pair>& abPointPairs);
+
 template <typename A1, typename A2>
 struct Range;
 

diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
@@ -368,6 +368,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
+using Pose3Pair = std::pair<Pose3, Pose3>;
+
 // For MATLAB wrapper
 typedef std::vector<Pose3> Pose3Vector;
 

diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp
@@ -164,6 +164,26 @@ TEST (Point3, normalize) {
   EXPECT(assert_equal(expectedH, actualH, 1e-8));
 }
 
+//*************************************************************************
+TEST(Point3, mean) {
+  Point3 expected_a_mean(2, 2, 2);
+  Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3);
+  std::vector<Point3> a_points{a1, a2, a3};
+  Point3 actual_a_mean = mean(a_points);
+  EXPECT(assert_equal(expected_a_mean, actual_a_mean));
+}
+
+TEST(Point3, mean_pair) {
+  Point3 a_mean(2, 2, 2), b_mean(-1, 1, 0);
+  Point3Pair expected_mean = std::make_pair(a_mean, b_mean);
+  Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3);
+  Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0);
+  std::vector<Point3Pair> point_pairs{{a1,b1},{a2,b2},{a3,b3}};
+  Point3Pair actual_mean = mean(point_pairs);
+  EXPECT(assert_equal(expected_mean.first, actual_mean.first));
+  EXPECT(assert_equal(expected_mean.second, actual_mean.second));
+}
+
 //*************************************************************************
 double norm_proxy(const Point3& point) {
   return double(point.norm());

diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp
@@ -19,9 +19,61 @@
 
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/Manifold.h>
+#include <gtsam/slam/KarcherMeanFactor-inl.h>
 
 namespace gtsam {
 
+namespace {
+    /// Subtract centroids from point pairs.
+static std::vector<Point3Pair> subtractCentroids(const std::vector<Point3Pair>& abPointPairs, const Point3Pair& centroids) {
+  std::vector<Point3Pair> d_abPointPairs;
+  for (const Point3Pair& abPair : abPointPairs) {
+    Point3 da = abPair.first - centroids.first;
+    Point3 db = abPair.second - centroids.second;
+    d_abPointPairs.emplace_back(da, db);
+  }
+  return d_abPointPairs;
+}
+
+/// Form inner products x and y and calculate scale.
+static const double calculateScale(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb) {
+  double x = 0, y = 0;
+  Point3 da, db;
+  for (const Point3Pair& d_abPair : d_abPointPairs) {
+    std::tie(da, db) = d_abPair;
+    const Vector3 da_prime = aRb * db;
+    y += da.transpose() * da_prime;
+    x += da_prime.transpose() * da_prime;
+  }
+  const double s = y / x;
+  return s;
+}
+
+/// Form outer product H.
+static Matrix3 calculateH(const std::vector<Point3Pair>& d_abPointPairs) {
+  Matrix3 H = Z_3x3;
+  for (const Point3Pair& d_abPair : d_abPointPairs) {
+    H += d_abPair.first * d_abPair.second.transpose();
+  }
+  return H;
+}
+
+/// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids.
+static Similarity3 align(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb, const Point3Pair& centroids) {
+  const double s = calculateScale(d_abPointPairs, aRb);
+  const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
+  return Similarity3(aRb, aTb, s);
+}
+
+/// This method estimates the similarity transform from point pairs, given a known or estimated rotation.
+// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
+static Similarity3 alignGivenR(const std::vector<Point3Pair>& abPointPairs, const Rot3& aRb) {
+  auto centroids = mean(abPointPairs);
+  auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
+  return align(d_abPointPairs, aRb, centroids);
+}
+}  // namespace
+
 Similarity3::Similarity3() :
     t_(0,0,0), s_(1) {
 }
@@ -54,15 +106,15 @@ bool Similarity3::operator==(const Similarity3& other) const {
 void Similarity3::print(const std::string& s) const {
   std::cout << std::endl;
   std::cout << s;
-  rotation().print("R:\n");
-  std::cout << "t: " << translation().transpose() << "s: " << scale() << std::endl;
+  rotation().print("\nR:\n");
+  std::cout << "t: " << translation().transpose() << " s: " << scale() << std::endl;
 }
 
 Similarity3 Similarity3::identity() {
   return Similarity3();
 }
-Similarity3 Similarity3::operator*(const Similarity3& T) const {
-  return Similarity3(R_ * T.R_, ((1.0 / T.s_) * t_) + R_ * T.t_, s_ * T.s_);
+Similarity3 Similarity3::operator*(const Similarity3& S) const {
+  return Similarity3(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
 }
 
 Similarity3 Similarity3::inverse() const {
@@ -85,10 +137,47 @@ 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_ * T.translation() + t_));
+  return Pose3(R, t);
+}
+
 Point3 Similarity3::operator*(const Point3& p) const {
   return transformFrom(p);
 }
 
+Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
+  // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
+  if (abPointPairs.size() < 3) throw std::runtime_error("input should have at least 3 pairs of points");
+  auto centroids = mean(abPointPairs);
+  auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
+  Matrix3 H = calculateH(d_abPointPairs);
+  // ClosestTo finds rotation matrix closest to H in Frobenius sense
+  Rot3 aRb = Rot3::ClosestTo(H);
+  return align(d_abPointPairs, aRb, centroids);
+}
+
+Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) {
+  const size_t n = abPosePairs.size();
+  if (n < 2) throw std::runtime_error("input should have at least 2 pairs of poses");
+
+  // calculate rotation
+  vector<Rot3> rotations;
+  vector<Point3Pair> abPointPairs;
+  rotations.reserve(n);
+  abPointPairs.reserve(n);
+  Pose3 wTa, wTb;
+  for (const Pose3Pair& abPair : abPosePairs) {
+    std::tie(wTa, wTb) = abPair;
+    rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse()));
+    abPointPairs.emplace_back(wTa.translation(), wTb.translation());
+  }
+  const Rot3 aRb = FindKarcherMean<Rot3>(rotations);
+
+  return alignGivenR(abPointPairs, aRb);
+}
+
 Matrix4 Similarity3::wedge(const Vector7& xi) {
   // http://www.ethaneade.org/latex2html/lie/node29.html
   const auto w = xi.head<3>();

diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h
@@ -19,10 +19,12 @@
 
 #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>
 
+
 namespace gtsam {
 
 // Forward declarations
@@ -87,7 +89,7 @@ class Similarity3: public LieGroup<Similarity3, 7> {
   GTSAM_UNSTABLE_EXPORT static Similarity3 identity();
 
   /// Composition
-  GTSAM_UNSTABLE_EXPORT Similarity3 operator*(const Similarity3& T) const;
+  GTSAM_UNSTABLE_EXPORT Similarity3 operator*(const Similarity3& S) const;
 
   /// Return the inverse
   GTSAM_UNSTABLE_EXPORT Similarity3 inverse() const;
@@ -101,9 +103,32 @@ class Similarity3: public LieGroup<Similarity3, 7> {
       OptionalJacobian<3, 7> H1 = boost::none, //
       OptionalJacobian<3, 3> H2 = boost::none) const;
 
+  /** 
+   * Action on a pose T.
+   * |Rs  ts|   |R t|   |Rs*R Rs*t+ts| 
+   * |0  1/s| * |0 1| = | 0      1/s |, the result is still a Sim3 object.
+   * To retrieve a Pose3, we normalized the scale value into 1.
+   * |Rs*R Rs*t+ts|   |Rs*R s(Rs*t+ts)|
+   * | 0      1/s | = |  0       1    |
+   * 
+   * This group action satisfies the compatibility condition. 
+   * For more details, refer to: https://en.wikipedia.org/wiki/Group_action
+   */
+  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 at least three point pairs
+   */
+  GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
+
+  /**
+   *  Create Similarity3 by aligning at least two pose pairs
+   */
+  GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
+
   /// @}
   /// @name Lie Group
   /// @{
@@ -182,8 +207,8 @@ class Similarity3: public LieGroup<Similarity3, 7> {
   /// @name Helper functions
   /// @{
 
-  /// Calculate expmap and logmap coefficients.
 private:
+  /// Calculate expmap and logmap coefficients.
   static Matrix3 GetV(Vector3 w, double lambda);
 
   /// @}