qparticleviewer.cpp
Go to the documentation of this file.
00001 /*****************************************************************
00002  *
00003  * This file is part of the GMAPPING project
00004  *
00005  * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 
00006  * Cyrill Stachniss, and Wolfram Burgard
00007  *
00008  * This software is licensed under the "Creative Commons 
00009  * License (Attribution-NonCommercial-ShareAlike 2.0)" 
00010  * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 
00011  * and Wolfram Burgard.
00012  * 
00013  * Further information on this license can be found at:
00014  * http://creativecommons.org/licenses/by-nc-sa/2.0/
00015  * 
00016  * GMAPPING is distributed in the hope that it will be useful,
00017  * but WITHOUT ANY WARRANTY; without even the implied 
00018  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
00019  * PURPOSE.  
00020  *
00021  *****************************************************************/
00022 
00023 
00024 #include "qparticleviewer.h"
00025 #include "moc_qparticleviewer.cpp"
00026 #include <qimage.h>
00027 
00028 
00029 using namespace GMapping;
00030 
00031 QParticleViewer::QParticleViewer( QWidget * parent, const char * name , WFlags f, GridSlamProcessorThread* thread): QWidget(parent, name, f|WRepaintNoErase|WResizeNoErase){
00032   viewCenter=Point(0.,0.);
00033   setMinimumSize(500,500);
00034   mapscale=10.;
00035   m_pixmap=new QPixmap(500,500);
00036   m_pixmap->fill(Qt::white);
00037   gfs_thread=thread;
00038   tis=0;
00039   m_particleSize=0;
00040   m_refresh=false;
00041   bestMap=0;
00042   dragging=false;
00043   showPaths=0;
00044   showBestPath=1;
00045   count=0;
00046   writeToFile=0;
00047 }
00048 
00049 QParticleViewer::~QParticleViewer(){
00050   if (m_pixmap)
00051     delete m_pixmap;
00052 }
00053 
00054 void QParticleViewer::paintEvent ( QPaintEvent *paintevent ){
00055   if (! m_pixmap)
00056     return;
00057   bitBlt(this,0,0,m_pixmap,0,0,m_pixmap->width(),m_pixmap->height(),CopyROP);
00058 }
00059 
00060 void QParticleViewer::mousePressEvent ( QMouseEvent *event ){
00061   if (event->button()==LeftButton){
00062     dragging=true;
00063     draggingPos=event->pos();   
00064   }
00065 }
00066 void QParticleViewer::mouseMoveEvent ( QMouseEvent *event ){
00067   if (dragging){
00068     QPoint delta=event->pos()-draggingPos;
00069     draggingPos=event->pos();
00070     viewCenter.x-=delta.x()/mapscale;
00071     viewCenter.y+=delta.y()/mapscale;
00072     update();
00073   }
00074 }
00075 
00076 void QParticleViewer::mouseReleaseEvent ( QMouseEvent *event ){
00077   if (event->button()==LeftButton){
00078     dragging=false;
00079   }
00080 }
00081 
00082 void QParticleViewer::keyPressEvent ( QKeyEvent* e ){
00083   switch (e->key()){
00084   case Qt::Key_B: showBestPath=!showBestPath; break;
00085   case Qt::Key_P: showPaths=!showPaths; break;
00086   case Qt::Key_Plus: mapscale *=1.25;  break;
00087   case Qt::Key_Minus: mapscale/=1.25;  break;
00088   case Qt::Key_C: viewCenter=bestParticlePose; break;
00089   default:;
00090   }
00091 }
00092 
00093                 
00094 void QParticleViewer::resizeEvent(QResizeEvent * sizeev){
00095   if (!m_pixmap)
00096     return;
00097   cerr << "QParticleViewer::resizeEvent" <<  sizeev->size().width()<< " " << sizeev->size().height() << endl;
00098   m_pixmap->resize(sizeev->size());
00099 }
00100 
00101 void QParticleViewer::drawParticleMove(const QParticleViewer::OrientedPointVector& oldPose, const QParticleViewer::OrientedPointVector& newPose){
00102   assert(oldPose.size()==newPose.size());
00103   QPainter painter(m_pixmap);
00104   painter.setPen(Qt::red);
00105   OrientedPointVector::const_iterator nit=newPose.begin();
00106   for(OrientedPointVector::const_iterator it=oldPose.begin(); it!=oldPose.end(); it++, nit++){
00107     IntPoint p0=map2pic(*it);
00108     IntPoint p1=map2pic(*nit);
00109     painter.drawLine( 
00110                      (int)(p0.x), (int)(p0.y), (int)(p1.x), (int)(p1.y)
00111                      );
00112   }
00113 }
00114 
00115 void QParticleViewer::drawFromFile(){
00116   if(! tis)
00117     return;
00118   if (tis->atEnd())
00119     return;     
00120   QTextIStream& is=*tis;
00121         
00122   string line=is.readLine();
00123   istringstream lineStream(line);
00124   string recordType;
00125   lineStream >> recordType;
00126   if (recordType=="LASER_READING"){
00127     //do nothing with the laser
00128     cout << "l" << flush;
00129   }
00130   if (recordType=="ODO_UPDATE"){
00131     //just move the particles
00132     if (m_particleSize)
00133       m_refresh=true;
00134     m_oldPose=m_newPose;
00135     m_newPose.clear();
00136     unsigned int size;
00137     lineStream >> size;
00138     if (!m_particleSize)
00139       m_particleSize=size;
00140     assert(m_particleSize==size);
00141     for (unsigned int i=0; i< size; i++){
00142       OrientedPoint p;
00143       double w;
00144       lineStream >> p.x;
00145       lineStream >> p.y;
00146       lineStream >> p.theta;
00147       lineStream >> w;
00148       m_newPose.push_back(p);
00149     }
00150     cout << "o" << flush;
00151   }
00152   if (recordType=="SM_UPDATE"){
00153     if (m_particleSize)
00154       m_refresh=true;
00155     m_oldPose=m_newPose;
00156     m_newPose.clear();
00157     unsigned int size;
00158     lineStream >> size;
00159     if (!m_particleSize)
00160       m_particleSize=size;
00161     assert(m_particleSize==size);
00162     for (unsigned int i=0; i< size; i++){
00163       OrientedPoint p;
00164       double w;
00165       lineStream >> p.x;
00166       lineStream >> p.y;
00167       lineStream >> p.theta;
00168       lineStream >> w;
00169       m_newPose.push_back(p);
00170     }
00171     cout << "u" << flush;
00172   }
00173   if (recordType=="RESAMPLE"){
00174     unsigned int size;
00175     lineStream >> size;
00176     if (!m_particleSize)
00177       m_particleSize=size;
00178     assert(m_particleSize==size);
00179     OrientedPointVector temp(size);
00180     for (unsigned int i=0; i< size; i++){
00181       unsigned int ind;
00182       lineStream >> ind;
00183       temp[i]=m_newPose[ind];
00184     }
00185     m_newPose=temp;
00186     cout << "r" << flush;
00187   }
00188   if (m_refresh){
00189     drawParticleMove(m_oldPose, m_newPose);
00190     m_refresh=false;
00191   }
00192 }
00193 
00194 void QParticleViewer::drawMap(const ScanMatcherMap& map){
00195   //cout << "Map received" << map.getMapSizeX() << " " << map.getMapSizeY() << endl;
00196   QPainter painter(m_pixmap);
00197   painter.setPen(Qt::black);
00198   m_pixmap->fill(QColor(200,200,255));
00199   unsigned int count=0;
00200         
00201   Point wmin=Point(pic2map(IntPoint(-m_pixmap->width()/2,m_pixmap->height()/2)));
00202   Point wmax=Point(pic2map(IntPoint(m_pixmap->width()/2,-m_pixmap->height()/2)));
00203   IntPoint imin=map.world2map(wmin);
00204   IntPoint imax=map.world2map(wmax);
00205   /*    cout << __PRETTY_FUNCTION__ << endl;
00206         cout << " viewCenter=" << viewCenter.x << "," << viewCenter.y <<   endl;        
00207         cout << " wmin=" << wmin.x << "," << wmin.y <<  " wmax=" << wmax.x << "," << wmax.y << endl;    
00208         cout << " imin=" << imin.x << "," << imin.y <<  " imax=" << imax.x << "," << imax.y << endl;
00209         cout << " mapSize=" << map.getMapSizeX() << "," << map.getMapSizeY() << endl;*/
00210   for(int x=0; x<m_pixmap->width(); x++)
00211     for(int y=0; y<m_pixmap->height(); y++){
00212       //IntPoint ip=IntPoint(x,y)+imin;
00213       //Point p=map.map2world(ip);
00214       Point p=pic2map(IntPoint(x-m_pixmap->width()/2,
00215                                y-m_pixmap->height()/2));
00216 
00217       //if (map.storage().isInside(map.world2map(p))){
00218       double v=map.cell(p);
00219       if (v>=0){
00220         int grayValue=255-(int)(255.*v);
00221         painter.setPen(QColor(grayValue, grayValue, grayValue));
00222         painter.drawPoint(x,y);
00223         count++;
00224       }
00225     }
00226 }
00227 
00228 
00229 void QParticleViewer::drawFromMemory(){
00230   if (! gfs_thread)
00231     return;
00232   m_pixmap->fill(Qt::white);
00233   GridSlamProcessorThread::EventDeque events=gfs_thread->getEvents();
00234   for (GridSlamProcessorThread::EventDeque::const_iterator it=events.begin(); it!=events.end();it++){
00235     GridSlamProcessorThread::MapEvent* mapEvent= dynamic_cast<GridSlamProcessorThread::MapEvent*>(*it);
00236     if (mapEvent){
00237       //cout << "Map: bestIdx=" << mapEvent->index <<endl;
00238       if (bestMap)
00239         delete bestMap;
00240       else {
00241                                 
00242       }
00243       bestMap=mapEvent->pmap;
00244       mapEvent->pmap=0;
00245       bestParticlePose=mapEvent->pose;
00246       delete mapEvent;
00247     }else{
00248       GridSlamProcessorThread::DoneEvent* doneEvent= dynamic_cast<GridSlamProcessorThread::DoneEvent*>(*it);
00249       if (doneEvent){
00250         gfs_thread->stop();
00251         delete doneEvent;
00252       } else
00253         history.push_back(*it);
00254     }   
00255                         
00256   }
00257   if (bestMap)
00258     drawMap(*bestMap);
00259         
00260   unsigned int particleSize=0;
00261   std::vector<OrientedPoint> oldPose, newPose;
00262   vector<unsigned int> indexes;
00263         
00264   GridSlamProcessorThread::EventDeque::reverse_iterator it=history.rbegin();
00265   while (!particleSize && it!=history.rend()){
00266     GridSlamProcessorThread::ParticleMoveEvent* move= dynamic_cast<GridSlamProcessorThread::ParticleMoveEvent*>(*it);
00267     GridSlamProcessorThread::ResampleEvent* resample= dynamic_cast<GridSlamProcessorThread::ResampleEvent*>(*it);
00268     if (move)
00269       particleSize=move->hypotheses.size();
00270     if (resample)
00271       particleSize=resample->indexes.size();
00272     it++;
00273   }
00274         
00275   //check for the best index
00276   double wmax=-1e2000;
00277   unsigned int bestIdx=0;
00278   bool emitted=false;
00279   for (unsigned int i=0; i<particleSize; i++){
00280     unsigned int currentIndex=i;
00281     bool done=false;
00282     for(GridSlamProcessorThread::EventDeque::reverse_iterator it=history.rbegin(); it!=history.rend()&& !done; it++){
00283       GridSlamProcessorThread::ParticleMoveEvent* move= dynamic_cast<GridSlamProcessorThread::ParticleMoveEvent*>(*it);
00284       if (move && move->scanmatched){
00285         double cw=move->weightSums[currentIndex];
00286         if (cw>wmax){
00287           wmax=cw;
00288           bestIdx=currentIndex;
00289         } 
00290         done=true;
00291         if (! emitted){
00292           emit neffChanged(move->neff/particleSize);
00293           emitted=true;
00294         }
00295       }
00296       GridSlamProcessorThread::ResampleEvent* resample= dynamic_cast<GridSlamProcessorThread::ResampleEvent*>(*it);
00297       if (resample){
00298         currentIndex=resample->indexes[currentIndex];
00299       }
00300     }
00301   }
00302   //cout << "bestIdx=" << bestIdx << endl;
00303   QPainter painter(m_pixmap);
00304         
00305   for (unsigned int i=0; i<particleSize+1; i++){
00306     painter.setPen(Qt::yellow);
00307     unsigned int currentIndex=i;
00308     if (i==particleSize && showBestPath){
00309       currentIndex=bestIdx;
00310       painter.setPen(Qt::red);
00311     }
00312     bool first=true;
00313     OrientedPoint pnew(0,0,0);
00314     for(GridSlamProcessorThread::EventDeque::reverse_iterator it=history.rbegin(); it!=history.rend(); it++){
00315       GridSlamProcessorThread::ParticleMoveEvent* move= dynamic_cast<GridSlamProcessorThread::ParticleMoveEvent*>(*it);
00316       if (move){
00317         OrientedPoint pold=move->hypotheses[currentIndex];
00318         IntPoint p0=map2pic(pold)+IntPoint(m_pixmap->width()/2,m_pixmap->height()/2);
00319         IntPoint p1=map2pic(pnew)+IntPoint(m_pixmap->width()/2,m_pixmap->height()/2);;
00320         if (first){
00321           painter.drawPoint(p0.x, p0.y);
00322         } else {
00323           painter.drawLine(p0.x, p0.y, p1.x, p1.y);
00324         }
00325         first=false;
00326         if (!(showPaths || showBestPath&&i==particleSize))
00327           break;
00328         pnew=pold;
00329       }
00330       GridSlamProcessorThread::ResampleEvent* resample= dynamic_cast<GridSlamProcessorThread::ResampleEvent*>(*it);
00331       if (resample && ! first){
00332         currentIndex=resample->indexes[currentIndex];
00333       }
00334     }
00335   }
00336   if (writeToFile && bestMap){
00337     if (! (count%writeToFile) ){
00338       char name[100];
00339       sprintf(name,"dump-%05d.png", count/writeToFile);
00340       cout << " Writing " << name <<" ..." << flush;
00341       QImage image=m_pixmap->convertToImage();
00342       bool rv=image.save(name,"PNG");
00343       if (rv)
00344         cout << " Done";
00345       else
00346         cout << " ERROR";
00347       cout << endl;
00348     }
00349     count++;
00350   }
00351 }
00352 
00353 void QParticleViewer::timerEvent(QTimerEvent * te) {
00354   if (te->timerId()==timer) {
00355     if ( tis)
00356       drawFromFile();
00357     else{
00358       drawFromMemory();
00359       update();
00360     }
00361   }
00362 }
00363 
00364 
00365 void QParticleViewer::start(int period){
00366   timer=startTimer(period);
00367 }
00368 
00369 void QParticleViewer::refreshParameters(){
00370   //scanmatcher
00371   matchingParameters.maxrange=gfs_thread->getlaserMaxRange();
00372   matchingParameters.urange=gfs_thread->getusableRange();
00373   matchingParameters.ssigma=gfs_thread->getgaussianSigma();
00374   matchingParameters.sreg=gfs_thread->getregScore();
00375   matchingParameters.scrit=gfs_thread->getcritScore();
00376   matchingParameters.ksize=gfs_thread->getkernelSize();
00377   matchingParameters.lstep=gfs_thread->getoptLinearDelta();
00378   matchingParameters.astep=gfs_thread->getoptAngularDelta();
00379   matchingParameters.iterations=gfs_thread->getoptRecursiveIterations();
00380 
00381   //start
00382   startParameters.srr=gfs_thread->getsrr();
00383   startParameters.stt=gfs_thread->getstt();
00384   startParameters.str=gfs_thread->getstr();
00385   startParameters.srt=gfs_thread->getsrt();
00386         
00387   startParameters.xmin=gfs_thread->getxmin();
00388   startParameters.ymin=gfs_thread->getymin();
00389   startParameters.xmax=gfs_thread->getxmax();
00390   startParameters.ymax=gfs_thread->getymax();
00391   startParameters.delta=gfs_thread->getdelta();
00392         
00393   startParameters.particles=gfs_thread->getParticles().size();
00394   startParameters.resampleThreshold=gfs_thread->getresampleThreshold();
00395   startParameters.outFileName=0;
00396 }
00397 
00398 void QParticleViewer::start(){
00399   gfs_thread->setMatchingParameters(
00400                                     matchingParameters.urange, 
00401                                     matchingParameters.maxrange, 
00402                                     matchingParameters.ssigma, 
00403                                     matchingParameters.ksize, 
00404                                     matchingParameters.lstep, 
00405                                     matchingParameters.astep, 
00406                                     matchingParameters.iterations, 
00407                                     startParameters.lsigma,
00408                                     startParameters.lgain,
00409                                     startParameters.lskip);
00410   gfs_thread->setMotionModelParameters(
00411                                        startParameters.srr,
00412                                        startParameters.srt,
00413                                        startParameters.srt,
00414                                        startParameters.stt);
00415   gfs_thread->setUpdateDistances(
00416                                  startParameters.linearUpdate,
00417                                  startParameters.angularUpdate,
00418                                  startParameters.resampleThreshold
00419                                  );
00420   ((GridSlamProcessor*)(gfs_thread))->init(
00421                                            startParameters.particles,
00422                                            startParameters.xmin, 
00423                                            startParameters.ymin, 
00424                                            startParameters.xmax, 
00425                                            startParameters.ymax, 
00426                                            startParameters.delta, 
00427                                            startParameters.initialPose);
00428   gfs_thread->start();
00429 }
00430 
00431 void QParticleViewer::setMatchingParameters(const QParticleViewer::MatchingParameters& mp){
00432   matchingParameters=mp;
00433 }
00434 
00435 void QParticleViewer::setStartParameters(const QParticleViewer::StartParameters& sp){
00436   startParameters=sp;
00437 }
00438 
00439 void QParticleViewer::stop(){
00440   gfs_thread->stop();
00441 }
00442 
00443 void QParticleViewer::loadFile(const char * fn){
00444   gfs_thread->loadFiles(fn);
00445   /*    
00446     startParameters.initialPose=
00447     gfs_thread->boundingBox(
00448     startParameters.xmin, 
00449     startParameters.ymin, 
00450     startParameters.xmax,
00451     startParameters.ymax);
00452   */    
00453 }


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:13