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

Similarity2 fixes

release/4.3a0
Varun Agrawal 2022-02-06 00:08:54 -05:00
parent 784bdc64c5
commit bf668e5869
2 changed files with 146 additions and 144 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

180
gtsam/geometry/Similarity2.cpp
View File

@ -15,21 +15,21 @@
* @author John Lambert * @author John Lambert
*/ */
#include <gtsam/geometry/Similarity2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Manifold.h> #include <gtsam/base/Manifold.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Similarity2.h>
#include <gtsam/slam/KarcherMeanFactor-inl.h> #include <gtsam/slam/KarcherMeanFactor-inl.h>
namespace gtsam { namespace gtsam {
using std::vector; using std::vector;
namespace { namespace internal {
/// Subtract centroids from point pairs. /// Subtract centroids from point pairs.
static Point2Pairs subtractCentroids(const Point2Pairs &abPointPairs, static Point2Pairs subtractCentroids(const Point2Pairs& abPointPairs,
const Point2Pair &centroids) { const Point2Pair& centroids) {
Point2Pairs d_abPointPairs; Point2Pairs d_abPointPairs;
for (const Point2Pair& abPair : abPointPairs) { for (const Point2Pair& abPair : abPointPairs) {
Point2 da = abPair.first - centroids.first; 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. /// Form inner products x and y and calculate scale.
static const double calculateScale(const Point2Pairs &d_abPointPairs, static double calculateScale(const Point2Pairs& d_abPointPairs,
const Rot2 &aRb) { const Rot2& aRb) {
double x = 0, y = 0; double x = 0, y = 0;
Point2 da, db; Point2 da, db;
for (const Point2Pair& d_abPair : d_abPointPairs) { for (const Point2Pair& d_abPair : d_abPointPairs) {
std::tie(da, db) = d_abPair; std::tie(da, db) = d_abPair;
const Vector2 da_prime = aRb * db; const Vector2 da_prime = aRb * db;
@ -55,7 +56,7 @@ static const double calculateScale(const Point2Pairs &d_abPointPairs,
} }
/// Form outer product H. /// Form outer product H.
static Matrix2 calculateH(const Point2Pairs &d_abPointPairs) { static Matrix2 calculateH(const Point2Pairs& d_abPointPairs) {
Matrix2 H = Z_2x2; Matrix2 H = Z_2x2;
for (const Point2Pair& d_abPair : d_abPointPairs) { for (const Point2Pair& d_abPair : d_abPointPairs) {
H += d_abPair.first * d_abPair.second.transpose(); H += d_abPair.first * d_abPair.second.transpose();
@ -63,10 +64,17 @@ static Matrix2 calculateH(const Point2Pairs &d_abPointPairs) {
return H; return H;
} }
/// This method estimates the similarity transform from differences point pairs, /**
// given a known or estimated rotation and point centroids. * @brief This method estimates the similarity transform from differences point
static Similarity2 align(const Point2Pairs &d_abPointPairs, const Rot2 &aRb, * pairs, given a known or estimated rotation and point centroids.
const Point2Pair &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); const double s = calculateScale(d_abPointPairs, aRb);
// dividing aTb by s is required because the registration cost function // dividing aTb by s is required because the registration cost function
// minimizes ||a - sRb - t||, whereas Sim(2) computes s(Rb + t) // 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); 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 * @brief This method estimates the similarity transform from point pairs,
static Similarity2 alignGivenR(const Point2Pairs &abPointPairs, * given a known or estimated rotation.
const Rot2 &aRb) { * 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 centroids = means(abPointPairs);
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids); auto d_abPointPairs = internal::subtractCentroids(abPointPairs, centroids);
return align(d_abPointPairs, aRb, centroids); return internal::align(d_abPointPairs, aRb, centroids);
} }
} // namespace } // namespace internal
Similarity2::Similarity2() : Similarity2::Similarity2() : t_(0, 0), s_(1) {}
t_(0,0), s_(1) {
}
Similarity2::Similarity2(double s) : Similarity2::Similarity2(double s) : t_(0, 0), s_(s) {}
t_(0,0), s_(s) {
}
Similarity2::Similarity2(const Rot2& R, const Point2& t, double s) : 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_(Rot2::ClosestTo(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) &&
&& s_ < (other.s_ + tol) && s_ > (other.s_ - tol); traits<Point2>::Equals(t_, other.t_, tol) && s_ < (other.s_ + tol) &&
s_ > (other.s_ - tol);
} }
bool Similarity2::operator==(const Similarity2& other) const { 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 << std::endl;
std::cout << s; std::cout << s;
rotation().print("\nR:\n"); 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() { Similarity2 Similarity2::identity() { return Similarity2(); }
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 { Similarity2 Similarity2::inverse() const {
const Rot2 Rt = R_.inverse(); const Rot2 Rt = R_.inverse();
@ -148,57 +161,56 @@ 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 = internal::subtractCentroids(abPointPairs, centroids);
// Matrix3 H = calculateH(d_abPointPairs); Matrix2 H = internal::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 internal::align(d_abPointPairs, aRb, centroids);
// } }
// Similarity2 Similarity2::Align(const vector<Pose2Pair> &abPosePairs) { Similarity2 Similarity2::Align(const Pose2Pairs& 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
// Pose2 aTi, bTi; // in frame "a" or frame "b".
// for (const Pose2Pair &abPair : abPosePairs) { Pose2 aTi, bTi;
// std::tie(aTi, bTi) = abPair; for (const Pose2Pair& abPair : abPosePairs) {
// const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse()); std::tie(aTi, bTi) = abPair;
// rotations.emplace_back(aRb); const Rot2 aRb = aTi.rotation().compose(bTi.rotation().inverse());
// abPointPairs.emplace_back(aTi.translation(), bTi.translation()); rotations.emplace_back(aRb);
// } 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 internal::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() << " " << p.translation().transpose() << " "
<< p.translation().transpose() << " " << p.scale() << "]\';"; << p.scale() << "]\';";
return os; return os;
} }
const Matrix3 Similarity2::matrix() const { Matrix3 Similarity2::matrix() const {
Matrix3 T; Matrix3 T;
T.topRows<2>() << R_.matrix(), t_; T.topRows<2>() << R_.matrix(), t_;
T.bottomRows<1>() << 0, 0, 1.0 / s_; T.bottomRows<1>() << 0, 0, 1.0 / s_;
return T; return T;
} }
Similarity2::operator Pose2() const { Similarity2::operator Pose2() const { return Pose2(R_, s_ * t_); }
return Pose2(R_, s_ * t_);
}
} // namespace gtsam } // namespace gtsam

100
gtsam/geometry/Similarity2.h
View File

@ -17,13 +17,12 @@
#pragma once #pragma once
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Lie.h> #include <gtsam/base/Lie.h>
#include <gtsam/base/Manifold.h> #include <gtsam/base/Manifold.h>
#include <gtsam/dllexport.h> #include <gtsam/dllexport.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Rot2.h>
namespace gtsam { namespace gtsam {
@ -33,21 +32,19 @@ class Pose2;
/** /**
* 2D similarity transform * 2D similarity transform
*/ */
class Similarity2: public LieGroup<Similarity2, 4> { class Similarity2 : public LieGroup<Similarity2, 4> {
/// @name Pose Concept /// @name Pose Concept
/// @{ /// @{
typedef Rot2 Rotation; typedef Rot2 Rotation;
typedef Point2 Translation; typedef Point2 Translation;
/// @} /// @}
private: private:
Rot2 R_; Rot2 R_;
Point2 t_; Point2 t_;
double s_; double s_;
public: public:
/// @name Constructors /// @name Constructors
/// @{ /// @{
@ -60,11 +57,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
@ -79,7 +76,8 @@ public:
/// Print with optional string /// Print with optional string
GTSAM_EXPORT void print(const std::string& s) const; 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 /// @name Group
@ -94,9 +92,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;
@ -117,22 +115,26 @@ public:
/* 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 Point2Pairs& 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,
// * will compute the best-fit Similarity2 aSb transformation to align them. Align()
// * First, the rotation aRb will be computed as the average (Karcher mean) of * will compute the best-fit Similarity2 aSb transformation to align them.
// * many estimates aRb (from each pair). Afterwards, the scale factor will be computed * First, the rotation aRb will be computed as the average (Karcher mean)
// * using the algorithm described here: of
// * http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf * many estimates aRb (from each pair). Afterwards, the scale factor will
// */ be computed
// GTSAM_EXPORT static Similarity2 Align(const std::vector<Pose2Pair>& abPosePairs); * 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<Pose2Pair>& abPosePairs);
/// @} /// @}
/// @name Lie Group /// @name Lie Group
@ -145,45 +147,33 @@ public:
/// @{ /// @{
/// Calculate 4*4 matrix group equivalent /// Calculate 4*4 matrix group equivalent
GTSAM_EXPORT const Matrix3 matrix() const; GTSAM_EXPORT Matrix3 matrix() const;
/// Return a GTSAM rotation /// Return a GTSAM rotation
const Rot2& rotation() const { Rot2 rotation() const { return R_; }
return R_;
}
/// Return a GTSAM translation /// Return a GTSAM translation
const Point2& translation() const { Point2 translation() const { return t_; }
return t_;
}
/// Return the scale /// Return the scale
double scale() const { double scale() const { return s_; }
return s_;
}
/// Convert to a rigid body pose (R, s*t) /// 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; GTSAM_EXPORT operator Pose2() const;
/// Dimensionality of tangent space = 4 DOF - used to autodetect sizes /// Dimensionality of tangent space = 4 DOF - used to autodetect sizes
inline static size_t Dim() { inline static size_t Dim() { return 4; }
return 4;
}
/// Dimensionality of tangent space = 4 DOF /// Dimensionality of tangent space = 4 DOF
inline size_t dim() const { inline size_t dim() const { return 4; }
return 4;
}
/// @} /// @}
}; };
template <>
struct traits<Similarity2> : public internal::LieGroup<Similarity2> {};
// template<> template <>
// struct traits<Similarity2> : public internal::LieGroup<Similarity2> {}; struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
// template<> } // namespace gtsam
// struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
} // namespace gtsam