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

Merge pull request #1090 from borglab/add-Similarity2-classes-2 

Get Sim2 working
release/4.3a0
John Lambert 2022-02-07 12:26:09 -05:00 committed by GitHub
parent 5a1797f9d9 37ae7038fa
commit 10554d85cf
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
14 changed files with 209 additions and 207 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/geometry/Pose2.h
View File

@ -324,8 +324,8 @@ GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
// Convenience typedef // Convenience typedef
using Pose2Pair = std::pair<Pose2, Pose2>; using Pose2Pair = std::pair<Pose2, Pose2>;
using Pose2Pairs = std::vector<std::pair<Pose2, Pose2> >; using Pose2Pairs = std::vector<Pose2Pair>;
template <> template <>
struct traits<Pose2> : public internal::LieGroup<Pose2> {}; struct traits<Pose2> : public internal::LieGroup<Pose2> {};

16
gtsam/geometry/Rot2.cpp
View File

@ -130,15 +130,15 @@ Rot2 Rot2::relativeBearing(const Point2& d, OptionalJacobian<1, 2> H) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
static Rot2 ClosestTo(const Matrix2& M) { Rot2 Rot2::ClosestTo(const Matrix2& M) {
double c = M(0,0); double c = M(0, 0);
double s = M(1,0); double s = M(1, 0);
double theta_rad = atan2(s, c); double theta_rad = std::atan2(s, c);
c = cos(theta_rad); c = cos(theta_rad);
s = sin(theta_rad); s = sin(theta_rad);
return Rot2::fromCosSin(c, s); return Rot2::fromCosSin(c, s);
} }
/* ************************************************************************* */ /* ************************************************************************* */
} // gtsam } // gtsam

2
gtsam/geometry/Rot2.h
View File

@ -209,7 +209,7 @@ namespace gtsam {
/** return 2*2 transpose (inverse) rotation matrix */ /** return 2*2 transpose (inverse) rotation matrix */
Matrix2 transpose() const; Matrix2 transpose() const;
/** Find closest valid rotation matrix, given a 2x2 matrix */ /** Find closest valid rotation matrix, given a 2x2 matrix */
static Rot2 ClosestTo(const Matrix2& M); static Rot2 ClosestTo(const Matrix2& M);

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

110
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,22 +92,22 @@ 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;
/** /**
* Action on a pose T. * Action on a pose T.
* |Rs ts| |R t| |Rs*R Rs*t+ts| * |Rs ts| |R t| |Rs*R Rs*t+ts|
* |0 1/s| * |0 1| = | 0 1/s |, the result is still a Sim2 object. * |0 1/s| * |0 1| = | 0 1/s |, the result is still a Sim2 object.
* To retrieve a Pose2, we normalized the scale value into 1. * To retrieve a Pose2, we normalized the scale value into 1.
* |Rs*R Rs*t+ts| |Rs*R s(Rs*t+ts)| * |Rs*R Rs*t+ts| |Rs*R s(Rs*t+ts)|
* | 0 1/s | = | 0 1 | * | 0 1/s | = | 0 1 |
* *
* This group action satisfies the compatibility condition. * This group action satisfies the compatibility condition.
* For more details, refer to: https://en.wikipedia.org/wiki/Group_action * For more details, refer to: https://en.wikipedia.org/wiki/Group_action
*/ */
GTSAM_EXPORT Pose2 transformFrom(const Pose2& T) const; GTSAM_EXPORT Pose2 transformFrom(const Pose2& T) 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

16
gtsam/geometry/Similarity3.cpp
View File

@ -26,7 +26,7 @@ namespace gtsam {
using std::vector; using std::vector;
namespace { namespace internal {
/// Subtract centroids from point pairs. /// Subtract centroids from point pairs.
static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs, static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
const Point3Pair &centroids) { const Point3Pair &centroids) {
@ -81,10 +81,10 @@ static Similarity3 align(const Point3Pairs &d_abPointPairs, const Rot3 &aRb,
static Similarity3 alignGivenR(const Point3Pairs &abPointPairs, static Similarity3 alignGivenR(const Point3Pairs &abPointPairs,
const Rot3 &aRb) { const Rot3 &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 align(d_abPointPairs, aRb, centroids);
} }
} // namespace } // namespace internal
Similarity3::Similarity3() : Similarity3::Similarity3() :
t_(0,0,0), s_(1) { t_(0,0,0), s_(1) {
@ -165,11 +165,11 @@ Similarity3 Similarity3::Align(const Point3Pairs &abPointPairs) {
if (abPointPairs.size() < 3) if (abPointPairs.size() < 3)
throw std::runtime_error("input should have at least 3 pairs of points"); throw std::runtime_error("input should have at least 3 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); Matrix3 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
Rot3 aRb = Rot3::ClosestTo(H); Rot3 aRb = Rot3::ClosestTo(H);
return align(d_abPointPairs, aRb, centroids); return internal::align(d_abPointPairs, aRb, centroids);
} }
Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) { Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
@ -192,7 +192,7 @@ Similarity3 Similarity3::Align(const vector<Pose3Pair> &abPosePairs) {
} }
const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations); const Rot3 aRb_estimate = FindKarcherMean<Rot3>(rotations);
return alignGivenR(abPointPairs, aRb_estimate); return internal::alignGivenR(abPointPairs, aRb_estimate);
} }
Matrix4 Similarity3::wedge(const Vector7 &xi) { Matrix4 Similarity3::wedge(const Vector7 &xi) {
@ -283,7 +283,7 @@ std::ostream &operator<<(std::ostream &os, const Similarity3& p) {
return os; return os;
} }
const Matrix4 Similarity3::matrix() const { Matrix4 Similarity3::matrix() const {
Matrix4 T; Matrix4 T;
T.topRows<3>() << R_.matrix(), t_; T.topRows<3>() << R_.matrix(), t_;
T.bottomRows<1>() << 0, 0, 0, 1.0 / s_; T.bottomRows<1>() << 0, 0, 0, 1.0 / s_;

6
gtsam/geometry/Similarity3.h
View File

@ -180,15 +180,15 @@ public:
/// @{ /// @{
/// Calculate 4*4 matrix group equivalent /// Calculate 4*4 matrix group equivalent
GTSAM_EXPORT const Matrix4 matrix() const; GTSAM_EXPORT Matrix4 matrix() const;
/// Return a GTSAM rotation /// Return a GTSAM rotation
const Rot3& rotation() const { Rot3 rotation() const {
return R_; return R_;
} }
/// Return a GTSAM translation /// Return a GTSAM translation
const Point3& translation() const { Point3 translation() const {
return t_; return t_;
} }

14
gtsam/geometry/geometry.i
View File

@ -862,9 +862,10 @@ class Similarity2 {
static gtsam::Similarity2 Align(const gtsam::Pose2Pairs& abPosePairs); static gtsam::Similarity2 Align(const gtsam::Pose2Pairs& abPosePairs);
// Standard Interface // Standard Interface
const Matrix matrix() const; bool equals(const gtsam::Similarity2& sim, double tol) const;
const gtsam::Rot2& rotation(); Matrix matrix() const;
const gtsam::Point2& translation(); gtsam::Rot2& rotation();
gtsam::Point2& translation();
double scale() const; double scale() const;
}; };
@ -884,9 +885,10 @@ class Similarity3 {
static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs); static gtsam::Similarity3 Align(const gtsam::Pose3Pairs& abPosePairs);
// Standard Interface // Standard Interface
const Matrix matrix() const; bool equals(const gtsam::Similarity3& sim, double tol) const;
const gtsam::Rot3& rotation(); Matrix matrix() const;
const gtsam::Point3& translation(); gtsam::Rot3& rotation();
gtsam::Point3& translation();
double scale() const; double scale() const;
}; };

5
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -27,7 +27,6 @@
#include <GeographicLib/Config.h> #include <GeographicLib/Config.h>
#include <GeographicLib/LocalCartesian.hpp> #include <GeographicLib/LocalCartesian.hpp>
using namespace std::placeholders;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace GeographicLib; using namespace GeographicLib;
@ -71,7 +70,7 @@ TEST( GPSFactor, Constructor ) {
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,Pose3>( Matrix expectedH = numericalDerivative11<Vector, Pose3>(
std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T); std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative
@ -100,7 +99,7 @@ TEST( GPSFactor2, Constructor ) {
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5)); EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,NavState>( Matrix expectedH = numericalDerivative11<Vector, NavState>(
std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), T); std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), T);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative

23
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -26,7 +26,6 @@
#include <GeographicLib/LocalCartesian.hpp> #include <GeographicLib/LocalCartesian.hpp>
using namespace std::placeholders;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace GeographicLib; using namespace GeographicLib;
@ -63,7 +62,7 @@ TEST( MagFactor, unrotate ) {
Matrix H; Matrix H;
Point3 expected(22735.5, 314.502, 44202.5); Point3 expected(22735.5, 314.502, 44202.5);
EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1)); EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1));
EXPECT( assert_equal(numericalDerivative11<Point3,Rot2> // EXPECT(assert_equal(numericalDerivative11<Point3, Rot2> //
(std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6)); (std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6));
} }
@ -74,27 +73,27 @@ TEST( MagFactor, Factors ) {
// MagFactor // MagFactor
MagFactor f(1, measured, s, dir, bias, model); MagFactor f(1, measured, s, dir, bias, model);
EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); EXPECT(assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> // EXPECT(assert_equal((Matrix)numericalDerivative11<Vector, Rot2> //
(std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7)); (std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7));
// MagFactor1 // MagFactor1
MagFactor1 f1(1, measured, s, dir, bias, model); MagFactor1 f1(1, measured, s, dir, bias, model);
EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); EXPECT(assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> // EXPECT(assert_equal(numericalDerivative11<Vector, Rot3> //
(std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7)); (std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7));
// MagFactor2 // MagFactor2
MagFactor2 f2(1, 2, measured, nRb, model); MagFactor2 f2(1, 2, measured, nRb, model);
EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT(assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> // EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
(std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),// (std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),//
H1, 1e-7)); H1, 1e-7));
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> // EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
(std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),// (std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),//
H2, 1e-7)); H2, 1e-7));
// MagFactor2 // MagFactor3
MagFactor3 f3(1, 2, 3, measured, nRb, model); MagFactor3 f3(1, 2, 3, measured, nRb, model);
EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> // EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //

3
gtsam/slam/KarcherMeanFactor-inl.h
View File

@ -40,8 +40,7 @@ T FindKarcherMeanImpl(const vector<T, ALLOC>& rotations) {
return result.at<T>(kKey); return result.at<T>(kKey);
} }
template <class T, template <class T>
typename = typename std::enable_if< std::is_same<gtsam::Rot3, T>::value >::type >
T FindKarcherMean(const std::vector<T>& rotations) { T FindKarcherMean(const std::vector<T>& rotations) {
return FindKarcherMeanImpl(rotations); return FindKarcherMeanImpl(rotations);
} }

4
python/gtsam/preamble/geometry.h
View File

@ -23,8 +23,8 @@ PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>); std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>);
PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Pose2Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs); PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>); PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
PYBIND11_MAKE_OPAQUE( PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>);

1
python/gtsam/specializations/geometry.h
View File

@ -16,6 +16,7 @@ py::bind_vector<
m_, "Point2Vector"); m_, "Point2Vector");
py::bind_vector<std::vector<gtsam::Point2Pair>>(m_, "Point2Pairs"); py::bind_vector<std::vector<gtsam::Point2Pair>>(m_, "Point2Pairs");
py::bind_vector<std::vector<gtsam::Point3Pair>>(m_, "Point3Pairs"); py::bind_vector<std::vector<gtsam::Point3Pair>>(m_, "Point3Pairs");
py::bind_vector<std::vector<gtsam::Pose2Pair>>(m_, "Pose2Pairs");
py::bind_vector<std::vector<gtsam::Pose3Pair>>(m_, "Pose3Pairs"); py::bind_vector<std::vector<gtsam::Pose3Pair>>(m_, "Pose3Pairs");
py::bind_vector<std::vector<gtsam::Pose3>>(m_, "Pose3Vector"); py::bind_vector<std::vector<gtsam::Pose3>>(m_, "Pose3Vector");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>>( py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>>(

32
python/gtsam/tests/test_Sim2.py
View File

@ -27,10 +27,10 @@ class TestSim2(GtsamTestCase):
Scenario: Scenario:
3 object poses 3 object poses
same scale (no gauge ambiguity) same scale (no gauge ambiguity)
world frame has poses rotated about x-axis (90 degree roll) world frame has poses rotated about 180 degrees.
world and egovehicle frame translated by 15 meters w.r.t. each other world and egovehicle frame translated by 15 meters w.r.t. each other
""" """
Rx90 = Rot2.fromDegrees(90) R180 = Rot2.fromDegrees(180)
# Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame) # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
# Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
@ -41,10 +41,10 @@ class TestSim2(GtsamTestCase):
eToi_list = [eTo0, eTo1, eTo2] eToi_list = [eTo0, eTo1, eTo2]
# Create destination poses # Create destination poses
# (same three objects, but instead living in the world/city "w" frame) # (same three objects, but instead living in the world "w" frame)
wTo0 = Pose2(Rx90, np.array([-10, 0])) wTo0 = Pose2(R180, np.array([-10, 0]))
wTo1 = Pose2(Rx90, np.array([-5, 0])) wTo1 = Pose2(R180, np.array([-5, 0]))
wTo2 = Pose2(Rx90, np.array([0, 0])) wTo2 = Pose2(R180, np.array([0, 0]))
wToi_list = [wTo0, wTo1, wTo2] wToi_list = [wTo0, wTo1, wTo2]
@ -62,10 +62,10 @@ class TestSim2(GtsamTestCase):
Scenario: Scenario:
3 object poses 3 object poses
with gauge ambiguity (2x scale) with gauge ambiguity (2x scale)
world frame has poses rotated about z-axis (90 degree yaw) world frame has poses rotated by 90 degrees.
world and egovehicle frame translated by 11 meters w.r.t. each other world and egovehicle frame translated by 11 meters w.r.t. each other
""" """
Rz90 = Rot3.Rz(np.deg2rad(90)) R90 = Rot2.fromDegrees(90)
# Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame) # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame)
# Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame
@ -77,9 +77,9 @@ class TestSim2(GtsamTestCase):
# Create destination poses # Create destination poses
# (same three objects, but instead living in the world/city "w" frame) # (same three objects, but instead living in the world/city "w" frame)
wTo0 = Pose2(Rz90, np.array([0, 12])) wTo0 = Pose2(R90, np.array([0, 12]))
wTo1 = Pose2(Rz90, np.array([0, 14])) wTo1 = Pose2(R90, np.array([0, 14]))
wTo2 = Pose2(Rz90, np.array([0, 18])) wTo2 = Pose2(R90, np.array([0, 18]))
wToi_list = [wTo0, wTo1, wTo2] wToi_list = [wTo0, wTo1, wTo2]
@ -107,10 +107,10 @@ class TestSim2(GtsamTestCase):
R180 = Rot2.fromDegrees(180) R180 = Rot2.fromDegrees(180)
R270 = Rot2.fromDegrees(270) R270 = Rot2.fromDegrees(270)
aTi0 = Pose3(R0, np.array([2, 3])) aTi0 = Pose2(R0, np.array([2, 3]))
aTi1 = Pose3(R90, np.array([12, 3])) aTi1 = Pose2(R90, np.array([12, 3]))
aTi2 = Pose3(R180, np.array([12, 13])) aTi2 = Pose2(R180, np.array([12, 13]))
aTi3 = Pose3(R270, np.array([2, 13])) aTi3 = Pose2(R270, np.array([2, 13]))
aTi_list = [aTi0, aTi1, aTi2, aTi3] aTi_list = [aTi0, aTi1, aTi2, aTi3]
@ -144,7 +144,7 @@ class TestSim2(GtsamTestCase):
"""Ensure object equality works properly (are equal).""" """Ensure object equality works properly (are equal)."""
bSa = Similarity2(R=Rot2(), t=np.array([1, 2]), s=3.0) bSa = Similarity2(R=Rot2(), t=np.array([1, 2]), s=3.0)
bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3) bSa_ = Similarity2(R=Rot2(), t=np.array([1.0, 2.0]), s=3)
assert bSa == bSa_ self.gtsamAssertEquals(bSa, bSa_)
def test_not_eq_translation(self) -> None: def test_not_eq_translation(self) -> None:
"""Ensure object equality works properly (not equal translation).""" """Ensure object equality works properly (not equal translation)."""