Skip to content

Commit

Permalink
Merge pull request #1150 from borglab/feature/align_align
Browse files Browse the repository at this point in the history 
Pose2::Align
  • Loading branch information
@dellaert
dellaert authored Apr 1, 2022
2 parents a7c2bc5 + e8d7857 commit b2b878e
Show file tree
Hide file tree
Showing 6 changed files with 126 additions and 82 deletions.
81 changes: 52 additions & 29 deletions gtsam/geometry/Pose2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -309,54 +309,77 @@ double Pose2::range(const Pose2& pose,
}

/* *************************************************************************
* New explanation, from scan.ml
* It finds the angle using a linear method:
* q = Pose2::transformFrom(p) = t + R*p
* Align finds the angle using a linear method:
* a = Pose2::transformFrom(b) = t + R*b
* We need to remove the centroids from the data to find the rotation
* using dp=[dpx;dpy] and q=[dqx;dqy] we have
* |dqx| |c -s| |dpx| |dpx -dpy| |c|
* using db=[dbx;dby] and a=[dax;day] we have
* |dax| |c -s| |dbx| |dbx -dby| |c|
* | | = | | * | | = | | * | | = H_i*cs
* |dqy| |s c| |dpy| |dpy dpx| |s|
* |day| |s c| |dby| |dby dbx| |s|
* where the Hi are the 2*2 matrices. Then we will minimize the criterion
* J = \sum_i norm(q_i - H_i * cs)
* J = \sum_i norm(a_i - H_i * cs)
* Taking the derivative with respect to cs and setting to zero we have
* cs = (\sum_i H_i' * q_i)/(\sum H_i'*H_i)
* cs = (\sum_i H_i' * a_i)/(\sum H_i'*H_i)
* The hessian is diagonal and just divides by a constant, but this
* normalization constant is irrelevant, since we take atan2.
* i.e., cos ~ sum(dpx*dqx + dpy*dqy) and sin ~ sum(-dpy*dqx + dpx*dqy)
* i.e., cos ~ sum(dbx*dax + dby*day) and sin ~ sum(-dby*dax + dbx*day)
* The translation is then found from the centroids
* as they also satisfy cq = t + R*cp, hence t = cq - R*cp
* as they also satisfy ca = t + R*cb, hence t = ca - R*cb
*/

boost::optional<Pose2> align(const vector<Point2Pair>& pairs) {

size_t n = pairs.size();
if (n<2) return boost::none; // we need at least two pairs
boost::optional<Pose2> Pose2::Align(const Point2Pairs &ab_pairs) {
const size_t n = ab_pairs.size();
if (n < 2) {
return boost::none; // we need at least 2 pairs
}

// calculate centroids
Point2 cp(0,0), cq(0,0);
for(const Point2Pair& pair: pairs) {
cp += pair.first;
cq += pair.second;
Point2 ca(0, 0), cb(0, 0);
for (const Point2Pair& pair : ab_pairs) {
ca += pair.first;
cb += pair.second;
}
double f = 1.0/n;
cp *= f; cq *= f;
const double f = 1.0/n;
ca *= f;
cb *= f;

// calculate cos and sin
double c=0,s=0;
for(const Point2Pair& pair: pairs) {
Point2 dp = pair.first - cp;
Point2 dq = pair.second - cq;
c += dp.x() * dq.x() + dp.y() * dq.y();
s += -dp.y() * dq.x() + dp.x() * dq.y();
double c = 0, s = 0;
for (const Point2Pair& pair : ab_pairs) {
Point2 da = pair.first - ca;
Point2 db = pair.second - cb;
c += db.x() * da.x() + db.y() * da.y();
s += -db.y() * da.x() + db.x() * da.y();
}

// calculate angle and translation
double theta = atan2(s,c);
Rot2 R = Rot2::fromAngle(theta);
Point2 t = cq - R*cp;
const double theta = atan2(s, c);
const Rot2 R = Rot2::fromAngle(theta);
const Point2 t = ca - R*cb;
return Pose2(R, t);
}

boost::optional<Pose2> Pose2::Align(const Matrix& a, const Matrix& b) {
if (a.rows() != 2 || b.rows() != 2 || a.cols() != b.cols()) {
throw std::invalid_argument(
"Pose2:Align expects 2*N matrices of equal shape.");
}
Point2Pairs ab_pairs;
for (size_t j=0; j < a.cols(); j++) {
ab_pairs.emplace_back(a.col(j), b.col(j));
}
return Pose2::Align(ab_pairs);
}

#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
boost::optional<Pose2> align(const Point2Pairs& ba_pairs) {
Point2Pairs ab_pairs;
for (const Point2Pair &baPair : ba_pairs) {
ab_pairs.emplace_back(baPair.second, baPair.first);
}
return Pose2::Align(ab_pairs);
}
#endif

/* ************************************************************************* */
} // namespace gtsam
19 changes: 17 additions & 2 deletions gtsam/geometry/Pose2.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,18 @@ class Pose2: public LieGroup<Pose2, 3> {
*this = Expmap(v);
}

/**
* Create Pose2 by aligning two point pairs
* A pose aTb is estimated between pairs (a_point, b_point) such that
* a_point = aTb * b_point
* Note this allows for noise on the points but in that case the mapping
* will not be exact.
*/
static boost::optional<Pose2> Align(const Point2Pairs& abPointPairs);

// Version of Pose2::Align that takes 2 matrices.
static boost::optional<Pose2> Align(const Matrix& a, const Matrix& b);

/// @}
/// @name Testable
/// @{
Expand Down Expand Up @@ -331,12 +343,15 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
return Matrix(Pose2::wedge(xi(0),xi(1),xi(2))).eval();
}

#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/**
* @deprecated Use static constructor (with reversed pairs!)
* Calculate pose between a vector of 2D point correspondences (p,q)
* where q = Pose2::transformFrom(p) = t + R*p
*/
typedef std::pair<Point2,Point2> Point2Pair;
GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
GTSAM_EXPORT boost::optional<Pose2>
GTSAM_DEPRECATED align(const Point2Pairs& pairs);
#endif

template <>
struct traits<Pose2> : public internal::LieGroup<Pose2> {};
Expand Down
2 changes: 2 additions & 0 deletions gtsam/geometry/Pose3.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -479,13 +479,15 @@ boost::optional<Pose3> Pose3::Align(const Matrix& a, const Matrix& b) {
return Pose3::Align(abPointPairs);
}

#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
Point3Pairs abPointPairs;
for (const Point3Pair &baPair : baPointPairs) {
abPointPairs.emplace_back(baPair.second, baPair.first);
}
return Pose3::Align(abPointPairs);
}
#endif

/* ************************************************************************* */
std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
Expand Down
5 changes: 3 additions & 2 deletions gtsam/geometry/geometry.i
View file
Original file line number Diff line number Diff line change
Expand Up @@ -372,6 +372,9 @@ class Pose2 {
Pose2(const gtsam::Rot2& r, const gtsam::Point2& t);
Pose2(Vector v);

static boost::optional<gtsam::Pose2> Align(const gtsam::Point2Pairs& abPointPairs);
static boost::optional<gtsam::Pose2> Align(const gtsam::Matrix& a, const gtsam::Matrix& b);

// Testable
void print(string s = "") const;
bool equals(const gtsam::Pose2& pose, double tol) const;
Expand Down Expand Up @@ -424,8 +427,6 @@ class Pose2 {
void serialize() const;
};

boost::optional<gtsam::Pose2> align(const gtsam::Point2Pairs& pairs);

#include <gtsam/geometry/Pose3.h>
class Pose3 {
// Standard Constructors
Expand Down
72 changes: 33 additions & 39 deletions gtsam/geometry/tests/testPose2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -717,81 +717,75 @@ TEST( Pose2, range_pose )
/* ************************************************************************* */

TEST(Pose2, align_1) {
Pose2 expected(Rot2::fromAngle(0), Point2(10,10));

vector<Point2Pair> correspondences;
Point2Pair pq1(make_pair(Point2(0,0), Point2(10,10)));
Point2Pair pq2(make_pair(Point2(20,10), Point2(30,20)));
correspondences += pq1, pq2;

boost::optional<Pose2> actual = align(correspondences);
EXPECT(assert_equal(expected, *actual));
Pose2 expected(Rot2::fromAngle(0), Point2(10, 10));
Point2Pairs ab_pairs {{Point2(10, 10), Point2(0, 0)},
{Point2(30, 20), Point2(20, 10)}};
boost::optional<Pose2> aTb = Pose2::Align(ab_pairs);
EXPECT(assert_equal(expected, *aTb));
}

TEST(Pose2, align_2) {
Point2 t(20,10);
Point2 t(20, 10);
Rot2 R = Rot2::fromAngle(M_PI/2.0);
Pose2 expected(R, t);

vector<Point2Pair> correspondences;
Point2 p1(0,0), p2(10,0);
Point2 q1 = expected.transformFrom(p1), q2 = expected.transformFrom(p2);
EXPECT(assert_equal(Point2(20,10),q1));
EXPECT(assert_equal(Point2(20,20),q2));
Point2Pair pq1(make_pair(p1, q1));
Point2Pair pq2(make_pair(p2, q2));
correspondences += pq1, pq2;
Point2 b1(0, 0), b2(10, 0);
Point2Pairs ab_pairs {{expected.transformFrom(b1), b1},
{expected.transformFrom(b2), b2}};

boost::optional<Pose2> actual = align(correspondences);
EXPECT(assert_equal(expected, *actual));
boost::optional<Pose2> aTb = Pose2::Align(ab_pairs);
EXPECT(assert_equal(expected, *aTb));
}

namespace align_3 {
Point2 t(10,10);
Point2 t(10, 10);
Pose2 expected(Rot2::fromAngle(2*M_PI/3), t);
Point2 p1(0,0), p2(10,0), p3(10,10);
Point2 q1 = expected.transformFrom(p1), q2 = expected.transformFrom(p2), q3 = expected.transformFrom(p3);
Point2 b1(0, 0), b2(10, 0), b3(10, 10);
Point2 a1 = expected.transformFrom(b1),
a2 = expected.transformFrom(b2),
a3 = expected.transformFrom(b3);
}

TEST(Pose2, align_3) {
using namespace align_3;

vector<Point2Pair> correspondences;
Point2Pair pq1(make_pair(p1, q1));
Point2Pair pq2(make_pair(p2, q2));
Point2Pair pq3(make_pair(p3, q3));
correspondences += pq1, pq2, pq3;
Point2Pairs ab_pairs;
Point2Pair ab1(make_pair(a1, b1));
Point2Pair ab2(make_pair(a2, b2));
Point2Pair ab3(make_pair(a3, b3));
ab_pairs += ab1, ab2, ab3;

boost::optional<Pose2> actual = align(correspondences);
EXPECT(assert_equal(expected, *actual));
boost::optional<Pose2> aTb = Pose2::Align(ab_pairs);
EXPECT(assert_equal(expected, *aTb));
}

namespace {
/* ************************************************************************* */
// Prototype code to align two triangles using a rigid transform
/* ************************************************************************* */
struct Triangle { size_t i_,j_,k_;};
struct Triangle { size_t i_, j_, k_;};

boost::optional<Pose2> align2(const Point2Vector& ps, const Point2Vector& qs,
boost::optional<Pose2> align2(const Point2Vector& as, const Point2Vector& bs,
const pair<Triangle, Triangle>& trianglePair) {
const Triangle& t1 = trianglePair.first, t2 = trianglePair.second;
vector<Point2Pair> correspondences;
correspondences += make_pair(ps[t1.i_],qs[t2.i_]), make_pair(ps[t1.j_],qs[t2.j_]), make_pair(ps[t1.k_],qs[t2.k_]);
return align(correspondences);
Point2Pairs ab_pairs = {{as[t1.i_], bs[t2.i_]},
{as[t1.j_], bs[t2.j_]},
{as[t1.k_], bs[t2.k_]}};
return Pose2::Align(ab_pairs);
}
}

TEST(Pose2, align_4) {
using namespace align_3;

Point2Vector ps,qs;
ps += p1, p2, p3;
qs += q3, q1, q2; // note in 3,1,2 order !
Point2Vector as, bs;
as += a1, a2, a3;
bs += b3, b1, b2; // note in 3,1,2 order !

Triangle t1; t1.i_=0; t1.j_=1; t1.k_=2;
Triangle t2; t2.i_=1; t2.j_=2; t2.k_=0;

boost::optional<Pose2> actual = align2(ps, qs, make_pair(t1,t2));
boost::optional<Pose2> actual = align2(as, bs, {t1, t2});
EXPECT(assert_equal(expected, *actual));
}

Expand Down
29 changes: 19 additions & 10 deletions python/gtsam/tests/test_Pose2.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,27 +70,36 @@ def test_align(self) -> None:
O---O
"""
pts_a = [
Point2(3, 1),
Point2(1, 1),
Point2(1, 3),
Point2(3, 3),
]
pts_b = [
Point2(1, -3),
Point2(1, -5),
Point2(-1, -5),
Point2(-1, -3),
]
pts_b = [
Point2(3, 1),
Point2(1, 1),
Point2(1, 3),
Point2(3, 3),
]

# fmt: on
ab_pairs = Point2Pairs(list(zip(pts_a, pts_b)))
bTa = gtsam.align(ab_pairs)
aTb = bTa.inverse()
assert aTb is not None
aTb = Pose2.Align(ab_pairs)
self.assertIsNotNone(aTb)

for pt_a, pt_b in zip(pts_a, pts_b):
pt_a_ = aTb.transformFrom(pt_b)
np.testing.assert_allclose(pt_a, pt_a_)

# Matrix version
A = np.array(pts_a).T
B = np.array(pts_b).T
aTb = Pose2.Align(A, B)
self.assertIsNotNone(aTb)

for pt_a, pt_b in zip(pts_a, pts_b):
pt_a_ = aTb.transformFrom(pt_b)
assert np.allclose(pt_a, pt_a_)
np.testing.assert_allclose(pt_a, pt_a_)


if __name__ == "__main__":
Expand Down

0 comments on commit b2b878e

Please sign in to comment.