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
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> {};

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) {
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
gtsam/geometry/Rot2.h
View File

@ -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);

180
gtsam/geometry/Similarity2.cpp
View File

@ -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 &centroids) {
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 &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)
@ -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

110
gtsam/geometry/Similarity2.h
View File

@ -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,22 +92,22 @@ 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;
/**
/**
* 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.
* To retrieve a Pose2, we normalized the scale value into 1.
* |Rs*R Rs*t+ts| |Rs*R s(Rs*t+ts)|
* | 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
*/
GTSAM_EXPORT Pose2 transformFrom(const Pose2& T) 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 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 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);
/// @}
/// @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

16
gtsam/geometry/Similarity3.cpp
View File

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

14
gtsam/geometry/geometry.i
View File

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

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

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

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

@ -26,7 +26,6 @@
#include <GeographicLib/LocalCartesian.hpp>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
using namespace GeographicLib;
@ -63,7 +62,7 @@ TEST( MagFactor, unrotate ) {
Matrix H;
Point3 expected(22735.5, 314.502, 44202.5);
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));
}
@ -74,27 +73,27 @@ TEST( MagFactor, Factors ) {
// MagFactor
MagFactor f(1, measured, s, dir, bias, model);
EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> //
EXPECT(assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector, Rot2> //
(std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7));
// MagFactor1
// MagFactor1
MagFactor1 f1(1, measured, s, dir, bias, model);
EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
EXPECT(assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
EXPECT(assert_equal(numericalDerivative11<Vector, Rot3> //
(std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7));
// MagFactor2
// MagFactor2
MagFactor2 f2(1, 2, measured, nRb, model);
EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
EXPECT(assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT(assert_equal(numericalDerivative11<Vector, Point3> //
(std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),//
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),//
H2, 1e-7));
// MagFactor2
// MagFactor3
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((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);
}
template <class T,
typename = typename std::enable_if< std::is_same<gtsam::Rot3, T>::value >::type >
template <class T>
T FindKarcherMean(const std::vector<T>& 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>>);
PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Pose2Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
PYBIND11_MAKE_OPAQUE(
gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
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");
py::bind_vector<std::vector<gtsam::Point2Pair>>(m_, "Point2Pairs");
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::Pose3>>(m_, "Pose3Vector");
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:
3 object poses
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
"""
Rx90 = Rot2.fromDegrees(90)
R180 = Rot2.fromDegrees(180)
# 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
@ -41,10 +41,10 @@ class TestSim2(GtsamTestCase):
eToi_list = [eTo0, eTo1, eTo2]
# Create destination poses
# (same three objects, but instead living in the world/city "w" frame)
wTo0 = Pose2(Rx90, np.array([-10, 0]))
wTo1 = Pose2(Rx90, np.array([-5, 0]))
wTo2 = Pose2(Rx90, np.array([0, 0]))
# (same three objects, but instead living in the world "w" frame)
wTo0 = Pose2(R180, np.array([-10, 0]))
wTo1 = Pose2(R180, np.array([-5, 0]))
wTo2 = Pose2(R180, np.array([0, 0]))
wToi_list = [wTo0, wTo1, wTo2]
@ -62,10 +62,10 @@ class TestSim2(GtsamTestCase):
Scenario:
3 object poses
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
"""
Rz90 = Rot3.Rz(np.deg2rad(90))
R90 = Rot2.fromDegrees(90)
# 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
@ -77,9 +77,9 @@ class TestSim2(GtsamTestCase):
# Create destination poses
# (same three objects, but instead living in the world/city "w" frame)
wTo0 = Pose2(Rz90, np.array([0, 12]))
wTo1 = Pose2(Rz90, np.array([0, 14]))
wTo2 = Pose2(Rz90, np.array([0, 18]))
wTo0 = Pose2(R90, np.array([0, 12]))
wTo1 = Pose2(R90, np.array([0, 14]))
wTo2 = Pose2(R90, np.array([0, 18]))
wToi_list = [wTo0, wTo1, wTo2]
@ -107,10 +107,10 @@ class TestSim2(GtsamTestCase):
R180 = Rot2.fromDegrees(180)
R270 = Rot2.fromDegrees(270)
aTi0 = Pose3(R0, np.array([2, 3]))
aTi1 = Pose3(R90, np.array([12, 3]))
aTi2 = Pose3(R180, np.array([12, 13]))
aTi3 = Pose3(R270, np.array([2, 13]))
aTi0 = Pose2(R0, np.array([2, 3]))
aTi1 = Pose2(R90, np.array([12, 3]))
aTi2 = Pose2(R180, np.array([12, 13]))
aTi3 = Pose2(R270, np.array([2, 13]))
aTi_list = [aTi0, aTi1, aTi2, aTi3]
@ -144,7 +144,7 @@ class TestSim2(GtsamTestCase):
"""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.0, 2.0]), s=3)
assert bSa == bSa_
self.gtsamAssertEquals(bSa, bSa_)
def test_not_eq_translation(self) -> None:
"""Ensure object equality works properly (not equal translation)."""