icp.h
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 }//end namespace
00084 
00085 #endif


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:13