Merge pull request #1148 from borglab/feature/transformAll

dellaert · web-flow · commit c2c54bcd7dc7 · 2022-03-27T00:24:34.000-04:00
diff --git 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
@@ -199,13 +199,29 @@ class Pose2: public LieGroup<Pose2, 3> {
       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/Pose3.cpp b/gtsam/geometry/Pose3.cpp
@@ -354,6 +354,14 @@ Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself,
   return R_ * point + t_;
 }
 
+Matrix Pose3::transformFrom(const Matrix& points) const {
+  if (points.rows() != 3) {
+    throw std::invalid_argument("Pose3:transformFrom expects 3*N matrix.");
+  }
+  const Matrix3 R = R_.matrix();
+  return (R * points).colwise() + t_;  // Eigen broadcasting!
+}
+
 /* ************************************************************************* */
 Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself,
     OptionalJacobian<3, 3> Hpoint) const {
@@ -374,6 +382,14 @@ Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself,
   return q;
 }
 
+Matrix Pose3::transformTo(const Matrix& points) const {
+  if (points.rows() != 3) {
+    throw std::invalid_argument("Pose3:transformTo expects 3*N matrix.");
+  }
+  const Matrix3 Rt = R_.transpose();
+  return Rt * (points.colwise() - t_);  // Eigen broadcasting!
+}
+
 /* ************************************************************************* */
 double Pose3::range(const Point3& point, OptionalJacobian<1, 6> Hself,
                     OptionalJacobian<1, 3> Hpoint) const {
@@ -431,7 +447,7 @@ Unit3 Pose3::bearing(const Pose3& pose, OptionalJacobian<2, 6> Hself,
 boost::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
   const size_t n = abPointPairs.size();
   if (n < 3) {
-    return boost::none; // we need at least three pairs
+    return boost::none;  // we need at least three pairs
   }
 
   // calculate centroids
@@ -451,6 +467,18 @@ boost::optional<Pose3> Pose3::Align(const Point3Pairs &abPointPairs) {
   return Pose3(aRb, aTb);
 }
 
+boost::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
+  if (a.rows() != 3 || b.rows() != 3 || a.cols() != b.cols()) {
+    throw std::invalid_argument(
+      "Pose3:Align expects 3*N matrices of equal shape.");
+  }
+  Point3Pairs abPointPairs;
+  for (size_t j=0; j < a.cols(); j++) {
+    abPointPairs.emplace_back(a.col(j), b.col(j));
+  }
+  return Pose3::Align(abPointPairs);
+}
+
 boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
   Point3Pairs abPointPairs;
   for (const Point3Pair &baPair : baPointPairs) {
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
@@ -85,6 +85,9 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
    */
   static boost::optional<Pose3> Align(const std::vector<Point3Pair>& abPointPairs);
 
+  // Version of Pose3::Align that takes 2 matrices.
+  static boost::optional<Pose3> Align(const Matrix& a, const Matrix& b);
+
   /// @}
   /// @name Testable
   /// @{
@@ -249,6 +252,13 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
   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 transformFrom(const Matrix& points) const;
+
   /** syntactic sugar for transformFrom */
   inline Point3 operator*(const Point3& point) const {
     return transformFrom(point);
@@ -264,6 +274,13 @@ class GTSAM_EXPORT Pose3: public LieGroup<Pose3, 6> {
   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 transformTo(const Matrix& points) const;
+
   /// @}
   /// @name Standard Interface
   /// @{
diff --git 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;
@@ -431,6 +435,9 @@ class Pose3 {
   Pose3(const gtsam::Pose2& pose2);
   Pose3(Matrix mat);
 
+  static boost::optional<gtsam::Pose3> Align(const gtsam::Point3Pairs& abPointPairs);
+  static boost::optional<gtsam::Pose3> Align(const gtsam::Matrix& a, const gtsam::Matrix& b);
+
   // Testable
   void print(string s = "") const;
   bool equals(const gtsam::Pose3& pose, double tol) const;
@@ -470,6 +477,10 @@ class Pose3 {
   gtsam::Point3 transformFrom(const gtsam::Point3& point) const;
   gtsam::Point3 transformTo(const gtsam::Point3& point) const;
 
+  // Matrix versions
+  Matrix transformFrom(const Matrix& points) const;
+  Matrix transformTo(const Matrix& points) const;
+
   // Standard Interface
   gtsam::Rot3 rotation() const;
   gtsam::Point3 translation() const;
diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py
@@ -8,11 +8,11 @@
 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 @@ def test_adjoint(self) -> None:
         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
@@ -15,7 +15,7 @@
 import numpy as np
 
 import gtsam
-from gtsam import Point3, Pose3, Rot3
+from gtsam import Point3, Pose3, Rot3, Point3Pairs
 from gtsam.utils.test_case import GtsamTestCase
 
 
@@ -30,13 +30,34 @@ def test_between(self):
         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))
-        actual = transform.transformTo(Point3(3, 2, 10))
+        pose = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), Point3(2, 4, 0))
+        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.transformTo(points)
+        self.assertEqual(actual_array.shape, (3, 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 = Pose3(Rot3.Rodrigues(0, 0, -math.pi/2), 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.transformFrom(points)
+        self.assertEqual(actual_array.shape, (3, 2))
+        expected_array = np.stack([expected, expected]).T
+        np.testing.assert_allclose(actual_array, expected_array, atol=1e-6)
+
     def test_range(self):
         """Test range method."""
         l1 = Point3(1, 0, 0)
@@ -81,6 +102,24 @@ def test_serialization(self):
         actual.deserialize(serialized)
         self.gtsamAssertEquals(expected, actual, 1e-10)
 
+    def test_align_squares(self):
+        """Test if Align method can align 2 squares."""
+        square = np.array([[0,0,0],[0,1,0],[1,1,0],[1,0,0]], float).T
+        sTt = Pose3(Rot3.Rodrigues(0, 0, -math.pi), Point3(2, 4, 0))
+        transformed = sTt.transformTo(square)
+
+        st_pairs = Point3Pairs()
+        for j in range(4):
+            st_pairs.append((square[:,j], transformed[:,j]))
+
+        # Recover the transformation sTt
+        estimated_sTt = Pose3.Align(st_pairs)
+        self.gtsamAssertEquals(estimated_sTt, sTt, 1e-10)
+
+        # Matrix version
+        estimated_sTt = Pose3.Align(square, transformed)
+        self.gtsamAssertEquals(estimated_sTt, sTt, 1e-10)
+
 
 if __name__ == "__main__":
     unittest.main()
