fix Eigen error
John Lambert
parent
c2040fb910
commit
7ebbe1869e
|
|
@ -17,6 +17,7 @@
|
|||
|
||||
#include <gtsam/geometry/Similarity2.h>
|
||||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/base/Manifold.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) {
|
||||
}
|
||||
|
||||
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<Point2>::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<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 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");
|
||||
|
||||
// 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 alignGivenR(abPointPairs, aRb_estimate);
|
||||
// }
|
||||
|
||||
std::ostream &operator<<(std::ostream &os, const Similarity2& p) {
|
||||
os << "[" << p.rotation().theta() << " "
|
||||
|
|
|
|||
|
|
@ -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<Point2Pair>& abPointPairs);
|
||||
// /**
|
||||
// * Create Similarity2 by aligning at least two point pairs
|
||||
// */
|
||||
// GTSAM_EXPORT static Similarity2 Align(const std::vector<Point2Pair>& 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
|
||||
|
|
@ -180,10 +180,10 @@ public:
|
|||
};
|
||||
|
||||
|
||||
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
|
||||
|
|
|
|||
Loading…
Reference in New Issue