diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index a0b3d502e..cc7f8e474 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -213,6 +213,14 @@ Point2 Pose2::transformTo(const Point2& point, return q; } +Matrix Pose2::transformTo(const Matrix& points) const { + if (points.rows() != 2) { + throw std::invalid_argument("Pose2:transformTo expects 2*N matrix."); + } + const Matrix2 Rt = rotation().transpose(); + return Rt * (points.colwise() - t_); // Eigen broadcasting! +} + /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transformFrom(const Point2& point, @@ -224,6 +232,15 @@ Point2 Pose2::transformFrom(const Point2& point, return q + t_; } + +Matrix Pose2::transformFrom(const Matrix& points) const { + if (points.rows() != 2) { + throw std::invalid_argument("Pose2:transformFrom expects 2*N matrix."); + } + const Matrix2 R = rotation().matrix(); + return (R * points).colwise() + t_; // Eigen broadcasting! +} + /* ************************************************************************* */ Rot2 Pose2::bearing(const Point2& point, OptionalJacobian<1, 3> Hpose, OptionalJacobian<1, 2> Hpoint) const { diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index a54951728..1e79836f5 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -199,13 +199,29 @@ public: OptionalJacobian<2, 3> Dpose = boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const; + /** + * @brief transform many points in world coordinates and transform to Pose. + * @param points 2*N matrix in world coordinates + * @return points in Pose coordinates, as 2*N Matrix + */ + Matrix transformTo(const Matrix& points) const; + /** Return point coordinates in global frame */ GTSAM_EXPORT Point2 transformFrom(const Point2& point, OptionalJacobian<2, 3> Dpose = boost::none, OptionalJacobian<2, 2> Dpoint = boost::none) const; + /** + * @brief transform many points in Pose coordinates and transform to world. + * @param points 2*N matrix in Pose coordinates + * @return points in world coordinates, as 2*N Matrix + */ + Matrix transformFrom(const Matrix& points) const; + /** syntactic sugar for transformFrom */ - inline Point2 operator*(const Point2& point) const { return transformFrom(point);} + inline Point2 operator*(const Point2& point) const { + return transformFrom(point); + } /// @} /// @name Standard Interface diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 6a0f1d3ad..5aeac37ec 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -406,6 +406,10 @@ class Pose2 { gtsam::Point2 transformFrom(const gtsam::Point2& p) const; gtsam::Point2 transformTo(const gtsam::Point2& p) const; + // Matrix versions + Matrix transformFrom(const Matrix& points) const; + Matrix transformTo(const Matrix& points) const; + // Standard Interface double x() const; double y() const; diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index e5ffbad7d..860487db2 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -8,11 +8,11 @@ See LICENSE for the license information Pose2 unit tests. Author: Frank Dellaert & Duy Nguyen Ta & John Lambert """ +import math import unittest -import numpy as np - import gtsam +import numpy as np from gtsam import Point2, Point2Pairs, Pose2 from gtsam.utils.test_case import GtsamTestCase @@ -26,6 +26,34 @@ class TestPose2(GtsamTestCase): actual = Pose2.adjoint_(xi, xi) np.testing.assert_array_equal(actual, expected) + def test_transformTo(self): + """Test transformTo method.""" + pose = Pose2(2, 4, -math.pi/2) + actual = pose.transformTo(Point2(3, 2)) + expected = Point2(2, 1) + self.gtsamAssertEquals(actual, expected, 1e-6) + + # multi-point version + points = np.stack([Point2(3, 2), Point2(3, 2)]).T + actual_array = pose.transformTo(points) + self.assertEqual(actual_array.shape, (2, 2)) + expected_array = np.stack([expected, expected]).T + np.testing.assert_allclose(actual_array, expected_array, atol=1e-6) + + def test_transformFrom(self): + """Test transformFrom method.""" + pose = Pose2(2, 4, -math.pi/2) + actual = pose.transformFrom(Point2(2, 1)) + expected = Point2(3, 2) + self.gtsamAssertEquals(actual, expected, 1e-6) + + # multi-point version + points = np.stack([Point2(2, 1), Point2(2, 1)]).T + actual_array = pose.transformFrom(points) + self.assertEqual(actual_array.shape, (2, 2)) + expected_array = np.stack([expected, expected]).T + np.testing.assert_allclose(actual_array, expected_array, atol=1e-6) + def test_align(self) -> None: """Ensure estimation of the Pose2 element to align two 2d point clouds succeeds. diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py index c37b95c79..cde71de53 100644 --- a/python/gtsam/tests/test_Pose3.py +++ b/python/gtsam/tests/test_Pose3.py @@ -45,7 +45,7 @@ class TestPose3(GtsamTestCase): np.testing.assert_allclose(actual_array, expected_array, atol=1e-6) def test_transformFrom(self): - """Test transformTo method.""" + """Test transformFrom method.""" pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0)) actual = pose.transformFrom(Point3(2, 1, 10)) expected = Point3(3, 2, 10)