From 13c164f25d4683bd1720b564ee3dcbf67b4ffff5 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 30 Aug 2021 16:38:47 -0400 Subject: [PATCH 01/14] add Pose2.align() to wrapper --- gtsam/geometry/geometry.i | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 0e303cbcd..c495c22d9 100644 --- a/gtsam/geometry/geometry.i +++ b/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 class Point2Vector { @@ -428,6 +436,8 @@ class Pose2 { // enable pickling in python void pickle() const; + + gtsam::Pose2 align(const gtsam::Point2Pairs& pairs); }; #include From ce495cb7bc0ab3ed5fac8482e083f629aafeb684 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 30 Aug 2021 16:41:27 -0400 Subject: [PATCH 02/14] add Point2Pairs typedef to Point2.h --- gtsam/geometry/Point2.h | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h index 17f87f656..cdb9f4480 100644 --- a/gtsam/geometry/Point2.h +++ b/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; +GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); + +using Point2Pairs = std::vector; /// 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 Point2Pair; -GTSAM_EXPORT std::ostream &operator<<(std::ostream &os, const gtsam::Point2Pair &p); - // For MATLAB wrapper typedef std::vector > Point2Vector; From 8bd2e6a976071ae4f5fa6063ec6d40c5a38de0c9 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 30 Aug 2021 16:44:11 -0400 Subject: [PATCH 03/14] add gtsam::Point2Pairs to CMakeLists.txt --- python/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 4254a21c6..c3524adad 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -43,6 +43,7 @@ set(ignore gtsam::BetweenFactorPose2s gtsam::BetweenFactorPose3s gtsam::Point2Vector + gtsam::Point2Pairs gtsam::Point3Pairs gtsam::Pose3Pairs gtsam::Pose3Vector From 55785f81809777fdc0115a0761a16f10f5ad8ab6 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 30 Aug 2021 16:44:58 -0400 Subject: [PATCH 04/14] add Point2Pairs to specializations --- python/gtsam/specializations/geometry.h | 1 + 1 file changed, 1 insertion(+) diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h index e11d3cc4f..a492ce8eb 100644 --- a/python/gtsam/specializations/geometry.h +++ b/python/gtsam/specializations/geometry.h @@ -14,6 +14,7 @@ py::bind_vector< std::vector>>( m_, "Point2Vector"); +py::bind_vector>(m_, "Point2Pairs"); py::bind_vector>(m_, "Point3Pairs"); py::bind_vector>(m_, "Pose3Pairs"); py::bind_vector>(m_, "Pose3Vector"); From 50f3b93324a6041aad6465159b1cddc86724dca4 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Mon, 30 Aug 2021 18:01:17 -0600 Subject: [PATCH 05/14] move align as function, not as class method --- gtsam/geometry/geometry.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index c495c22d9..bf906d67f 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -436,9 +436,9 @@ class Pose2 { // enable pickling in python void pickle() const; - - gtsam::Pose2 align(const gtsam::Point2Pairs& pairs); }; + +gtsam::Pose2 align(const gtsam::Point2Pairs& pairs); #include class Pose3 { From cff3c5b4f43662c81a095fd9123afeae94f9970c Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 31 Aug 2021 06:26:52 -0600 Subject: [PATCH 06/14] start python unit test for align() --- python/gtsam/tests/test_Pose2.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index 9652b594a..b44914a94 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -6,7 +6,7 @@ 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 @@ -20,13 +20,20 @@ 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. + + """ + pass + + if __name__ == "__main__": unittest.main() From 2d2ca21d1aea4d5bd4b92523075553598afe60c0 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 31 Aug 2021 09:14:59 -0400 Subject: [PATCH 07/14] add python unit test on Pose2.align() --- python/gtsam/tests/test_Pose2.py | 36 ++++++++++++++++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index b44914a94..fc9c7acf9 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -30,9 +30,41 @@ class TestPose2(GtsamTestCase): def test_align(self) -> None: """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds. - """ - pass + Two point clouds represent horseshoe-shapes of the same size, just rotated and translated: + | X---X + | | + | X---X + ------------------ + | + | + O | O + | | | + O---O + """ + # fmt: off + 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 = Pose2.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__": From 5c737c3cc4592a588a6bfd781b277de1feeb67a1 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 31 Aug 2021 09:42:49 -0400 Subject: [PATCH 08/14] fix missing imports --- python/gtsam/tests/test_Pose2.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index fc9c7acf9..f18eab079 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -13,7 +13,7 @@ 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 @@ -47,7 +47,7 @@ class TestPose2(GtsamTestCase): Point2(3, 1), Point2(1, 1), Point2(1, 3), - Point2(3, 3), + Point2(3, 3) ] pts_b = [ Point2(1, -3), From 3fc7692b4a72f62ef944532d554652f369543c6d Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 31 Aug 2021 08:35:43 -0600 Subject: [PATCH 09/14] import align from gtsam directly --- python/gtsam/tests/test_Pose2.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index f18eab079..213de655e 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -13,7 +13,7 @@ import unittest import numpy as np import gtsam -from gtsam import Point2, Point2Pairs, Pose2 +from gtsam import align, Point2, Point2Pairs, Pose2 from gtsam.utils.test_case import GtsamTestCase @@ -58,7 +58,7 @@ class TestPose2(GtsamTestCase): # fmt: on ab_pairs = Point2Pairs(list(zip(pts_a, pts_b))) - bTa = Pose2.align(ab_pairs) + bTa = align(ab_pairs) aTb = bTa.inverse() assert aTb is not None From bc641f893dc8b8e16f51ff579f8fd802573a6e78 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 31 Aug 2021 14:14:37 -0400 Subject: [PATCH 10/14] directly import only classes from gtsam --- python/gtsam/tests/test_Pose2.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index 213de655e..569046596 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -13,7 +13,7 @@ import unittest import numpy as np import gtsam -from gtsam import align, Point2, Point2Pairs, Pose2 +from gtsam import Point2, Point2Pairs, Pose2 from gtsam.utils.test_case import GtsamTestCase @@ -58,7 +58,7 @@ class TestPose2(GtsamTestCase): # fmt: on ab_pairs = Point2Pairs(list(zip(pts_a, pts_b))) - bTa = align(ab_pairs) + bTa = gtsam.align(ab_pairs) aTb = bTa.inverse() assert aTb is not None From 6d57016a51b936c014167a693e7315fe40521a66 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Wed, 1 Sep 2021 10:15:00 -0600 Subject: [PATCH 11/14] use boost::optional in .i file directly --- gtsam/geometry/geometry.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index bf906d67f..9baa49e8e 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -438,7 +438,7 @@ class Pose2 { void pickle() const; }; -gtsam::Pose2 align(const gtsam::Point2Pairs& pairs); +boost::optional align(const gtsam::Point2Pairs& pairs); #include class Pose3 { From fbdef91c54aff13b6cdb1e80095627b4cd12b217 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Sep 2021 15:29:29 -0400 Subject: [PATCH 12/14] add support for boost::optional return type in geometry.i --- python/gtsam/preamble/geometry.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h index 498c7dc58..9ddb6e208 100644 --- a/python/gtsam/preamble/geometry.h +++ b/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 + +// Support for binding boost::optional types. +// https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html +namespace pybind11 { namespace detail { + template + struct type_caster> : optional_caster> {}; +}} PYBIND11_MAKE_OPAQUE( std::vector>); +PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); PYBIND11_MAKE_OPAQUE(std::vector); From 9f661c01cf0e7ce144a94a39f1a83343e03f29ed Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Sep 2021 15:29:39 -0400 Subject: [PATCH 13/14] formatting --- python/gtsam/tests/test_Pose2.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index 569046596..e5ffbad7d 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -19,7 +19,6 @@ from gtsam.utils.test_case import GtsamTestCase class TestPose2(GtsamTestCase): """Test selected Pose2 methods.""" - def test_adjoint(self) -> None: """Test adjoint method.""" xi = np.array([1, 2, 3]) @@ -29,9 +28,9 @@ class TestPose2(GtsamTestCase): 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 @@ -42,18 +41,17 @@ class TestPose2(GtsamTestCase): | | | O---O """ - # fmt: off pts_a = [ Point2(3, 1), Point2(1, 1), Point2(1, 3), - Point2(3, 3) + Point2(3, 3), ] pts_b = [ Point2(1, -3), Point2(1, -5), Point2(-1, -5), - Point2(-1, -3) + Point2(-1, -3), ] # fmt: on @@ -65,7 +63,7 @@ class TestPose2(GtsamTestCase): 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() From 1205df2c07757d20b01d43827bc88f8b0f696884 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 1 Sep 2021 15:32:43 -0400 Subject: [PATCH 14/14] update documentation for boost::optional binding --- python/gtsam/preamble/geometry.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h index 9ddb6e208..35fe2a577 100644 --- a/python/gtsam/preamble/geometry.h +++ b/python/gtsam/preamble/geometry.h @@ -12,7 +12,7 @@ */ #include -// Support for binding boost::optional types. +// Support for binding boost::optional types in C++11. // https://pybind11.readthedocs.io/en/stable/advanced/cast/stl.html namespace pybind11 { namespace detail { template