Frank Dellaert
GitHub
commit
b89478c0fa
|
|
@ -75,6 +75,19 @@ double dot(const Point3 &p, const Point3 &q, OptionalJacobian<1, 3> H1,
|
|||
return p.x() * q.x() + p.y() * q.y() + p.z() * q.z();
|
||||
}
|
||||
|
||||
Point3Pair mean(const std::vector<Point3Pair> &abPointPairs) {
|
||||
const size_t n = abPointPairs.size();
|
||||
Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0);
|
||||
for (const Point3Pair &abPair : abPointPairs) {
|
||||
aCentroid += abPair.first;
|
||||
bCentroid += abPair.second;
|
||||
}
|
||||
const double f = 1.0 / n;
|
||||
aCentroid *= f;
|
||||
bCentroid *= f;
|
||||
return make_pair(aCentroid, bCentroid);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
ostream &operator<<(ostream &os, const gtsam::Point3Pair &p) {
|
||||
os << p.first << " <-> " << p.second;
|
||||
|
|
|
|||
|
|
@ -26,6 +26,7 @@
|
|||
#include <gtsam/base/Vector.h>
|
||||
#include <gtsam/dllexport.h>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
#include <numeric>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -58,6 +59,17 @@ GTSAM_EXPORT double dot(const Point3& p, const Point3& q,
|
|||
OptionalJacobian<1, 3> H_p = boost::none,
|
||||
OptionalJacobian<1, 3> H_q = boost::none);
|
||||
|
||||
/// mean
|
||||
template <class CONTAINER>
|
||||
GTSAM_EXPORT Point3 mean(const CONTAINER& points) {
|
||||
Point3 sum(0, 0, 0);
|
||||
sum = std::accumulate(points.begin(), points.end(), sum);
|
||||
return sum / points.size();
|
||||
}
|
||||
|
||||
/// mean of Point3 pair
|
||||
GTSAM_EXPORT Point3Pair mean(const std::vector<Point3Pair>& abPointPairs);
|
||||
|
||||
template <typename A1, typename A2>
|
||||
struct Range;
|
||||
|
||||
|
|
|
|||
|
|
@ -368,6 +368,9 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
|
|||
return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5));
|
||||
}
|
||||
|
||||
// Convenience typedef
|
||||
using Pose3Pair = std::pair<Pose3, Pose3>;
|
||||
|
||||
// For MATLAB wrapper
|
||||
typedef std::vector<Pose3> Pose3Vector;
|
||||
|
||||
|
|
|
|||
|
|
@ -164,6 +164,26 @@ TEST (Point3, normalize) {
|
|||
EXPECT(assert_equal(expectedH, actualH, 1e-8));
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
TEST(Point3, mean) {
|
||||
Point3 expected_a_mean(2, 2, 2);
|
||||
Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3);
|
||||
std::vector<Point3> a_points{a1, a2, a3};
|
||||
Point3 actual_a_mean = mean(a_points);
|
||||
EXPECT(assert_equal(expected_a_mean, actual_a_mean));
|
||||
}
|
||||
|
||||
TEST(Point3, mean_pair) {
|
||||
Point3 a_mean(2, 2, 2), b_mean(-1, 1, 0);
|
||||
Point3Pair expected_mean = std::make_pair(a_mean, b_mean);
|
||||
Point3 a1(0, 0, 0), a2(1, 2, 3), a3(5, 4, 3);
|
||||
Point3 b1(-1, 0, 0), b2(-2, 4, 0), b3(0, -1, 0);
|
||||
std::vector<Point3Pair> point_pairs{{a1,b1},{a2,b2},{a3,b3}};
|
||||
Point3Pair actual_mean = mean(point_pairs);
|
||||
EXPECT(assert_equal(expected_mean.first, actual_mean.first));
|
||||
EXPECT(assert_equal(expected_mean.second, actual_mean.second));
|
||||
}
|
||||
|
||||
//*************************************************************************
|
||||
double norm_proxy(const Point3& point) {
|
||||
return double(point.norm());
|
||||
|
|
|
|||
|
|
@ -19,9 +19,61 @@
|
|||
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam/slam/KarcherMeanFactor-inl.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
namespace {
|
||||
/// Subtract centroids from point pairs.
|
||||
static std::vector<Point3Pair> subtractCentroids(const std::vector<Point3Pair>& abPointPairs, const Point3Pair& centroids) {
|
||||
std::vector<Point3Pair> d_abPointPairs;
|
||||
for (const Point3Pair& abPair : abPointPairs) {
|
||||
Point3 da = abPair.first - centroids.first;
|
||||
Point3 db = abPair.second - centroids.second;
|
||||
d_abPointPairs.emplace_back(da, db);
|
||||
}
|
||||
return d_abPointPairs;
|
||||
}
|
||||
|
||||
/// Form inner products x and y and calculate scale.
|
||||
static const double calculateScale(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb) {
|
||||
double x = 0, y = 0;
|
||||
Point3 da, db;
|
||||
for (const Point3Pair& d_abPair : d_abPointPairs) {
|
||||
std::tie(da, db) = d_abPair;
|
||||
const Vector3 da_prime = aRb * db;
|
||||
y += da.transpose() * da_prime;
|
||||
x += da_prime.transpose() * da_prime;
|
||||
}
|
||||
const double s = y / x;
|
||||
return s;
|
||||
}
|
||||
|
||||
/// Form outer product H.
|
||||
static Matrix3 calculateH(const std::vector<Point3Pair>& d_abPointPairs) {
|
||||
Matrix3 H = Z_3x3;
|
||||
for (const Point3Pair& 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 Similarity3 align(const std::vector<Point3Pair>& d_abPointPairs, const Rot3& aRb, const Point3Pair& centroids) {
|
||||
const double s = calculateScale(d_abPointPairs, aRb);
|
||||
const Point3 aTb = (centroids.first - s * (aRb * centroids.second)) / s;
|
||||
return Similarity3(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 Similarity3 alignGivenR(const std::vector<Point3Pair>& abPointPairs, const Rot3& aRb) {
|
||||
auto centroids = mean(abPointPairs);
|
||||
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
|
||||
return align(d_abPointPairs, aRb, centroids);
|
||||
}
|
||||
} // namespace
|
||||
|
||||
Similarity3::Similarity3() :
|
||||
t_(0,0,0), s_(1) {
|
||||
}
|
||||
|
|
@ -54,15 +106,15 @@ bool Similarity3::operator==(const Similarity3& other) const {
|
|||
void Similarity3::print(const std::string& s) const {
|
||||
std::cout << std::endl;
|
||||
std::cout << s;
|
||||
rotation().print("R:\n");
|
||||
rotation().print("\nR:\n");
|
||||
std::cout << "t: " << translation().transpose() << " s: " << scale() << std::endl;
|
||||
}
|
||||
|
||||
Similarity3 Similarity3::identity() {
|
||||
return Similarity3();
|
||||
}
|
||||
Similarity3 Similarity3::operator*(const Similarity3& T) const {
|
||||
return Similarity3(R_ * T.R_, ((1.0 / T.s_) * t_) + R_ * T.t_, s_ * T.s_);
|
||||
Similarity3 Similarity3::operator*(const Similarity3& S) const {
|
||||
return Similarity3(R_ * S.R_, ((1.0 / S.s_) * t_) + R_ * S.t_, s_ * S.s_);
|
||||
}
|
||||
|
||||
Similarity3 Similarity3::inverse() const {
|
||||
|
|
@ -85,10 +137,47 @@ Point3 Similarity3::transformFrom(const Point3& p, //
|
|||
return s_ * q;
|
||||
}
|
||||
|
||||
Pose3 Similarity3::transformFrom(const Pose3& T) const {
|
||||
Rot3 R = R_.compose(T.rotation());
|
||||
Point3 t = Point3(s_ * (R_ * T.translation() + t_));
|
||||
return Pose3(R, t);
|
||||
}
|
||||
|
||||
Point3 Similarity3::operator*(const Point3& p) const {
|
||||
return transformFrom(p);
|
||||
}
|
||||
|
||||
Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
|
||||
// Refer to: http://www5.informatik.uni-erlangen.de/Forschung/Publikationen/2005/Zinsser05-PSR.pdf Chapter 3
|
||||
if (abPointPairs.size() < 3) throw std::runtime_error("input should have at least 3 pairs of points");
|
||||
auto centroids = mean(abPointPairs);
|
||||
auto d_abPointPairs = subtractCentroids(abPointPairs, centroids);
|
||||
Matrix3 H = calculateH(d_abPointPairs);
|
||||
// ClosestTo finds rotation matrix closest to H in Frobenius sense
|
||||
Rot3 aRb = Rot3::ClosestTo(H);
|
||||
return align(d_abPointPairs, aRb, centroids);
|
||||
}
|
||||
|
||||
Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& 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<Rot3> rotations;
|
||||
vector<Point3Pair> abPointPairs;
|
||||
rotations.reserve(n);
|
||||
abPointPairs.reserve(n);
|
||||
Pose3 wTa, wTb;
|
||||
for (const Pose3Pair& abPair : abPosePairs) {
|
||||
std::tie(wTa, wTb) = abPair;
|
||||
rotations.emplace_back(wTa.rotation().compose(wTb.rotation().inverse()));
|
||||
abPointPairs.emplace_back(wTa.translation(), wTb.translation());
|
||||
}
|
||||
const Rot3 aRb = FindKarcherMean<Rot3>(rotations);
|
||||
|
||||
return alignGivenR(abPointPairs, aRb);
|
||||
}
|
||||
|
||||
Matrix4 Similarity3::wedge(const Vector7& xi) {
|
||||
// http://www.ethaneade.org/latex2html/lie/node29.html
|
||||
const auto w = xi.head<3>();
|
||||
|
|
|
|||
|
|
@ -19,10 +19,12 @@
|
|||
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/base/Manifold.h>
|
||||
#include <gtsam_unstable/dllexport.h>
|
||||
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declarations
|
||||
|
|
@ -87,7 +89,7 @@ public:
|
|||
GTSAM_UNSTABLE_EXPORT static Similarity3 identity();
|
||||
|
||||
/// Composition
|
||||
GTSAM_UNSTABLE_EXPORT Similarity3 operator*(const Similarity3& T) const;
|
||||
GTSAM_UNSTABLE_EXPORT Similarity3 operator*(const Similarity3& S) const;
|
||||
|
||||
/// Return the inverse
|
||||
GTSAM_UNSTABLE_EXPORT Similarity3 inverse() const;
|
||||
|
|
@ -101,9 +103,32 @@ public:
|
|||
OptionalJacobian<3, 7> H1 = boost::none, //
|
||||
OptionalJacobian<3, 3> H2 = boost::none) const;
|
||||
|
||||
/**
|
||||
* Action on a pose T.
|
||||
* |Rs ts| |R t| |Rs*R Rs*t+ts|
|
||||
* |0 1/s| * |0 1| = | 0 1/s |, the result is still a Sim3 object.
|
||||
* To retrieve a Pose3, 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.
|
||||
* For more details, refer to: https://en.wikipedia.org/wiki/Group_action
|
||||
*/
|
||||
GTSAM_UNSTABLE_EXPORT Pose3 transformFrom(const Pose3& T) const;
|
||||
|
||||
/** syntactic sugar for transformFrom */
|
||||
GTSAM_UNSTABLE_EXPORT Point3 operator*(const Point3& p) const;
|
||||
|
||||
/**
|
||||
* Create Similarity3 by aligning at least three point pairs
|
||||
*/
|
||||
GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
|
||||
|
||||
/**
|
||||
* Create Similarity3 by aligning at least two pose pairs
|
||||
*/
|
||||
GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
|
|
@ -182,8 +207,8 @@ public:
|
|||
/// @name Helper functions
|
||||
/// @{
|
||||
|
||||
/// Calculate expmap and logmap coefficients.
|
||||
private:
|
||||
/// Calculate expmap and logmap coefficients.
|
||||
static Matrix3 GetV(Vector3 w, double lambda);
|
||||
|
||||
/// @}
|
||||
|
|
|
|||
|
|
@ -51,6 +51,8 @@ static const Similarity3 T4(R, P, s);
|
|||
static const Similarity3 T5(R, P, 10);
|
||||
static const Similarity3 T6(Rot3(), Point3(1, 1, 0), 2); // Simpler transform
|
||||
|
||||
const double degree = M_PI / 180;
|
||||
|
||||
//******************************************************************************
|
||||
TEST(Similarity3, Concepts) {
|
||||
BOOST_CONCEPT_ASSERT((IsGroup<Similarity3 >));
|
||||
|
|
@ -255,6 +257,114 @@ TEST(Similarity3, GroupAction) {
|
|||
}
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
// Group action on Pose3
|
||||
TEST(Similarity3, GroupActionPose3) {
|
||||
Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
|
||||
|
||||
// Create source poses
|
||||
Pose3 Ta1(Rot3(), Point3(0, 0, 0));
|
||||
Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
|
||||
|
||||
// Create destination poses
|
||||
Pose3 expected_Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
|
||||
Pose3 expected_Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
|
||||
|
||||
EXPECT(assert_equal(expected_Tb1, bSa.transformFrom(Ta1)));
|
||||
EXPECT(assert_equal(expected_Tb2, bSa.transformFrom(Ta2)));
|
||||
}
|
||||
|
||||
// Test left group action compatibility.
|
||||
// cSa*Ta = cSb*bSa*Ta
|
||||
TEST(Similarity3, GroupActionPose3_Compatibility) {
|
||||
Similarity3 bSa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
|
||||
Similarity3 cSb(Rot3::Ry(90 * degree), Point3(-10, -4, 0), 3.0);
|
||||
Similarity3 cSa(Rot3::Ry(270 * degree), Point3(0, 1, -2), 6.0);
|
||||
|
||||
// Create poses
|
||||
Pose3 Ta1(Rot3(), Point3(0, 0, 0));
|
||||
Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
|
||||
Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
|
||||
Pose3 Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
|
||||
Pose3 Tc1(Rot3(0, 0, -1, 0, 1, 0, 1, 0, 0), Point3(0, 6, -12));
|
||||
Pose3 Tc2(Rot3(0, 0, -1, 0, -1, 0, -1, 0, 0), Point3(0, 6, 12));
|
||||
|
||||
EXPECT(assert_equal(Tc1, cSb.transformFrom(Tb1)));
|
||||
EXPECT(assert_equal(Tc2, cSb.transformFrom(Tb2)));
|
||||
|
||||
EXPECT(assert_equal(cSa.transformFrom(Ta1), cSb.transformFrom(Tb1)));
|
||||
EXPECT(assert_equal(cSa.transformFrom(Ta2), cSb.transformFrom(Tb2)));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
// Align with Point3 Pairs
|
||||
TEST(Similarity3, AlignPoint3_1) {
|
||||
Similarity3 expected_aSb(Rot3::Rz(-90 * degree), Point3(3, 4, 5), 2.0);
|
||||
|
||||
Point3 b1(0, 0, 0), b2(3, 0, 0), b3(3, 0, 4);
|
||||
|
||||
Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1));
|
||||
Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2));
|
||||
Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3));
|
||||
|
||||
vector<Point3Pair> correspondences{ab1, ab2, ab3};
|
||||
|
||||
Similarity3 actual_aSb = Similarity3::Align(correspondences);
|
||||
EXPECT(assert_equal(expected_aSb, actual_aSb));
|
||||
}
|
||||
|
||||
TEST(Similarity3, AlignPoint3_2) {
|
||||
Similarity3 expected_aSb(Rot3(), Point3(10, 10, 0), 1.0);
|
||||
|
||||
Point3 b1(0, 0, 0), b2(20, 10, 0), b3(10, 20, 0);
|
||||
|
||||
Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1));
|
||||
Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2));
|
||||
Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3));
|
||||
|
||||
vector<Point3Pair> correspondences{ab1, ab2, ab3};
|
||||
|
||||
Similarity3 actual_aSb = Similarity3::Align(correspondences);
|
||||
EXPECT(assert_equal(expected_aSb, actual_aSb));
|
||||
}
|
||||
|
||||
TEST(Similarity3, AlignPoint3_3) {
|
||||
Similarity3 expected_aSb(Rot3::RzRyRx(0.3, 0.2, 0.1), Point3(20, 10, 5), 1.0);
|
||||
|
||||
Point3 b1(0, 0, 1), b2(10, 0, 2), b3(20, -10, 30);
|
||||
|
||||
Point3Pair ab1(make_pair(expected_aSb.transformFrom(b1), b1));
|
||||
Point3Pair ab2(make_pair(expected_aSb.transformFrom(b2), b2));
|
||||
Point3Pair ab3(make_pair(expected_aSb.transformFrom(b3), b3));
|
||||
|
||||
vector<Point3Pair> correspondences{ab1, ab2, ab3};
|
||||
|
||||
Similarity3 actual_aSb = Similarity3::Align(correspondences);
|
||||
EXPECT(assert_equal(expected_aSb, actual_aSb));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
// Align with Pose3 Pairs
|
||||
TEST(Similarity3, AlignPose3) {
|
||||
Similarity3 expected_aSb(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
|
||||
|
||||
// Create source poses
|
||||
Pose3 Ta1(Rot3(), Point3(0, 0, 0));
|
||||
Pose3 Ta2(Rot3(-1, 0, 0, 0, -1, 0, 0, 0, 1), Point3(4, 0, 0));
|
||||
|
||||
// Create destination poses
|
||||
Pose3 Tb1(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
|
||||
Pose3 Tb2(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(-4, 6, 10));
|
||||
|
||||
Pose3Pair bTa1(make_pair(Tb1, Ta1));
|
||||
Pose3Pair bTa2(make_pair(Tb2, Ta2));
|
||||
|
||||
vector<Pose3Pair> correspondences{bTa1, bTa2};
|
||||
|
||||
Similarity3 actual_aSb = Similarity3::Align(correspondences);
|
||||
EXPECT(assert_equal(expected_aSb, actual_aSb));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
// Test very simple prior optimization example
|
||||
TEST(Similarity3, Optimization) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue