12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix Eigen error

release/4.3a0
John Lambert 2021-12-07 19:07:50 -05:00
parent c2040fb910
commit 7ebbe1869e
2 changed files with 69 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
gtsam/geometry/Similarity2.cpp
View File

@ -17,6 +17,7 @@
#include <gtsam/geometry/Similarity2.h> #include <gtsam/geometry/Similarity2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Manifold.h> #include <gtsam/base/Manifold.h>
#include <gtsam/slam/KarcherMeanFactor-inl.h> #include <gtsam/slam/KarcherMeanFactor-inl.h>
@ -95,13 +96,13 @@ Similarity2::Similarity2(const Rot2& R, const Point2& t, double s) :
R_(R), t_(t), s_(s) { R_(R), t_(t), s_(s) {
} }
Similarity2::Similarity2(const Matrix2& R, const Vector2& t, double s) : // Similarity2::Similarity2(const Matrix2& R, const Vector2& t, double s) :
R_(Rot2.ClosestTo(R)), t_(t), s_(s) { // R_(Rot2::ClosestTo(R)), t_(t), s_(s) {
} // }
Similarity2::Similarity2(const Matrix3& T) : // Similarity2::Similarity2(const Matrix3& T) :
R_(T.topLeftCorner<2, 2>()), t_(T.topRightCorner<2, 1>()), s_(1.0 / T(2, 2)) { // 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 { bool Similarity2::equals(const Similarity2& other, double tol) const {
return R_.equals(other.R_, tol) && traits<Point2>::Equals(t_, other.t_, tol) return R_.equals(other.R_, tol) && traits<Point2>::Equals(t_, other.t_, tol)
@ -122,9 +123,9 @@ void Similarity2::print(const std::string& s) const {
Similarity2 Similarity2::identity() { Similarity2 Similarity2::identity() {
return Similarity2(); return Similarity2();
} }
Similarity2 Similarity2::operator*(const Similarity2& S) const { // Similarity2 Similarity2::operator*(const Similarity2& S) const {
return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_); // return Similarity2(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
} // }
Similarity2 Similarity2::inverse() const { Similarity2 Similarity2::inverse() const {
const Rot2 Rt = R_.inverse(); const Rot2 Rt = R_.inverse();
@ -147,41 +148,41 @@ Point2 Similarity2::operator*(const Point2& p) const {
return transformFrom(p); return transformFrom(p);
} }
Similarity2 Similarity2::Align(const Point2Pairs &abPointPairs) { // Similarity2 Similarity2::Align(const Point2Pairs &abPointPairs) {
// Refer to Chapter 3 of // // Refer to Chapter 3 of
// http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf // // http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
if (abPointPairs.size() < 2) // if (abPointPairs.size() < 2)
throw std::runtime_error("input should have at least 2 pairs of points"); // throw std::runtime_error("input should have at least 2 pairs of points");
auto centroids = means(abPointPairs); // auto centroids = means(abPointPairs);
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); // auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
Matrix3 H = calculateH(d_abPointPairs); // Matrix3 H = calculateH(d_abPointPairs);
// ClosestTo finds rotation matrix closest to H in Frobenius sense // // ClosestTo finds rotation matrix closest to H in Frobenius sense
Rot2 aRb = Rot2::ClosestTo(H); // Rot2 aRb = Rot2::ClosestTo(H);
return align(d_abPointPairs, aRb, centroids); // return align(d_abPointPairs, aRb, centroids);
} // }
Similarity2 Similarity2::Align(const vector<Pose2Pair> &abPosePairs) { // Similarity2 Similarity2::Align(const vector<Pose2Pair> &abPosePairs) {
const size_t n = abPosePairs.size(); // const size_t n = abPosePairs.size();
if (n < 2) // if (n < 2)
throw std::runtime_error("input should have at least 2 pairs of poses"); // throw std::runtime_error("input should have at least 2 pairs of poses");
// calculate rotation // // calculate rotation
vector<Rot2> rotations; // vector<Rot2> rotations;
Point2Pairs abPointPairs; // Point2Pairs abPointPairs;
rotations.reserve(n); // rotations.reserve(n);
abPointPairs.reserve(n); // abPointPairs.reserve(n);
// Below denotes the pose of the i'th object/camera/etc in frame "a" or frame "b" // // Below denotes the pose of the i'th object/camera/etc in frame "a" or frame "b"
Pose2 aTi, bTi; // Pose2 aTi, bTi;
for (const Pose2Pair &abPair : abPosePairs) { // for (const Pose2Pair &abPair : abPosePairs) {
std::tie(aTi, bTi) = abPair; // std::tie(aTi, bTi) = abPair;
const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse()); // const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse());
rotations.emplace_back(aRb); // rotations.emplace_back(aRb);
abPointPairs.emplace_back(aTi.translation(), bTi.translation()); // abPointPairs.emplace_back(aTi.translation(), bTi.translation());
} // }
const Rot2 aRb_estimate = FindKarcherMean<Rot2>(rotations); // const Rot2 aRb_estimate = FindKarcherMean<Rot2>(rotations);
return alignGivenR(abPointPairs, aRb_estimate); // return alignGivenR(abPointPairs, aRb_estimate);
} // }
std::ostream &operator<<(std::ostream &os, const Similarity2& p) { std::ostream &operator<<(std::ostream &os, const Similarity2& p) {
os << "[" << p.rotation().theta() << " " os << "[" << p.rotation().theta() << " "

54
gtsam/geometry/Similarity2.h
View File

@ -60,11 +60,11 @@ public:
/// Construct from GTSAM types /// Construct from GTSAM types
GTSAM_EXPORT Similarity2(const Rot2& R, const Point2& t, double s); GTSAM_EXPORT Similarity2(const Rot2& R, const Point2& t, double s);
/// Construct from Eigen types // /// Construct from Eigen types
GTSAM_EXPORT Similarity2(const Matrix2& R, const Vector2& t, double s); // GTSAM_EXPORT Similarity2(const Matrix2& R, const Vector2& t, double s);
/// Construct from matrix [R t; 0 s^-1] // /// Construct from matrix [R t; 0 s^-1]
GTSAM_EXPORT Similarity2(const Matrix3& T); // GTSAM_EXPORT Similarity2(const Matrix3& T);
/// @} /// @}
/// @name Testable /// @name Testable
@ -94,9 +94,9 @@ public:
/// Return the inverse /// Return the inverse
GTSAM_EXPORT Similarity2 inverse() const; 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) /// Action on a point p is s*(R*p+t)
GTSAM_EXPORT Point2 transformFrom(const Point2& p) const; GTSAM_EXPORT Point2 transformFrom(const Point2& p) const;
@ -114,25 +114,25 @@ public:
*/ */
GTSAM_EXPORT Pose2 transformFrom(const Pose2& T) const; GTSAM_EXPORT Pose2 transformFrom(const Pose2& T) const;
/** syntactic sugar for transformFrom */ /* syntactic sugar for transformFrom */
GTSAM_EXPORT Point2 operator*(const Point2& p) const; GTSAM_EXPORT Point2 operator*(const Point2& p) const;
/** // /**
* Create Similarity2 by aligning at least two point pairs // * Create Similarity2 by aligning at least two point pairs
*/ // */
GTSAM_EXPORT static Similarity2 Align(const std::vector<Point2Pair>& abPointPairs); // GTSAM_EXPORT static Similarity2 Align(const std::vector<Point2Pair>& abPointPairs);
/** // /**
* Create the Similarity2 object that aligns at least two pose pairs. // * Create the Similarity2 object that aligns at least two pose pairs.
* Each pair is of the form (aTi, bTi). // * 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() // * 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. // * will compute the best-fit Similarity2 aSb transformation to align them.
* First, the rotation aRb will be computed as the average (Karcher mean) of // * 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 // * many estimates aRb (from each pair). Afterwards, the scale factor will be computed
* using the algorithm described here: // * using the algorithm described here:
* http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf // * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf
*/ // */
GTSAM_EXPORT static Similarity2 Align(const std::vector<Pose2Pair>& abPosePairs); // GTSAM_EXPORT static Similarity2 Align(const std::vector<Pose2Pair>& abPosePairs);
/// @} /// @}
/// @name Lie Group /// @name Lie Group
@ -180,10 +180,10 @@ public:
}; };
template<> // template<>
struct traits<Similarity2> : public internal::LieGroup<Similarity2> {}; // struct traits<Similarity2> : public internal::LieGroup<Similarity2> {};
template<> // template<>
struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {}; // struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
} // namespace gtsam } // namespace gtsam