lumiles.h
Go to the documentation of this file.
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


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