00001 #include "PoseEstimation.h"
00002
00003 double compute2DPose(const std::vector< std::pair<Point2D, Point2D> > &correspondences, OrientedPoint2D& transformation)
00004 {
00005 Point2D point1mean, point2mean;
00006 for(unsigned int i = 0; i < correspondences.size(); i++){
00007 point1mean = point1mean + correspondences[i].first;
00008 point2mean = point2mean + correspondences[i].second;
00009 }
00010 point1mean = point1mean * (1./double(correspondences.size()));
00011 point2mean = point2mean * (1./double(correspondences.size()));
00012
00013 double A = 0, B = 0;
00014 for(unsigned int i = 0; i < correspondences.size(); i++){
00015 Point2D delta1 = correspondences[i].first - point1mean;
00016 Point2D delta2 = correspondences[i].second - point2mean;
00017 A += delta1 * delta2;
00018 B += delta1.ortho() * delta2;
00019 }
00020
00021 A /= double(correspondences.size());
00022 B /= double(correspondences.size());
00023
00024 double denom = sqrt(A*A + B*B);
00025 double sinalpha1 = B/denom;
00026 double cosalpha1 = -A/denom;
00027 double sinalpha2 = -sinalpha1;
00028 double cosalpha2 = -cosalpha1;
00029
00030 Point2D point1rotated(cosalpha1 * point1mean.x - sinalpha1 * point1mean.y,
00031 sinalpha1 * point1mean.x + cosalpha1 * point1mean.y);
00032 Point2D point2rotated(cosalpha2 * point1mean.x - sinalpha2 * point1mean.y,
00033 sinalpha2 * point1mean.x + cosalpha2 * point1mean.y);
00034
00035 Point2D translation1(point2mean - point1rotated);
00036 Point2D translation2(point2mean - point2rotated);
00037
00038 double error1 = 0, error2 = 0;
00039 for(unsigned int i = 0; i < correspondences.size(); i++){
00040 const Point2D& point1 = correspondences[i].first;
00041 const Point2D& point2 = correspondences[i].second;
00042 Point2D delta1 = point2 - Point2D(cosalpha1 * point1.x - sinalpha1 * point1.y, sinalpha1 * point1.x + cosalpha1 * point1.y) - translation1;
00043 Point2D delta2 = point2 - Point2D(cosalpha2 * point1.x - sinalpha2 * point1.y, sinalpha2 * point1.x + cosalpha2 * point1.y) - translation2;
00044 error1 = error1 + delta1 * delta1;
00045 error2 = error2 + delta2 * delta2;
00046 }
00047 if(error1 <= error2){
00048 transformation.x = translation1.x; transformation.y = translation1.y; transformation.theta = atan2(sinalpha1, cosalpha1);
00049 return error1;
00050 } else {
00051 transformation.x = translation2.x; transformation.y = translation2.y; transformation.theta = atan2(sinalpha2, cosalpha2);
00052 return error2;
00053 }
00054 }
