diff --git a/gtsam/geometry/Point3.cpp b/gtsam/geometry/Point3.cpp index 7a46f5988..ce4ceee89 100644 --- a/gtsam/geometry/Point3.cpp +++ b/gtsam/geometry/Point3.cpp @@ -75,6 +75,19 @@ double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1, return p.x() * q.x() + p.y() * q.y() + p.z() * q.z(); } +Point3Pair mean(const std::vector &abPointPairs) { + const size_t n = abPointPairs.size(); + Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0); + for (const Point3Pair &abPair : abPointPairs) { + aCentroid += abPair.first; + bCentroid += abPair.second; + } + const double f = 1.0 / n; + aCentroid *= f; + bCentroid *= f; + return make_pair(aCentroid, bCentroid); +} + /* ************************************************************************* */ ostream &operator<<(ostream &os, const gtsam::Point3Pair &p) { os << p.first << " <-> " << p.second; diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h index 19e328022..7f58497e9 100644 --- a/gtsam/geometry/Point3.h +++ b/gtsam/geometry/Point3.h @@ -26,6 +26,7 @@ #include #include #include +#include namespace gtsam { @@ -58,6 +59,17 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q, OptionalJacobian<1, 3> H_p = boost::none, OptionalJacobian<1, 3> H_q = boost::none); +/// mean +template +GTSAM_EXPORT Point3 mean(const CONTAINER& points) { + Point3 sum(0, 0, 0); + sum = std::accumulate(points.begin(), points.end(), sum); + return sum / points.size(); +} + +/// mean of Point3 pair +GTSAM_EXPORT Point3Pair mean(const std::vector& abPointPairs); + template struct Range; diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 2f0802cab..990ffdfe2 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -368,6 +368,9 @@ inline Matrix wedge(const Vector& xi) { return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5)); } +// Convenience typedef +using Pose3Pair = std::pair; + // For MATLAB wrapper typedef std::vector Pose3Vector; diff --git a/gtsam/geometry/tests/testPoint3.cpp b/gtsam/geometry/tests/testPoint3.cpp index a7c2ac50c..a481a8072 100644 --- a/gtsam/geometry/tests/testPoint3.cpp +++ b/gtsam/geometry/tests/testPoint3.cpp @@ -164,6 +164,26 @@ TEST (Point3, normalize) { EXPECT(assert_equal(expectedH, actualH, 1e-8)); } +//************************************************************************* +TEST(Point3, mean) { + Point3 expected_a_mean(2, 2, 2); + Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3); + std::vector a_points{a1, a2, a3}; + Point3 actual_a_mean = mean(a_points); + EXPECT(assert_equal(expected_a_mean, actual_a_mean)); +} + +TEST(Point3, mean_pair) { + Point3 a_mean(2, 2, 2), b_mean(-1, 1, 0); + Point3Pair expected_mean = std::make_pair(a_mean, b_mean); + Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3); + Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0); + std::vector point_pairs{{a1,b1},{a2,b2},{a3,b3}}; + Point3Pair actual_mean = mean(point_pairs); + EXPECT(assert_equal(expected_mean.first, actual_mean.first)); + EXPECT(assert_equal(expected_mean.second, actual_mean.second)); +} + //************************************************************************* double norm_proxy(const Point3& point) { return double(point.norm()); diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index d40fb0b59..b2d7dc080 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -19,9 +19,61 @@ #include #include +#include namespace gtsam { +namespace { +/// Subtract centroids from point pairs. +static std::vector subtractCentroids(const std::vector& abPointPairs, const Point3Pair& centroids) { + std::vector d_abPointPairs; + for (const Point3Pair& abPair : abPointPairs) { + Point3 da = abPair.first - centroids.first; + Point3 db = abPair.second - centroids.second; + d_abPointPairs.emplace_back(da, db); + } + return d_abPointPairs; +} + +/// Form inner products x and y and calculate scale. +static const double calculateScale(const std::vector& d_abPointPairs, const Rot3& aRb) { + double x = 0, y = 0; + Point3 da, db; + for (const Point3Pair& d_abPair : d_abPointPairs) { + std::tie(da, db) = d_abPair; + const Vector3 da_prime = aRb * db; + y += da.transpose() * da_prime; + x += da_prime.transpose() * da_prime; + } + const double s = y / x; + return s; +} + +/// Form outer product H. +static Matrix3 calculateH(const std::vector& d_abPointPairs) { + Matrix3 H = Z_3x3; + for (const Point3Pair& d_abPair : d_abPointPairs) { + H += d_abPair.first * d_abPair.second.transpose(); + } + return H; +} + +/// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. +static Similarity3 align(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3Pair& centroids) { + const double s = calculateScale(d_abPointPairs, aRb); + const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s; + return Similarity3(aRb, aTb, s); +} + +/// This method estimates the similarity transform from point pairs, given a known or estimated rotation. +// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 +static Similarity3 alignGivenR(const std::vector& abPointPairs, const Rot3& aRb) { + auto centroids = mean(abPointPairs); + auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); + return align(d_abPointPairs, aRb, centroids); +} +} // namespace + Similarity3::Similarity3() : t_(0,0,0), s_(1) { } @@ -54,15 +106,15 @@ bool Similarity3::operator==(const Similarity3& other) const { void Similarity3::print(const std::string& s) const { std::cout << std::endl; std::cout << s; - rotation().print("R:\n"); - std::cout << "t: " << translation().transpose() << "s: " << scale() << std::endl; + rotation().print("\nR:\n"); + std::cout << "t: " << translation().transpose() << " s: " << scale() << std::endl; } Similarity3 Similarity3::identity() { return Similarity3(); } -Similarity3 Similarity3::operator*(const Similarity3& T) const { - return Similarity3(R_ * T.R_, ((1.0 / T.s_) * t_) + R_ * T.t_, s_ * T.s_); +Similarity3 Similarity3::operator*(const Similarity3& S) const { + return Similarity3(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_); } Similarity3 Similarity3::inverse() const { @@ -85,10 +137,47 @@ 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_ * T.translation() + t_)); + return Pose3(R, t); +} + Point3 Similarity3::operator*(const Point3& p) const { return transformFrom(p); } +Similarity3 Similarity3::Align(const std::vector& abPointPairs) { + // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 + if (abPointPairs.size() < 3) throw std::runtime_error("input should have at least 3 pairs of points"); + auto centroids = mean(abPointPairs); + auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); + Matrix3 H = calculateH(d_abPointPairs); + // ClosestTo finds rotation matrix closest to H in Frobenius sense + Rot3 aRb = Rot3::ClosestTo(H); + return align(d_abPointPairs, aRb, centroids); +} + +Similarity3 Similarity3::Align(const std::vector& abPosePairs) { + const size_t n = abPosePairs.size(); + if (n < 2) throw std::runtime_error("input should have at least 2 pairs of poses"); + + // calculate rotation + vector rotations; + vector abPointPairs; + rotations.reserve(n); + abPointPairs.reserve(n); + Pose3 wTa, wTb; + for (const Pose3Pair& abPair : abPosePairs) { + std::tie(wTa, wTb) = abPair; + rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse())); + abPointPairs.emplace_back(wTa.translation(), wTb.translation()); + } + const Rot3 aRb = FindKarcherMean(rotations); + + return alignGivenR(abPointPairs, aRb); +} + 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..b82862ddb 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -19,10 +19,12 @@ #include #include +#include #include #include #include + namespace gtsam { // Forward declarations @@ -87,7 +89,7 @@ public: GTSAM_UNSTABLE_EXPORT static Similarity3 identity(); /// Composition - GTSAM_UNSTABLE_EXPORT Similarity3 operator*(const Similarity3& T) const; + GTSAM_UNSTABLE_EXPORT Similarity3 operator*(const Similarity3& S) const; /// Return the inverse GTSAM_UNSTABLE_EXPORT Similarity3 inverse() const; @@ -101,9 +103,32 @@ public: OptionalJacobian<3, 7> H1 = boost::none, // OptionalJacobian<3, 3> H2 = boost::none) const; + /** + * Action on a pose T. + * |Rs ts| |R t| |Rs*R Rs*t+ts| + * |0 1/s| * |0 1| = | 0 1/s |, the result is still a Sim3 object. + * To retrieve a Pose3, we normalized the scale value into 1. + * |Rs*R Rs*t+ts| |Rs*R s(Rs*t+ts)| + * | 0 1/s | = | 0 1 | + * + * This group action satisfies the compatibility condition. + * For more details, refer to: https://en.wikipedia.org/wiki/Group_action + */ + 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 at least three point pairs + */ + GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector& abPointPairs); + + /** + * Create Similarity3 by aligning at least two pose pairs + */ + GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector& abPosePairs); + /// @} /// @name Lie Group /// @{ @@ -182,8 +207,8 @@ public: /// @name Helper functions /// @{ - /// Calculate expmap and logmap coefficients. private: + /// Calculate expmap and logmap coefficients. static Matrix3 GetV(Vector3 w, double lambda); /// @} diff --git a/gtsam_unstable/geometry/tests/testSimilarity3.cpp b/gtsam_unstable/geometry/tests/testSimilarity3.cpp index d23346896..b985eb374 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,114 @@ TEST(Similarity3, GroupAction) { } } +//****************************************************************************** +// Group action on Pose3 +TEST(Similarity3, GroupActionPose3) { + Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); + + // Create source poses + Pose3 Ta1(Rot3(), Point3(0, 0, 0)); + Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0)); + + // Create destination poses + Pose3 expected_Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); + Pose3 expected_Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10)); + + EXPECT(assert_equal(expected_Tb1, bSa.transformFrom(Ta1))); + EXPECT(assert_equal(expected_Tb2, bSa.transformFrom(Ta2))); +} + +// Test left group action compatibility. +// cSa*Ta = cSb*bSa*Ta +TEST(Similarity3, GroupActionPose3_Compatibility) { + Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); + Similarity3 cSb(Rot3::Ry(90 * degree), Point3(-10, -4, 0), 3.0); + Similarity3 cSa(Rot3::Ry(270 * degree), Point3(0, 1, -2), 6.0); + + // Create poses + Pose3 Ta1(Rot3(), Point3(0, 0, 0)); + Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0)); + Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); + Pose3 Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10)); + Pose3 Tc1(Rot3(0, 0, -1, 0, 1, 0, 1, 0, 0), Point3(0, 6, -12)); + Pose3 Tc2(Rot3(0, 0, -1, 0, -1, 0, -1, 0, 0), Point3(0, 6, 12)); + + EXPECT(assert_equal(Tc1, cSb.transformFrom(Tb1))); + EXPECT(assert_equal(Tc2, cSb.transformFrom(Tb2))); + + EXPECT(assert_equal(cSa.transformFrom(Ta1), cSb.transformFrom(Tb1))); + EXPECT(assert_equal(cSa.transformFrom(Ta2), cSb.transformFrom(Tb2))); +} + +//****************************************************************************** +// Align with Point3 Pairs +TEST(Similarity3, AlignPoint3_1) { + Similarity3 expected_aSb(Rot3::Rz(-90 * degree), Point3(3, 4, 5), 2.0); + + Point3 b1(0, 0, 0), b2(3, 0, 0), b3(3, 0, 4); + + Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1)); + Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2)); + Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3)); + + vector correspondences{ab1, ab2, ab3}; + + Similarity3 actual_aSb = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected_aSb, actual_aSb)); +} + +TEST(Similarity3, AlignPoint3_2) { + Similarity3 expected_aSb(Rot3(), Point3(10, 10, 0), 1.0); + + Point3 b1(0, 0, 0), b2(20, 10, 0), b3(10, 20, 0); + + Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1)); + Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2)); + Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3)); + + vector correspondences{ab1, ab2, ab3}; + + Similarity3 actual_aSb = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected_aSb, actual_aSb)); +} + +TEST(Similarity3, AlignPoint3_3) { + Similarity3 expected_aSb(Rot3::RzRyRx(0.3, 0.2, 0.1), Point3(20, 10, 5), 1.0); + + Point3 b1(0, 0, 1), b2(10, 0, 2), b3(20, -10, 30); + + Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1)); + Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2)); + Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3)); + + vector correspondences{ab1, ab2, ab3}; + + Similarity3 actual_aSb = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected_aSb, actual_aSb)); +} + +//****************************************************************************** +// Align with Pose3 Pairs +TEST(Similarity3, AlignPose3) { + Similarity3 expected_aSb(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0); + + // Create source poses + Pose3 Ta1(Rot3(), Point3(0, 0, 0)); + Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0)); + + // Create destination poses + Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10)); + Pose3 Tb2(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_aSb = Similarity3::Align(correspondences); + EXPECT(assert_equal(expected_aSb, actual_aSb)); +} + //****************************************************************************** // Test very simple prior optimization example TEST(Similarity3, Optimization) {