icptest.cpp
Go to the documentation of this file.
00001 #include <cstdlib>
00002 #include <iostream>
00003 #include <fstream>
00004 
00005 #include <list>
00006 #include <gmapping/scanmatcher/icp.h>
00007 
00008 using namespace GMapping;
00009 using namespace std;
00010 
00011 typedef std::list<PointPair> PointPairList;
00012 
00013 PointPairList generateRandomPointPairs(int size, OrientedPoint t, double noise=0.){
00014         PointPairList ppl;
00015         double s=sin(t.theta), c=cos(t.theta);
00016         for (int i=0; i<size; i++){
00017                 Point noiseDraw(noise*(drand48()-.5),noise*(drand48()-.5));
00018                 PointPair pp;
00019                 pp.first.x=100.*(drand48()-.5)+200;
00020                 pp.first.y=10.*(drand48()-.5);
00021                 pp.second.x= c*pp.first.x-s*pp.first.y;
00022                 pp.second.y= s*pp.first.x+c*pp.first.y;
00023                 pp.second=pp.second+t+noiseDraw;
00024                 //cerr << "p1=" << pp.first.x << " " << pp.first.y << endl;
00025                 //cerr << "p2=" << pp.second.x << " " << pp.second.y << endl;
00026                 ppl.push_back(pp);
00027         }
00028         return ppl;
00029 }
00030 
00031 
00032 int main(int argc, const char ** argv){
00033         while (1){
00034                 OrientedPoint t;
00035                 int size;
00036                 cerr << "Insert size, t.x, t.y, t.theta" << endl;
00037                 cin >> size >> t.x >> t.y >> t.theta;
00038                 PointPairList ppl=generateRandomPointPairs(size, t, 3);
00039                 OrientedPoint tc;
00040                 OrientedPoint ttot(0.,0.,0.);
00041                 bool method=true;
00042                 while(1){
00043                         char buf[10];
00044                         cerr << "iterate?" << endl;
00045                         cin.getline(buf,10);
00046                         if (buf[0]=='n')
00047                                 method=false;
00048                         else if (buf[0]=='l')
00049                                 method=true;
00050                         else if (buf[0]!=char(0))
00051                                 break;
00052                         cout << "plot '-' w l, '-' w p, '-' w p" << endl;
00053                         for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
00054                                 cout << it->first.x << " " << it->first.y<< endl;
00055                                 cout << it->second.x << " " << it->second.y<< endl;
00056                                 cout << endl;
00057                         }
00058                         cout << "e" << endl;
00059                         for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
00060                                 cout << it->first.x << " " << it->first.y<< endl;
00061                         }
00062                         cout << "e" << endl;
00063                         for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
00064                                 cout << it->second.x << " " << it->second.y<< endl;
00065                         }
00066                         cout << "e" << endl;
00067                         
00068                         double error;
00069                         if (!method){
00070                                 cerr << "Nonlinear Optimization" << endl;
00071                                 error=icpNonlinearStep(tc,ppl);
00072                         }else {
00073                                 cerr << "Linear Optimization" << endl;
00074                                 error=icpStep(tc,ppl);
00075                         }
00076                         cerr << "ICP err=" << error << " t.x=" << tc.x << " t.y=" << tc.y << " t.theta=" << tc.theta << endl;
00077                         cerr << "\t" << error << " ttot.x=" << ttot.x << " ttot.y=" << ttot.y << " ttot.theta=" << ttot.theta << endl;
00078                         double s=sin(tc.theta), c=cos(tc.theta);
00079                         for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
00080                                 Point p1(c*it->first.x-s*it->first.y+tc.x,
00081                                          s*it->first.x+c*it->first.y+tc.y);
00082                                 it->first=p1;
00083                         }
00084                         ttot.x+=tc.x;
00085                         ttot.y+=tc.y;
00086                         ttot.theta+=tc.theta;
00087                         ttot.theta=atan2(sin(ttot.theta), cos(ttot.theta));
00088                 }
00089         }
00090         return 0;
00091 }


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Fri Aug 28 2015 11:56:21