Add Similarity(2) support in gtsam/geometry

gtsam/geometry/Point2.cpp

@@ -113,6 +113,18 @@ list<Point2> circleCircleIntersection(Point2 c1, double r1, Point2 c2,
   return circleCircleIntersection(c1, c2, fh);
 }
 
+Point2Pair means(const std::vector<Point2Pair> &abPointPairs) {
+  const size_t n = abPointPairs.size();
+  if (n == 0) throw std::invalid_argument("Point2::mean input Point2Pair vector is empty");
+  Point2 aSum(0, 0), bSum(0, 0);
+  for (const Point2Pair &abPair : abPointPairs) {
+    aSum += abPair.first;
+    bSum += abPair.second;
+  }
+  const double f = 1.0 / n;
+  return {aSum * f, bSum * f};
+}
+
 /* ************************************************************************* */
 ostream &operator<<(ostream &os, const gtsam::Point2Pair &p) {
   os << p.first << " <-> " << p.second;

gtsam/geometry/Point2.h

@@ -71,6 +71,9 @@ GTSAM_EXPORT boost::optional<Point2> circleCircleIntersection(double R_d, double
  * @return list of solutions (0,1, or 2). Identical circles will return empty list, as well.
  */
 GTSAM_EXPORT std::list<Point2> circleCircleIntersection(Point2 c1, Point2 c2, boost::optional<Point2> fh);
+
+/// Calculate the two means of a set of Point2 pairs
+GTSAM_EXPORT Point2Pair means(const std::vector<Point2Pair> &abPointPairs);
 
 /**
  * @brief Intersect 2 circles

gtsam/geometry/Pose2.h

@@ -322,6 +322,10 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
 typedef std::pair<Point2,Point2> Point2Pair;
 GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
 
+// Convenience typedef
+using Pose2Pair = std::pair<Pose2, Pose2>;
+using Pose2Pairs = std::vector<Pose2Pair>;
+
 template <>
 struct traits<Pose2> : public internal::LieGroup<Pose2> {};
 

gtsam/geometry/Rot2.cpp

@@ -129,6 +129,16 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
   }
 }
 
+/* ************************************************************************* */
+Rot2 Rot2::ClosestTo(const Matrix2& M) {
+  double c = M(0, 0);
+  double s = M(1, 0);
+  double theta_rad = std::atan2(s, c);
+  c = cos(theta_rad);
+  s = sin(theta_rad);
+  return Rot2::fromCosSin(c, s);
+}
+
 /* ************************************************************************* */
 
 } // gtsam

gtsam/geometry/Rot2.h

@@ -14,6 +14,7 @@
  * @brief 2D rotation
  * @date Dec 9, 2009
  * @author Frank Dellaert
+ * @author John Lambert
  */
 
 #pragma once
@@ -209,6 +210,9 @@ namespace gtsam {
     /** return 2*2 transpose (inverse) rotation matrix   */
     Matrix2 transpose() const;
 
+    /** Find closest valid rotation matrix, given a 2x2 matrix */
+    static Rot2 ClosestTo(const Matrix2& M);
+
   private:
     /** Serialization function */
     friend class boost::serialization::access;

gtsam/geometry/Similarity2.cpp

@@ -0,0 +1,216 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file   Similarity2.cpp
+ * @brief  Implementation of Similarity2 transform
+ * @author John Lambert
+ */
+
+#include <gtsam/base/Manifold.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Rot3.h>
+#include <gtsam/geometry/Similarity2.h>
+#include <gtsam/slam/KarcherMeanFactor-inl.h>
+
+namespace gtsam {
+
+using std::vector;
+
+namespace internal {
+
+/// Subtract centroids from point pairs.
+static Point2Pairs subtractCentroids(const Point2Pairs& abPointPairs,
+                                     const Point2Pair& centroids) {
+  Point2Pairs d_abPointPairs;
+  for (const Point2Pair& abPair : abPointPairs) {
+    Point2 da = abPair.first - centroids.first;
+    Point2 db = abPair.second - centroids.second;
+    d_abPointPairs.emplace_back(da, db);
+  }
+  return d_abPointPairs;
+}
+
+/// Form inner products x and y and calculate scale.
+static double calculateScale(const Point2Pairs& d_abPointPairs,
+                             const Rot2& aRb) {
+  double x = 0, y = 0;
+  Point2 da, db;
+
+  for (const Point2Pair& d_abPair : d_abPointPairs) {
+    std::tie(da, db) = d_abPair;
+    const Vector2 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 Matrix2 calculateH(const Point2Pairs& d_abPointPairs) {
+  Matrix2 H = Z_2x2;
+  for (const Point2Pair& d_abPair : d_abPointPairs) {
+    H += d_abPair.first * d_abPair.second.transpose();
+  }
+  return H;
+}
+
+/**
+ * @brief This method estimates the similarity transform from differences point
+ * pairs, given a known or estimated rotation and point centroids.
+ *
+ * @param d_abPointPairs
+ * @param aRb
+ * @param centroids
+ * @return Similarity2
+ */
+static Similarity2 align(const Point2Pairs& d_abPointPairs, const Rot2& aRb,
+                         const Point2Pair& centroids) {
+  const double s = calculateScale(d_abPointPairs, aRb);
+  // dividing aTb by s is required because the registration cost function
+  // minimizes ||a - sRb - t||, whereas Sim(2) computes s(Rb + t)
+  const Point2 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
+  return Similarity2(aRb, aTb, s);
+}
+
+/**
+ * @brief 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
+ *
+ * @param abPointPairs
+ * @param aRb
+ * @return Similarity2
+ */
+static Similarity2 alignGivenR(const Point2Pairs& abPointPairs,
+                               const Rot2& aRb) {
+  auto centroids = means(abPointPairs);
+  auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
+  return internal::align(d_abPointPairs, aRb, centroids);
+}
+}  // namespace internal
+
+Similarity2::Similarity2() : t_(0, 0), s_(1) {}
+
+Similarity2::Similarity2(double s) : t_(0, 0), s_(s) {}
+
+Similarity2::Similarity2(const Rot2& R, const Point2& t, double s)
+    : R_(R), t_(t), s_(s) {}
+
+Similarity2::Similarity2(const Matrix2& R, const Vector2& t, double s)
+    : R_(Rot2::ClosestTo(R)), t_(t), s_(s) {}
+
+Similarity2::Similarity2(const Matrix3& T)
+    : R_(Rot2::ClosestTo(T.topLeftCorner<2, 2>())),
+      t_(T.topRightCorner<2, 1>()),
+      s_(1.0 / T(2, 2)) {}
+
+bool Similarity2::equals(const Similarity2& other, double tol) const {
+  return R_.equals(other.R_, tol) &&
+         traits<Point2>::Equals(t_, other.t_, tol) && s_ < (other.s_ + tol) &&
+         s_ > (other.s_ - tol);
+}
+
+bool Similarity2::operator==(const Similarity2& other) const {
+  return R_.matrix() == other.R_.matrix() && t_ == other.t_ && s_ == other.s_;
+}
+
+void Similarity2::print(const std::string& s) const {
+  std::cout << std::endl;
+  std::cout << s;
+  rotation().print("\nR:\n");
+  std::cout << "t: " << translation().transpose() << " s: " << scale()
+            << std::endl;
+}
+
+Similarity2 Similarity2::identity() { return Similarity2(); }
+
+Similarity2 Similarity2::operator*(const Similarity2& S) const {
+  return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
+}
+
+Similarity2 Similarity2::inverse() const {
+  const Rot2 Rt = R_.inverse();
+  const Point2 sRt = Rt * (-s_ * t_);
+  return Similarity2(Rt, sRt, 1.0 / s_);
+}
+
+Point2 Similarity2::transformFrom(const Point2& p) const {
+  const Point2 q = R_ * p + t_;
+  return s_ * q;
+}
+
+Pose2 Similarity2::transformFrom(const Pose2& T) const {
+  Rot2 R = R_.compose(T.rotation());
+  Point2 t = Point2(s_ * (R_ * T.translation() + t_));
+  return Pose2(R, t);
+}
+
+Point2 Similarity2::operator*(const Point2& p) const {
+  return transformFrom(p);
+}
+
+Similarity2 Similarity2::Align(const Point2Pairs& abPointPairs) {
+  // Refer to Chapter 3 of
+  // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
+  if (abPointPairs.size() < 2)
+    throw std::runtime_error("input should have at least 2 pairs of points");
+  auto centroids = means(abPointPairs);
+  auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
+  Matrix2 H = internal::calculateH(d_abPointPairs);
+  // ClosestTo finds rotation matrix closest to H in Frobenius sense
+  Rot2 aRb = Rot2::ClosestTo(H);
+  return internal::align(d_abPointPairs, aRb, centroids);
+}
+
+Similarity2 Similarity2::Align(const Pose2Pairs& 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<Rot2> rotations;
+  Point2Pairs abPointPairs;
+  rotations.reserve(n);
+  abPointPairs.reserve(n);
+  // Below denotes the pose of the i'th object/camera/etc
+  // in frame "a" or frame "b".
+  Pose2 aTi, bTi;
+  for (const Pose2Pair& abPair : abPosePairs) {
+    std::tie(aTi, bTi) = abPair;
+    const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse());
+    rotations.emplace_back(aRb);
+    abPointPairs.emplace_back(aTi.translation(), bTi.translation());
+  }
+  const Rot2 aRb_estimate = FindKarcherMean<Rot2>(rotations);
+
+  return internal::alignGivenR(abPointPairs, aRb_estimate);
+}
+
+std::ostream& operator<<(std::ostream& os, const Similarity2& p) {
+  os << "[" << p.rotation().theta() << " " << p.translation().transpose() << " "
+     << p.scale() << "]\';";
+  return os;
+}
+
+Matrix3 Similarity2::matrix() const {
+  Matrix3 T;
+  T.topRows<2>() << R_.matrix(), t_;
+  T.bottomRows<1>() << 0, 0, 1.0 / s_;
+  return T;
+}
+
+Similarity2::operator Pose2() const { return Pose2(R_, s_ * t_); }
+
+}  // namespace gtsam