diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index a0b3d502ea..cc7f8e474e 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -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 { diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index a54951728c..1e79836f5e 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -199,13 +199,29 @@ class Pose2: public LieGroup { 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 diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 88f1281918..5369475976 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -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::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::Align(const Point3Pairs &abPointPairs) { return Pose3(aRb, aTb); } +boost::optional 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 align(const Point3Pairs &baPointPairs) { Point3Pairs abPointPairs; for (const Point3Pair &baPair : baPointPairs) { diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 884d0760ca..c360473499 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -85,6 +85,9 @@ class GTSAM_EXPORT Pose3: public LieGroup { */ static boost::optional Align(const std::vector& abPointPairs); + // Version of Pose3::Align that takes 2 matrices. + static boost::optional Align(const Matrix& a, const Matrix& b); + /// @} /// @name Testable /// @{ @@ -249,6 +252,13 @@ class GTSAM_EXPORT Pose3: public LieGroup { 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 @@ class GTSAM_EXPORT Pose3: public LieGroup { 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 /// @{ diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 1e42966f84..5aeac37ec5 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -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 Align(const gtsam::Point3Pairs& abPointPairs); + static boost::optional 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; diff --git a/python/gtsam/tests/test_Pose2.py b/python/gtsam/tests/test_Pose2.py index e5ffbad7d1..860487db2e 100644 --- a/python/gtsam/tests/test_Pose2.py +++ b/python/gtsam/tests/test_Pose2.py @@ -8,11 +8,11 @@ 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 @@ def test_adjoint(self) -> None: 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. diff --git a/python/gtsam/tests/test_Pose3.py b/python/gtsam/tests/test_Pose3.py index 4118288908..cde71de53a 100644 --- a/python/gtsam/tests/test_Pose3.py +++ b/python/gtsam/tests/test_Pose3.py @@ -15,7 +15,7 @@ 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 @@ def test_between(self): 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 @@ def test_serialization(self): 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()