Go to the documentation of this file.00001 #ifndef _ICP_H_
00002 #define _ICP_H_
00003
00004 #include <gmapping/utils/point.h>
00005 #include <utility>
00006 #include <list>
00007 #include <vector>
00008
00009 namespace GMapping{
00010 typedef std::pair<Point,Point> PointPair;
00011
00012 template <typename PointPairContainer>
00013 double icpStep(OrientedPoint & retval, const PointPairContainer& container){
00014 typedef typename PointPairContainer::const_iterator ContainerIterator;
00015 PointPair mean=std::make_pair(Point(0.,0.), Point(0.,0.));
00016 int size=0;
00017 for (ContainerIterator it=container.begin(); it!=container.end(); it++){
00018 mean.first=mean.first+it->first;
00019 mean.second=mean.second+it->second;
00020 size++;
00021 }
00022 mean.first=mean.first*(1./size);
00023 mean.second=mean.second*(1./size);
00024 double sxx=0, sxy=0, syx=0, syy=0;
00025
00026 for (ContainerIterator it=container.begin(); it!=container.end(); it++){
00027 PointPair mf=std::make_pair(it->first-mean.first, it->second-mean.second);
00028 sxx+=mf.first.x*mf.second.x;
00029 sxy+=mf.first.x*mf.second.y;
00030 syx+=mf.first.y*mf.second.x;
00031 syy+=mf.first.y*mf.second.y;
00032 }
00033 retval.theta=atan2(sxy-syx, sxx+sxy);
00034 double s=sin(retval.theta), c=cos(retval.theta);
00035 retval.x=mean.second.x-(c*mean.first.x-s*mean.first.y);
00036 retval.y=mean.second.y-(s*mean.first.x+c*mean.first.y);
00037
00038 double error=0;
00039 for (ContainerIterator it=container.begin(); it!=container.end(); it++){
00040 Point delta(
00041 c*it->first.x-s*it->first.y+retval.x-it->second.x, s*it->first.x+c*it->first.y+retval.y-it->second.y);
00042 error+=delta*delta;
00043 }
00044 return error;
00045 }
00046
00047 template <typename PointPairContainer>
00048 double icpNonlinearStep(OrientedPoint & retval, const PointPairContainer& container){
00049 typedef typename PointPairContainer::const_iterator ContainerIterator;
00050 PointPair mean=std::make_pair(Point(0.,0.), Point(0.,0.));
00051 int size=0;
00052 for (ContainerIterator it=container.begin(); it!=container.end(); it++){
00053 mean.first=mean.first+it->first;
00054 mean.second=mean.second+it->second;
00055 size++;
00056 }
00057
00058 mean.first=mean.first*(1./size);
00059 mean.second=mean.second*(1./size);
00060
00061 double ms=0,mc=0;
00062 for (ContainerIterator it=container.begin(); it!=container.end(); it++){
00063 PointPair mf=std::make_pair(it->first-mean.first, it->second-mean.second);
00064 double dalpha=atan2(mf.second.y, mf.second.x) - atan2(mf.first.y, mf.first.x);
00065 double gain=sqrt(mean.first*mean.first);
00066 ms+=gain*sin(dalpha);
00067 mc+=gain*cos(dalpha);
00068 }
00069 retval.theta=atan2(ms, mc);
00070 double s=sin(retval.theta), c=cos(retval.theta);
00071 retval.x=mean.second.x-(c*mean.first.x-s*mean.first.y);
00072 retval.y=mean.second.y-(s*mean.first.x+c*mean.first.y);
00073
00074 double error=0;
00075 for (ContainerIterator it=container.begin(); it!=container.end(); it++){
00076 Point delta(
00077 c*it->first.x-s*it->first.y+retval.x-it->second.x, s*it->first.x+c*it->first.y+retval.y-it->second.y);
00078 error+=delta*delta;
00079 }
00080 return error;
00081 }
00082
00083 }
00084
00085 #endif