icptest.cpp
Go to the documentation of this file.
1 #include <cstdlib>
2 #include <iostream>
3 #include <fstream>
4 
5 #include <list>
7 
8 using namespace GMapping;
9 using namespace std;
10 
11 typedef std::list<PointPair> PointPairList;
12 
14  PointPairList ppl;
15  double s=sin(t.theta), c=cos(t.theta);
16  for (int i=0; i<size; i++){
17  Point noiseDraw(noise*(drand48()-.5),noise*(drand48()-.5));
18  PointPair pp;
19  pp.first.x=100.*(drand48()-.5)+200;
20  pp.first.y=10.*(drand48()-.5);
21  pp.second.x= c*pp.first.x-s*pp.first.y;
22  pp.second.y= s*pp.first.x+c*pp.first.y;
23  pp.second=pp.second+t+noiseDraw;
24  //cerr << "p1=" << pp.first.x << " " << pp.first.y << endl;
25  //cerr << "p2=" << pp.second.x << " " << pp.second.y << endl;
26  ppl.push_back(pp);
27  }
28  return ppl;
29 }
30 
31 
32 int main(int argc, const char ** argv){
33  while (1){
34  OrientedPoint t;
35  int size;
36  cerr << "Insert size, t.x, t.y, t.theta" << endl;
37  cin >> size >> t.x >> t.y >> t.theta;
39  OrientedPoint tc;
40  OrientedPoint ttot(0.,0.,0.);
41  bool method=true;
42  while(1){
43  char buf[10];
44  cerr << "iterate?" << endl;
45  cin.getline(buf,10);
46  if (buf[0]=='n')
47  method=false;
48  else if (buf[0]=='l')
49  method=true;
50  else if (buf[0]!=char(0))
51  break;
52  cout << "plot '-' w l, '-' w p, '-' w p" << endl;
53  for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
54  cout << it->first.x << " " << it->first.y<< endl;
55  cout << it->second.x << " " << it->second.y<< endl;
56  cout << endl;
57  }
58  cout << "e" << endl;
59  for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
60  cout << it->first.x << " " << it->first.y<< endl;
61  }
62  cout << "e" << endl;
63  for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
64  cout << it->second.x << " " << it->second.y<< endl;
65  }
66  cout << "e" << endl;
67 
68  double error;
69  if (!method){
70  cerr << "Nonlinear Optimization" << endl;
71  error=icpNonlinearStep(tc,ppl);
72  }else {
73  cerr << "Linear Optimization" << endl;
74  error=icpStep(tc,ppl);
75  }
76  cerr << "ICP err=" << error << " t.x=" << tc.x << " t.y=" << tc.y << " t.theta=" << tc.theta << endl;
77  cerr << "\t" << error << " ttot.x=" << ttot.x << " ttot.y=" << ttot.y << " ttot.theta=" << ttot.theta << endl;
78  double s=sin(tc.theta), c=cos(tc.theta);
79  for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
80  Point p1(c*it->first.x-s*it->first.y+tc.x,
81  s*it->first.x+c*it->first.y+tc.y);
82  it->first=p1;
83  }
84  ttot.x+=tc.x;
85  ttot.y+=tc.y;
86  ttot.theta+=tc.theta;
87  ttot.theta=atan2(sin(ttot.theta), cos(ttot.theta));
88  }
89  }
90  return 0;
91 }
std::pair< Point, Point > PointPair
Definition: icp.h:10
std::list< PointPair > PointPairList
Definition: icptest.cpp:11
double icpStep(OrientedPoint &retval, const PointPairContainer &container)
Definition: icp.h:13
PointPairList generateRandomPointPairs(int size, OrientedPoint t, double noise=0.)
Definition: icptest.cpp:13
unsigned int c
Definition: gfs2stream.cpp:41
double icpNonlinearStep(OrientedPoint &retval, const PointPairContainer &container)
Definition: icp.h:48
int main(int argc, conat char **argv)
Definition: lumiles.h:45


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