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

Expose Align, and add matrix version

release/4.3a0
Frank Dellaert 2022-03-26 16:33:20 -04:00
parent 96fb72eb54
commit bbf4e48d3c
4 changed files with 35 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/geometry/Pose3.cpp
View File

@ -473,6 +473,17 @@ 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) {

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

3
gtsam/geometry/geometry.i
View File

@ -431,6 +431,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;

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

@ -102,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()