Pose3 improvements

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) {

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
   /// @{

gtsam/geometry/geometry.i

@@ -431,6 +431,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 +473,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;

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 transformTo 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()