Merge pull request #918 from borglab/add-Similarity2-classes

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

@@ -72,6 +72,9 @@ GTSAM_EXPORT boost::optional<Point2> circleCircleIntersection(double R_d, double
  */
 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
  * @param c1 center of first circle

gtsam/geometry/Pose2.h

@@ -353,6 +353,10 @@ GTSAM_EXPORT boost::optional<Pose2>
 GTSAM_DEPRECATED align(const Point2Pairs& pairs);
 #endif
 
+// 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,19 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
   }
 }
 
+/* ************************************************************************* */
+Rot2 Rot2::ClosestTo(const Matrix2& M) {
+  Eigen::JacobiSVD<Matrix2> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
+  const Matrix2& U = svd.matrixU();
+  const Matrix2& V = svd.matrixV();
+  const double det = (U * V.transpose()).determinant();
+  Matrix2 M_prime = (U * Vector2(1, det).asDiagonal() * V.transpose());
+
+  double c = M_prime(0, 0);
+  double s = M_prime(1, 0);
+  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,242 @@
+/* ----------------------------------------------------------------------------
+
+ * 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, Varun Agrawal
+ */
+
+#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);
+}
+
+Vector4 Similarity2::Logmap(const Similarity2& S,  //
+                            OptionalJacobian<4, 4> Hm) {
+  const Vector2 u = S.t_;
+  const Vector1 w = Rot2::Logmap(S.R_);
+  const double s = log(S.s_);
+  Vector4 result;
+  result << u, w, s;
+  if (Hm) {
+    throw std::runtime_error("Similarity2::Logmap: derivative not implemented");
+  }
+  return result;
+}
+
+Similarity2 Similarity2::Expmap(const Vector4& v,  //
+                                OptionalJacobian<4, 4> Hm) {
+  const Vector2 t = v.head<2>();
+  const Rot2 R = Rot2::Expmap(v.segment<1>(2));
+  const double s = v[3];
+  if (Hm) {
+    throw std::runtime_error("Similarity2::Expmap: derivative not implemented");
+  }
+  return Similarity2(R, t, s);
+}
+
+Matrix4 Similarity2::AdjointMap() const {
+  throw std::runtime_error("Similarity2::AdjointMap not implemented");
+}
+
+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;
+}
+
+}  // namespace gtsam

gtsam/geometry/Similarity2.h

@@ -0,0 +1,200 @@
+/* ----------------------------------------------------------------------------
+
+ * 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.h
+ * @brief  Implementation of Similarity2 transform
+ * @author John Lambert, Varun Agrawal
+ */
+
+#pragma once
+
+#include <gtsam/base/Lie.h>
+#include <gtsam/base/Manifold.h>
+#include <gtsam/dllexport.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Rot2.h>
+
+namespace gtsam {
+
+// Forward declarations
+class Pose2;
+
+/**
+ * 2D similarity transform
+ */
+class GTSAM_EXPORT Similarity2 : public LieGroup<Similarity2, 4> {
+  /// @name Pose Concept
+  /// @{
+  typedef Rot2 Rotation;
+  typedef Point2 Translation;
+  /// @}
+
+ private:
+  Rot2 R_;
+  Point2 t_;
+  double s_;
+
+ public:
+  /// @name Constructors
+  /// @{
+
+  /// Default constructor
+  Similarity2();
+
+  /// Construct pure scaling
+  Similarity2(double s);
+
+  /// Construct from GTSAM types
+  Similarity2(const Rot2& R, const Point2& t, double s);
+
+  /// Construct from Eigen types
+  Similarity2(const Matrix2& R, const Vector2& t, double s);
+
+  /// Construct from matrix [R t; 0 s^-1]
+  Similarity2(const Matrix3& T);
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// Compare with tolerance
+  bool equals(const Similarity2& sim, double tol) const;
+
+  /// Exact equality
+  bool operator==(const Similarity2& other) const;
+
+  /// Print with optional string
+  void print(const std::string& s) const;
+
+  friend std::ostream& operator<<(std::ostream& os, const Similarity2& p);
+
+  /// @}
+  /// @name Group
+  /// @{
+
+  /// Return an identity transform
+  static Similarity2 identity();
+
+  /// Composition
+  Similarity2 operator*(const Similarity2& S) const;
+
+  /// Return the inverse
+  Similarity2 inverse() const;
+
+  /// @}
+  /// @name Group action on Point2
+  /// @{
+
+  /// Action on a point p is s*(R*p+t)
+  Point2 transformFrom(const Point2& p) 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 Sim2 object.
+   * To retrieve a Pose2, 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
+   */
+  Pose2 transformFrom(const Pose2& T) const;
+
+  /* syntactic sugar for transformFrom */
+  Point2 operator*(const Point2& p) const;
+
+  /**
+   *  Create Similarity2 by aligning at least two point pairs
+   */
+  static Similarity2 Align(const Point2Pairs& abPointPairs);
+
+  /**
+   * Create the Similarity2 object that aligns at least two pose pairs.
+   * Each pair is of the form (aTi, bTi).
+   * Given a list of pairs in frame a, and a list of pairs in frame b,
+   Align()
+   * will compute the best-fit Similarity2 aSb transformation to align them.
+   * First, the rotation aRb will be computed as the average (Karcher mean)
+   of
+   * many estimates aRb (from each pair). Afterwards, the scale factor will
+   be computed
+   * using the algorithm described here:
+   * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
+   */
+  static Similarity2 Align(const std::vector<Pose2Pair>& abPosePairs);
+
+  /// @}
+  /// @name Lie Group
+  /// @{
+
+  /**
+   * Log map at the identity
+   * \f$ [t_x, t_y, \delta, \lambda] \f$
+   */
+  static Vector4 Logmap(const Similarity2& S,  //
+                        OptionalJacobian<4, 4> Hm = boost::none);
+
+  /// Exponential map at the identity
+  static Similarity2 Expmap(const Vector4& v,  //
+                            OptionalJacobian<4, 4> Hm = boost::none);
+
+  /// Chart at the origin
+  struct ChartAtOrigin {
+    static Similarity2 Retract(const Vector4& v,
+                               ChartJacobian H = boost::none) {
+      return Similarity2::Expmap(v, H);
+    }
+    static Vector4 Local(const Similarity2& other,
+                         ChartJacobian H = boost::none) {
+      return Similarity2::Logmap(other, H);
+    }
+  };
+
+  /// Project from one tangent space to another
+  Matrix4 AdjointMap() const;
+
+  using LieGroup<Similarity2, 4>::inverse;
+
+  /// @}
+  /// @name Standard interface
+  /// @{
+
+  /// Calculate 4*4 matrix group equivalent
+  Matrix3 matrix() const;
+
+  /// Return a GTSAM rotation
+  Rot2 rotation() const { return R_; }
+
+  /// Return a GTSAM translation
+  Point2 translation() const { return t_; }
+
+  /// Return the scale
+  double scale() const { return s_; }
+
+  /// Dimensionality of tangent space = 4 DOF - used to autodetect sizes
+  inline static size_t Dim() { return 4; }
+
+  /// Dimensionality of tangent space = 4 DOF
+  inline size_t dim() const { return 4; }
+
+  /// @}
+};
+
+template <>
+struct traits<Similarity2> : public internal::LieGroup<Similarity2> {};
+
+template <>
+struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
+
+}  // namespace gtsam

gtsam/geometry/Similarity3.cpp

@@ -26,7 +26,7 @@ namespace gtsam {
 
 using std::vector;
 
-namespace {
+namespace internal {
 /// Subtract centroids from point pairs.
 static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
                                     const Point3Pair &centroids) {
@@ -81,10 +81,10 @@ static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb,
 static Similarity3 alignGivenR(const Point3Pairs &abPointPairs,
                                const Rot3 &aRb) {
   auto centroids = means(abPointPairs);
-  auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
+  auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
   return align(d_abPointPairs, aRb, centroids);
 }
-}  // namespace
+}  // namespace internal
 
 Similarity3::Similarity3() :
     t_(0,0,0), s_(1) {
@@ -165,11 +165,11 @@ Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) {
   if (abPointPairs.size() < 3)
     throw std::runtime_error("input should have at least 3 pairs of points");
   auto centroids = means(abPointPairs);
-  auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
+  auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
-  Matrix3 H = calculateH(d_abPointPairs);
+  Matrix3 H = internal::calculateH(d_abPointPairs);
   // ClosestTo finds rotation matrix closest to H in Frobenius sense
   Rot3 aRb = Rot3::ClosestTo(H);
-  return align(d_abPointPairs, aRb, centroids);
+  return internal::align(d_abPointPairs, aRb, centroids);
 }
 
 Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
@@ -192,7 +192,7 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
   }
   const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations);
 
-  return alignGivenR(abPointPairs, aRb_estimate);
+  return internal::alignGivenR(abPointPairs, aRb_estimate);
 }
 
 Matrix4 Similarity3::wedge(const Vector7 &xi) {
@@ -283,15 +283,11 @@ std::ostream &operator<<(std::ostream &os, const Similarity3& p) {
   return os;
 }
 
-const Matrix4 Similarity3::matrix() const {
+Matrix4 Similarity3::matrix() const {
   Matrix4 T;
   T.topRows<3>() << R_.matrix(), t_;
   T.bottomRows<1>() << 0, 0, 0, 1.0 / s_;
   return T;
 }
 
-Similarity3::operator Pose3() const {
-  return Pose3(R_, s_ * t_);
-}
-
 } // namespace gtsam

gtsam/geometry/Similarity3.h

@@ -18,13 +18,12 @@
 
 #pragma once
 
-#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/dllexport.h>
-
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Rot3.h>
 
 namespace gtsam {
 
@@ -34,8 +33,7 @@ class Pose3;
 /**
  * 3D similarity transform
  */
-class Similarity3: public LieGroup<Similarity3, 7> {
+class GTSAM_EXPORT Similarity3 : public LieGroup<Similarity3, 7> {
-
   /// @name Pose Concept
   /// @{
   typedef Rot3 Rotation;
@@ -48,59 +46,58 @@ private:
   double s_;
 
  public:
-
   /// @name Constructors
   /// @{
 
   /// Default constructor
-  GTSAM_EXPORT Similarity3();
+  Similarity3();
 
   /// Construct pure scaling
-  GTSAM_EXPORT Similarity3(double s);
+  Similarity3(double s);
 
   /// Construct from GTSAM types
-  GTSAM_EXPORT Similarity3(const Rot3& R, const Point3& t, double s);
+  Similarity3(const Rot3& R, const Point3& t, double s);
 
   /// Construct from Eigen types
-  GTSAM_EXPORT Similarity3(const Matrix3& R, const Vector3& t, double s);
+  Similarity3(const Matrix3& R, const Vector3& t, double s);
 
   /// Construct from matrix [R t; 0 s^-1]
-  GTSAM_EXPORT Similarity3(const Matrix4& T);
+  Similarity3(const Matrix4& T);
 
   /// @}
   /// @name Testable
   /// @{
 
   /// Compare with tolerance
-  GTSAM_EXPORT bool equals(const Similarity3& sim, double tol) const;
+  bool equals(const Similarity3& sim, double tol) const;
 
   /// Exact equality
-  GTSAM_EXPORT bool operator==(const Similarity3& other) const;
+  bool operator==(const Similarity3& other) const;
 
   /// Print with optional string
-  GTSAM_EXPORT void print(const std::string& s) const;
+  void print(const std::string& s) const;
 
-  GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity3& p);
+  friend std::ostream& operator<<(std::ostream& os, const Similarity3& p);
 
   /// @}
   /// @name Group
   /// @{
 
   /// Return an identity transform
-  GTSAM_EXPORT static Similarity3 identity();
+  static Similarity3 identity();
 
   /// Composition
-  GTSAM_EXPORT Similarity3 operator*(const Similarity3& S) const;
+  Similarity3 operator*(const Similarity3& S) const;
 
   /// Return the inverse
-  GTSAM_EXPORT Similarity3 inverse() const;
+  Similarity3 inverse() const;
 
   /// @}
   /// @name Group action on Point3
   /// @{
 
   /// Action on a point p is s*(R*p+t)
-  GTSAM_EXPORT Point3 transformFrom(const Point3& p, //
+  Point3 transformFrom(const Point3& p,                          //
                        OptionalJacobian<3, 7> H1 = boost::none,  //
                        OptionalJacobian<3, 3> H2 = boost::none) const;
 
@@ -115,15 +112,15 @@ public:
    * This group action satisfies the compatibility condition.
    * For more details, refer to: https://en.wikipedia.org/wiki/Group_action
    */
-  GTSAM_EXPORT Pose3 transformFrom(const Pose3& T) const;
+  Pose3 transformFrom(const Pose3& T) const;
 
   /** syntactic sugar for transformFrom */
-  GTSAM_EXPORT Point3 operator*(const Point3& p) const;
+  Point3 operator*(const Point3& p) const;
 
   /**
    *  Create Similarity3 by aligning at least three point pairs
    */
-  GTSAM_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
+  static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
 
   /**
    * Create the Similarity3 object that aligns at least two pose pairs.
@@ -131,11 +128,11 @@ public:
    * Given a list of pairs in frame a, and a list of pairs in frame b, Align()
    * will compute the best-fit Similarity3 aSb transformation to align them.
    * First, the rotation aRb will be computed as the average (Karcher mean) of
-   * many estimates aRb (from each pair). Afterwards, the scale factor will be computed
+   * many estimates aRb (from each pair). Afterwards, the scale factor will be
-   * using the algorithm described here:
+   * computed using the algorithm described here:
    * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
    */
-  GTSAM_EXPORT static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
+  static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
 
   /// @}
   /// @name Lie Group
@@ -144,20 +141,22 @@ public:
   /** Log map at the identity
    * \f$ [R_x,R_y,R_z, t_x, t_y, t_z, \lambda] \f$
    */
-  GTSAM_EXPORT static Vector7 Logmap(const Similarity3& s, //
+  static Vector7 Logmap(const Similarity3& s,  //
                         OptionalJacobian<7, 7> Hm = boost::none);
 
   /** Exponential map at the identity
    */
-  GTSAM_EXPORT static Similarity3 Expmap(const Vector7& v, //
+  static Similarity3 Expmap(const Vector7& v,  //
                             OptionalJacobian<7, 7> Hm = boost::none);
 
   /// Chart at the origin
   struct ChartAtOrigin {
-    static Similarity3 Retract(const Vector7& v, ChartJacobian H = boost::none) {
+    static Similarity3 Retract(const Vector7& v,
+                               ChartJacobian H = boost::none) {
       return Similarity3::Expmap(v, H);
     }
-    static Vector7 Local(const Similarity3& other, ChartJacobian H = boost::none) {
+    static Vector7 Local(const Similarity3& other,
+                         ChartJacobian H = boost::none) {
       return Similarity3::Logmap(other, H);
     }
   };
@@ -170,46 +169,32 @@ public:
    * @return 4*4 element of Lie algebra that can be exponentiated
    * TODO(frank): rename to Hat, make part of traits
    */
-  GTSAM_EXPORT static Matrix4 wedge(const Vector7& xi);
+  static Matrix4 wedge(const Vector7& xi);
 
   /// Project from one tangent space to another
-  GTSAM_EXPORT  Matrix7 AdjointMap() const;
+  Matrix7 AdjointMap() const;
 
   /// @}
   /// @name Standard interface
   /// @{
 
   /// Calculate 4*4 matrix group equivalent
-  GTSAM_EXPORT const Matrix4 matrix() const;
+  Matrix4 matrix() const;
 
   /// Return a GTSAM rotation
-  const Rot3& rotation() const {
+  Rot3 rotation() const { return R_; }
-    return R_;
-  }
 
   /// Return a GTSAM translation
-  const Point3& translation() const {
+  Point3 translation() const { return t_; }
-    return t_;
-  }
 
   /// Return the scale
-  double scale() const {
+  double scale() const { return s_; }
-    return s_;
-  }
-
-  /// Convert to a rigid body pose (R, s*t)
-  /// TODO(frank): why is this here? Red flag! Definitely don't have it as a cast.
-  GTSAM_EXPORT operator Pose3() const;
 
   /// Dimensionality of tangent space = 7 DOF - used to autodetect sizes
-  inline static size_t Dim() {
+  inline static size_t Dim() { return 7; }
-    return 7;
-  }
 
   /// Dimensionality of tangent space = 7 DOF
-  inline size_t dim() const {
+  inline size_t dim() const { return 7; }
-    return 7;
-  }
 
   /// @}
   /// @name Helper functions

gtsam/geometry/geometry.i

@@ -915,6 +915,29 @@ class PinholeCamera {
   void serialize() const;
 };
   
+#include <gtsam/geometry/Similarity2.h>
+class Similarity2 {
+  // Standard Constructors
+  Similarity2();
+  Similarity2(double s);
+  Similarity2(const gtsam::Rot2& R, const gtsam::Point2& t, double s);
+  Similarity2(const Matrix& R, const Vector& t, double s);
+  Similarity2(const Matrix& T);
+
+  gtsam::Point2 transformFrom(const gtsam::Point2& p) const;
+  gtsam::Pose2 transformFrom(const gtsam::Pose2& T);
+
+  static gtsam::Similarity2 Align(const gtsam::Point2Pairs& abPointPairs);
+  static gtsam::Similarity2 Align(const gtsam::Pose2Pairs& abPosePairs);
+
+  // Standard Interface
+  bool equals(const gtsam::Similarity2& sim, double tol) const;
+  Matrix matrix() const;
+  gtsam::Rot2& rotation();
+  gtsam::Point2& translation();
+  double scale() const;
+};
+
 #include <gtsam/geometry/Similarity3.h>
 class Similarity3 {
   // Standard Constructors
@@ -931,9 +954,10 @@ class Similarity3 {
   static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs);
 
   // Standard Interface
-  const Matrix matrix() const;
+  bool equals(const gtsam::Similarity3& sim, double tol) const;
-  const gtsam::Rot3& rotation();
+  Matrix matrix() const;
-  const gtsam::Point3& translation();
+  gtsam::Rot3& rotation();
+  gtsam::Point3& translation();
   double scale() const;
 };
 

gtsam/geometry/tests/testSimilarity2.cpp

@@ -0,0 +1,66 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testSimilarity2.cpp
+ * @brief  Unit tests for Similarity2 class
+ * @author Varun Agrawal
+ */
+
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/base/testLie.h>
+#include <gtsam/geometry/Similarity2.h>
+
+#include <functional>
+
+using namespace std::placeholders;
+using namespace gtsam;
+using namespace std;
+
+GTSAM_CONCEPT_TESTABLE_INST(Similarity2)
+
+static const Point2 P(0.2, 0.7);
+static const Rot2 R = Rot2::fromAngle(0.3);
+static const double s = 4;
+
+const double degree = M_PI / 180;
+
+//******************************************************************************
+TEST(Similarity2, Concepts) {
+  BOOST_CONCEPT_ASSERT((IsGroup<Similarity2>));
+  BOOST_CONCEPT_ASSERT((IsManifold<Similarity2>));
+  BOOST_CONCEPT_ASSERT((IsLieGroup<Similarity2>));
+}
+
+//******************************************************************************
+TEST(Similarity2, Constructors) {
+  Similarity2 sim2_Construct1;
+  Similarity2 sim2_Construct2(s);
+  Similarity2 sim2_Construct3(R, P, s);
+  Similarity2 sim2_Construct4(R.matrix(), P, s);
+}
+
+//******************************************************************************
+TEST(Similarity2, Getters) {
+  Similarity2 sim2_default;
+  EXPECT(assert_equal(Rot2(), sim2_default.rotation()));
+  EXPECT(assert_equal(Point2(0, 0), sim2_default.translation()));
+  EXPECT_DOUBLES_EQUAL(1.0, sim2_default.scale(), 1e-9);
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************

gtsam/geometry/tests/testSimilarity3.cpp

@@ -458,18 +458,18 @@ TEST(Similarity3, Optimization2) {
   Values result;
   result = LevenbergMarquardtOptimizer(graph, initial).optimize();
   //result.print("Optimized Estimate\n");
-  Pose3 p1, p2, p3, p4, p5;
+  Similarity3 p1, p2, p3, p4, p5;
-  p1 = Pose3(result.at<Similarity3>(X(1)));
+  p1 = result.at<Similarity3>(X(1));
-  p2 = Pose3(result.at<Similarity3>(X(2)));
+  p2 = result.at<Similarity3>(X(2));
-  p3 = Pose3(result.at<Similarity3>(X(3)));
+  p3 = result.at<Similarity3>(X(3));
-  p4 = Pose3(result.at<Similarity3>(X(4)));
+  p4 = result.at<Similarity3>(X(4));
-  p5 = Pose3(result.at<Similarity3>(X(5)));
+  p5 = result.at<Similarity3>(X(5));
 
-  //p1.print("Pose1");
+  //p1.print("Similarity1");
-  //p2.print("Pose2");
+  //p2.print("Similarity2");
-  //p3.print("Pose3");
+  //p3.print("Similarity3");
-  //p4.print("Pose4");
+  //p4.print("Similarity4");
-  //p5.print("Pose5");
+  //p5.print("Similarity5");
 
   Similarity3 expected(0.7);
   EXPECT(assert_equal(expected, result.at<Similarity3>(X(5)), 0.4));

gtsam/navigation/tests/testGPSFactor.cpp

@@ -27,7 +27,6 @@
 #include <GeographicLib/Config.h>
 #include <GeographicLib/LocalCartesian.hpp>
 
-using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 using namespace GeographicLib;

gtsam/navigation/tests/testMagFactor.cpp

@@ -26,7 +26,6 @@
 
 #include <GeographicLib/LocalCartesian.hpp>
 
-using namespace std::placeholders;
 using namespace std;
 using namespace gtsam;
 using namespace GeographicLib;
@@ -95,7 +94,7 @@ TEST( MagFactor, Factors ) {
       (std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),//
       H2, 1e-7));
 
-// MagFactor2
+  // MagFactor3
   MagFactor3 f3(1, 2, 3, measured, nRb, model);
   EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
   EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //

gtsam/slam/KarcherMeanFactor-inl.h

@@ -40,8 +40,7 @@ T FindKarcherMeanImpl(const vector<T, ALLOC>& rotations) {
   return result.at<T>(kKey);
 }
 
-template <class T,
+template <class T>
-        typename = typename std::enable_if< std::is_same<gtsam::Rot3, T>::value >::type >
 T FindKarcherMean(const std::vector<T>& rotations) {
   return FindKarcherMeanImpl(rotations);
 }

python/gtsam/preamble/geometry.h

@@ -23,8 +23,8 @@ PYBIND11_MAKE_OPAQUE(
     std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>);
 PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs);
 PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs);
+PYBIND11_MAKE_OPAQUE(gtsam::Pose2Pairs);
 PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs);
 PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
-PYBIND11_MAKE_OPAQUE(
+PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
-    gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
 PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>);

python/gtsam/specializations/geometry.h

@@ -16,6 +16,7 @@ py::bind_vector<
     m_, "Point2Vector");
 py::bind_vector<std::vector<gtsam::Point2Pair>>(m_, "Point2Pairs");
 py::bind_vector<std::vector<gtsam::Point3Pair>>(m_, "Point3Pairs");
+py::bind_vector<std::vector<gtsam::Pose2Pair>>(m_, "Pose2Pairs");
 py::bind_vector<std::vector<gtsam::Pose3Pair>>(m_, "Pose3Pairs");
 py::bind_vector<std::vector<gtsam::Pose3>>(m_, "Pose3Vector");
 py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>>(

python/gtsam/tests/test_Sim2.py

@@ -0,0 +1,194 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+Sim3 unit tests.
+Author: John Lambert
+"""
+# pylint: disable=no-name-in-module
+import unittest
+
+import numpy as np
+from gtsam import Pose2, Pose2Pairs, Rot2, Similarity2
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestSim2(GtsamTestCase):
+    """Test selected Sim2 methods."""
+
+    def test_align_poses_along_straight_line(self) -> None:
+        """Test Align Pose2Pairs method.
+
+        Scenario:
+           3 object poses
+           same scale (no gauge ambiguity)
+           world frame has poses rotated about 180 degrees.
+           world and egovehicle frame translated by 15 meters w.r.t. each other
+        """
+        R180 = Rot2.fromDegrees(180)
+
+        # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
+        # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
+        eTo0 = Pose2(Rot2(), np.array([5, 0]))
+        eTo1 = Pose2(Rot2(), np.array([10, 0]))
+        eTo2 = Pose2(Rot2(), np.array([15, 0]))
+
+        eToi_list = [eTo0, eTo1, eTo2]
+
+        # Create destination poses
+        # (same three objects, but instead living in the world "w" frame)
+        wTo0 = Pose2(R180, np.array([-10, 0]))
+        wTo1 = Pose2(R180, np.array([-5, 0]))
+        wTo2 = Pose2(R180, np.array([0, 0]))
+
+        wToi_list = [wTo0, wTo1, wTo2]
+
+        we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list)))
+
+        # Recover the transformation wSe (i.e. world_S_egovehicle)
+        wSe = Similarity2.Align(we_pairs)
+
+        for wToi, eToi in zip(wToi_list, eToi_list):
+            self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
+
+    def test_align_poses_along_straight_line_gauge(self):
+        """Test if Align Pose3Pairs method can account for gauge ambiguity.
+
+        Scenario:
+           3 object poses
+           with gauge ambiguity (2x scale)
+           world frame has poses rotated by 90 degrees.
+           world and egovehicle frame translated by 11 meters w.r.t. each other
+        """
+        R90 = Rot2.fromDegrees(90)
+
+        # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
+        # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
+        eTo0 = Pose2(Rot2(), np.array([1, 0]))
+        eTo1 = Pose2(Rot2(), np.array([2, 0]))
+        eTo2 = Pose2(Rot2(), np.array([4, 0]))
+
+        eToi_list = [eTo0, eTo1, eTo2]
+
+        # Create destination poses
+        # (same three objects, but instead living in the world/city "w" frame)
+        wTo0 = Pose2(R90, np.array([0, 12]))
+        wTo1 = Pose2(R90, np.array([0, 14]))
+        wTo2 = Pose2(R90, np.array([0, 18]))
+
+        wToi_list = [wTo0, wTo1, wTo2]
+
+        we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list)))
+
+        # Recover the transformation wSe (i.e. world_S_egovehicle)
+        wSe = Similarity2.Align(we_pairs)
+
+        for wToi, eToi in zip(wToi_list, eToi_list):
+            self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
+
+    def test_align_poses_scaled_squares(self):
+        """Test if Align Pose2Pairs method can account for gauge ambiguity.
+
+        Make sure a big and small square can be aligned.
+        The u's represent a big square (10x10), and v's represents a small square (4x4).
+
+        Scenario:
+           4 object poses
+           with gauge ambiguity (2.5x scale)
+        """
+        # 0, 90, 180, and 270 degrees yaw
+        R0 = Rot2.fromDegrees(0)
+        R90 = Rot2.fromDegrees(90)
+        R180 = Rot2.fromDegrees(180)
+        R270 = Rot2.fromDegrees(270)
+
+        aTi0 = Pose2(R0, np.array([2, 3]))
+        aTi1 = Pose2(R90, np.array([12, 3]))
+        aTi2 = Pose2(R180, np.array([12, 13]))
+        aTi3 = Pose2(R270, np.array([2, 13]))
+
+        aTi_list = [aTi0, aTi1, aTi2, aTi3]
+
+        bTi0 = Pose2(R0, np.array([4, 3]))
+        bTi1 = Pose2(R90, np.array([8, 3]))
+        bTi2 = Pose2(R180, np.array([8, 7]))
+        bTi3 = Pose2(R270, np.array([4, 7]))
+
+        bTi_list = [bTi0, bTi1, bTi2, bTi3]
+
+        ab_pairs = Pose2Pairs(list(zip(aTi_list, bTi_list)))
+
+        # Recover the transformation wSe (i.e. world_S_egovehicle)
+        aSb = Similarity2.Align(ab_pairs)
+
+        for aTi, bTi in zip(aTi_list, bTi_list):
+            self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi))
+
+    def test_constructor(self) -> None:
+        """Sim(2) to perform p_b = bSa * p_a"""
+        bRa = Rot2()
+        bta = np.array([1, 2])
+        bsa = 3.0
+        bSa = Similarity2(R=bRa, t=bta, s=bsa)
+        self.assertIsInstance(bSa, Similarity2)
+        np.testing.assert_allclose(bSa.rotation().matrix(), bRa.matrix())
+        np.testing.assert_allclose(bSa.translation(), bta)
+        np.testing.assert_allclose(bSa.scale(), bsa)
+
+    def test_is_eq(self) -> None:
+        """Ensure object equality works properly (are equal)."""
+        bSa = Similarity2(R=Rot2(), t=np.array([1, 2]), s=3.0)
+        bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3)
+        self.gtsamAssertEquals(bSa, bSa_)
+
+    def test_not_eq_translation(self) -> None:
+        """Ensure object equality works properly (not equal translation)."""
+        bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0)
+        bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3)
+        self.assertNotEqual(bSa, bSa_)
+
+    def test_not_eq_rotation(self) -> None:
+        """Ensure object equality works properly (not equal rotation)."""
+        bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0)
+        bSa_ = Similarity2(R=Rot2.fromDegrees(180), t=np.array([2.0, 1.0]), s=3)
+        self.assertNotEqual(bSa, bSa_)
+
+    def test_not_eq_scale(self) -> None:
+        """Ensure object equality works properly (not equal scale)."""
+        bSa = Similarity2(R=Rot2(), t=np.array([2, 1]), s=3.0)
+        bSa_ = Similarity2(R=Rot2(), t=np.array([2.0, 1.0]), s=1.0)
+        self.assertNotEqual(bSa, bSa_)
+
+    def test_rotation(self) -> None:
+        """Ensure rotation component is returned properly."""
+        R = Rot2.fromDegrees(90)
+        t = np.array([1, 2])
+        bSa = Similarity2(R=R, t=t, s=3.0)
+
+        # evaluates to [[0, -1], [1, 0]]
+        expected_R = Rot2.fromDegrees(90)
+        np.testing.assert_allclose(expected_R.matrix(), bSa.rotation().matrix())
+
+    def test_translation(self) -> None:
+        """Ensure translation component is returned properly."""
+        R = Rot2.fromDegrees(90)
+        t = np.array([1, 2])
+        bSa = Similarity2(R=R, t=t, s=3.0)
+
+        expected_t = np.array([1, 2])
+        np.testing.assert_allclose(expected_t, bSa.translation())
+
+    def test_scale(self) -> None:
+        """Ensure the scale factor is returned properly."""
+        bRa = Rot2()
+        bta = np.array([1, 2])
+        bsa = 3.0
+        bSa = Similarity2(R=bRa, t=bta, s=bsa)
+        self.assertEqual(bSa.scale(), 3.0)
+
+
+if __name__ == "__main__":
+    unittest.main()