From bf668e58697f5236ba20363b7b100714ea70820b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 6 Feb 2022 00:08:54 -0500 Subject: [PATCH] Similarity2 fixes --- gtsam/geometry/Similarity2.cpp | 180 ++++++++++++++++++--------------- gtsam/geometry/Similarity2.h | 110 +++++++++----------- 2 files changed, 146 insertions(+), 144 deletions(-) diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp index 5a63e70cb..12529f52d 100644 --- a/gtsam/geometry/Similarity2.cpp +++ b/gtsam/geometry/Similarity2.cpp @@ -15,21 +15,21 @@ * @author John Lambert */ -#include - -#include -#include #include +#include +#include +#include #include namespace gtsam { using std::vector; -namespace { +namespace internal { + /// Subtract centroids from point pairs. -static Point2Pairs subtractCentroids(const Point2Pairs &abPointPairs, - const Point2Pair ¢roids) { +static Point2Pairs subtractCentroids(const Point2Pairs& abPointPairs, + const Point2Pair& centroids) { Point2Pairs d_abPointPairs; for (const Point2Pair& abPair : abPointPairs) { Point2 da = abPair.first - centroids.first; @@ -40,10 +40,11 @@ static Point2Pairs subtractCentroids(const Point2Pairs &abPointPairs, } /// Form inner products x and y and calculate scale. -static const double calculateScale(const Point2Pairs &d_abPointPairs, - const Rot2 &aRb) { +static double calculateScale(const Point2Pairs& d_abPointPairs, + const Rot2& aRb) { double x = 0, y = 0; Point2 da, db; + for (const Point2Pair& d_abPair : d_abPointPairs) { std::tie(da, db) = d_abPair; const Vector2 da_prime = aRb * db; @@ -55,7 +56,7 @@ static const double calculateScale(const Point2Pairs &d_abPointPairs, } /// Form outer product H. -static Matrix2 calculateH(const Point2Pairs &d_abPointPairs) { +static Matrix2 calculateH(const Point2Pairs& d_abPointPairs) { Matrix2 H = Z_2x2; for (const Point2Pair& d_abPair : d_abPointPairs) { H += d_abPair.first * d_abPair.second.transpose(); @@ -63,10 +64,17 @@ static Matrix2 calculateH(const Point2Pairs &d_abPointPairs) { return H; } -/// This method estimates the similarity transform from differences point pairs, -// given a known or estimated rotation and point centroids. -static Similarity2 align(const Point2Pairs &d_abPointPairs, const Rot2 &aRb, - const Point2Pair ¢roids) { +/** + * @brief This method estimates the similarity transform from differences point + * pairs, given a known or estimated rotation and point centroids. + * + * @param d_abPointPairs + * @param aRb + * @param centroids + * @return Similarity2 + */ +static Similarity2 align(const Point2Pairs& d_abPointPairs, const Rot2& aRb, + const Point2Pair& centroids) { const double s = calculateScale(d_abPointPairs, aRb); // dividing aTb by s is required because the registration cost function // minimizes ||a - sRb - t||, whereas Sim(2) computes s(Rb + t) @@ -74,39 +82,44 @@ static Similarity2 align(const Point2Pairs &d_abPointPairs, const Rot2 &aRb, return Similarity2(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 Similarity2 alignGivenR(const Point2Pairs &abPointPairs, - const Rot2 &aRb) { +/** + * @brief 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 + * + * @param abPointPairs + * @param aRb + * @return Similarity2 + */ +static Similarity2 alignGivenR(const Point2Pairs& abPointPairs, + const Rot2& aRb) { auto centroids = means(abPointPairs); - auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); - return align(d_abPointPairs, aRb, centroids); + auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids); + return internal::align(d_abPointPairs, aRb, centroids); } -} // namespace +} // namespace internal -Similarity2::Similarity2() : - t_(0,0), s_(1) { -} +Similarity2::Similarity2() : t_(0, 0), s_(1) {} -Similarity2::Similarity2(double s) : - t_(0,0), s_(s) { -} +Similarity2::Similarity2(double s) : t_(0, 0), s_(s) {} -Similarity2::Similarity2(const Rot2& R, const Point2& t, double s) : - R_(R), t_(t), s_(s) { -} +Similarity2::Similarity2(const Rot2& R, const Point2& t, double s) + : R_(R), t_(t), s_(s) {} -// Similarity2::Similarity2(const Matrix2& R, const Vector2& t, double s) : -// R_(Rot2::ClosestTo(R)), t_(t), s_(s) { -// } +Similarity2::Similarity2(const Matrix2& R, const Vector2& t, double s) + : R_(Rot2::ClosestTo(R)), t_(t), s_(s) {} -// Similarity2::Similarity2(const Matrix3& T) : -// R_(Rot2::ClosestTo(T.topLeftCorner<2, 2>())), t_(T.topRightCorner<2, 1>()), s_(1.0 / T(2, 2)) { -// } +Similarity2::Similarity2(const Matrix3& T) + : R_(Rot2::ClosestTo(T.topLeftCorner<2, 2>())), + t_(T.topRightCorner<2, 1>()), + s_(1.0 / T(2, 2)) {} bool Similarity2::equals(const Similarity2& other, double tol) const { - return R_.equals(other.R_, tol) && traits::Equals(t_, other.t_, tol) - && s_ < (other.s_ + tol) && s_ > (other.s_ - tol); + return R_.equals(other.R_, tol) && + traits::Equals(t_, other.t_, tol) && s_ < (other.s_ + tol) && + s_ > (other.s_ - tol); } bool Similarity2::operator==(const Similarity2& other) const { @@ -117,15 +130,15 @@ void Similarity2::print(const std::string& s) const { std::cout << std::endl; std::cout << s; rotation().print("\nR:\n"); - std::cout << "t: " << translation().transpose() << " s: " << scale() << std::endl; + std::cout << "t: " << translation().transpose() << " s: " << scale() + << std::endl; } -Similarity2 Similarity2::identity() { - return Similarity2(); +Similarity2 Similarity2::identity() { return Similarity2(); } + +Similarity2 Similarity2::operator*(const Similarity2& S) const { + return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_); } -// Similarity2 Similarity2::operator*(const Similarity2& S) const { -// return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_); -// } Similarity2 Similarity2::inverse() const { const Rot2 Rt = R_.inverse(); @@ -148,57 +161,56 @@ Point2 Similarity2::operator*(const Point2& p) const { return transformFrom(p); } -// Similarity2 Similarity2::Align(const Point2Pairs &abPointPairs) { -// // Refer to Chapter 3 of -// // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf -// if (abPointPairs.size() < 2) -// throw std::runtime_error("input should have at least 2 pairs of points"); -// auto centroids = means(abPointPairs); -// auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); -// Matrix3 H = calculateH(d_abPointPairs); -// // ClosestTo finds rotation matrix closest to H in Frobenius sense -// Rot2 aRb = Rot2::ClosestTo(H); -// return align(d_abPointPairs, aRb, centroids); -// } +Similarity2 Similarity2::Align(const Point2Pairs& abPointPairs) { + // Refer to Chapter 3 of + // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf + if (abPointPairs.size() < 2) + throw std::runtime_error("input should have at least 2 pairs of points"); + auto centroids = means(abPointPairs); + auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids); + Matrix2 H = internal::calculateH(d_abPointPairs); + // ClosestTo finds rotation matrix closest to H in Frobenius sense + Rot2 aRb = Rot2::ClosestTo(H); + return internal::align(d_abPointPairs, aRb, centroids); +} -// Similarity2 Similarity2::Align(const vector &abPosePairs) { -// const size_t n = abPosePairs.size(); -// if (n < 2) -// throw std::runtime_error("input should have at least 2 pairs of poses"); +Similarity2 Similarity2::Align(const Pose2Pairs& 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; -// Point2Pairs abPointPairs; -// rotations.reserve(n); -// abPointPairs.reserve(n); -// // Below denotes the pose of the i'th object/camera/etc in frame "a" or frame "b" -// Pose2 aTi, bTi; -// for (const Pose2Pair &abPair : abPosePairs) { -// std::tie(aTi, bTi) = abPair; -// const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse()); -// rotations.emplace_back(aRb); -// abPointPairs.emplace_back(aTi.translation(), bTi.translation()); -// } -// const Rot2 aRb_estimate = FindKarcherMean(rotations); + // calculate rotation + vector rotations; + Point2Pairs abPointPairs; + rotations.reserve(n); + abPointPairs.reserve(n); + // Below denotes the pose of the i'th object/camera/etc + // in frame "a" or frame "b". + Pose2 aTi, bTi; + for (const Pose2Pair& abPair : abPosePairs) { + std::tie(aTi, bTi) = abPair; + const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse()); + rotations.emplace_back(aRb); + abPointPairs.emplace_back(aTi.translation(), bTi.translation()); + } + const Rot2 aRb_estimate; // = FindKarcherMean(rotations); -// return alignGivenR(abPointPairs, aRb_estimate); -// } + return internal::alignGivenR(abPointPairs, aRb_estimate); +} -std::ostream &operator<<(std::ostream &os, const Similarity2& p) { - os << "[" << p.rotation().theta() << " " - << p.translation().transpose() << " " << p.scale() << "]\';"; +std::ostream& operator<<(std::ostream& os, const Similarity2& p) { + os << "[" << p.rotation().theta() << " " << p.translation().transpose() << " " + << p.scale() << "]\';"; return os; } -const Matrix3 Similarity2::matrix() const { +Matrix3 Similarity2::matrix() const { Matrix3 T; T.topRows<2>() << R_.matrix(), t_; T.bottomRows<1>() << 0, 0, 1.0 / s_; return T; } -Similarity2::operator Pose2() const { - return Pose2(R_, s_ * t_); -} +Similarity2::operator Pose2() const { return Pose2(R_, s_ * t_); } -} // namespace gtsam +} // namespace gtsam diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h index 85a6324c5..93a1227f5 100644 --- a/gtsam/geometry/Similarity2.h +++ b/gtsam/geometry/Similarity2.h @@ -17,13 +17,12 @@ #pragma once -#include -#include -#include #include #include #include - +#include +#include +#include namespace gtsam { @@ -33,21 +32,19 @@ class Pose2; /** * 2D similarity transform */ -class Similarity2: public LieGroup { - +class Similarity2 : public LieGroup { /// @name Pose Concept /// @{ typedef Rot2 Rotation; typedef Point2 Translation; /// @} -private: + private: Rot2 R_; Point2 t_; double s_; -public: - + public: /// @name Constructors /// @{ @@ -60,11 +57,11 @@ public: /// Construct from GTSAM types GTSAM_EXPORT Similarity2(const Rot2& R, const Point2& t, double s); - // /// Construct from Eigen types - // GTSAM_EXPORT Similarity2(const Matrix2& R, const Vector2& t, double s); + /// Construct from Eigen types + GTSAM_EXPORT Similarity2(const Matrix2& R, const Vector2& t, double s); - // /// Construct from matrix [R t; 0 s^-1] - // GTSAM_EXPORT Similarity2(const Matrix3& T); + /// Construct from matrix [R t; 0 s^-1] + GTSAM_EXPORT Similarity2(const Matrix3& T); /// @} /// @name Testable @@ -79,7 +76,8 @@ public: /// Print with optional string GTSAM_EXPORT void print(const std::string& s) const; - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Similarity2& p); + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, + const Similarity2& p); /// @} /// @name Group @@ -94,22 +92,22 @@ public: /// Return the inverse GTSAM_EXPORT Similarity2 inverse() const; - // /// @} - // /// @name Group action on Point2 - // /// @{ + /// @} + /// @name Group action on Point2 + /// @{ /// Action on a point p is s*(R*p+t) GTSAM_EXPORT Point2 transformFrom(const Point2& p) const; - /** + /** * Action on a pose T. - * |Rs ts| |R t| |Rs*R Rs*t+ts| + * |Rs ts| |R t| |Rs*R Rs*t+ts| * |0 1/s| * |0 1| = | 0 1/s |, the result is still a Sim2 object. * To retrieve a Pose2, 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. + * + * This group action satisfies the compatibility condition. * For more details, refer to: https://en.wikipedia.org/wiki/Group_action */ GTSAM_EXPORT Pose2 transformFrom(const Pose2& T) const; @@ -117,22 +115,26 @@ public: /* syntactic sugar for transformFrom */ GTSAM_EXPORT Point2 operator*(const Point2& p) const; - // /** - // * Create Similarity2 by aligning at least two point pairs - // */ - // GTSAM_EXPORT static Similarity2 Align(const std::vector& abPointPairs); - - // /** - // * Create the Similarity2 object that aligns at least two pose pairs. - // * Each pair is of the form (aTi, bTi). - // * Given a list of pairs in frame a, and a list of pairs in frame b, Align() - // * will compute the best-fit Similarity2 aSb transformation to align them. - // * First, the rotation aRb will be computed as the average (Karcher mean) of - // * many estimates aRb (from each pair). Afterwards, the scale factor will be computed - // * using the algorithm described here: - // * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf - // */ - // GTSAM_EXPORT static Similarity2 Align(const std::vector& abPosePairs); + /** + * Create Similarity2 by aligning at least two point pairs + */ + GTSAM_EXPORT static Similarity2 Align(const Point2Pairs& abPointPairs); + + /** + * Create the Similarity2 object that aligns at least two pose pairs. + * Each pair is of the form (aTi, bTi). + * Given a list of pairs in frame a, and a list of pairs in frame b, + Align() + * will compute the best-fit Similarity2 aSb transformation to align them. + * First, the rotation aRb will be computed as the average (Karcher mean) + of + * many estimates aRb (from each pair). Afterwards, the scale factor will + be computed + * using the algorithm described here: + * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf + */ + GTSAM_EXPORT static Similarity2 Align( + const std::vector& abPosePairs); /// @} /// @name Lie Group @@ -145,45 +147,33 @@ public: /// @{ /// Calculate 4*4 matrix group equivalent - GTSAM_EXPORT const Matrix3 matrix() const; + GTSAM_EXPORT Matrix3 matrix() const; /// Return a GTSAM rotation - const Rot2& rotation() const { - return R_; - } + Rot2 rotation() const { return R_; } /// Return a GTSAM translation - const Point2& translation() const { - return t_; - } + Point2 translation() const { return t_; } /// Return the scale - double scale() const { - return s_; - } + double scale() const { return s_; } /// Convert to a rigid body pose (R, s*t) - /// TODO(frank): why is this here? Red flag! Definitely don't have it as a cast. GTSAM_EXPORT operator Pose2() const; /// Dimensionality of tangent space = 4 DOF - used to autodetect sizes - inline static size_t Dim() { - return 4; - } + inline static size_t Dim() { return 4; } /// Dimensionality of tangent space = 4 DOF - inline size_t dim() const { - return 4; - } + inline size_t dim() const { return 4; } /// @} }; +template <> +struct traits : public internal::LieGroup {}; -// template<> -// struct traits : public internal::LieGroup {}; +template <> +struct traits : public internal::LieGroup {}; -// template<> -// struct traits : public internal::LieGroup {}; - -} // namespace gtsam +} // namespace gtsam