From bf0651b626d1adf34dcd550f0d3f654eab084206 Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Sun, 20 Sep 2020 11:45:30 -0400 Subject: [PATCH] Refactor Align with short functions. --- gtsam_unstable/geometry/Similarity3.cpp | 82 ++++++++++++------------- gtsam_unstable/geometry/Similarity3.h | 14 ++++- 2 files changed, 52 insertions(+), 44 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 5b35b9978..d6515e022 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -96,33 +96,19 @@ Point3 Similarity3::operator*(const Point3& p) const { return transformFrom(p); } -// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 -Similarity3 Similarity3::AlignGivenR(const std::vector& abPointPairs, const Rot3& aRb) { - // calculate centroids - Point3 aCentroid, bCentroid; - std::tie(aCentroid, bCentroid) = mean(abPointPairs); - - // calculate scale - double x = 0, y = 0; +std::vector Similarity3::SubtractCentroids(const std::vector& abPointPairs, const Point3& aCentroid, const Point3& bCentroid) { + std::vector d_abPointPairs; Point3 aPoint, bPoint; for (const Point3Pair& abPair : abPointPairs) { std::tie(aPoint, bPoint) = abPair; - const Point3 da = aPoint - aCentroid; - const Point3 db = bPoint - bCentroid; - Vector3 Rdb = aRb * db; - y += da.transpose() * Rdb; - x += Rdb.transpose() * Rdb; + Point3 da = aPoint - aCentroid; + Point3 db = bPoint - bCentroid; + d_abPointPairs.emplace_back(da, db); } - const double s = y / x; - - // calculate translation - const Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s; - return Similarity3(aRb, aTb, s); + return d_abPointPairs; } -// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 -Similarity3 Similarity3::AlignGivenR(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid) { - // calculate scale +std::pair Similarity3::GetXY(const std::vector& d_abPointPairs, const Rot3& aRb) { double x = 0, y = 0; Point3 da, db; for (const Point3Pair& d_abPair : d_abPointPairs) { @@ -131,38 +117,52 @@ Similarity3 Similarity3::AlignGivenR(const std::vector& d_abPointPai y += da.transpose() * Rdb; x += Rdb.transpose() * Rdb; } - const double s = y / x; + return std::make_pair(x, y); +} +Matrix3 Similarity3::GetH(const std::vector& d_abPointPairs) { + Matrix3 H = Z_3x3; + Point3 da, db; + for (const Point3Pair& d_abPair : d_abPointPairs) { + std::tie(da, db) = d_abPair; + H += da * db.transpose(); + } + return H; +} + +Similarity3 Similarity3::AlignGivenRandCentroids(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid) { + double x, y; + std::tie(x, y) = GetXY(d_abPointPairs, aRb); + const double s = y / x; // calculate translation const Point3 aTb = (aCentroid - s * (aRb * bCentroid)) / s; return Similarity3(aRb, aTb, s); } -// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 -Similarity3 Similarity3::Align(const std::vector& abPointPairs) { - const size_t n = abPointPairs.size(); - if (n < 3) throw std::runtime_error("input should have at least 3 pairs of points"); // we need at least three pairs - +Similarity3 Similarity3::AlignGivenR(const std::vector& abPointPairs, const Rot3& aRb) { + // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 // calculate centroids Point3 aCentroid, bCentroid; std::tie(aCentroid, bCentroid) = mean(abPointPairs); + // substract centroids + std::vector d_abPointPairs = SubtractCentroids(abPointPairs, aCentroid, bCentroid); + return AlignGivenRandCentroids(d_abPointPairs, aRb, aCentroid, bCentroid); +} - // Add to form H matrix - Matrix3 H = Z_3x3; - Point3 aPoint, bPoint; - std::vector d_abPointPairs; - for (const Point3Pair& abPair : abPointPairs) { - std::tie(aPoint, bPoint) = abPair; - Point3 da = aPoint - aCentroid; - Point3 db = bPoint - bCentroid; - d_abPointPairs.emplace_back(da, db); - H += da * db.transpose(); - } - +Similarity3 Similarity3::Align(const std::vector& abPointPairs) { + // Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3 + const size_t n = abPointPairs.size(); + if (n < 3) throw std::runtime_error("input should have at least 3 pairs of points"); // we need at least three pairs + // calculate centroids + Point3 aCentroid, bCentroid; + std::tie(aCentroid, bCentroid) = mean(abPointPairs); + // substract centroids + std::vector d_abPointPairs = SubtractCentroids(abPointPairs, aCentroid, bCentroid); + // form H matrix + Matrix3 H = GetH(d_abPointPairs); // ClosestTo finds rotation matrix closest to H in Frobenius sense Rot3 aRb = Rot3::ClosestTo(H); - - return AlignGivenR(d_abPointPairs, aRb, aCentroid, bCentroid); + return AlignGivenRandCentroids(d_abPointPairs, aRb, aCentroid, bCentroid); } Similarity3 Similarity3::Align(const std::vector& abPosePairs) { diff --git a/gtsam_unstable/geometry/Similarity3.h b/gtsam_unstable/geometry/Similarity3.h index 9b34f3095..d596d1193 100644 --- a/gtsam_unstable/geometry/Similarity3.h +++ b/gtsam_unstable/geometry/Similarity3.h @@ -211,12 +211,20 @@ private: /// Calculate expmap and logmap coefficients. static Matrix3 GetV(Vector3 w, double lambda); - /// This method estimates the similarity transform from point pairs, given a known or estimated rotation. - static Similarity3 AlignGivenR(const std::vector& abPointPairs, const Rot3& aRb); + /// Subtract centroids from point pairs. + static std::vector SubtractCentroids(const std::vector& abPointPairs, const Point3& aCentroid, const Point3& bCentroid); + + /// Form inner products x and y. + static std::pair GetXY(const std::vector& d_abPointPairs, const Rot3& aRb); + + /// Form outer product H. + static Matrix3 GetH(const std::vector& d_abPointPairs); /// This method estimates the similarity transform from differences point pairs, given a known or estimated rotation and point centroids. - static Similarity3 AlignGivenR(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid); + static Similarity3 AlignGivenRandCentroids(const std::vector& d_abPointPairs, const Rot3& aRb, const Point3& aCentroid, const Point3& bCentroid); + /// This method estimates the similarity transform from point pairs, given a known or estimated rotation. + static Similarity3 AlignGivenR(const std::vector& abPointPairs, const Rot3& aRb); /// @} };