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

Merge pull request #1148 from borglab/feature/transformAll

release/4.3a0
Frank Dellaert 2022-03-27 00:24:34 -04:00 committed by GitHub
parent a9a4075ff6 628ffb1495
commit c2c54bcd7d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 164 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/geometry/Pose2.cpp
View File

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

18
gtsam/geometry/Pose2.h
View File

@ -199,13 +199,29 @@ public:
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

30
gtsam/geometry/Pose3.cpp
View File

@ -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> 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> Pose3::Align(const Point3Pairs &abPointPairs) {
return Pose3(aRb, aTb);
}
boost::optional<Pose3> 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<Pose3> align(const Point3Pairs &baPointPairs) {
Point3Pairs abPointPairs;
for (const Point3Pair &baPair : baPointPairs) {

17
gtsam/geometry/Pose3.h
View File

@ -85,6 +85,9 @@ public:
*/
static boost::optional<Pose3> Align(const std::vector<Point3Pair>& abPointPairs);
// Version of Pose3::Align that takes 2 matrices.
static boost::optional<Pose3> Align(const Matrix& a, const Matrix& b);
/// @}
/// @name Testable
/// @{
@ -249,6 +252,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 transformFrom(const Matrix& points) const;
/** syntactic sugar for transformFrom */
inline Point3 operator*(const Point3& point) const {
return transformFrom(point);
@ -264,6 +274,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 transformTo(const Matrix& points) const;
/// @}
/// @name Standard Interface
/// @{

11
gtsam/geometry/geometry.i
View File

@ -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<gtsam::Pose3> Align(const gtsam::Point3Pairs& abPointPairs);
static boost::optional<gtsam::Pose3> 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;

32
python/gtsam/tests/test_Pose2.py
View File

@ -8,11 +8,11 @@ See LICENSE for the license information
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 @@ class TestPose2(GtsamTestCase):
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.

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

@ -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
@ -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, -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 @@ class TestPose3(GtsamTestCase):
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()