From 8236d69fa17fc1d5164d032f820e030126ebcc6c Mon Sep 17 00:00:00 2001 From: alexma3312 Date: Sat, 26 Sep 2020 14:09:37 -0400 Subject: [PATCH] Refactor code to increase speed. --- gtsam_unstable/geometry/Similarity3.cpp | 116 +++++++++++------------- 1 file changed, 54 insertions(+), 62 deletions(-) diff --git a/gtsam_unstable/geometry/Similarity3.cpp b/gtsam_unstable/geometry/Similarity3.cpp index 6fc720e75..b2d7dc080 100644 --- a/gtsam_unstable/geometry/Similarity3.cpp +++ b/gtsam_unstable/geometry/Similarity3.cpp @@ -23,61 +23,56 @@ 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; - Point3 da, db; - for (const Point3Pair& d_abPair : d_abPointPairs) { - std::tie(da, db) = d_abPair; - H += da * db.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); - // calculate translation - 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. - static 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 - auto centroids = mean(abPointPairs); - // substract centroids - std::vector d_abPointPairs = subtractCentroids(abPointPairs, centroids); - return align(d_abPointPairs, aRb, centroids); +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) { @@ -154,13 +149,9 @@ Point3 Similarity3::operator*(const Point3& p) const { 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 + if (abPointPairs.size() < 3) throw std::runtime_error("input should have at least 3 pairs of points"); auto centroids = mean(abPointPairs); - // substract centroids - std::vector d_abPointPairs = subtractCentroids(abPointPairs, centroids); - // form H matrix + 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); @@ -169,19 +160,20 @@ Similarity3 Similarity3::Align(const std::vector& abPointPairs) { 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"); // we need at least two pairs + if (n < 2) throw std::runtime_error("input should have at least 2 pairs of poses"); // calculate rotation - vector rotationList; + vector rotations; vector abPointPairs; + rotations.reserve(n); abPointPairs.reserve(n); Pose3 wTa, wTb; for (const Pose3Pair& abPair : abPosePairs) { std::tie(wTa, wTb) = abPair; - rotationList.emplace_back(wTa.rotation().compose(wTb.rotation().inverse())); + rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse())); abPointPairs.emplace_back(wTa.translation(), wTb.translation()); } - const Rot3 aRb = FindKarcherMean(rotationList); + const Rot3 aRb = FindKarcherMean(rotations); return alignGivenR(abPointPairs, aRb); }