lumiles.h
Go to the documentation of this file.
1 #ifndef LUMILESPROCESSOR
2 #define LUMILESPROCESSOR
3 
4 namespace GMapping{
5 
7  typedef std:vector<Point> PointVector;
8  static OrientedPoint step(const PointVector& src, const PointVector& dest);
9 };
10 
11 OrientedPoint LuMilesProcessors::step(const PointVector& src, const PointVector& dest){
12  assert(src.size()==dest.size());
13  unsigned int size=dest.size();
14  double smx=0, smy=0, dmx=0, dmy=0;
15  for (PointVector::const_iterator it=src.begin(); it!=src.end(); it++){
16  smx+=it->x;
17  smy+=it->y;
18  }
19  smx/=src.size();
20  smy/=src.size();
21 
22  for (PointVector::const_iterator it=dest.begin(); it!=dest.end(); it++){
23  dmx+=it->x;
24  dmy+=it->y;
25  }
26  dmx/=src.size();
27  dmy/=src.size();
28 
29  double sxx=0, sxy=0;
30  double syx=0, syy=0;
31  for (unsigned int i=0; i<size(); i++){
32  sxx+=(src[i].x-smx)*(dest[i].x-dmx);
33  sxy+=(src[i].x-smx)*(dest[i].y-dmy);
34  syx+=(src[i].y-smy)*(dest[i].x-dmx);
35  syy+=(src[i].y-smy)*(dest[i].y-dmy);
36  }
37  double omega=atan2(sxy-syx,sxx+syy);
38  return OrientedPoint(
39  dmx-smx*cos(omega)+smx*sin(omega)),
40  dmy-smx*sin(omega)-smy*cos(omega)),
41  omega
42  )
43 };
44 
45 int main(int argc, conat char ** argv){
46 }
47 
48 };
49 
50 #endif
static OrientedPoint step(const PointVector &src, const PointVector &dest)
int main(int argc, conat char **argv)
Definition: lumiles.h:45
orientedpoint< double, double > OrientedPoint
Definition: point.h:203


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20