00001 #ifndef LUMILESPROCESSOR
00002 #define LUMILESPROCESSOR
00003
00004 namespace GMapping{
00005
00006 class LuMilesProcessor{
00007 typedef std:vector<Point> PointVector;
00008 static OrientedPoint step(const PointVector& src, const PointVector& dest);
00009 };
00010
00011 OrientedPoint LuMilesProcessors::step(const PointVector& src, const PointVector& dest){
00012 assert(src.size()==dest.size());
00013 unsigned int size=dest.size();
00014 double smx=0, smy=0, dmx=0, dmy=0;
00015 for (PointVector::const_iterator it=src.begin(); it!=src.end(); it++){
00016 smx+=it->x;
00017 smy+=it->y;
00018 }
00019 smx/=src.size();
00020 smy/=src.size();
00021
00022 for (PointVector::const_iterator it=dest.begin(); it!=dest.end(); it++){
00023 dmx+=it->x;
00024 dmy+=it->y;
00025 }
00026 dmx/=src.size();
00027 dmy/=src.size();
00028
00029 double sxx=0, sxy=0;
00030 double syx=0, syy=0;
00031 for (unsigned int i=0; i<size(); i++){
00032 sxx+=(src[i].x-smx)*(dest[i].x-dmx);
00033 sxy+=(src[i].x-smx)*(dest[i].y-dmy);
00034 syx+=(src[i].y-smy)*(dest[i].x-dmx);
00035 syy+=(src[i].y-smy)*(dest[i].y-dmy);
00036 }
00037 double omega=atan2(sxy-syx,sxx+syy);
00038 return OrientedPoint(
00039 dmx-smx*cos(omega)+smx*sin(omega)),
00040 dmy-smx*sin(omega)-smy*cos(omega)),
00041 omega
00042 )
00043 };
00044
00045 int main(int argc, conat char ** argv){
00046 }
00047
00048 };
00049
00050 #endif
