Merge remote-tracking branch 'origin/develop' into fan/prototype-hybrid-tr

gtsam/base/OptionalJacobian.h

@@ -20,6 +20,8 @@
 #pragma once
 #include <gtsam/config.h>      // Configuration from CMake
 #include <Eigen/Dense>
+#include <stdexcept>
+#include <string>
 
 #ifndef OPTIONALJACOBIAN_NOBOOST
 #include <boost/optional.hpp>
@@ -96,6 +98,24 @@ public:
     usurp(dynamic->data());
   }
 
+  /**
+   * @brief Constructor from an Eigen::Ref *value*. Will not usurp if dimension is wrong
+   * @note This is important so we don't overwrite someone else's memory!
+   */
+  template<class MATRIX>
+  OptionalJacobian(Eigen::Ref<MATRIX> dynamic_ref) :
+      map_(nullptr) {
+    if (dynamic_ref.rows() == Rows && dynamic_ref.cols() == Cols && !dynamic_ref.IsRowMajor) {
+      usurp(dynamic_ref.data());
+    } else {
+      throw std::invalid_argument(
+          std::string("OptionalJacobian called with wrong dimensions or "
+                      "storage order.\n"
+                      "Expected: ") +
+          "(" + std::to_string(Rows) + ", " + std::to_string(Cols) + ")");
+    }
+  }
+
 #ifndef OPTIONALJACOBIAN_NOBOOST
 
   /// Constructor with boost::none just makes empty

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.cpp

@@ -365,7 +365,7 @@ boost::optional<Pose2> Pose2::Align(const Matrix& a, const Matrix& b) {
       "Pose2:Align expects 2*N matrices of equal shape.");
   }
   Point2Pairs ab_pairs;
-  for (size_t j=0; j < a.cols(); j++) {
+  for (Eigen::Index j = 0; j < a.cols(); j++) {
     ab_pairs.emplace_back(a.col(j), b.col(j));
   }
   return Pose2::Align(ab_pairs);

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/Pose3.cpp

@@ -473,7 +473,7 @@ boost::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
       "Pose3:Align expects 3*N matrices of equal shape.");
   }
   Point3Pairs abPointPairs;
-  for (size_t j=0; j < a.cols(); j++) {
+  for (Eigen::Index j = 0; j < a.cols(); j++) {
     abPointPairs.emplace_back(a.col(j), b.col(j));
   }
   return Pose3::Align(abPointPairs);

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,75 +33,73 @@ 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;
   typedef Point3 Translation;
   /// @}
 
-private:
+ private:
   Rot3 R_;
   Point3 t_;
   double s_;
 
-public:
+ 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, 7> H1 = boost::none,  //
-      OptionalJacobian<3, 3> H2 = boost::none) const;
+                       OptionalJacobian<3, 3> H2 = boost::none) const;
 
   /**
    * Action on a pose T.
@@ -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);
+                        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);
+                            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,67 +169,53 @@ 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
   /// @{
 
-private:
+ private:
   /// Calculate expmap and logmap coefficients.
   static Matrix3 GetV(Vector3 w, double lambda);
 
   /// @}
 };
 
-template<>
+template <>
 inline Matrix wedge<Similarity3>(const Vector& xi) {
   return Similarity3::wedge(xi);
 }
 
-template<>
+template <>
 struct traits<Similarity3> : public internal::LieGroup<Similarity3> {};
 
-template<>
+template <>
 struct traits<const Similarity3> : public internal::LieGroup<Similarity3> {};
 
-} // namespace gtsam
+}  // namespace gtsam

gtsam/geometry/geometry.i

@@ -547,6 +547,12 @@ class EssentialMatrix {
   // Standard Constructors
   EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
 
+  // Constructors from Pose3
+  gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_);
+
+  gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_,
+                            Eigen::Ref<Eigen::MatrixXd> H);
+
   // Testable
   void print(string s = "") const;
   bool equals(const gtsam::EssentialMatrix& pose, double tol) const;
@@ -584,7 +590,13 @@ class Cal3_S2 {
 
   // Action on Point2
   gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p,
+                          Eigen::Ref<Eigen::MatrixXd> Dcal,
+                          Eigen::Ref<Eigen::MatrixXd> Dp) const;
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p,
+                            Eigen::Ref<Eigen::MatrixXd> Dcal,
+                            Eigen::Ref<Eigen::MatrixXd> Dp) const;
 
   // Standard Interface
   double fx() const;
@@ -623,7 +635,13 @@ virtual class Cal3DS2_Base {
 
   // Action on Point2
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p,
+                            Eigen::Ref<Eigen::MatrixXd> Dcal,
+                            Eigen::Ref<Eigen::MatrixXd> Dp) const;
   gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p,
+                          Eigen::Ref<Eigen::MatrixXd> Dcal,
+                          Eigen::Ref<Eigen::MatrixXd> Dp) const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -680,7 +698,13 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base {
   // Note: the signature of this functions differ from the functions
   // with equal name in the base class.
   gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p,
+                          Eigen::Ref<Eigen::MatrixXd> Dcal,
+                          Eigen::Ref<Eigen::MatrixXd> Dp) const;
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p,
+                            Eigen::Ref<Eigen::MatrixXd> Dcal,
+                            Eigen::Ref<Eigen::MatrixXd> Dp) const;
 
   // enabling serialization functionality
   void serialize() const;
@@ -706,7 +730,13 @@ class Cal3Fisheye {
 
   // Action on Point2
   gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p,
+                          Eigen::Ref<Eigen::MatrixXd> Dcal,
+                          Eigen::Ref<Eigen::MatrixXd> Dp) const;
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p,
+                            Eigen::Ref<Eigen::MatrixXd> Dcal,
+                            Eigen::Ref<Eigen::MatrixXd> Dp) const;
 
   // Standard Interface
   double fx() const;
@@ -769,7 +799,13 @@ class Cal3Bundler {
 
   // Action on Point2
   gtsam::Point2 calibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 calibrate(const gtsam::Point2& p,
+                          Eigen::Ref<Eigen::MatrixXd> Dcal,
+                          Eigen::Ref<Eigen::MatrixXd> Dp) const;
   gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
+  gtsam::Point2 uncalibrate(const gtsam::Point2& p,
+                            Eigen::Ref<Eigen::MatrixXd> Dcal,
+                            Eigen::Ref<Eigen::MatrixXd> Dp) const;
 
   // Standard Interface
   double fx() const;
@@ -807,12 +843,25 @@ class CalibratedCamera {
 
   // Action on Point3
   gtsam::Point2 project(const gtsam::Point3& point) const;
+  gtsam::Point2 project(const gtsam::Point3& point,
+                        Eigen::Ref<Eigen::MatrixXd> Dcamera,
+                        Eigen::Ref<Eigen::MatrixXd> Dpoint);
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth);
+
   static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
 
   // Standard Interface
   gtsam::Pose3 pose() const;
   double range(const gtsam::Point3& point) const;
+  double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpoint);
   double range(const gtsam::Pose3& pose) const;
+  double range(const gtsam::Pose3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpose);
   double range(const gtsam::CalibratedCamera& camera) const;
 
   // enabling serialization functionality
@@ -824,6 +873,7 @@ template <CALIBRATION>
 class PinholeCamera {
   // Standard Constructors and Named Constructors
   PinholeCamera();
+  PinholeCamera(const gtsam::PinholeCamera<CALIBRATION> other);
   PinholeCamera(const gtsam::Pose3& pose);
   PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K);
   static This Level(const CALIBRATION& K, const gtsam::Pose2& pose,
@@ -850,14 +900,123 @@ class PinholeCamera {
   static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
   pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
   gtsam::Point2 project(const gtsam::Point3& point);
+  gtsam::Point2 project(const gtsam::Point3& point,
+                        Eigen::Ref<Eigen::MatrixXd> Dpose,
+                        Eigen::Ref<Eigen::MatrixXd> Dpoint,
+                        Eigen::Ref<Eigen::MatrixXd> Dcal);
   gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dcal);
+
+  gtsam::Point2 reprojectionError(const gtsam::Point3& pw, const gtsam::Point2& measured,
+                                  Eigen::Ref<Eigen::MatrixXd> Dpose,
+                                  Eigen::Ref<Eigen::MatrixXd> Dpoint,
+                                  Eigen::Ref<Eigen::MatrixXd> Dcal);
+
   double range(const gtsam::Point3& point);
+  double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpoint);
   double range(const gtsam::Pose3& pose);
+  double range(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpose);
 
   // enabling serialization functionality
   void serialize() const;
 };
 
+// Forward declaration of PinholeCameraCalX is defined here.
+#include <gtsam/geometry/SimpleCamera.h>
+// Some typedefs for common camera types
+// PinholeCameraCal3_S2 is the same as SimpleCamera above
+typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
+typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
+typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
+typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
+typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
+
+#include <gtsam/geometry/PinholePose.h>
+template <CALIBRATION>
+class PinholePose {
+  // Standard Constructors and Named Constructors
+  PinholePose();
+  PinholePose(const gtsam::PinholePose<CALIBRATION> other);
+  PinholePose(const gtsam::Pose3& pose);
+  PinholePose(const gtsam::Pose3& pose, const CALIBRATION* K);
+  static This Level(const gtsam::Pose2& pose, double height);
+  static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
+                     const gtsam::Point3& upVector, const CALIBRATION* K);
+
+  // Testable
+  void print(string s = "PinholePose") const;
+  bool equals(const This& camera, double tol) const;
+
+  // Standard Interface
+  gtsam::Pose3 pose() const;
+  CALIBRATION calibration() const;
+
+  // Manifold
+  This retract(Vector d) const;
+  Vector localCoordinates(const This& T2) const;
+  size_t dim() const;
+  static size_t Dim();
+
+  // Transformations and measurement functions
+  static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
+  pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
+  gtsam::Point2 project(const gtsam::Point3& point);
+  gtsam::Point2 project(const gtsam::Point3& point,
+                        Eigen::Ref<Eigen::MatrixXd> Dpose,
+                        Eigen::Ref<Eigen::MatrixXd> Dpoint,
+                        Eigen::Ref<Eigen::MatrixXd> Dcal);
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
+  gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dp,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_ddepth,
+                            Eigen::Ref<Eigen::MatrixXd> Dresult_dcal);
+  double range(const gtsam::Point3& point);
+  double range(const gtsam::Point3& point, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpoint);
+  double range(const gtsam::Pose3& pose);
+  double range(const gtsam::Pose3& pose, Eigen::Ref<Eigen::MatrixXd> Dcamera,
+               Eigen::Ref<Eigen::MatrixXd> Dpose);
+
+  // enabling serialization functionality
+  void serialize() const;
+};
+
+typedef gtsam::PinholePose<gtsam::Cal3_S2> PinholePoseCal3_S2;
+typedef gtsam::PinholePose<gtsam::Cal3DS2> PinholePoseCal3DS2;
+typedef gtsam::PinholePose<gtsam::Cal3Unified> PinholePoseCal3Unified;
+typedef gtsam::PinholePose<gtsam::Cal3Bundler> PinholePoseCal3Bundler;
+typedef gtsam::PinholePose<gtsam::Cal3Fisheye> PinholePoseCal3Fisheye;
+
+#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
@@ -874,22 +1033,13 @@ 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;
 };
 
-// Forward declaration of PinholeCameraCalX is defined here.
-#include <gtsam/geometry/SimpleCamera.h>
-// Some typedefs for common camera types
-// PinholeCameraCal3_S2 is the same as SimpleCamera above
-typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
-typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
-typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
-typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
-typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
-
 template <T>
 class CameraSet {
   CameraSet();
@@ -921,9 +1071,18 @@ class StereoCamera {
   static size_t Dim();
 
   // Transformations and measurement functions
-  gtsam::StereoPoint2 project(const gtsam::Point3& point);
+  gtsam::StereoPoint2 project(const gtsam::Point3& point) const;
   gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const;
 
+  // project with Jacobian
+  gtsam::StereoPoint2 project2(const gtsam::Point3& point,
+                              Eigen::Ref<Eigen::MatrixXd> H1,
+                              Eigen::Ref<Eigen::MatrixXd> H2) const;
+
+  gtsam::Point3 backproject2(const gtsam::StereoPoint2& p,
+                             Eigen::Ref<Eigen::MatrixXd> H1,
+                             Eigen::Ref<Eigen::MatrixXd> H2) const;
+
   // enabling serialization functionality
   void serialize() 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/linear/LossFunctions.h

@@ -54,23 +54,31 @@ namespace noiseModel {
 // clang-format on
 namespace mEstimator {
 
-//---------------------------------------------------------------------------------------
+/**
-
+ * Pure virtual class for all robust error function classes.
+ *
+ * It provides the machinery for block vs scalar reweighting strategies, in
+ * addition to defining the interface of derived classes.
+ */
 class GTSAM_EXPORT Base {
  public:
+  /** the rows can be weighted independently according to the error
+   * or uniformly with the norm of the right hand side */
   enum ReweightScheme { Scalar, Block };
   typedef boost::shared_ptr<Base> shared_ptr;
 
  protected:
-  /** the rows can be weighted independently according to the error
+  /// Strategy for reweighting \sa ReweightScheme
-   * or uniformly with the norm of the right hand side */
   ReweightScheme reweight_;
 
  public:
   Base(const ReweightScheme reweight = Block) : reweight_(reweight) {}
   virtual ~Base() {}
 
-  /*
+  /// Returns the reweight scheme, as explained in ReweightScheme
+  ReweightScheme reweightScheme() const { return reweight_; }
+
+  /**
    * This method is responsible for returning the total penalty for a given
    * amount of error. For example, this method is responsible for implementing
    * the quadratic function for an L2 penalty, the absolute value function for
@@ -80,16 +88,20 @@ class GTSAM_EXPORT Base {
    * error vector, then it prevents implementations of asymmeric loss
    * functions. It would be better for this function to accept the vector and
    * internally call the norm if necessary.
+   *
+   * This returns \rho(x) in \ref mEstimator
    */
-  virtual double loss(double distance) const { return 0; };
+  virtual double loss(double distance) const { return 0; }
 
-  /*
+  /**
    * This method is responsible for returning the weight function for a given
    * amount of error. The weight function is related to the analytic derivative
    * of the loss function. See
    *  https://members.loria.fr/MOBerger/Enseignement/Master2/Documents/ZhangIVC-97-01.pdf
    * for details. This method is required when optimizing cost functions with
    * robust penalties using iteratively re-weighted least squares.
+   *
+   * This returns w(x) in \ref mEstimator
    */
   virtual double weight(double distance) const = 0;
 
@@ -124,7 +136,15 @@ class GTSAM_EXPORT Base {
   }
 };
 
-/// Null class should behave as Gaussian
+/** "Null" robust loss function, equivalent to a Gaussian pdf noise model, or
+ *  plain least-squares (non-robust).
+ *
+ *  This model has no additional parameters.
+ *
+ * - Loss       \rho(x)          = 0.5 x²
+ * - Derivative \phi(x)          = x
+ * - Weight     w(x) = \phi(x)/x = 1
+ */
 class GTSAM_EXPORT Null : public Base {
  public:
   typedef boost::shared_ptr<Null> shared_ptr;
@@ -146,7 +166,14 @@ class GTSAM_EXPORT Null : public Base {
   }
 };
 
-/// Fair implements the "Fair" robust error model (Zhang97ivc)
+/** Implementation of the "Fair" robust error model (Zhang97ivc)
+ *
+ *  This model has a scalar parameter "c".
+ *
+ * - Loss       \rho(x) = c² (|x|/c - log(1+|x|/c))
+ * - Derivative \phi(x) = x/(1+|x|/c)
+ * - Weight     w(x) = \phi(x)/x = 1/(1+|x|/c)
+ */
 class GTSAM_EXPORT Fair : public Base {
  protected:
   double c_;
@@ -160,6 +187,7 @@ class GTSAM_EXPORT Fair : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double c, const ReweightScheme reweight = Block);
+  double modelParameter() const { return c_; }
 
  private:
   /** Serialization function */
@@ -171,7 +199,14 @@ class GTSAM_EXPORT Fair : public Base {
   }
 };
 
-/// Huber implements the "Huber" robust error model (Zhang97ivc)
+/** The "Huber" robust error model (Zhang97ivc).
+ *
+ *  This model has a scalar parameter "k".
+ *
+ * - Loss       \rho(x)          = 0.5 x²  if |x|<k, 0.5 k² + k|x-k|  otherwise
+ * - Derivative \phi(x)          = x       if |x|<k, k sgn(x)         otherwise
+ * - Weight     w(x) = \phi(x)/x = 1       if |x|<k, k/|x|            otherwise
+ */
 class GTSAM_EXPORT Huber : public Base {
  protected:
   double k_;
@@ -185,6 +220,7 @@ class GTSAM_EXPORT Huber : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return k_; }
 
  private:
   /** Serialization function */
@@ -196,12 +232,19 @@ class GTSAM_EXPORT Huber : public Base {
   }
 };
 
-/// Cauchy implements the "Cauchy" robust error model (Lee2013IROS). Contributed
+/** Implementation of the "Cauchy" robust error model (Lee2013IROS).
-/// by:
+ * Contributed by:
-///   Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
+ *  Dipl.-Inform. Jan Oberlaender (M.Sc.), FZI Research Center for
-///   Information Technology, Karlsruhe, Germany.
+ *  Information Technology, Karlsruhe, Germany.
-///   oberlaender@fzi.de
+ *  oberlaender@fzi.de
-/// Thanks Jan!
+ *  Thanks Jan!
+ *
+ *  This model has a scalar parameter "k".
+ *
+ * - Loss       \rho(x) = 0.5 k² log(1+x²/k²)
+ * - Derivative \phi(x) = (k²x)/(x²+k²)
+ * - Weight     w(x) = \phi(x)/x = k²/(x²+k²)
+ */
 class GTSAM_EXPORT Cauchy : public Base {
  protected:
   double k_, ksquared_;
@@ -215,6 +258,7 @@ class GTSAM_EXPORT Cauchy : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return k_; }
 
  private:
   /** Serialization function */
@@ -227,7 +271,14 @@ class GTSAM_EXPORT Cauchy : public Base {
   }
 };
 
-/// Tukey implements the "Tukey" robust error model (Zhang97ivc)
+/** Implementation of the "Tukey" robust error model (Zhang97ivc).
+ *
+ *  This model has a scalar parameter "c".
+ *
+ * - Loss       \rho(x) = c² (1 - (1-x²/c²)³)/6  if |x|<c,  c²/6   otherwise
+ * - Derivative \phi(x) = x(1-x²/c²)² if |x|<c,  0   otherwise
+ * - Weight     w(x) = \phi(x)/x = (1-x²/c²)² if |x|<c,  0   otherwise
+ */
 class GTSAM_EXPORT Tukey : public Base {
  protected:
   double c_, csquared_;
@@ -241,6 +292,7 @@ class GTSAM_EXPORT Tukey : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return c_; }
 
  private:
   /** Serialization function */
@@ -252,7 +304,14 @@ class GTSAM_EXPORT Tukey : public Base {
   }
 };
 
-/// Welsch implements the "Welsch" robust error model (Zhang97ivc)
+/** Implementation of the "Welsch" robust error model (Zhang97ivc).
+ *
+ *  This model has a scalar parameter "c".
+ *
+ * - Loss       \rho(x) = -0.5 c² (exp(-x²/c²) - 1)
+ * - Derivative \phi(x) = x exp(-x²/c²)
+ * - Weight     w(x) = \phi(x)/x = exp(-x²/c²)
+ */
 class GTSAM_EXPORT Welsch : public Base {
  protected:
   double c_, csquared_;
@@ -266,6 +325,7 @@ class GTSAM_EXPORT Welsch : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return c_; }
 
  private:
   /** Serialization function */
@@ -278,12 +338,16 @@ class GTSAM_EXPORT Welsch : public Base {
   }
 };
 
-/// GemanMcClure implements the "Geman-McClure" robust error model
+/** Implementation of the "Geman-McClure" robust error model (Zhang97ivc).
-/// (Zhang97ivc).
+ *
-///
+ * Note that Geman-McClure weight function uses the parameter c == 1.0,
-/// Note that Geman-McClure weight function uses the parameter c == 1.0,
+ * but here it's allowed to use different values, so we actually have
-/// but here it's allowed to use different values, so we actually have
+ * the generalized Geman-McClure from (Agarwal15phd).
-/// the generalized Geman-McClure from (Agarwal15phd).
+ *
+ * - Loss       \rho(x) = 0.5 (c²x²)/(c²+x²)
+ * - Derivative \phi(x) = xc⁴/(c²+x²)²
+ * - Weight     w(x) = \phi(x)/x = c⁴/(c²+x²)²
+ */
 class GTSAM_EXPORT GemanMcClure : public Base {
  public:
   typedef boost::shared_ptr<GemanMcClure> shared_ptr;
@@ -295,6 +359,7 @@ class GTSAM_EXPORT GemanMcClure : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return c_; }
 
  protected:
   double c_;
@@ -309,11 +374,18 @@ class GTSAM_EXPORT GemanMcClure : public Base {
   }
 };
 
-/// DCS implements the Dynamic Covariance Scaling robust error model
+/** DCS implements the Dynamic Covariance Scaling robust error model
-/// from the paper Robust Map Optimization (Agarwal13icra).
+ *  from the paper Robust Map Optimization (Agarwal13icra).
-///
+ *
-/// Under the special condition of the parameter c == 1.0 and not
+ *  Under the special condition of the parameter c == 1.0 and not
-/// forcing the output weight s <= 1.0, DCS is similar to Geman-McClure.
+ *  forcing the output weight s <= 1.0, DCS is similar to Geman-McClure.
+ *
+ *  This model has a scalar parameter "c" (with "units" of squared error).
+ *
+ * - Loss       \rho(x) = (c²x² + cx⁴)/(x²+c)²   (for any "x")
+ * - Derivative \phi(x) = 2c²x/(x²+c)²
+ * - Weight     w(x) = \phi(x)/x = 2c²/(x²+c)²  if x²>c,   1  otherwise
+ */
 class GTSAM_EXPORT DCS : public Base {
  public:
   typedef boost::shared_ptr<DCS> shared_ptr;
@@ -325,6 +397,7 @@ class GTSAM_EXPORT DCS : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return c_; }
 
  protected:
   double c_;
@@ -339,12 +412,19 @@ class GTSAM_EXPORT DCS : public Base {
   }
 };
 
-/// L2WithDeadZone implements a standard L2 penalty, but with a dead zone of
+/** L2WithDeadZone implements a standard L2 penalty, but with a dead zone of
-/// width 2*k, centered at the origin. The resulting penalty within the dead
+ *  width 2*k, centered at the origin. The resulting penalty within the dead
-/// zone is always zero, and grows quadratically outside the dead zone. In this
+ *  zone is always zero, and grows quadratically outside the dead zone. In this
-/// sense, the L2WithDeadZone penalty is "robust to inliers", rather than being
+ *  sense, the L2WithDeadZone penalty is "robust to inliers", rather than being
-/// robust to outliers. This penalty can be used to create barrier functions in
+ *  robust to outliers. This penalty can be used to create barrier functions in
-/// a general way.
+ *  a general way.
+ *
+ *  This model has a scalar parameter "k".
+ *
+ * - Loss       \rho(x) = 0 if |x|<k,    0.5(k-|x|)² otherwise
+ * - Derivative \phi(x) = 0 if |x|<k, (-k+x) if x>k,  (k+x) if x<-k
+ * - Weight     w(x) = \phi(x)/x = 0 if |x|<k, (-k+x)/x if x>k,  (k+x)/x if x<-k
+ */
 class GTSAM_EXPORT L2WithDeadZone : public Base {
  protected:
   double k_;
@@ -358,6 +438,7 @@ class GTSAM_EXPORT L2WithDeadZone : public Base {
   void print(const std::string &s) const override;
   bool equals(const Base &expected, double tol = 1e-8) const override;
   static shared_ptr Create(double k, const ReweightScheme reweight = Block);
+  double modelParameter() const { return k_; }
 
  private:
   /** Serialization function */

gtsam/linear/linear.i

@@ -671,6 +671,10 @@ virtual class DummyPreconditionerParameters : gtsam::PreconditionerParameters {
   DummyPreconditionerParameters();
 };
 
+virtual class BlockJacobiPreconditionerParameters : gtsam::PreconditionerParameters {
+  BlockJacobiPreconditionerParameters();
+};
+
 #include <gtsam/linear/PCGSolver.h>
 virtual class PCGSolverParameters : gtsam::ConjugateGradientParameters {
   PCGSolverParameters();

gtsam/navigation/AHRSFactor.h

@@ -90,7 +90,11 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
 
   /**
    * Add a single Gyroscope measurement to the preintegration.
-   * @param measureOmedga Measured angular velocity (in body frame)
+   * Measurements are taken to be in the sensor
+   * frame and conversion to the body frame is handled by `body_P_sensor` in
+   * `PreintegratedRotationParams` (if provided).
+   *
+   * @param measuredOmega Measured angular velocity (as given by the sensor)
    * @param deltaT Time step
    */
   void integrateMeasurement(const Vector3& measuredOmega, double deltaT);

gtsam/navigation/CombinedImuFactor.h

@@ -208,8 +208,11 @@ public:
 
   /**
    * Add a single IMU measurement to the preintegration.
-   * @param measuredAcc Measured acceleration (in body frame, as given by the
+   * Both accelerometer and gyroscope measurements are taken to be in the sensor
-   * sensor)
+   * frame and conversion to the body frame is handled by `body_P_sensor` in
+   * `PreintegrationParams`.
+   *
+   * @param measuredAcc Measured acceleration (as given by the sensor)
    * @param measuredOmega Measured angular velocity (as given by the sensor)
    * @param dt Time interval between two consecutive IMU measurements
    */

gtsam/navigation/ImuFactor.h

@@ -121,7 +121,11 @@ public:
 
   /**
    * Add a single IMU measurement to the preintegration.
-   * @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
+   * Both accelerometer and gyroscope measurements are taken to be in the sensor
+   * frame and conversion to the body frame is handled by `body_P_sensor` in
+   * `PreintegrationParams`.
+   *
+   * @param measuredAcc Measured acceleration (as given by the sensor)
    * @param measuredOmega Measured angular velocity (as given by the sensor)
    * @param dt Time interval between this and the last IMU measurement
    */

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;
@@ -71,7 +70,7 @@ TEST( GPSFactor, Constructor ) {
   EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
 
   // Calculate numerical derivatives
-  Matrix expectedH = numericalDerivative11<Vector,Pose3>(
+  Matrix expectedH = numericalDerivative11<Vector, Pose3>(
       std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T);
 
   // Use the factor to calculate the derivative
@@ -100,7 +99,7 @@ TEST( GPSFactor2, Constructor ) {
   EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
 
   // Calculate numerical derivatives
-  Matrix expectedH = numericalDerivative11<Vector,NavState>(
+  Matrix expectedH = numericalDerivative11<Vector, NavState>(
       std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), T);
 
   // Use the factor to calculate the derivative

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;
@@ -64,7 +63,7 @@ TEST( MagFactor, unrotate ) {
   Matrix H;
   Point3 expected(22735.5, 314.502, 44202.5);
   EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1));
-  EXPECT( assert_equal(numericalDerivative11<Point3,Rot2> //
+  EXPECT(assert_equal(numericalDerivative11<Point3, Rot2> //
       (std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6));
 }
 
@@ -75,27 +74,27 @@ TEST( MagFactor, Factors ) {
 
   // MagFactor
   MagFactor f(1, measured, s, dir, bias, model);
-  EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
+  EXPECT(assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
-  EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> //
+  EXPECT(assert_equal((Matrix)numericalDerivative11<Vector, Rot2> //
       (std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7));
 
-// MagFactor1
+  // MagFactor1
   MagFactor1 f1(1, measured, s, dir, bias, model);
-  EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
+  EXPECT(assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
-  EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
+  EXPECT(assert_equal(numericalDerivative11<Vector, Rot3> //
       (std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7));
 
-// MagFactor2
+  // MagFactor2
   MagFactor2 f2(1, 2, measured, nRb, model);
-  EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
+  EXPECT(assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
-  EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
+  EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
       (std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),//
       H1, 1e-7));
-  EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
+  EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
       (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/nonlinear/nonlinear.i

@@ -226,6 +226,10 @@ class Values {
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
   void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
+  void insert(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert(size_t j, const gtsam::NavState& nav_state);
   void insert(size_t j, double c);
@@ -269,6 +273,10 @@ class Values {
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
   void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
+  void update(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void update(size_t j, const gtsam::NavState& nav_state);
   void update(size_t j, Vector vector);
@@ -310,6 +318,10 @@ class Values {
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
   void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3_S2>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Bundler>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Fisheye>& camera);
+  void insert_or_assign(size_t j, const gtsam::PinholePose<gtsam::Cal3Unified>& camera);
   void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
   void insert_or_assign(size_t j, const gtsam::NavState& nav_state);
   void insert_or_assign(size_t j, Vector vector);
@@ -351,6 +363,10 @@ class Values {
                  gtsam::PinholeCamera<gtsam::Cal3Bundler>,
                  gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
                  gtsam::PinholeCamera<gtsam::Cal3Unified>,
+                 gtsam::PinholePose<gtsam::Cal3_S2>,
+                 gtsam::PinholePose<gtsam::Cal3Bundler>,
+                 gtsam::PinholePose<gtsam::Cal3Fisheye>,
+                 gtsam::PinholePose<gtsam::Cal3Unified>,
                  gtsam::imuBias::ConstantBias,
                  gtsam::NavState,
                  Vector,

gtsam/sfm/SfmData.h

@@ -77,10 +77,14 @@ struct GTSAM_EXPORT SfmData {
   size_t numberCameras() const { return cameras.size(); }
 
   /// The track formed by series of landmark measurements
-  SfmTrack track(size_t idx) const { return tracks[idx]; }
+  const SfmTrack& track(size_t idx) const { return tracks[idx]; }
 
   /// The camera pose at frame index `idx`
-  SfmCamera camera(size_t idx) const { return cameras[idx]; }
+  const SfmCamera& camera(size_t idx) const { return cameras[idx]; }
+
+  /// Getters
+  const std::vector<SfmCamera>& cameraList() const { return cameras; }
+  const std::vector<SfmTrack>& trackList() const { return tracks; }
 
   /**
    * @brief Create projection factors using keys i and P(j)

gtsam/sfm/sfm.i

@@ -10,6 +10,8 @@ class SfmTrack {
   SfmTrack(const gtsam::Point3& pt);
   const Point3& point3() const;
   
+  Point3 p;
+
   double r;
   double g;
   double b;
@@ -34,12 +36,15 @@ class SfmData {
   static gtsam::SfmData FromBundlerFile(string filename);
   static gtsam::SfmData FromBalFile(string filename);
 
+  std::vector<gtsam::SfmTrack>& trackList() const;
+  std::vector<gtsam::PinholeCamera<gtsam::Cal3Bundler>>& cameraList() const;
+
   void addTrack(const gtsam::SfmTrack& t);
   void addCamera(const gtsam::SfmCamera& cam);
   size_t numberTracks() const;
   size_t numberCameras() const;
-  gtsam::SfmTrack track(size_t idx) const;
+  gtsam::SfmTrack& track(size_t idx) const;
-  gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
+  gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera(size_t idx) const;
 
   gtsam::NonlinearFactorGraph generalSfmFactors(
       const gtsam::SharedNoiseModel& model =
@@ -91,6 +96,13 @@ class BinaryMeasurementsUnit3 {
   void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
 };
 
+class BinaryMeasurementsRot3 {
+  BinaryMeasurementsRot3();
+  size_t size() const;
+  gtsam::BinaryMeasurement<gtsam::Rot3> at(size_t idx) const;
+  void push_back(const gtsam::BinaryMeasurement<gtsam::Rot3>& measurement);
+};
+
 #include <gtsam/sfm/ShonanAveraging.h>
 
 // TODO(frank): copy/pasta below until we have integer template paremeters in
@@ -184,6 +196,10 @@ class ShonanAveraging2 {
 };
 
 class ShonanAveraging3 {
+  ShonanAveraging3(
+      const std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>& measurements,
+      const gtsam::ShonanAveragingParameters3& parameters =
+          gtsam::ShonanAveragingParameters3());
   ShonanAveraging3(string g2oFile);
   ShonanAveraging3(string g2oFile,
                    const gtsam::ShonanAveragingParameters3& parameters);

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);
 }

gtsam/slam/dataset.h

@@ -223,6 +223,7 @@ parse3DFactors(const std::string &filename,
                size_t maxIndex = 0);
 
 using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
+using BinaryMeasurementsRot3 = std::vector<BinaryMeasurement<Rot3>>;
 
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
 inline boost::optional<IndexedPose> GTSAM_DEPRECATED

gtsam/slam/slam.i

@@ -90,6 +90,22 @@ typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Unified>,
                                 gtsam::Point3>
     GeneralSFMFactorCal3Unified;
 
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3_S2>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3_S2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3DS2>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3DS2;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Bundler>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3Bundler;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Fisheye>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3Fisheye;
+typedef gtsam::GeneralSFMFactor<gtsam::PinholePose<gtsam::Cal3Unified>,
+                                gtsam::Point3>
+    GeneralSFMFactorPoseCal3Unified;
+
 template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
                          gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
 virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {

python/CMakeLists.txt

@@ -48,6 +48,7 @@ set(ignore
     gtsam::Rot3Vector
     gtsam::KeyVector
     gtsam::BinaryMeasurementsUnit3
+    gtsam::BinaryMeasurementsRot3
     gtsam::DiscreteKey
     gtsam::KeyPairDoubleMap)
 
@@ -99,11 +100,23 @@ set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam)
 copy_directory("${CMAKE_CURRENT_SOURCE_DIR}/gtsam"
         "${GTSAM_MODULE_PATH}")
 
-# Hack to get python test files copied every time they are modified
+# Hack to get python test and util files copied every time they are modified
 file(GLOB GTSAM_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/tests/*.py")
 foreach(test_file ${GTSAM_PYTHON_TEST_FILES})
         configure_file(${test_file} "${GTSAM_MODULE_PATH}/tests/${test_file}" COPYONLY)
 endforeach()
+file(GLOB GTSAM_PYTHON_UTIL_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/utils/*.py")
+foreach(util_file ${GTSAM_PYTHON_UTIL_FILES})
+        configure_file(${util_file} "${GTSAM_MODULE_PATH}/utils/${test_file}" COPYONLY)
+endforeach()
+file(GLOB GTSAM_PYTHON_PREAMBLE_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/preamble/*.h")
+foreach(util_file ${GTSAM_PYTHON_PREAMBLE_FILES})
+        configure_file(${util_file} "${GTSAM_MODULE_PATH}/preamble/${test_file}" COPYONLY)
+endforeach()
+file(GLOB GTSAM_PYTHON_SPECIALIZATION_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam/specializations/*.h")
+foreach(util_file ${GTSAM_PYTHON_SPECIALIZATION_FILES})
+        configure_file(${util_file} "${GTSAM_MODULE_PATH}/specializations/${test_file}" COPYONLY)
+endforeach()
 
 # Common directory for data/datasets stored with the package.
 # This will store the data in the Python site package directly.
@@ -126,6 +139,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
             gtsam::KeyVector
             gtsam::FixedLagSmootherKeyTimestampMapValue
             gtsam::BinaryMeasurementsUnit3
+            gtsam::BinaryMeasurementsRot3
             gtsam::CameraSetCal3_S2
             gtsam::CameraSetCal3Bundler
             gtsam::CameraSetCal3Unified
@@ -161,7 +175,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
 
     # Hack to get python test files copied every time they are modified
     file(GLOB GTSAM_UNSTABLE_PYTHON_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_unstable/tests/*.py")
-    foreach(test_file ${GTSAM_PYTHON_TEST_FILES})
+    foreach(test_file ${GTSAM_UNSTABLE_PYTHON_TEST_FILES})
         configure_file(${test_file} "${GTSAM_UNSTABLE_MODULE_PATH}/tests/${test_file}" COPYONLY)
     endforeach()
 

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/preamble/sfm.h

@@ -10,3 +10,17 @@
  * Without this they will be automatically converted to a Python object, and all
  * mutations on Python side will not be reflected on C++.
  */
+
+// Including <stl.h> can cause some serious hard-to-debug bugs!!!
+// #include <pybind11/stl.h>
+#include <pybind11/stl_bind.h>
+
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::SfmTrack>);
+
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::SfmCamera>);
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::BinaryMeasurement<gtsam::Unit3>>);
+PYBIND11_MAKE_OPAQUE(
+    std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>);

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/specializations/sfm.h

@@ -13,4 +13,19 @@
 
 py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
     m_, "BinaryMeasurementsUnit3");
+py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Rot3> > >(
+    m_, "BinaryMeasurementsRot3");
 py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
+
+py::bind_vector<
+    std::vector<gtsam::SfmTrack> >(
+    m_, "SfmTracks");
+
+py::bind_vector<
+    std::vector<gtsam::SfmCamera> >(
+    m_, "SfmCameras");
+
+py::bind_vector<
+    std::vector<std::pair<size_t, gtsam::Point2>>>(
+        m_, "SfmMeasurementVector"
+    );

python/gtsam/tests/test_Cal3Unified.py

@@ -139,6 +139,17 @@ class TestCal3Unified(GtsamTestCase):
         self.gtsamAssertEquals(z, np.zeros(2))
         self.gtsamAssertEquals(H @ H.T, 4*np.eye(2))
 
+        Dcal = np.zeros((2, 10), order='F')
+        Dp = np.zeros((2, 2), order='F')
+        camera.calibrate(img_point, Dcal, Dp)
+        
+        self.gtsamAssertEquals(Dcal, np.array(
+            [[ 0.,  0.,  0., -1.,  0.,  0.,  0.,  0.,  0.,  0.],
+            [ 0.,  0.,  0.,  0., -1.,  0.,  0.,  0.,  0.,  0.]]))
+        self.gtsamAssertEquals(Dp, np.array(
+            [[ 1., -0.],
+            [-0.,  1.]]))
+
     @unittest.skip("triangulatePoint3 currently seems to require perspective projections.")
     def test_triangulation(self):
         """Estimate spatial point from image measurements"""

python/gtsam/tests/test_PinholeCamera.py

@@ -0,0 +1,46 @@
+"""
+GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
+Atlanta, Georgia 30332-0415
+All Rights Reserved
+
+See LICENSE for the license information
+
+PinholeCamera unit tests.
+Author: Fan Jiang
+"""
+import unittest
+from math import pi
+
+import numpy as np
+
+import gtsam
+from gtsam.utils.test_case import GtsamTestCase
+
+
+class TestPinholeCamera(GtsamTestCase):
+    """
+    Tests if we can correctly get the camera Jacobians in Python
+    """
+    def test_jacobian(self):
+        cam1 = gtsam.PinholeCameraCal3Bundler()
+
+        # order is important because Eigen is column major!
+        Dpose = np.zeros((2, 6), order='F')
+        Dpoint = np.zeros((2, 3), order='F')
+        Dcal = np.zeros((2, 3), order='F')
+        cam1.project(np.array([1, 1, 1]), Dpose, Dpoint, Dcal)
+
+        self.gtsamAssertEquals(Dpoint, np.array([[1, 0, -1], [0, 1, -1]]))
+
+        self.gtsamAssertEquals(
+            Dpose,
+            np.array([
+                [1., -2., 1., -1., 0., 1.],  #
+                [2., -1., -1., 0., -1., 1.]
+            ]))
+
+        self.gtsamAssertEquals(Dcal, np.array([[1., 2., 4.], [1., 2., 4.]]))
+
+
+if __name__ == "__main__":
+    unittest.main()

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()