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

transformAllFrom/To

release/4.3a0
Frank Dellaert 2022-03-26 15:57:52 -04:00
parent a9a4075ff6
commit 08bbcc889e
4 changed files with 64 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/geometry/Pose3.cpp
View File

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

14
gtsam/geometry/Pose3.h
View File

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

4
gtsam/geometry/geometry.i
View File

@ -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;

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

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