Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Get Sim2 working #1090

Merged
merged 12 commits into from
Feb 7, 2022
Merged

Get Sim2 working #1090

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
784bdc6
minor fixes to Pose2 and Rot2
varunagrawal Feb 6, 2022
bf668e5
Similarity2 fixes
varunagrawal Feb 6, 2022
a57465e
Similarity3 fixes
varunagrawal Feb 6, 2022
5e75986
fix wrapper
varunagrawal Feb 6, 2022
b484a21
fix tests
varunagrawal Feb 6, 2022
1933597
Merge branch 'develop' into add-Similarity2-classes-2
varunagrawal Feb 6, 2022
c716f63
wrap Pose2Pairs
varunagrawal Feb 6, 2022
5098706
wrap equals for Sim2 and Sim3
varunagrawal Feb 6, 2022
464af9f
Fix syntactic errors in test_Sim2.py
varunagrawal Feb 6, 2022
b60ca0f
Merge branch 'add-Similarity2-classes' into add-Similarity2-classes-2
varunagrawal Feb 6, 2022
cc8d80f
Remove SFINAE from KarcherMean so we can also use it for Rot2
varunagrawal Feb 6, 2022
37ae703
Fix tests
varunagrawal Feb 6, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions gtsam/geometry/Pose2.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -324,8 +324,8 @@ GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);

// Convenience typedef
using Pose2Pair = std::pair<Pose2, Pose2>;
using Pose2Pairs = std::vector<std::pair<Pose2, Pose2> >;
using Pose2Pairs = std::vector<Pose2Pair>;

template <>
struct traits<Pose2> : public internal::LieGroup<Pose2> {};

Expand Down
16 changes: 8 additions & 8 deletions gtsam/geometry/Rot2.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -130,15 +130,15 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
}

/* ************************************************************************* */
static Rot2 ClosestTo(const Matrix2& M) {
double c = M(0,0);
double s = M(1,0);
double theta_rad = atan2(s, c);
c = cos(theta_rad);
s = sin(theta_rad);
return Rot2::fromCosSin(c, s);
Rot2 Rot2::ClosestTo(const Matrix2& M) {
double c = M(0, 0);
double s = M(1, 0);
double theta_rad = std::atan2(s, c);
c = cos(theta_rad);
s = sin(theta_rad);
return Rot2::fromCosSin(c, s);
}

/* ************************************************************************* */

} // gtsam
2 changes: 1 addition & 1 deletion gtsam/geometry/Rot2.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -209,7 +209,7 @@ namespace gtsam {

/** return 2*2 transpose (inverse) rotation matrix */
Matrix2 transpose() const;

/** Find closest valid rotation matrix, given a 2x2 matrix */
static Rot2 ClosestTo(const Matrix2& M);

Expand Down
188 changes: 100 additions & 88 deletions gtsam/geometry/Similarity2.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

thanks for the PR @varunagrawal. Appreciate you taking the time to help out here.

Was curious the rationale for the internal namespace (why its needed and the naming)

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I followed Frank's suggestion from your PR.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am assuming @varunagrawal this is because you need Sim3 for your A-> B :-)

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yup exactly :)


/// Subtract centroids from point pairs.
static Point2Pairs subtractCentroids(const Point2Pairs &abPointPairs,
const Point2Pair &centroids) {
static Point2Pairs subtractCentroids(const Point2Pairs& abPointPairs,
const Point2Pair& centroids) {
Point2Pairs d_abPointPairs;
for (const Point2Pair& abPair : abPointPairs) {
Point2 da = abPair.first - centroids.first;
Expand All @@ -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;
Expand All @@ -55,58 +56,70 @@ 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();
}
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 &centroids) {
/**
* @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)
const Point2 aTb = (centroids.first - s * (aRb * centroids.second)) / 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
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 {
Expand All @@ -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();
Expand All @@ -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 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);

// return alignGivenR(abPointPairs, aRb_estimate);
// }

std::ostream &operator<<(std::ostream &os, const Similarity2& p) {
os << "[" << p.rotation().theta() << " "
<< p.translation().transpose() << " " << p.scale() << "]\';";
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 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);

return internal::alignGivenR(abPointPairs, aRb_estimate);
}

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
Loading