Finish Sim3 align and transformFrom functions.
ss
parent
269dea3a24
commit
8425957e3f
|
|
@ -356,6 +356,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
|
||||
typedef std::pair<Pose3, Pose3> Pose3Pair;
|
||||
|
||||
// For MATLAB wrapper
|
||||
typedef std::vector<Pose3> Pose3Vector;
|
||||
|
||||
|
|
|
|||
|
|
@ -85,10 +85,105 @@ 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_.matrix() * T.translation().vector() + t_.vector()));
|
||||
return Pose3(R, t);
|
||||
}
|
||||
|
||||
Point3 Similarity3::operator*(const Point3& p) const {
|
||||
return transformFrom(p);
|
||||
}
|
||||
|
||||
Similarity3 Similarity3::Align(const std::vector<Point3Pair>& abPointPairs) {
|
||||
const size_t n = abPointPairs.size();
|
||||
if (n < 3) throw std::runtime_error("less than 3 pairs"); // we need at least three pairs
|
||||
|
||||
// calculate centroids
|
||||
Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0);
|
||||
for (const Point3Pair& abPair : abPointPairs) {
|
||||
aCentroid += abPair.first;
|
||||
bCentroid += abPair.second;
|
||||
}
|
||||
double f = 1.0 / n;
|
||||
aCentroid *= f;
|
||||
bCentroid *= f;
|
||||
|
||||
// Add to form H matrix
|
||||
Matrix3 H = Z_3x3;
|
||||
for (const Point3Pair& abPair : abPointPairs) {
|
||||
Point3 da = abPair.first - aCentroid;
|
||||
Point3 db = abPair.second - bCentroid;
|
||||
H += da * db.transpose();
|
||||
}
|
||||
|
||||
// ClosestTo finds rotation matrix closest to H in Frobenius sense
|
||||
Rot3 aRb = Rot3::ClosestTo(H);
|
||||
|
||||
// Calculate scale
|
||||
double x = 0;
|
||||
double y = 0;
|
||||
for (const Point3Pair& abPair : abPointPairs) {
|
||||
Point3 da = abPair.first - aCentroid;
|
||||
Point3 db = abPair.second - bCentroid;
|
||||
Vector3 Rdb = aRb * db;
|
||||
y += da.transpose() * Rdb;
|
||||
x += Rdb.transpose() * Rdb;
|
||||
}
|
||||
double s = y / x;
|
||||
|
||||
Point3 aTb = (Point3(aCentroid) - s * (aRb * Point3(bCentroid))) / s;
|
||||
return Similarity3(aRb, aTb, s);
|
||||
}
|
||||
|
||||
Rot3 Similarity3::rotationAveraging(const std::vector<Rot3>& rotations, double error) {
|
||||
Rot3 R = rotations[0];
|
||||
const size_t n = rotations.size();
|
||||
Vector3 r;
|
||||
r << 0, 0, 0;
|
||||
while (1) {
|
||||
for (const Rot3 R_i : rotations) {
|
||||
r += Rot3::Logmap(R.inverse().compose(R_i));
|
||||
}
|
||||
r /= n;
|
||||
if (r.norm() < error) return R;
|
||||
R = R.compose(Rot3::Expmap(r));
|
||||
}
|
||||
}
|
||||
|
||||
Similarity3 Similarity3::Align(const std::vector<Pose3Pair>& abPosePairs) {
|
||||
const size_t n = abPosePairs.size();
|
||||
if (n < 2) throw std::runtime_error("less than 2 pairs"); // we need at least two pairs
|
||||
|
||||
// calculate centroids
|
||||
Point3 aCentroid(0, 0, 0), bCentroid(0, 0, 0);
|
||||
vector<Rot3> rotationList;
|
||||
for (const Pose3Pair& abPair : abPosePairs) {
|
||||
aCentroid += abPair.first.translation();
|
||||
bCentroid += abPair.second.translation();
|
||||
rotationList.push_back(abPair.first.rotation().compose(abPair.second.rotation().inverse()));
|
||||
}
|
||||
double f = 1.0 / n;
|
||||
aCentroid *= f;
|
||||
bCentroid *= f;
|
||||
Rot3 aRb = Similarity3::rotationAveraging(rotationList);
|
||||
|
||||
// Calculate scale
|
||||
double x = 0;
|
||||
double y = 0;
|
||||
for (const Pose3Pair& abPair : abPosePairs) {
|
||||
Point3 da = abPair.first.translation() - aCentroid;
|
||||
Point3 db = abPair.second.translation() - bCentroid;
|
||||
Vector3 Rdb = aRb * db;
|
||||
y += da.transpose() * Rdb;
|
||||
x += Rdb.transpose() * Rdb;
|
||||
}
|
||||
double s = y / x;
|
||||
|
||||
Point3 aTb = (Point3(aCentroid) - s * (aRb * Point3(bCentroid))) / s;
|
||||
return Similarity3(aRb, aTb, s);
|
||||
}
|
||||
|
||||
Matrix4 Similarity3::wedge(const Vector7& xi) {
|
||||
// http://www.ethaneade.org/latex2html/lie/node29.html
|
||||
const auto w = xi.head<3>();
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@
|
|||
|
||||
#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>
|
||||
|
|
@ -101,9 +102,27 @@ public:
|
|||
OptionalJacobian<3, 7> H1 = boost::none, //
|
||||
OptionalJacobian<3, 3> H2 = boost::none) const;
|
||||
|
||||
/// Action on a pose T
|
||||
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 two point pairs
|
||||
*/
|
||||
GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
|
||||
|
||||
/**
|
||||
* Calculate the average rotation from a list of rotations
|
||||
*/
|
||||
GTSAM_UNSTABLE_EXPORT static Rot3 rotationAveraging(const std::vector<Rot3>& rotations, double error = 1e-10);
|
||||
|
||||
/**
|
||||
* Create Similarity3 by aligning two pose pairs
|
||||
*/
|
||||
GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
/// @{
|
||||
|
|
|
|||
|
|
@ -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,107 @@ TEST(Similarity3, GroupAction) {
|
|||
}
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
// Group action on Pose3
|
||||
TEST(Similarity3, GroupActionPose3) {
|
||||
Similarity3 bTa(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
|
||||
|
||||
// Create source poses
|
||||
Pose3 Ta1 = Pose3(Rot3(), Point3(0, 0, 0));
|
||||
Pose3 Ta2 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0));
|
||||
|
||||
// Create destination poses
|
||||
Pose3 expectedTb1 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
|
||||
Pose3 expectedTb2 = Pose3(Rot3(1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(-4, 6, 10));
|
||||
|
||||
EXPECT(assert_equal(expectedTb1, bTa.transformFrom(Ta1)));
|
||||
EXPECT(assert_equal(expectedTb2, bTa.transformFrom(Ta2)));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
// Align with Point3 Pairs
|
||||
TEST(Similarity3, AlignPoint3_1) {
|
||||
Similarity3 expected(Rot3::Rz(-90 * degree), Point3(3, 4, 5), 2.0);
|
||||
|
||||
Point3 p1 = Point3(0, 0, 0);
|
||||
Point3 p2 = Point3(3, 0, 0);
|
||||
Point3 p3 = Point3(3, 0, 4);
|
||||
|
||||
Point3Pair ab1(make_pair(expected.transformFrom(p1), p1));
|
||||
Point3Pair ab2(make_pair(expected.transformFrom(p2), p2));
|
||||
Point3Pair ab3(make_pair(expected.transformFrom(p3), p3));
|
||||
|
||||
vector<Point3Pair> correspondences{ab1, ab2, ab3};
|
||||
|
||||
Similarity3 actual = Similarity3::Align(correspondences);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
TEST(Similarity3, AlignPoint3_2) {
|
||||
Similarity3 expected(Rot3(), Point3(10, 10, 0), 1.0);
|
||||
|
||||
Point3 p1 = Point3(0, 0, 0);
|
||||
Point3 p2 = Point3(20, 10, 0);
|
||||
Point3 p3 = Point3(10, 20, 0);
|
||||
|
||||
Point3Pair ab1(make_pair(expected.transformFrom(p1), p1));
|
||||
Point3Pair ab2(make_pair(expected.transformFrom(p2), p2));
|
||||
Point3Pair ab3(make_pair(expected.transformFrom(p3), p3));
|
||||
|
||||
vector<Point3Pair> correspondences{ab1, ab2, ab3};
|
||||
|
||||
Similarity3 actual = Similarity3::Align(correspondences);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
TEST(Similarity3, AlignPoint3_3) {
|
||||
Similarity3 expected(Rot3::RzRyRx(0.3, 0.2, 0.1), Point3(20, 10, 5), 1.0);
|
||||
|
||||
Point3 p1 = Point3(0, 0, 1);
|
||||
Point3 p2 = Point3(10, 0, 2);
|
||||
Point3 p3 = Point3(20, -10, 30);
|
||||
|
||||
Point3Pair ab1(make_pair(expected.transformFrom(p1), p1));
|
||||
Point3Pair ab2(make_pair(expected.transformFrom(p2), p2));
|
||||
Point3Pair ab3(make_pair(expected.transformFrom(p3), p3));
|
||||
|
||||
vector<Point3Pair> correspondences{ab1, ab2, ab3};
|
||||
|
||||
Similarity3 actual = Similarity3::Align(correspondences);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
// Rotation Averaging
|
||||
TEST(Similarity3, RotationAveraging) {
|
||||
Rot3 expected = Rot3::Rx(90 * degree);
|
||||
vector<Rot3> rotations{Rot3(), Rot3::Rx(90 * degree), Rot3::Rx(180 * degree)};
|
||||
Rot3 actual = Similarity3::rotationAveraging(rotations);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
// Align with Pose3 Pairs
|
||||
TEST(Similarity3, AlignPose3) {
|
||||
Similarity3 expected(Rot3::Ry(180 * degree), Point3(2, 3, 5), 2.0);
|
||||
|
||||
// Create source poses
|
||||
Pose3 Ta1 = Pose3(Rot3(), Point3(0, 0, 0));
|
||||
Pose3 Ta2 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, 1), Point3(4, 0, 0));
|
||||
|
||||
// Create destination poses
|
||||
Pose3 Tb1 = Pose3(Rot3(-1, 0, 0, 0, 1, 0, 0, 0, -1), Point3(4, 6, 10));
|
||||
Pose3 Tb2 = Pose3(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 = Similarity3::Align(correspondences);
|
||||
EXPECT(assert_equal(expected, actual));
|
||||
}
|
||||
|
||||
//******************************************************************************
|
||||
// Test very simple prior optimization example
|
||||
TEST(Similarity3, Optimization) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue