12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

align finds Pose2 between *correct* 2D point correspondences

release/4.3a0
Frank Dellaert 2010-09-11 05:29:38 +00:00
parent 7f25b3f086
commit bfe91d6337
4 changed files with 121 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
geometry/Point2.h
View File

@ -87,8 +87,10 @@ namespace gtsam {
Vector vector() const { return Vector_(2, x_, y_); }
/** operators */
inline void operator += (const Point2& q) {x_+=q.x_;y_+=q.y_;}
inline void operator *= (double s) {x_*=s;y_*=s;}
inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;}
inline Point2 operator- () const {return Point2(-x_,-y_);}
inline bool operator ==(const Point2& q) const {return x_==q.x_ && q.y_==q.y_;}
inline Point2 operator + (const Point2& q) const {return Point2(x_+q.x_,y_+q.y_);}
inline Point2 operator - (const Point2& q) const {return Point2(x_-q.x_,y_-q.y_);}
inline Point2 operator * (double s) const {return Point2(x_*s,y_*s);}

70
geometry/Pose2.cpp
View File

@ -3,6 +3,7 @@
* @brief 2D Pose
*/
#include <boost/foreach.hpp>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Lie-inl.h>
@ -192,5 +193,74 @@ namespace gtsam {
return n;
}
// Re-factor of Michael Sobers' code, in turn based on Frank Dellaert's ML code
// and the Applied Estimation course notes of Dr. Mark Costello
//
// q = Pose2::transform_from(p) = t + R*p
//
// | qx | cqx + | cos -sin | | px-cpx | |cqx - cos*cpx + sin*cpy| | cos -sin | | px |
// | | = | | * | | = | | + | | * | |
// | qy | cqy + | sin cos | | py-cpy | |cqy - sin*cpx - cos*cpy| | sin cos | | py |
//
// where the cos/sin rotation matrix takes the points p-cp into the same frame as the (u,v) points
//
// This is reformulated as a linear least-squares regression problem with two parameters (cos,sin),
// using the (u,v) points as "measurements" of the angle that rotates the (x,y):
//
// | dqx | | dpx -dpy | | cos | | cos |
// | | = | | * | | = H * | |
// | dqy | | dpy dpx | | sin | | sin |
//
// The solution is: | cos | | dqx |
// | | = inv(H'H)*H'*| |
// | sin | | dqy |
//
// where the rotation angle is found by using atan2(sin,cos).
//
// As it turns out, H'H is symmetric: H'H = | sum(dpx^2 + dpy^2) 0 |
// | 0 sum(dpx^2 + dpy^2) |
//
// | dqx | | sum( dpx*dqx + dpy*dqy) |
// Also, H'*| | = | |
// | dqy | | sum(-dpy*dqx + dpx*dqy) |
//
// so that cos = sum(dpx*dqx + dpy*dqy)/D and sin = sum(-dpy*dqx + dpx*dqy)/D
// where D = sum(dpx^2 + dpy^2)
//
// We need to remove the centroids from the data sets for this to work.
//
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
// calculate centroids
Point2 cp,cq;
BOOST_FOREACH(const Point2Pair& pair, pairs) {
cp += pair.first;
cq += pair.second;
}
double f = 1.0/n;
cp *= f; cq *= f;
// calculate cos and sin
double ct=0,st=0,D=0;
BOOST_FOREACH(const Point2Pair& pair, pairs) {
Point2 dq = pair.first - cp;
Point2 dp = pair.second - cq;
ct += dp.x() * dq.x() + dp.y() * dq.y();
st += dp.y() * dq.x() - dp.x() * dq.y(); // this works but is negative from formula above !! :-(
D += dp.x()*dp.x() + dp.y()*dp.y();
}
ct /= D; st /= D;
// calculate angle and translation
double theta = atan2(st,ct);
Rot2 R = Rot2::fromAngle(theta);
Point2 t = cq - R*cp;
return Pose2(R, t);
}
/* ************************************************************************* */
} // namespace gtsam

7
geometry/Pose2.h
View File

@ -195,5 +195,12 @@ namespace gtsam {
return Pose2::wedge(xi(0),xi(1),xi(2));
}
/**
* Calculate pose between a vector of 2D point correspondences (p,q)
* where q = Pose2::transform_from(p) = t + R*p
*/
typedef std::pair<Point2,Point2> Point2Pair;
boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
} // namespace gtsam

51
geometry/tests/testPose2.cpp
View File

@ -483,20 +483,51 @@ TEST( Pose2, range )
}
/* ************************************************************************* */
typedef pair<Point2,Point2> Point2Pair;
boost::optional<Pose2> align(const vector<Point2Pair> &) {
return boost::none;
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));
}
TEST(Pose2, align) {
vector<Point2Pair> correspondences;
Point2Pair p1(make_pair(Point2(0,0), Point2(10,0)));
Point2Pair p2(make_pair(Point2(20,20), Point2(30,20)));
correspondences += p1, p2;
TEST(Pose2, align_2) {
Point2 t(20,10);
Rot2 R = Rot2::fromAngle(M_PI_2);
Pose2 expected(R, t);
vector<Point2Pair> correspondences;
Point2 p1(0,0), p2(10,0);
Point2 q1 = expected.transform_from(p1), q2 = expected.transform_from(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;
Pose2 expected(Rot2::fromAngle(0), Point2(0,0));
boost::optional<Pose2> actual = align(correspondences);
//EXPECT(assert_equal(expected, *actual));
EXPECT(assert_equal(expected, *actual));
}
TEST(Pose2, align_3) {
Point2 t(10,10);
Pose2 expected(Rot2::fromAngle(2*M_PI/3), t);
vector<Point2Pair> correspondences;
Point2 p1(0,0), p2(10,0), p3(10,10);
Point2 q1 = expected.transform_from(p1), q2 = expected.transform_from(p2), q3 = expected.transform_from(p3);
Point2Pair pq1(make_pair(p1, q1));
Point2Pair pq2(make_pair(p2, q2));
Point2Pair pq3(make_pair(p3, q3));
correspondences += pq1, pq2, pq3;
boost::optional<Pose2> actual = align(correspondences);
EXPECT(assert_equal(expected, *actual));
}
/* ************************************************************************* */