qparticleviewer.cpp
Go to the documentation of this file.
1 /*****************************************************************
2  *
3  * This file is part of the GMAPPING project
4  *
5  * GMAPPING Copyright (c) 2004 Giorgio Grisetti,
6  * Cyrill Stachniss, and Wolfram Burgard
7  *
8  * This software is licensed under the 3-Clause BSD License
9  * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss,
10  * and Wolfram Burgard.
11  *
12  * Further information on this license can be found at:
13  * https://opensource.org/licenses/BSD-3-Clause
14  *
15  * GMAPPING is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied
17  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
18  * PURPOSE.
19  *
20  *****************************************************************/
21 
22 
24 #include "moc_qparticleviewer.cpp"
25 #include <qimage.h>
26 
27 
28 using namespace GMapping;
29 
30 QParticleViewer::QParticleViewer( QWidget * parent, const char * name , WFlags f, GridSlamProcessorThread* thread): QWidget(parent, name, f|WRepaintNoErase|WResizeNoErase){
31  viewCenter=Point(0.,0.);
32  setMinimumSize(500,500);
33  mapscale=10.;
34  m_pixmap=new QPixmap(500,500);
35  m_pixmap->fill(Qt::white);
36  gfs_thread=thread;
37  tis=0;
39  m_refresh=false;
40  bestMap=0;
41  dragging=false;
42  showPaths=0;
43  showBestPath=1;
44  count=0;
45  writeToFile=0;
46 }
47 
49  if (m_pixmap)
50  delete m_pixmap;
51 }
52 
53 void QParticleViewer::paintEvent ( QPaintEvent *paintevent ){
54  if (! m_pixmap)
55  return;
56  bitBlt(this,0,0,m_pixmap,0,0,m_pixmap->width(),m_pixmap->height(),CopyROP);
57 }
58 
59 void QParticleViewer::mousePressEvent ( QMouseEvent *event ){
60  if (event->button()==LeftButton){
61  dragging=true;
62  draggingPos=event->pos();
63  }
64 }
65 void QParticleViewer::mouseMoveEvent ( QMouseEvent *event ){
66  if (dragging){
67  QPoint delta=event->pos()-draggingPos;
68  draggingPos=event->pos();
69  viewCenter.x-=delta.x()/mapscale;
70  viewCenter.y+=delta.y()/mapscale;
71  update();
72  }
73 }
74 
75 void QParticleViewer::mouseReleaseEvent ( QMouseEvent *event ){
76  if (event->button()==LeftButton){
77  dragging=false;
78  }
79 }
80 
81 void QParticleViewer::keyPressEvent ( QKeyEvent* e ){
82  switch (e->key()){
83  case Qt::Key_B: showBestPath=!showBestPath; break;
84  case Qt::Key_P: showPaths=!showPaths; break;
85  case Qt::Key_Plus: mapscale *=1.25; break;
86  case Qt::Key_Minus: mapscale/=1.25; break;
87  case Qt::Key_C: viewCenter=bestParticlePose; break;
88  default:;
89  }
90 }
91 
92 
93 void QParticleViewer::resizeEvent(QResizeEvent * sizeev){
94  if (!m_pixmap)
95  return;
96  cerr << "QParticleViewer::resizeEvent" << sizeev->size().width()<< " " << sizeev->size().height() << endl;
97  m_pixmap->resize(sizeev->size());
98 }
99 
101  assert(oldPose.size()==newPose.size());
102  QPainter painter(m_pixmap);
103  painter.setPen(Qt::red);
104  OrientedPointVector::const_iterator nit=newPose.begin();
105  for(OrientedPointVector::const_iterator it=oldPose.begin(); it!=oldPose.end(); it++, nit++){
106  IntPoint p0=map2pic(*it);
107  IntPoint p1=map2pic(*nit);
108  painter.drawLine(
109  (int)(p0.x), (int)(p0.y), (int)(p1.x), (int)(p1.y)
110  );
111  }
112 }
113 
115  if(! tis)
116  return;
117  if (tis->atEnd())
118  return;
119  QTextIStream& is=*tis;
120 
121  string line=is.readLine();
122  istringstream lineStream(line);
123  string recordType;
124  lineStream >> recordType;
125  if (recordType=="LASER_READING"){
126  //do nothing with the laser
127  cout << "l" << flush;
128  }
129  if (recordType=="ODO_UPDATE"){
130  //just move the particles
131  if (m_particleSize)
132  m_refresh=true;
134  m_newPose.clear();
135  unsigned int size;
136  lineStream >> size;
137  if (!m_particleSize)
138  m_particleSize=size;
139  assert(m_particleSize==size);
140  for (unsigned int i=0; i< size; i++){
141  OrientedPoint p;
142  double w;
143  lineStream >> p.x;
144  lineStream >> p.y;
145  lineStream >> p.theta;
146  lineStream >> w;
147  m_newPose.push_back(p);
148  }
149  cout << "o" << flush;
150  }
151  if (recordType=="SM_UPDATE"){
152  if (m_particleSize)
153  m_refresh=true;
155  m_newPose.clear();
156  unsigned int size;
157  lineStream >> size;
158  if (!m_particleSize)
159  m_particleSize=size;
160  assert(m_particleSize==size);
161  for (unsigned int i=0; i< size; i++){
162  OrientedPoint p;
163  double w;
164  lineStream >> p.x;
165  lineStream >> p.y;
166  lineStream >> p.theta;
167  lineStream >> w;
168  m_newPose.push_back(p);
169  }
170  cout << "u" << flush;
171  }
172  if (recordType=="RESAMPLE"){
173  unsigned int size;
174  lineStream >> size;
175  if (!m_particleSize)
176  m_particleSize=size;
177  assert(m_particleSize==size);
178  OrientedPointVector temp(size);
179  for (unsigned int i=0; i< size; i++){
180  unsigned int ind;
181  lineStream >> ind;
182  temp[i]=m_newPose[ind];
183  }
184  m_newPose=temp;
185  cout << "r" << flush;
186  }
187  if (m_refresh){
189  m_refresh=false;
190  }
191 }
192 
194  //cout << "Map received" << map.getMapSizeX() << " " << map.getMapSizeY() << endl;
195  QPainter painter(m_pixmap);
196  painter.setPen(Qt::black);
197  m_pixmap->fill(QColor(200,200,255));
198  unsigned int count=0;
199 
200  Point wmin=Point(pic2map(IntPoint(-m_pixmap->width()/2,m_pixmap->height()/2)));
201  Point wmax=Point(pic2map(IntPoint(m_pixmap->width()/2,-m_pixmap->height()/2)));
202  IntPoint imin=map.world2map(wmin);
203  IntPoint imax=map.world2map(wmax);
204  /* cout << __func__ << endl;
205  cout << " viewCenter=" << viewCenter.x << "," << viewCenter.y << endl;
206  cout << " wmin=" << wmin.x << "," << wmin.y << " wmax=" << wmax.x << "," << wmax.y << endl;
207  cout << " imin=" << imin.x << "," << imin.y << " imax=" << imax.x << "," << imax.y << endl;
208  cout << " mapSize=" << map.getMapSizeX() << "," << map.getMapSizeY() << endl;*/
209  for(int x=0; x<m_pixmap->width(); x++)
210  for(int y=0; y<m_pixmap->height(); y++){
211  //IntPoint ip=IntPoint(x,y)+imin;
212  //Point p=map.map2world(ip);
213  Point p=pic2map(IntPoint(x-m_pixmap->width()/2,
214  y-m_pixmap->height()/2));
215 
216  //if (map.storage().isInside(map.world2map(p))){
217  double v=map.cell(p);
218  if (v>=0){
219  int grayValue=255-(int)(255.*v);
220  painter.setPen(QColor(grayValue, grayValue, grayValue));
221  painter.drawPoint(x,y);
222  count++;
223  }
224  }
225 }
226 
227 
229  if (! gfs_thread)
230  return;
231  m_pixmap->fill(Qt::white);
233  for (GridSlamProcessorThread::EventDeque::const_iterator it=events.begin(); it!=events.end();it++){
235  if (mapEvent){
236  //cout << "Map: bestIdx=" << mapEvent->index <<endl;
237  if (bestMap)
238  delete bestMap;
239  else {
240 
241  }
242  bestMap=mapEvent->pmap;
243  mapEvent->pmap=0;
244  bestParticlePose=mapEvent->pose;
245  delete mapEvent;
246  }else{
248  if (doneEvent){
249  gfs_thread->stop();
250  delete doneEvent;
251  } else
252  history.push_back(*it);
253  }
254 
255  }
256  if (bestMap)
257  drawMap(*bestMap);
258 
259  unsigned int particleSize=0;
260  std::vector<OrientedPoint> oldPose, newPose;
261  vector<unsigned int> indexes;
262 
263  GridSlamProcessorThread::EventDeque::reverse_iterator it=history.rbegin();
264  while (!particleSize && it!=history.rend()){
267  if (move)
268  particleSize=move->hypotheses.size();
269  if (resample)
270  particleSize=resample->indexes.size();
271  it++;
272  }
273 
274  //check for the best index
275  double wmax=-1e2000;
276  unsigned int bestIdx=0;
277  bool emitted=false;
278  for (unsigned int i=0; i<particleSize; i++){
279  unsigned int currentIndex=i;
280  bool done=false;
281  for(GridSlamProcessorThread::EventDeque::reverse_iterator it=history.rbegin(); it!=history.rend()&& !done; it++){
283  if (move && move->scanmatched){
284  double cw=move->weightSums[currentIndex];
285  if (cw>wmax){
286  wmax=cw;
287  bestIdx=currentIndex;
288  }
289  done=true;
290  if (! emitted){
291  emit neffChanged(move->neff/particleSize);
292  emitted=true;
293  }
294  }
296  if (resample){
297  currentIndex=resample->indexes[currentIndex];
298  }
299  }
300  }
301  //cout << "bestIdx=" << bestIdx << endl;
302  QPainter painter(m_pixmap);
303 
304  for (unsigned int i=0; i<particleSize+1; i++){
305  painter.setPen(Qt::yellow);
306  unsigned int currentIndex=i;
307  if (i==particleSize && showBestPath){
308  currentIndex=bestIdx;
309  painter.setPen(Qt::red);
310  }
311  bool first=true;
312  OrientedPoint pnew(0,0,0);
313  for(GridSlamProcessorThread::EventDeque::reverse_iterator it=history.rbegin(); it!=history.rend(); it++){
315  if (move){
316  OrientedPoint pold=move->hypotheses[currentIndex];
317  IntPoint p0=map2pic(pold)+IntPoint(m_pixmap->width()/2,m_pixmap->height()/2);
318  IntPoint p1=map2pic(pnew)+IntPoint(m_pixmap->width()/2,m_pixmap->height()/2);;
319  if (first){
320  painter.drawPoint(p0.x, p0.y);
321  } else {
322  painter.drawLine(p0.x, p0.y, p1.x, p1.y);
323  }
324  first=false;
325  if (!(showPaths || showBestPath&&i==particleSize))
326  break;
327  pnew=pold;
328  }
330  if (resample && ! first){
331  currentIndex=resample->indexes[currentIndex];
332  }
333  }
334  }
335  if (writeToFile && bestMap){
336  if (! (count%writeToFile) ){
337  char name[100];
338  sprintf(name,"dump-%05d.png", count/writeToFile);
339  cout << " Writing " << name <<" ..." << flush;
340  QImage image=m_pixmap->convertToImage();
341  bool rv=image.save(name,"PNG");
342  if (rv)
343  cout << " Done";
344  else
345  cout << " ERROR";
346  cout << endl;
347  }
348  count++;
349  }
350 }
351 
352 void QParticleViewer::timerEvent(QTimerEvent * te) {
353  if (te->timerId()==timer) {
354  if ( tis)
355  drawFromFile();
356  else{
357  drawFromMemory();
358  update();
359  }
360  }
361 }
362 
363 
364 void QParticleViewer::start(int period){
365  timer=startTimer(period);
366 }
367 
369  //scanmatcher
370  matchingParameters.maxrange=gfs_thread->getlaserMaxRange();
371  matchingParameters.urange=gfs_thread->getusableRange();
372  matchingParameters.ssigma=gfs_thread->getgaussianSigma();
373  matchingParameters.sreg=gfs_thread->getregScore();
374  matchingParameters.scrit=gfs_thread->getcritScore();
375  matchingParameters.ksize=gfs_thread->getkernelSize();
376  matchingParameters.lstep=gfs_thread->getoptLinearDelta();
377  matchingParameters.astep=gfs_thread->getoptAngularDelta();
378  matchingParameters.iterations=gfs_thread->getoptRecursiveIterations();
379 
380  //start
381  startParameters.srr=gfs_thread->getsrr();
382  startParameters.stt=gfs_thread->getstt();
383  startParameters.str=gfs_thread->getstr();
384  startParameters.srt=gfs_thread->getsrt();
385 
386  startParameters.xmin=gfs_thread->getxmin();
387  startParameters.ymin=gfs_thread->getymin();
388  startParameters.xmax=gfs_thread->getxmax();
389  startParameters.ymax=gfs_thread->getymax();
390  startParameters.delta=gfs_thread->getdelta();
391 
393  startParameters.resampleThreshold=gfs_thread->getresampleThreshold();
395 }
396 
418  );
419  ((GridSlamProcessor*)(gfs_thread))->init(
427  gfs_thread->start();
428 }
429 
432 }
433 
435  startParameters=sp;
436 }
437 
439  gfs_thread->stop();
440 }
441 
442 void QParticleViewer::loadFile(const char * fn){
443  gfs_thread->loadFiles(fn);
444  /*
445  startParameters.initialPose=
446  gfs_thread->boundingBox(
447  startParameters.xmin,
448  startParameters.ymin,
449  startParameters.xmax,
450  startParameters.ymax);
451  */
452 }
const char *const *argv double delta
Definition: gfs2stream.cpp:19
Cell & cell(int x, int y)
Definition: map.h:46
void setMatchingParameters(const MatchingParameters &mp)
point< double > Point
Definition: point.h:202
void drawMap(const ScanMatcherMap &map)
int loadFiles(const char *fn=0)
Definition: gsp_thread.cpp:153
void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma=1, double likelihoodGain=1, unsigned int likelihoodSkip=0)
MatchingParameters matchingParameters
QParticleViewer::OrientedPointVector m_oldPose
point< int > IntPoint
Definition: point.h:201
virtual void resizeEvent(QResizeEvent *)
QParticleViewer::OrientedPointVector m_newPose
std::vector< OrientedPoint > OrientedPointVector
void loadFile(const char *)
const ParticleVector & getParticles() const
void resample(std::vector< int > &indexes, const WeightVector &weights, unsigned int nparticles=0)
QParticleViewer(QWidget *parent=0, const char *name=0, WFlags f=0, GridSlamProcessorThread *thread=0)
virtual void mouseReleaseEvent(QMouseEvent *)
void setUpdateDistances(double linear, double angular, double resampleThreshold)
StartParameters startParameters
IntPoint map2pic(const Point &p)
void setStartParameters(const StartParameters &mp)
deque< Event * > EventDeque
Definition: gsp_thread.h:74
virtual void paintEvent(QPaintEvent *paintevent)
void drawParticleMove(const OrientedPointVector &start, const OrientedPointVector &end)
virtual void mousePressEvent(QMouseEvent *)
virtual void mouseMoveEvent(QMouseEvent *)
std::vector< unsigned int > indexes
Definition: gsp_thread.h:61
GridSlamProcessorThread * gfs_thread
std::vector< OrientedPoint > hypotheses
Definition: gsp_thread.h:52
Point pic2map(const IntPoint &p)
ifstream is(argv[c])
GridSlamProcessorThread::EventDeque history
void setMotionModelParameters(double srr, double srt, double str, double stt)
IntPoint world2map(const Point &p) const
Definition: map.h:179
virtual void timerEvent(QTimerEvent *te)
virtual void keyPressEvent(QKeyEvent *e)


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