From 7ebbe1869eeac0393c5e992944f672d0ac093db5 Mon Sep 17 00:00:00 2001 From: John Lambert Date: Tue, 7 Dec 2021 19:07:50 -0500 Subject: [PATCH] fix Eigen error --- gtsam/geometry/Similarity2.cpp | 83 +++++++++++++++++----------------- gtsam/geometry/Similarity2.h | 54 +++++++++++----------- 2 files changed, 69 insertions(+), 68 deletions(-) diff --git a/gtsam/geometry/Similarity2.cpp b/gtsam/geometry/Similarity2.cpp index 1ed0dd1b0..5a63e70cb 100644 --- a/gtsam/geometry/Similarity2.cpp +++ b/gtsam/geometry/Similarity2.cpp @@ -17,6 +17,7 @@ #include +#include #include #include #include @@ -95,13 +96,13 @@ 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_(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) @@ -122,9 +123,9 @@ void Similarity2::print(const std::string& s) const { 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(); @@ -147,41 +148,41 @@ 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 = 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 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 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; - 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 alignGivenR(abPointPairs, aRb_estimate); +// } std::ostream &operator<<(std::ostream &os, const Similarity2& p) { os << "[" << p.rotation().theta() << " " diff --git a/gtsam/geometry/Similarity2.h b/gtsam/geometry/Similarity2.h index 120e6690a..85a6324c5 100644 --- a/gtsam/geometry/Similarity2.h +++ b/gtsam/geometry/Similarity2.h @@ -60,11 +60,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 @@ -94,9 +94,9 @@ 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; @@ -114,25 +114,25 @@ public: */ GTSAM_EXPORT Pose2 transformFrom(const Pose2& T) const; - /** syntactic sugar for transformFrom */ + /* 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 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 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 @@ -180,10 +180,10 @@ public: }; -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