12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

name change - no "All" needed

release/4.3a0
Frank Dellaert 2022-03-26 16:06:06 -04:00
parent 08bbcc889e
commit 96fb72eb54
4 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Pose3.cpp
View File

@ -354,7 +354,7 @@ Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself,
return R_ * point + t_; return R_ * point + t_;
} }
Matrix Pose3::transformAllFrom(const Matrix& points) const { Matrix Pose3::transformFrom(const Matrix& points) const {
if (points.rows() != 3) { if (points.rows() != 3) {
throw std::invalid_argument("Pose3:transformFrom expects 3*N matrix."); 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; return q;
} }
Matrix Pose3::transformAllTo(const Matrix& points) const { Matrix Pose3::transformTo(const Matrix& points) const {
if (points.rows() != 3) { if (points.rows() != 3) {
throw std::invalid_argument("Pose3:transformTo expects 3*N matrix."); throw std::invalid_argument("Pose3:transformTo expects 3*N matrix.");
} }

4
gtsam/geometry/Pose3.h
View File

@ -254,7 +254,7 @@ public:
* @param points 3*N matrix in Pose coordinates * @param points 3*N matrix in Pose coordinates
* @return points in world coordinates, as 3*N Matrix * @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 */ /** syntactic sugar for transformFrom */
inline Point3 operator*(const Point3& point) const { inline Point3 operator*(const Point3& point) const {
@ -276,7 +276,7 @@ public:
* @param points 3*N matrix in world coordinates * @param points 3*N matrix in world coordinates
* @return points in Pose coordinates, as 3*N Matrix * @return points in Pose coordinates, as 3*N Matrix
*/ */
Matrix transformAllTo(const Matrix& points) const; Matrix transformTo(const Matrix& points) const;
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface

4
gtsam/geometry/geometry.i
View File

@ -471,8 +471,8 @@ class Pose3 {
gtsam::Point3 transformTo(const gtsam::Point3& point) const; gtsam::Point3 transformTo(const gtsam::Point3& point) const;
// Matrix versions // Matrix versions
Matrix transformAllFrom(const Matrix& points) const; Matrix transformFrom(const Matrix& points) const;
Matrix transformAllTo(const Matrix& points) const; Matrix transformTo(const Matrix& points) const;
// Standard Interface // Standard Interface
gtsam::Rot3 rotation() const; gtsam::Rot3 rotation() const;

14
python/gtsam/tests/test_Pose3.py
View File

@ -15,7 +15,7 @@ import unittest
import numpy as np import numpy as np
import gtsam import gtsam
from gtsam import Point3, Pose3, Rot3 from gtsam import Point3, Pose3, Rot3, Point3Pairs
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
@ -32,31 +32,31 @@ class TestPose3(GtsamTestCase):
def test_transformTo(self): def test_transformTo(self):
"""Test transformTo method.""" """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)) actual = pose.transformTo(Point3(3, 2, 10))
expected = Point3(2, 1, 10) expected = Point3(2, 1, 10)
self.gtsamAssertEquals(actual, expected, 1e-6) self.gtsamAssertEquals(actual, expected, 1e-6)
# multi-point version # multi-point version
points = np.stack([Point3(3, 2, 10), Point3(3, 2, 10)]).T 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)) self.assertEqual(actual_array.shape, (3, 2))
expected_array = np.stack([expected, expected]).T 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): def test_transformFrom(self):
"""Test transformTo method.""" """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)) actual = pose.transformFrom(Point3(2, 1, 10))
expected = Point3(3, 2, 10) expected = Point3(3, 2, 10)
self.gtsamAssertEquals(actual, expected, 1e-6) self.gtsamAssertEquals(actual, expected, 1e-6)
# multi-point version # multi-point version
points = np.stack([Point3(2, 1, 10), Point3(2, 1, 10)]).T 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)) self.assertEqual(actual_array.shape, (3, 2))
expected_array = np.stack([expected, expected]).T 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): def test_range(self):
"""Test range method.""" """Test range method."""