scanmatcherprocessor.cpp
Go to the documentation of this file.
1 #include <iostream>
4 
5 //#define SCANMATHCERPROCESSOR_DEBUG
6 namespace GMapping {
7 
8 using namespace std;
9 
11  : m_map(m.getCenter(), m.getWorldSizeX(), m.getWorldSizeY(), m.getResolution()),
12  m_pose(0,0,0){
13  m_regScore=300;
15  m_maxMove=1;
16  m_beams=0;
17  m_computeCovariance=false;
18  //m_eigenspace=gsl_eigen_symmv_alloc(3);
19  useICP=false;
20 }
21 
22 
24  (double xmin, double ymin, double xmax, double ymax, double delta, double patchdelta):
25  m_map(Point((xmax+xmin)*.5, (ymax+ymin)*.5), xmax-xmin, ymax-ymin, patchdelta), m_pose(0,0,0){
26  m_regScore=300;
28  m_maxMove=1;
29  m_beams=0;
30  m_computeCovariance=false;
31  //m_eigenspace=gsl_eigen_symmv_alloc(3);
32  useICP=false;
33 }
34 
36  //gsl_eigen_symmv_free(m_eigenspace);
37 }
38 
39 
40 void ScanMatcherProcessor::setSensorMap(const SensorMap& smap, std::string sensorName){
41  m_sensorMap=smap;
42 
43  /*
44  Construct the angle table for the sensor
45 
46  FIXME has to be extended to more than one laser...
47  */
48 
49  SensorMap::const_iterator laser_it=m_sensorMap.find(sensorName);
50  assert(laser_it!=m_sensorMap.end());
51  const RangeSensor* rangeSensor=dynamic_cast<const RangeSensor*>((laser_it->second));
52  assert(rangeSensor && rangeSensor->beams().size());
53 
54  m_beams=static_cast<unsigned int>(rangeSensor->beams().size());
55  double* angles=new double[rangeSensor->beams().size()];
56  for (unsigned int i=0; i<m_beams; i++){
57  angles[i]=rangeSensor->beams()[i].pose.theta;
58  }
59  m_matcher.setLaserParameters(m_beams, angles, rangeSensor->getPose());
60  delete [] angles;
61 
62 
63 }
64 
66  m_first=true;
67  m_pose=OrientedPoint(0,0,0);
68  m_count=0;
69 }
70 
73  OrientedPoint relPose=reading.getPose();
74  if (!m_count){
75  m_odoPose=relPose;
76  }
77 
78  //compute the move in the scan m_matcher
79  //reference frame
80 
81  OrientedPoint move=relPose-m_odoPose;
82 
83  double dth=m_odoPose.theta-m_pose.theta;
84  // cout << "rel-move x="<< move.x << " y=" << move.y << " theta=" << move.theta << endl;
85 
86  double lin_move=move*move;
87  if (lin_move>m_maxMove){
88  cerr << "Too big jump in the log file: " << lin_move << endl;
89  cerr << "relPose=" << relPose.x << " " <<relPose.y << endl;
90  cerr << "ignoring" << endl;
91  return;
92  //assert(0);
93  dth=0;
94  move.x=move.y=move.theta=0;
95  }
96 
97  double s=sin(dth), c=cos(dth);
98  OrientedPoint dPose;
99  dPose.x=c*move.x-s*move.y;
100  dPose.y=s*move.x+c*move.y;
101  dPose.theta=move.theta;
102 
103 #ifdef SCANMATHCERPROCESSOR_DEBUG
104  cout << "abs-move x="<< dPose.x << " y=" << dPose.y << " theta=" << dPose.theta << endl;
105 #endif
106  m_pose=m_pose+dPose;
107  m_pose.theta=atan2(sin(m_pose.theta), cos(m_pose.theta));
108 
109 #ifdef SCANMATHCERPROCESSOR_DEBUG
110  cout << "StartPose: x="
111  << m_pose.x << " y=" << m_pose.y << " theta=" << m_pose.theta << endl;
112 #endif
113 
114  m_odoPose=relPose; //update the past pose for the next iteration
115 
116 
117  //FIXME here I assume that everithing is referred to the center of the robot,
118  //while the offset of the laser has to be taken into account
119 
120  assert(reading.size()==m_beams);
121 /*
122  double * plainReading = new double[m_beams];
123 #ifdef SCANMATHCERPROCESSOR_DEBUG
124  cout << "PackedReadings ";
125 #endif
126  for(unsigned int i=0; i<m_beams; i++){
127  plainReading[i]=reading[i];
128 #ifdef SCANMATHCERPROCESSOR_DEBUG
129  cout << plainReading[i] << " ";
130 #endif
131  }
132 */
133  double * plainReading = new double[m_beams];
134  reading.rawView(plainReading, m_map.getDelta());
135 
136 
137 #ifdef SCANMATHCERPROCESSOR_DEBUG
138  cout << endl;
139 #endif
140  //the final stuff: scan match the pose
141  double score=0;
142  OrientedPoint newPose=m_pose;
143  if (m_count){
146  score=m_matcher.optimize(newPose, cov, m_map, m_pose, plainReading);
147  /*
148  gsl_matrix* m=gsl_matrix_alloc(3,3);
149  gsl_matrix_set(m,0,0,cov.xx); gsl_matrix_set(m,0,1,cov.xy); gsl_matrix_set(m,0,2,cov.xt);
150  gsl_matrix_set(m,1,0,cov.xy); gsl_matrix_set(m,1,1,cov.yy); gsl_matrix_set(m,1,2,cov.yt);
151  gsl_matrix_set(m,2,0,cov.xt); gsl_matrix_set(m,2,1,cov.yt); gsl_matrix_set(m,2,2,cov.tt);
152  gsl_matrix* evec=gsl_matrix_alloc(3,3);
153  gsl_vector* eval=gsl_vector_alloc(3);
154  */
155  double m[3][3];
156  double evec[3][3];
157  double eval[3];
158  m[0][0] = cov.xx;
159  m[0][1] = cov.xy;
160  m[0][2] = cov.xt;
161  m[1][0] = cov.xy;
162  m[1][1] = cov.yy;
163  m[1][2] = cov.yt;
164  m[2][0] = cov.xt;
165  m[2][1] = cov.yt;
166  m[2][2] = cov.tt;
167 
168  //gsl_eigen_symmv (m, eval, evec, m_eigenspace);
169  eigen_decomposition(m,evec,eval);
170 #ifdef SCANMATHCERPROCESSOR_DEBUG
171  //cout << "evals=" << gsl_vector_get(eval, 0) << " " << gsl_vector_get(eval, 1)<< " " << gsl_vector_get(eval, 2)<<endl;
172  cout << "evals=" << eval[0] << " " << eval[1]<< " " << eval[2]<<endl;
173 #endif
174  //gsl_matrix_free(m);
175  //gsl_matrix_free(evec);
176  //gsl_vector_free(eval);
177  } else {
178  if (useICP){
179  cerr << "USING ICP" << endl;
180  score=m_matcher.icpOptimize(newPose, m_map, m_pose, plainReading);
181  }else
182  score=m_matcher.optimize(newPose, m_map, m_pose, plainReading);
183  }
184 
185 
186  }
187  //...and register the scan
188  if (!m_count || score<m_regScore){
189 #ifdef SCANMATHCERPROCESSOR_DEBUG
190  cout << "Registering" << endl;
191 #endif
193  if (score<m_critScore){
194 #ifdef SCANMATHCERPROCESSOR_DEBUG
195  cout << "New Scan added, using odo pose" << endl;
196 #endif
197  m_matcher.registerScan(m_map, m_pose, plainReading);
198  } else {
199  m_matcher.registerScan(m_map, newPose, plainReading);
200 #ifdef SCANMATHCERPROCESSOR_DEBUG
201  cout << "New Scan added, using matched pose" << endl;
202 #endif
203  }
204  }
205 
206 #ifdef SCANMATHCERPROCESSOR_DEBUG
207  cout << " FinalPose: x="
208  << newPose.x << " y=" << newPose.y << " theta=" << newPose.theta << endl;
209  cout << "score=" << score << endl;
210 #endif
211  m_pose=newPose;
212  delete [] plainReading;
213  m_count++;
214 }
215 
217  (double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, bool computeCovariance){
218  m_matcher.setMatchingParameters(urange, range, sigma, kernsize, lopt, aopt, iterations);
219  m_computeCovariance=computeCovariance;
220 }
221 
222 void ScanMatcherProcessor::setRegistrationParameters(double regScore, double critScore){
223  m_regScore=regScore;
224  m_critScore=critScore;
225 }
226 
227 
229  return m_pose;
230 }
231 
232 };
233 
const char *const *argv double delta
Definition: gfs2stream.cpp:19
double registerScan(ScanMatcherMap &map, const OrientedPoint &p, const double *readings)
double getDelta() const
Definition: map.h:38
point< double > Point
Definition: point.h:202
void setSensorMap(const SensorMap &smap, std::string sensorName="FLASER")
void setRegistrationParameters(double regScore, double critScore)
unsigned int rawView(double *v, double density=0.) const
void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, bool computeCovariance=false)
void eigen_decomposition(double A[3][3], double V[3][3], double d[3])
void setLaserParameters(unsigned int beams, double *angles, const OrientedPoint &lpose)
double optimize(OrientedPoint &pnew, const ScanMatcherMap &map, const OrientedPoint &p, const double *readings) const
ScanMatcherProcessor(const ScanMatcherMap &m)
std::map< std::string, Sensor * > SensorMap
Definition: sensor.h:20
virtual void processScan(const RangeReading &reading)
void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma=1, unsigned int likelihoodSkip=0)
unsigned int c
Definition: gfs2stream.cpp:41
double icpOptimize(OrientedPoint &pnew, const ScanMatcherMap &map, const OrientedPoint &p, const double *readings) const
const OrientedPoint & getPose() const
Definition: rangereading.h:22
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