Pose2::Align

gtsam/geometry/Pose2.cpp

@@ -309,54 +309,77 @@ double Pose2::range(const Pose2& pose,
 }
 
 /* *************************************************************************
- * New explanation, from scan.ml
- * It finds the angle using a linear method:
- * q = Pose2::transformFrom(p) = t + R*p
+ * Align finds the angle using a linear method:
+ * a = Pose2::transformFrom(b) = t + R*b
  * We need to remove the centroids from the data to find the rotation
- * using dp=[dpx;dpy] and q=[dqx;dqy] we have
- *  |dqx|   |c  -s|     |dpx|     |dpx -dpy|     |c|
+ * using db=[dbx;dby] and a=[dax;day] we have
+ *  |dax|   |c  -s|     |dbx|     |dbx -dby|     |c|
  *  |   | = |     |  *  |   |  =  |        |  *  | | = H_i*cs
- *  |dqy|   |s   c|     |dpy|     |dpy  dpx|     |s|
+ *  |day|   |s   c|     |dby|     |dby  dbx|     |s|
  * where the Hi are the 2*2 matrices. Then we will minimize the criterion
- * J = \sum_i norm(q_i - H_i * cs)
+ * J = \sum_i norm(a_i - H_i * cs)
  * Taking the derivative with respect to cs and setting to zero we have
- * cs = (\sum_i H_i' * q_i)/(\sum H_i'*H_i)
+ * cs = (\sum_i H_i' * a_i)/(\sum H_i'*H_i)
  * The hessian is diagonal and just divides by a constant, but this
  * normalization constant is irrelevant, since we take atan2.
- * i.e., cos ~ sum(dpx*dqx + dpy*dqy) and sin ~ sum(-dpy*dqx + dpx*dqy)
+ * i.e., cos ~ sum(dbx*dax + dby*day) and sin ~ sum(-dby*dax + dbx*day)
  * The translation is then found from the centroids
- * as they also satisfy cq = t + R*cp, hence t = cq - R*cp
+ * as they also satisfy ca = t + R*cb, hence t = ca - R*cb
  */
 
-boost::optional<Pose2> align(const vector<Point2Pair>& pairs) {
-
-  size_t n = pairs.size();
-  if (n<2) return boost::none; // we need at least two pairs
+boost::optional<Pose2> Pose2::Align(const Point2Pairs &ab_pairs) {
+  const size_t n = ab_pairs.size();
+  if (n < 2) {
+    return boost::none;  // we need at least 2 pairs
+  }
 
   // calculate centroids
-  Point2 cp(0,0), cq(0,0);
-  for(const Point2Pair& pair: pairs) {
-    cp += pair.first;
-    cq += pair.second;
+  Point2 ca(0, 0), cb(0, 0);
+  for (const Point2Pair& pair : ab_pairs) {
+    ca += pair.first;
+    cb += pair.second;
   }
-  double f = 1.0/n;
-  cp *= f; cq *= f;
+  const double f = 1.0/n;
+  ca *= f;
+  cb *= f;
 
   // calculate cos and sin
-  double c=0,s=0;
-  for(const Point2Pair& pair: pairs) {
-    Point2 dp = pair.first - cp;
-    Point2 dq = pair.second - cq;
-    c += dp.x() * dq.x() + dp.y() * dq.y();
-    s += -dp.y() * dq.x() + dp.x() * dq.y();
+  double c = 0, s = 0;
+  for (const Point2Pair& pair : ab_pairs) {
+    Point2 da = pair.first - ca;
+    Point2 db = pair.second - cb;
+    c += db.x() * da.x() + db.y() * da.y();
+    s += -db.y() * da.x() + db.x() * da.y();
   }
 
   // calculate angle and translation
-  double theta = atan2(s,c);
-  Rot2 R = Rot2::fromAngle(theta);
-  Point2 t = cq - R*cp;
+  const double theta = atan2(s, c);
+  const Rot2 R = Rot2::fromAngle(theta);
+  const Point2 t = ca - R*cb;
   return Pose2(R, t);
 }
 
+boost::optional<Pose2> Pose2::Align(const Matrix& a, const Matrix& b) {
+  if (a.rows() != 2 || b.rows() != 2 || a.cols() != b.cols()) {
+    throw std::invalid_argument(
+      "Pose2:Align expects 2*N matrices of equal shape.");
+  }
+  Point2Pairs ab_pairs;
+  for (size_t j=0; j < a.cols(); j++) {
+    ab_pairs.emplace_back(a.col(j), b.col(j));
+  }
+  return Pose2::Align(ab_pairs);
+}
+
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
+boost::optional<Pose2> align(const Point2Pairs& ba_pairs) {
+  Point2Pairs ab_pairs;
+  for (const Point2Pair &baPair : ba_pairs) {
+    ab_pairs.emplace_back(baPair.second, baPair.first);
+  }
+  return Pose2::Align(ab_pairs);
+}
+#endif
+
 /* ************************************************************************* */
 } // namespace gtsam

gtsam/geometry/Pose2.h

@@ -92,6 +92,18 @@ class Pose2: public LieGroup<Pose2, 3> {
     *this = Expmap(v);
   }
 
+  /**
+   *  Create Pose2 by aligning two point pairs
+   *  A pose aTb is estimated between pairs (a_point, b_point) such that 
+   *    a_point = aTb * b_point
+   *  Note this allows for noise on the points but in that case the mapping 
+   *  will not be exact.
+   */
+  static boost::optional<Pose2> Align(const Point2Pairs& abPointPairs);
+
+  // Version of Pose2::Align that takes 2 matrices.
+  static boost::optional<Pose2> Align(const Matrix& a, const Matrix& b);
+
   /// @}
   /// @name Testable
   /// @{
@@ -331,12 +343,15 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
   return Matrix(Pose2::wedge(xi(0),xi(1),xi(2))).eval();
 }
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
 /**
+ * @deprecated Use static constructor (with reversed pairs!)
  * Calculate pose between a vector of 2D point correspondences (p,q)
  * where q = Pose2::transformFrom(p) = t + R*p
  */
-typedef std::pair<Point2,Point2> Point2Pair;
-GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
+GTSAM_EXPORT boost::optional<Pose2> 
+GTSAM_DEPRECATED align(const Point2Pairs& pairs);
+#endif
 
 template <>
 struct traits<Pose2> : public internal::LieGroup<Pose2> {};

gtsam/geometry/Pose3.cpp

@@ -479,13 +479,15 @@ boost::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
   return Pose3::Align(abPointPairs);
 }
 
+#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
 boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
   Point3Pairs abPointPairs;
   for (const Point3Pair &baPair : baPointPairs) {
     abPointPairs.emplace_back(baPair.second, baPair.first);
   }
   return Pose3::Align(abPointPairs);
 }
+#endif
 
 /* ************************************************************************* */
 std::ostream &operator<<(std::ostream &os, const Pose3& pose) {

gtsam/geometry/geometry.i

@@ -372,6 +372,9 @@ class Pose2 {
   Pose2(const gtsam::Rot2& r, const gtsam::Point2& t);
   Pose2(Vector v);
 
+  static boost::optional<gtsam::Pose2> Align(const gtsam::Point2Pairs& abPointPairs);
+  static boost::optional<gtsam::Pose2> Align(const gtsam::Matrix& a, const gtsam::Matrix& b);
+
   // Testable
   void print(string s = "") const;
   bool equals(const gtsam::Pose2& pose, double tol) const;
@@ -424,8 +427,6 @@ class Pose2 {
   void serialize() const;
 };
 
-boost::optional<gtsam::Pose2> align(const gtsam::Point2Pairs& pairs);
-
 #include <gtsam/geometry/Pose3.h>
 class Pose3 {
   // Standard Constructors

gtsam/geometry/tests/testPose2.cpp

@@ -717,81 +717,75 @@ TEST( Pose2, range_pose )
 /* ************************************************************************* */
 
 TEST(Pose2, align_1) {
-  Pose2 expected(Rot2::fromAngle(0), Point2(10,10));
-
-  vector<Point2Pair> correspondences;
-  Point2Pair pq1(make_pair(Point2(0,0), Point2(10,10)));
-  Point2Pair pq2(make_pair(Point2(20,10), Point2(30,20)));
-  correspondences += pq1, pq2;
-
-  boost::optional<Pose2> actual = align(correspondences);
-  EXPECT(assert_equal(expected, *actual));
+  Pose2 expected(Rot2::fromAngle(0), Point2(10, 10));
+  Point2Pairs ab_pairs {{Point2(10, 10), Point2(0, 0)},
+                        {Point2(30, 20), Point2(20, 10)}};
+  boost::optional<Pose2> aTb = Pose2::Align(ab_pairs);
+  EXPECT(assert_equal(expected, *aTb));
 }
 
 TEST(Pose2, align_2) {
-  Point2 t(20,10);
+  Point2 t(20, 10);
   Rot2 R = Rot2::fromAngle(M_PI/2.0);
   Pose2 expected(R, t);
 
-  vector<Point2Pair> correspondences;
-  Point2 p1(0,0), p2(10,0);
-  Point2 q1 = expected.transformFrom(p1), q2 = expected.transformFrom(p2);
-  EXPECT(assert_equal(Point2(20,10),q1));
-  EXPECT(assert_equal(Point2(20,20),q2));
-  Point2Pair pq1(make_pair(p1, q1));
-  Point2Pair pq2(make_pair(p2, q2));
-  correspondences += pq1, pq2;
+  Point2 b1(0, 0), b2(10, 0);
+  Point2Pairs ab_pairs {{expected.transformFrom(b1), b1},
+                        {expected.transformFrom(b2), b2}};
 
-  boost::optional<Pose2> actual = align(correspondences);
-  EXPECT(assert_equal(expected, *actual));
+  boost::optional<Pose2> aTb = Pose2::Align(ab_pairs);
+  EXPECT(assert_equal(expected, *aTb));
 }
 
 namespace align_3 {
-  Point2 t(10,10);
+  Point2 t(10, 10);
   Pose2 expected(Rot2::fromAngle(2*M_PI/3), t);
-  Point2 p1(0,0), p2(10,0), p3(10,10);
-  Point2 q1 = expected.transformFrom(p1), q2 = expected.transformFrom(p2), q3 = expected.transformFrom(p3);
+  Point2 b1(0, 0), b2(10, 0), b3(10, 10);
+  Point2 a1 = expected.transformFrom(b1),
+         a2 = expected.transformFrom(b2),
+         a3 = expected.transformFrom(b3);
 }
 
 TEST(Pose2, align_3) {
   using namespace align_3;
 
-  vector<Point2Pair> correspondences;
-  Point2Pair pq1(make_pair(p1, q1));
-  Point2Pair pq2(make_pair(p2, q2));
-  Point2Pair pq3(make_pair(p3, q3));
-  correspondences += pq1, pq2, pq3;
+  Point2Pairs ab_pairs;
+  Point2Pair ab1(make_pair(a1, b1));
+  Point2Pair ab2(make_pair(a2, b2));
+  Point2Pair ab3(make_pair(a3, b3));
+  ab_pairs += ab1, ab2, ab3;
 
-  boost::optional<Pose2> actual = align(correspondences);
-  EXPECT(assert_equal(expected, *actual));
+  boost::optional<Pose2> aTb = Pose2::Align(ab_pairs);
+  EXPECT(assert_equal(expected, *aTb));
 }
 
 namespace {
   /* ************************************************************************* */
   // Prototype code to align two triangles using a rigid transform
   /* ************************************************************************* */
-  struct Triangle { size_t i_,j_,k_;};
+  struct Triangle { size_t i_, j_, k_;};
 
-  boost::optional<Pose2> align2(const Point2Vector& ps, const Point2Vector& qs,
+  boost::optional<Pose2> align2(const Point2Vector& as, const Point2Vector& bs,
     const pair<Triangle, Triangle>& trianglePair) {
       const Triangle& t1 = trianglePair.first, t2 = trianglePair.second;
-      vector<Point2Pair> correspondences;
-      correspondences += make_pair(ps[t1.i_],qs[t2.i_]), make_pair(ps[t1.j_],qs[t2.j_]), make_pair(ps[t1.k_],qs[t2.k_]);
-      return align(correspondences);
+      Point2Pairs ab_pairs = {{as[t1.i_], bs[t2.i_]},
+                              {as[t1.j_], bs[t2.j_]},
+                              {as[t1.k_], bs[t2.k_]}};
+      return Pose2::Align(ab_pairs);
   }
 }
 
 TEST(Pose2, align_4) {
   using namespace align_3;
 
-  Point2Vector ps,qs;
-  ps += p1, p2, p3;
-  qs += q3, q1, q2; // note in 3,1,2 order !
+  Point2Vector as, bs;
+  as += a1, a2, a3;
+  bs += b3, b1, b2;  // note in 3,1,2 order !
 
   Triangle t1; t1.i_=0; t1.j_=1; t1.k_=2;
   Triangle t2; t2.i_=1; t2.j_=2; t2.k_=0;
 
-  boost::optional<Pose2> actual = align2(ps, qs, make_pair(t1,t2));
+  boost::optional<Pose2> actual = align2(as, bs, {t1, t2});
   EXPECT(assert_equal(expected, *actual));
 }
 

python/gtsam/tests/test_Pose2.py

@@ -70,27 +70,36 @@ def test_align(self) -> None:
               O---O
         """
         pts_a = [
-            Point2(3, 1),
-            Point2(1, 1),
-            Point2(1, 3),
-            Point2(3, 3),
-        ]
-        pts_b = [
             Point2(1, -3),
             Point2(1, -5),
             Point2(-1, -5),
             Point2(-1, -3),
         ]
+        pts_b = [
+            Point2(3, 1),
+            Point2(1, 1),
+            Point2(1, 3),
+            Point2(3, 3),
+        ]
 
         # fmt: on
         ab_pairs = Point2Pairs(list(zip(pts_a, pts_b)))
-        bTa = gtsam.align(ab_pairs)
-        aTb = bTa.inverse()
-        assert aTb is not None
+        aTb = Pose2.Align(ab_pairs)
+        self.assertIsNotNone(aTb)
+
+        for pt_a, pt_b in zip(pts_a, pts_b):
+            pt_a_ = aTb.transformFrom(pt_b)
+            np.testing.assert_allclose(pt_a, pt_a_)
+
+        # Matrix version
+        A = np.array(pts_a).T
+        B = np.array(pts_b).T
+        aTb = Pose2.Align(A, B)
+        self.assertIsNotNone(aTb)
 
         for pt_a, pt_b in zip(pts_a, pts_b):
             pt_a_ = aTb.transformFrom(pt_b)
-            assert np.allclose(pt_a, pt_a_)
+            np.testing.assert_allclose(pt_a, pt_a_)
 
 
 if __name__ == "__main__":