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


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Mon Jun 10 2019 14:04:22