diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 7a522b003..643416826 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -354,7 +354,7 @@ Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself, return R_ * point + t_; } -Matrix Pose3::transformAllFrom(const Matrix& points) const { +Matrix Pose3::transformFrom(const Matrix& points) const { if (points.rows() != 3) { throw std::invalid_argument("Pose3:transformFrom expects 3*N matrix."); } @@ -385,7 +385,7 @@ Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself, return q; } -Matrix Pose3::transformAllTo(const Matrix& points) const { +Matrix Pose3::transformTo(const Matrix& points) const { if (points.rows() != 3) { throw std::invalid_argument("Pose3:transformTo expects 3*N matrix."); } diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 6a9c19d8b..01980d7af 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -254,7 +254,7 @@ public: * @param points 3*N matrix in Pose coordinates * @return points in world coordinates, as 3*N Matrix */ - Matrix transformAllFrom(const Matrix& points) const; + Matrix transformFrom(const Matrix& points) const; /** syntactic sugar for transformFrom */ inline Point3 operator*(const Point3& point) const { @@ -276,7 +276,7 @@ public: * @param points 3*N matrix in world coordinates * @return points in Pose coordinates, as 3*N Matrix */ - Matrix transformAllTo(const Matrix& points) const; + Matrix transformTo(const Matrix& points) const; /// @} /// @name Standard Interface diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index da704d5ed..0fa931dcf 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -471,8 +471,8 @@ class Pose3 { gtsam::Point3 transformTo(const gtsam::Point3& point) const; // Matrix versions - Matrix transformAllFrom(const Matrix& points) const; - Matrix transformAllTo(const Matrix& points) const; + Matrix transformFrom(const Matrix& points) const; + Matrix transformTo(const Matrix& points) const; // Standard Interface gtsam::Rot3 rotation() const; diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py index 736782943..cf96cbadb 100644 --- a/python/gtsam/tests/test_Pose3.py +++ b/python/gtsam/tests/test_Pose3.py @@ -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 @@ -32,31 +32,31 @@ class TestPose3(GtsamTestCase): def test_transformTo(self): """Test transformTo method.""" - pose = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0)) + 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.transformAllTo(points) + 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) + np.testing.assert_allclose(actual_array, expected_array, atol=1e-6) def test_transformFrom(self): """Test transformTo method.""" - pose = Pose3(Rot3.Rodrigues(0, 0, -1.570796), Point3(2, 4, 0)) + 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.transformAllFrom(points) + 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) + np.testing.assert_allclose(actual_array, expected_array, atol=1e-6) def test_range(self): """Test range method."""