diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 159fd2927..57bec4dee 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -356,6 +356,9 @@ inline Matrix wedge(const Vector& xi) { return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5)); } +// Convenience typedef +typedef std::pair Pose3Pair; + // For MATLAB wrapper typedef std::vector Pose3Vector; diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index d40fb0b59..740960f0f 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -85,10 +85,105 @@ Point3 Similarity3::transformFrom(const Point3& p, // return s_ * q; } +Pose3 Similarity3::transformFrom(const Pose3& T) const { + Rot3 R = R_.compose(T.rotation()); + Point3 t = Point3(s_ * (R_.matrix() * T.translation().vector() + t_.vector())); + return Pose3(R, t); +} + Point3 Similarity3::operator*(const Point3& p) const { return transformFrom(p); } +Similarity3 Similarity3::Align(const std::vector& abPointPairs) { + const size_t n = abPointPairs.size(); + if (n < 3) throw std::runtime_error("less than 3 pairs"); // we need at least three pairs + + // calculate centroids + Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0); + for (const Point3Pair& abPair : abPointPairs) { + aCentroid += abPair.first; + bCentroid += abPair.second; + } + double f = 1.0 / n; + aCentroid *= f; + bCentroid *= f; + + // Add to form H matrix + Matrix3 H = Z_3x3; + for (const Point3Pair& abPair : abPointPairs) { + Point3 da = abPair.first - aCentroid; + Point3 db = abPair.second - bCentroid; + H += da * db.transpose(); + } + + // ClosestTo finds rotation matrix closest to H in Frobenius sense + Rot3 aRb = Rot3::ClosestTo(H); + + // Calculate scale + double x = 0; + double y = 0; + for (const Point3Pair& abPair : abPointPairs) { + Point3 da = abPair.first - aCentroid; + Point3 db = abPair.second - bCentroid; + Vector3 Rdb = aRb * db; + y += da.transpose() * Rdb; + x += Rdb.transpose() * Rdb; + } + double s = y / x; + + Point3 aTb = (Point3(aCentroid) - s * (aRb * Point3(bCentroid))) / s; + return Similarity3(aRb, aTb, s); +} + +Rot3 Similarity3::rotationAveraging(const std::vector& rotations, double error) { + Rot3 R = rotations[0]; + const size_t n = rotations.size(); + Vector3 r; + r << 0, 0, 0; + while (1) { + for (const Rot3 R_i : rotations) { + r += Rot3::Logmap(R.inverse().compose(R_i)); + } + r /= n; + if (r.norm() < error) return R; + R = R.compose(Rot3::Expmap(r)); + } +} + +Similarity3 Similarity3::Align(const std::vector& abPosePairs) { + const size_t n = abPosePairs.size(); + if (n < 2) throw std::runtime_error("less than 2 pairs"); // we need at least two pairs + + // calculate centroids + Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0); + vector rotationList; + for (const Pose3Pair& abPair : abPosePairs) { + aCentroid += abPair.first.translation(); + bCentroid += abPair.second.translation(); + rotationList.push_back(abPair.first.rotation().compose(abPair.second.rotation().inverse())); + } + double f = 1.0 / n; + aCentroid *= f; + bCentroid *= f; + Rot3 aRb = Similarity3::rotationAveraging(rotationList); + + // Calculate scale + double x = 0; + double y = 0; + for (const Pose3Pair& abPair : abPosePairs) { + Point3 da = abPair.first.translation() - aCentroid; + Point3 db = abPair.second.translation() - bCentroid; + Vector3 Rdb = aRb * db; + y += da.transpose() * Rdb; + x += Rdb.transpose() * Rdb; + } + double s = y / x; + + Point3 aTb = (Point3(aCentroid) - s * (aRb * Point3(bCentroid))) / s; + return Similarity3(aRb, aTb, s); +} + Matrix4 Similarity3::wedge(const Vector7& xi) { // http://www.ethaneade.org/latex2html/lie/node29.html const auto w = xi.head<3>(); diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index bf4937ed4..06b609eee 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -101,9 +102,27 @@ public: OptionalJacobian<3, 7> H1 = boost::none, // OptionalJacobian<3, 3> H2 = boost::none) const; + /// Action on a pose T + GTSAM_UNSTABLE_EXPORT Pose3 transformFrom(const Pose3& T) const; + /** syntactic sugar for transformFrom */ GTSAM_UNSTABLE_EXPORT Point3 operator*(const Point3& p) const; + /** + * Create Similarity3 by aligning two point pairs + */ + GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector& abPointPairs); + + /** + * Calculate the average rotation from a list of rotations + */ + GTSAM_UNSTABLE_EXPORT static Rot3 rotationAveraging(const std::vector& rotations, double error = 1e-10); + + /** + * Create Similarity3 by aligning two pose pairs + */ + GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector& abPosePairs); + /// @} /// @name Lie Group /// @{ diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index d23346896..770196e20 100644 --- a/gtsam_unstable/geometry/tests/testSimilarity3.cpp +++ b/gtsam_unstable/geometry/tests/testSimilarity3.cpp @@ -51,6 +51,8 @@ static const Similarity3 T4(R, P, s); static const Similarity3 T5(R, P, 10); static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform +const double degree = M_PI / 180; + //****************************************************************************** TEST(Similarity3, Concepts) { BOOST_CONCEPT_ASSERT((IsGroup)); @@ -255,6 +257,107 @@ TEST(Similarity3, GroupAction) { } } +//****************************************************************************** +// Group action on Pose3 +TEST(Similarity3, GroupActionPose3) { + Similarity3 bTa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); + + // Create source poses + Pose3 Ta1 = Pose3(Rot3(), Point3(0, 0, 0)); + Pose3 Ta2 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0)); + + // Create destination poses + Pose3 expectedTb1 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); + Pose3 expectedTb2 = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10)); + + EXPECT(assert_equal(expectedTb1, bTa.transformFrom(Ta1))); + EXPECT(assert_equal(expectedTb2, bTa.transformFrom(Ta2))); +} + +//****************************************************************************** +// Align with Point3 Pairs +TEST(Similarity3, AlignPoint3_1) { + Similarity3 expected(Rot3::Rz(-90 * degree), Point3(3, 4, 5), 2.0); + + Point3 p1 = Point3(0, 0, 0); + Point3 p2 = Point3(3, 0, 0); + Point3 p3 = Point3(3, 0, 4); + + Point3Pair ab1(make_pair(expected.transformFrom(p1), p1)); + Point3Pair ab2(make_pair(expected.transformFrom(p2), p2)); + Point3Pair ab3(make_pair(expected.transformFrom(p3), p3)); + + vector correspondences{ab1, ab2, ab3}; + + Similarity3 actual = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected, actual)); +} + +TEST(Similarity3, AlignPoint3_2) { + Similarity3 expected(Rot3(), Point3(10, 10, 0), 1.0); + + Point3 p1 = Point3(0, 0, 0); + Point3 p2 = Point3(20, 10, 0); + Point3 p3 = Point3(10, 20, 0); + + Point3Pair ab1(make_pair(expected.transformFrom(p1), p1)); + Point3Pair ab2(make_pair(expected.transformFrom(p2), p2)); + Point3Pair ab3(make_pair(expected.transformFrom(p3), p3)); + + vector correspondences{ab1, ab2, ab3}; + + Similarity3 actual = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected, actual)); +} + +TEST(Similarity3, AlignPoint3_3) { + Similarity3 expected(Rot3::RzRyRx(0.3, 0.2, 0.1), Point3(20, 10, 5), 1.0); + + Point3 p1 = Point3(0, 0, 1); + Point3 p2 = Point3(10, 0, 2); + Point3 p3 = Point3(20, -10, 30); + + Point3Pair ab1(make_pair(expected.transformFrom(p1), p1)); + Point3Pair ab2(make_pair(expected.transformFrom(p2), p2)); + Point3Pair ab3(make_pair(expected.transformFrom(p3), p3)); + + vector correspondences{ab1, ab2, ab3}; + + Similarity3 actual = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected, actual)); +} + +//****************************************************************************** +// Rotation Averaging +TEST(Similarity3, RotationAveraging) { + Rot3 expected = Rot3::Rx(90 * degree); + vector rotations{Rot3(), Rot3::Rx(90 * degree), Rot3::Rx(180 * degree)}; + Rot3 actual = Similarity3::rotationAveraging(rotations); + EXPECT(assert_equal(expected, actual)); +} + +//****************************************************************************** +// Align with Pose3 Pairs +TEST(Similarity3, AlignPose3) { + Similarity3 expected(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); + + // Create source poses + Pose3 Ta1 = Pose3(Rot3(), Point3(0, 0, 0)); + Pose3 Ta2 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0)); + + // Create destination poses + Pose3 Tb1 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); + Pose3 Tb2 = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10)); + + Pose3Pair bTa1(make_pair(Tb1, Ta1)); + Pose3Pair bTa2(make_pair(Tb2, Ta2)); + + vector correspondences{bTa1, bTa2}; + + Similarity3 actual = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected, actual)); +} + //****************************************************************************** // Test very simple prior optimization example TEST(Similarity3, Optimization) {