Merge pull request #1148 from borglab/feature/transformAll
						commit
						c2c54bcd7d
					
				|  | @ -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 { | ||||
|  |  | |||
|  | @ -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
 | ||||
|  |  | |||
|  | @ -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) { | ||||
|  |  | |||
|  | @ -85,6 +85,9 @@ public: | |||
|    */ | ||||
|   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 @@ 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 transformFrom(const Matrix& points) const; | ||||
| 
 | ||||
|   /** syntactic sugar for transformFrom */ | ||||
|   inline Point3 operator*(const Point3& point) const { | ||||
|     return transformFrom(point); | ||||
|  | @ -264,6 +274,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 transformTo(const Matrix& points) const; | ||||
| 
 | ||||
|   /// @}
 | ||||
|   /// @name Standard Interface
 | ||||
|   /// @{
 | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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. | ||||
| 
 | ||||
|  |  | |||
|  | @ -15,7 +15,7 @@ import unittest | |||
| 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 @@ 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)) | ||||
|         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 @@ class TestPose3(GtsamTestCase): | |||
|         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() | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue