transformAllFrom/To
							parent
							
								
									a9a4075ff6
								
							
						
					
					
						commit
						08bbcc889e
					
				|  | @ -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 { | ||||
|  |  | |||
|  | @ -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
 | ||||
|   /// @{
 | ||||
|  |  | |||
|  | @ -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; | ||||
|  |  | |||
|  | @ -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, -1.570796), 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.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) | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue