transformAllFrom/To

gtsam/geometry/Pose3.cpp

@@ -354,6 +354,17 @@ Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself,
   return R_ * point + t_;
 }
 
+Matrix Pose3::transformAllFrom(const Matrix& points) const {
+  if (points.rows() != 3) {
+    throw std::invalid_argument("Pose3:transformFrom expects 3*N matrix.");
+  }
+  Matrix result(3, points.cols());
+  for (size_t j=0; j < points.cols(); j++) {
+    result.col(j) = transformFrom(Point3(points.col(j)));
+  }
+  return result;
+}
+
 /* ************************************************************************* */
 Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself,
     OptionalJacobian<3, 3> Hpoint) const {
@@ -374,6 +385,17 @@ Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself,
   return q;
 }
 
+Matrix Pose3::transformAllTo(const Matrix& points) const {
+  if (points.rows() != 3) {
+    throw std::invalid_argument("Pose3:transformTo expects 3*N matrix.");
+  }
+  Matrix result(3, points.cols());
+  for (size_t j=0; j < points.cols(); j++) {
+    result.col(j) = transformTo(Point3(points.col(j)));
+  }
+  return result;
+}
+
 /* ************************************************************************* */
 double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself,
                     OptionalJacobian<1, 3> Hpoint) const {

gtsam/geometry/Pose3.h

@@ -249,6 +249,13 @@ public:
   Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself =
       boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
 
+  /**
+   * @brief transform many points in Pose coordinates and transform to world.
+   * @param points 3*N matrix in Pose coordinates
+   * @return points in world coordinates, as 3*N Matrix
+   */
+  Matrix transformAllFrom(const Matrix& points) const;
+
   /** syntactic sugar for transformFrom */
   inline Point3 operator*(const Point3& point) const {
     return transformFrom(point);
@@ -264,6 +271,13 @@ public:
   Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself =
       boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const;
 
+  /**
+   * @brief transform many points in world coordinates and transform to Pose.
+   * @param points 3*N matrix in world coordinates
+   * @return points in Pose coordinates, as 3*N Matrix
+   */
+  Matrix transformAllTo(const Matrix& points) const;
+
   /// @}
   /// @name Standard Interface
   /// @{

gtsam/geometry/geometry.i

@@ -470,6 +470,10 @@ class Pose3 {
   gtsam::Point3 transformFrom(const gtsam::Point3& point) const;
   gtsam::Point3 transformTo(const gtsam::Point3& point) const;
 
+  // Matrix versions
+  Matrix transformAllFrom(const Matrix& points) const;
+  Matrix transformAllTo(const Matrix& points) const;
+
   // Standard Interface
   gtsam::Rot3 rotation() const;
   gtsam::Point3 translation() const;

python/gtsam/tests/test_Pose3.py

@@ -30,13 +30,34 @@ class TestPose3(GtsamTestCase):
         actual = T2.between(T3)
         self.gtsamAssertEquals(actual, expected, 1e-6)
 
-    def test_transform_to(self):
+    def test_transformTo(self):
         """Test transformTo method."""
-        transform = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
+        pose = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
-        actual = transform.transformTo(Point3(3, 2, 10))
+        actual = pose.transformTo(Point3(3, 2, 10))
         expected = Point3(2, 1, 10)
         self.gtsamAssertEquals(actual, expected, 1e-6)
 
+        # multi-point version
+        points = np.stack([Point3(3, 2, 10), Point3(3, 2, 10)]).T
+        actual_array = pose.transformAllTo(points)
+        self.assertEqual(actual_array.shape, (3, 2))
+        expected_array = np.stack([expected, expected]).T
+        np.testing.assert_allclose(actual_array, expected_array)
+
+    def test_transformFrom(self):
+        """Test transformTo method."""
+        pose = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0))
+        actual = pose.transformFrom(Point3(2, 1, 10))
+        expected = Point3(3, 2, 10)
+        self.gtsamAssertEquals(actual, expected, 1e-6)
+
+        # multi-point version
+        points = np.stack([Point3(2, 1, 10), Point3(2, 1, 10)]).T
+        actual_array = pose.transformAllFrom(points)
+        self.assertEqual(actual_array.shape, (3, 2))
+        expected_array = np.stack([expected, expected]).T
+        np.testing.assert_allclose(actual_array, expected_array)
+
     def test_range(self):
         """Test range method."""
         l1 = Point3(1, 0, 0)