Similarity2 fixes
Varun Agrawal
parent
784bdc64c5
commit
bf668e5869
|
|
@ -15,21 +15,21 @@
|
|||
* @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/geometry/Pose2.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/geometry/Similarity2.h>
|
||||
#include <gtsam/slam/KarcherMeanFactor-inl.h>
|
||||
|
||||
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<Point2>::Equals(t_, other.t_, tol)
|
||||
&& s_ < (other.s_ + tol) && s_ > (other.s_ - tol);
|
||||
return R_.equals(other.R_, tol) &&
|
||||
traits<Point2>::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<Pose2Pair> &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<Rot2> 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<Rot2>(rotations);
|
||||
// calculate rotation
|
||||
vector<Rot2> 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<Rot2>(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
|
||||
|
|
|
|||
|
|
@ -17,13 +17,12 @@
|
|||
|
||||
#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/Manifold.h>
|
||||
#include <gtsam/dllexport.h>
|
||||
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/geometry/Rot2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -33,21 +32,19 @@ class Pose2;
|
|||
/**
|
||||
* 2D similarity transform
|
||||
*/
|
||||
class Similarity2: public LieGroup<Similarity2, 4> {
|
||||
|
||||
class Similarity2 : public LieGroup<Similarity2, 4> {
|
||||
/// @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,9 +92,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;
|
||||
|
|
@ -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<Point2Pair>& abPointPairs);
|
||||
/**
|
||||
* 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<Pose2Pair>& 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<Pose2Pair>& 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<Similarity2> : public internal::LieGroup<Similarity2> {};
|
||||
|
||||
// template<>
|
||||
// struct traits<Similarity2> : public internal::LieGroup<Similarity2> {};
|
||||
template <>
|
||||
struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
|
||||
|
||||
// template<>
|
||||
// struct traits<const Similarity2> : public internal::LieGroup<Similarity2> {};
|
||||
|
||||
} // namespace gtsam
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
Loading…
Reference in New Issue