Merge pull request #866 from borglab/Pose2-align

gtsam/geometry/Point2.h

@@ -25,6 +25,12 @@ namespace gtsam {
 /// As of GTSAM 4, in order to make GTSAM more lean,
 /// it is now possible to just typedef Point2 to Vector2
 typedef Vector2 Point2;
+  
+// Convenience typedef
+using Point2Pair = std::pair<Point2, Point2>;
+GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
+
+using Point2Pairs = std::vector<Point2Pair>;
 
 /// Distance of the point from the origin, with Jacobian
 GTSAM_EXPORT double norm2(const Point2& p, OptionalJacobian<1, 2> H = boost::none);
@@ -34,10 +40,6 @@ GTSAM_EXPORT double distance2(const Point2& p1, const Point2& q,
                  OptionalJacobian<1, 2> H1 = boost::none,
                  OptionalJacobian<1, 2> H2 = boost::none);
 
-// Convenience typedef
-typedef std::pair<Point2, Point2> Point2Pair;
-GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p);
-
 // For MATLAB wrapper
 typedef std::vector<Point2, Eigen::aligned_allocator<Point2> > Point2Vector;
 

gtsam/geometry/geometry.i

@@ -31,6 +31,14 @@ class Point2 {
   // enable pickling in python
   void pickle() const;
 };
+  
+class Point2Pairs {
+  Point2Pairs();
+  size_t size() const;
+  bool empty() const;
+  gtsam::Point2Pair at(size_t n) const;
+  void push_back(const gtsam::Point2Pair& point_pair);
+};
 
 // std::vector<gtsam::Point2>
 class Point2Vector {
@@ -429,6 +437,8 @@ class Pose2 {
   // enable pickling in python
   void pickle() const;
 };
+  
+boost::optional<gtsam::Pose2> align(const gtsam::Point2Pairs& pairs);
 
 #include <gtsam/geometry/Pose3.h>
 class Pose3 {

python/CMakeLists.txt

@@ -43,6 +43,7 @@ set(ignore
     gtsam::BetweenFactorPose2s
     gtsam::BetweenFactorPose3s
     gtsam::Point2Vector
+    gtsam::Point2Pairs
     gtsam::Point3Pairs
     gtsam::Pose3Pairs
     gtsam::Pose3Vector

python/gtsam/preamble/geometry.h

@@ -10,9 +10,18 @@
  * Without this they will be automatically converted to a Python object, and all
  * mutations on Python side will not be reflected on C++.
  */
+#include <pybind11/stl.h>
+
+// Support for binding boost::optional types in C++11.
+// https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html
+namespace pybind11 { namespace detail {
+    template <typename T>
+    struct type_caster<boost::optional<T>> : optional_caster<boost::optional<T>> {};
+}}
 
 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::Pose3Pairs);
 PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);

python/gtsam/specializations/geometry.h

@@ -14,6 +14,7 @@
 py::bind_vector<
     std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>>(
     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::Pose3Pair>>(m_, "Pose3Pairs");
 py::bind_vector<std::vector<gtsam::Pose3>>(m_, "Pose3Vector");

python/gtsam/tests/test_Pose2.py

@@ -6,27 +6,64 @@ All Rights Reserved
 See LICENSE for the license information
 
 Pose2 unit tests.
-Author: Frank Dellaert & Duy Nguyen Ta (Python)
+Author: Frank Dellaert & Duy Nguyen Ta & John Lambert
 """
 import unittest
 
 import numpy as np
 
 import gtsam
-from gtsam import Pose2
+from gtsam import Point2, Point2Pairs, Pose2
 from gtsam.utils.test_case import GtsamTestCase
 
 
 class TestPose2(GtsamTestCase):
     """Test selected Pose2 methods."""
-
-    def test_adjoint(self):
+    def test_adjoint(self) -> None:
         """Test adjoint method."""
         xi = np.array([1, 2, 3])
         expected = np.dot(Pose2.adjointMap_(xi), xi)
         actual = Pose2.adjoint_(xi, xi)
         np.testing.assert_array_equal(actual, expected)
 
+    def test_align(self) -> None:
+        """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds.
+
+        Two point clouds represent horseshoe-shapes of the same size, just rotated and translated:
+
+                |  X---X
+                |  |
+                |  X---X
+        ------------------
+                |
+                |
+              O | O
+              | | |
+              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),
+        ]
+
+        # fmt: on
+        ab_pairs = Point2Pairs(list(zip(pts_a, pts_b)))
+        bTa = gtsam.align(ab_pairs)
+        aTb = bTa.inverse()
+        assert aTb is not None
+
+        for pt_a, pt_b in zip(pts_a, pts_b):
+            pt_a_ = aTb.transformFrom(pt_b)
+            assert np.allclose(pt_a, pt_a_)
+
 
 if __name__ == "__main__":
     unittest.main()