X-lanni
92 lines
2.6 KiB
C++
92 lines
2.6 KiB
C++
#include <cstdlib>
|
|
#include <iostream>
|
|
#include <fstream>
|
|
|
|
#include <list>
|
|
#include <gmapping/scanmatcher/icp.h>
|
|
|
|
using namespace GMapping;
|
|
using namespace std;
|
|
|
|
typedef std::list<PointPair> PointPairList;
|
|
|
|
PointPairList generateRandomPointPairs(int size, OrientedPoint t, double noise=0.){
|
|
PointPairList ppl;
|
|
double s=sin(t.theta), c=cos(t.theta);
|
|
for (int i=0; i<size; i++){
|
|
Point noiseDraw(noise*(drand48()-.5),noise*(drand48()-.5));
|
|
PointPair pp;
|
|
pp.first.x=100.*(drand48()-.5)+200;
|
|
pp.first.y=10.*(drand48()-.5);
|
|
pp.second.x= c*pp.first.x-s*pp.first.y;
|
|
pp.second.y= s*pp.first.x+c*pp.first.y;
|
|
pp.second=pp.second+t+noiseDraw;
|
|
//cerr << "p1=" << pp.first.x << " " << pp.first.y << endl;
|
|
//cerr << "p2=" << pp.second.x << " " << pp.second.y << endl;
|
|
ppl.push_back(pp);
|
|
}
|
|
return ppl;
|
|
}
|
|
|
|
|
|
int main(int argc, const char ** argv){
|
|
while (1){
|
|
OrientedPoint t;
|
|
int size;
|
|
cerr << "Insert size, t.x, t.y, t.theta" << endl;
|
|
cin >> size >> t.x >> t.y >> t.theta;
|
|
PointPairList ppl=generateRandomPointPairs(size, t, 3);
|
|
OrientedPoint tc;
|
|
OrientedPoint ttot(0.,0.,0.);
|
|
bool method=true;
|
|
while(1){
|
|
char buf[10];
|
|
cerr << "iterate?" << endl;
|
|
cin.getline(buf,10);
|
|
if (buf[0]=='n')
|
|
method=false;
|
|
else if (buf[0]=='l')
|
|
method=true;
|
|
else if (buf[0]!=char(0))
|
|
break;
|
|
cout << "plot '-' w l, '-' w p, '-' w p" << endl;
|
|
for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
|
|
cout << it->first.x << " " << it->first.y<< endl;
|
|
cout << it->second.x << " " << it->second.y<< endl;
|
|
cout << endl;
|
|
}
|
|
cout << "e" << endl;
|
|
for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
|
|
cout << it->first.x << " " << it->first.y<< endl;
|
|
}
|
|
cout << "e" << endl;
|
|
for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
|
|
cout << it->second.x << " " << it->second.y<< endl;
|
|
}
|
|
cout << "e" << endl;
|
|
|
|
double error;
|
|
if (!method){
|
|
cerr << "Nonlinear Optimization" << endl;
|
|
error=icpNonlinearStep(tc,ppl);
|
|
}else {
|
|
cerr << "Linear Optimization" << endl;
|
|
error=icpStep(tc,ppl);
|
|
}
|
|
cerr << "ICP err=" << error << " t.x=" << tc.x << " t.y=" << tc.y << " t.theta=" << tc.theta << endl;
|
|
cerr << "\t" << error << " ttot.x=" << ttot.x << " ttot.y=" << ttot.y << " ttot.theta=" << ttot.theta << endl;
|
|
double s=sin(tc.theta), c=cos(tc.theta);
|
|
for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){
|
|
Point p1(c*it->first.x-s*it->first.y+tc.x,
|
|
s*it->first.x+c*it->first.y+tc.y);
|
|
it->first=p1;
|
|
}
|
|
ttot.x+=tc.x;
|
|
ttot.y+=tc.y;
|
|
ttot.theta+=tc.theta;
|
|
ttot.theta=atan2(sin(ttot.theta), cos(ttot.theta));
|
|
}
|
|
}
|
|
return 0;
|
|
}
|