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

New comments, no normalization any more

release/4.3a0
Frank Dellaert 2010-09-11 15:24:06 +00:00
parent b913c89749
commit e5374a55e8
1 changed files with 23 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
geometry/Pose2.cpp
View File

@ -193,42 +193,25 @@ namespace gtsam {
return n; return n;
} }
/* ************************************************************************* */ /* *************************************************************************
// Re-factor of Michael Sobers' code, in turn based on Frank Dellaert's ML code * New explanation, from scan.ml
// * It finds the angle using a linear method:
// q = Pose2::transform_from(p) = t + R*p * q = Pose2::transform_from(p) = t + R*p
// * We need to remove the centroids from the data to find the rotation
// | qx | cqx + | cos -sin | | px-cpx | |cqx - cos*cpx + sin*cpy| | cos -sin | | px | * using dp=[dpx;dpy] and q=[dqx;dqy] we have
// | | = | | * | | = | | + | | * | | * |dqx| |c -s| |dpx| |dpx -dpy| |c|
// | qy | cqy + | sin cos | | py-cpy | |cqy - sin*cpx - cos*cpy| | sin cos | | py | * | | = | | * | | = | | * | | = H_i*cs
// * |dqy| |s c| |dpy| |dpy dpx| |s|
// where the cos/sin rotation matrix takes the points p-cp into the same frame as the (u,v) points * where the Hi are the 2*2 matrices. Then we will minimize the criterion
// * J = \sum_i norm(q_i - H_i * cs)
// This is reformulated as a linear least-squares regression problem with two parameters (cos,sin), * Taking the derivative with respect to cs and setting to zero we have
// using the (u,v) points as "measurements" of the angle that rotates the (x,y): * cs = (\sum_i H_i' * q_i)/(\sum H_i'*H_i)
// * The hessian is diagonal and just divides by a constant, but this
// | dqx | | dpx -dpy | | cos | | cos | * normalization constant is irrelevant, since we take atan2.
// | | = | | * | | = H * | | * i.e., cos ~ sum(dpx*dqx + dpy*dqy) and sin ~ sum(-dpy*dqx + dpx*dqy)
// | dqy | | dpy dpx | | sin | | sin | * The translation is then found from the centroids
// * as they also satisfy cq = t + R*cp, hence t = cq - R*cp
// 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) { boost::optional<Pose2> align(const vector<Point2Pair>& pairs) {
@ -245,18 +228,16 @@ namespace gtsam {
cp *= f; cq *= f; cp *= f; cq *= f;
// calculate cos and sin // calculate cos and sin
double ct=0,st=0,D=0; double c=0,s=0;
BOOST_FOREACH(const Point2Pair& pair, pairs) { BOOST_FOREACH(const Point2Pair& pair, pairs) {
Point2 dq = pair.first - cp; Point2 dq = pair.first - cp;
Point2 dp = pair.second - cq; Point2 dp = pair.second - cq;
ct += dp.x() * dq.x() + dp.y() * dq.y(); c += 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 !! :-( s += 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 // calculate angle and translation
double theta = atan2(st,ct); double theta = atan2(s,c);
Rot2 R = Rot2::fromAngle(theta); Rot2 R = Rot2::fromAngle(theta);
Point2 t = cq - R*cp; Point2 t = cq - R*cp;
return Pose2(R, t); return Pose2(R, t);