Frank Dellaert
GitHub
commit
b2b878efc7
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -92,6 +92,18 @@ public:
|
|||
*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
|
||||
/// @{
|
||||
|
|
@ -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> {};
|
||||
|
|
|
|||
|
|
@ -479,6 +479,7 @@ 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) {
|
||||
|
|
@ -486,6 +487,7 @@ boost::optional<Pose3> align(const Point3Pairs &baPointPairs) {
|
|||
}
|
||||
return Pose3::Align(abPointPairs);
|
||||
}
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
std::ostream &operator<<(std::ostream &os, const Pose3& pose) {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -70,27 +70,36 @@ class TestPose2(GtsamTestCase):
|
|||
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)
|
||||
assert np.allclose(pt_a, pt_a_)
|
||||
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)
|
||||
np.testing.assert_allclose(pt_a, pt_a_)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
|
|||
Loading…
Reference in New Issue