icp.h
Go to the documentation of this file.
1 #ifndef _ICP_H_
2 #define _ICP_H_
3 
4 #include <gmapping/utils/point.h>
5 #include <utility>
6 #include <list>
7 #include <vector>
8 
9 namespace GMapping{
10 typedef std::pair<Point,Point> PointPair;
11 
12 template <typename PointPairContainer>
13 double icpStep(OrientedPoint & retval, const PointPairContainer& container){
14  typedef typename PointPairContainer::const_iterator ContainerIterator;
15  PointPair mean=std::make_pair(Point(0.,0.), Point(0.,0.));
16  int size=0;
17  for (ContainerIterator it=container.begin(); it!=container.end(); it++){
18  mean.first=mean.first+it->first;
19  mean.second=mean.second+it->second;
20  size++;
21  }
22  mean.first=mean.first*(1./size);
23  mean.second=mean.second*(1./size);
24  double sxx=0, sxy=0, syx=0, syy=0;
25 
26  for (ContainerIterator it=container.begin(); it!=container.end(); it++){
27  PointPair mf=std::make_pair(it->first-mean.first, it->second-mean.second);
28  sxx+=mf.first.x*mf.second.x;
29  sxy+=mf.first.x*mf.second.y;
30  syx+=mf.first.y*mf.second.x;
31  syy+=mf.first.y*mf.second.y;
32  }
33  retval.theta=atan2(sxy-syx, sxx+sxy);
34  double s=sin(retval.theta), c=cos(retval.theta);
35  retval.x=mean.second.x-(c*mean.first.x-s*mean.first.y);
36  retval.y=mean.second.y-(s*mean.first.x+c*mean.first.y);
37 
38  double error=0;
39  for (ContainerIterator it=container.begin(); it!=container.end(); it++){
40  Point delta(
41  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);
42  error+=delta*delta;
43  }
44  return error;
45 }
46 
47 template <typename PointPairContainer>
48 double icpNonlinearStep(OrientedPoint & retval, const PointPairContainer& container){
49  typedef typename PointPairContainer::const_iterator ContainerIterator;
50  PointPair mean=std::make_pair(Point(0.,0.), Point(0.,0.));
51  int size=0;
52  for (ContainerIterator it=container.begin(); it!=container.end(); it++){
53  mean.first=mean.first+it->first;
54  mean.second=mean.second+it->second;
55  size++;
56  }
57 
58  mean.first=mean.first*(1./size);
59  mean.second=mean.second*(1./size);
60 
61  double ms=0,mc=0;
62  for (ContainerIterator it=container.begin(); it!=container.end(); it++){
63  PointPair mf=std::make_pair(it->first-mean.first, it->second-mean.second);
64  double dalpha=atan2(mf.second.y, mf.second.x) - atan2(mf.first.y, mf.first.x);
65  double gain=sqrt(mean.first*mean.first);
66  ms+=gain*sin(dalpha);
67  mc+=gain*cos(dalpha);
68  }
69  retval.theta=atan2(ms, mc);
70  double s=sin(retval.theta), c=cos(retval.theta);
71  retval.x=mean.second.x-(c*mean.first.x-s*mean.first.y);
72  retval.y=mean.second.y-(s*mean.first.x+c*mean.first.y);
73 
74  double error=0;
75  for (ContainerIterator it=container.begin(); it!=container.end(); it++){
76  Point delta(
77  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);
78  error+=delta*delta;
79  }
80  return error;
81 }
82 
83 }//end namespace
84 
85 #endif
const char *const *argv double delta
Definition: gfs2stream.cpp:19
point< double > Point
Definition: point.h:202
std::pair< Point, Point > PointPair
Definition: icp.h:10
double icpStep(OrientedPoint &retval, const PointPairContainer &container)
Definition: icp.h:13
unsigned int c
Definition: gfs2stream.cpp:41
double icpNonlinearStep(OrientedPoint &retval, const PointPairContainer &container)
Definition: icp.h:48


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