Matrix versions for Pose2 group action

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 {

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

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;

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.
 

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)