GraphViewer.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include "rtabmap/gui/GraphViewer.h"
00029 
00030 #include <QGraphicsView>
00031 #include <QVBoxLayout>
00032 #include <QGraphicsScene>
00033 #include <QGraphicsEllipseItem>
00034 #include <QtGui/QWheelEvent>
00035 #include <QGraphicsSceneHoverEvent>
00036 #include <QMenu>
00037 #include <QtGui/QDesktopServices>
00038 #include <QtGui/QContextMenuEvent>
00039 #include <QColorDialog>
00040 #ifdef QT_SVG_LIB
00041 #include <QtSvg/QSvgGenerator>
00042 #endif
00043 #include <QInputDialog>
00044 #include <QMessageBox>
00045 
00046 #include <QtCore/QDir>
00047 #include <QtCore/QDateTime>
00048 #include <QtCore/QUrl>
00049 
00050 #include <rtabmap/core/util3d.h>
00051 #include <rtabmap/core/GeodeticCoords.h>
00052 #include <rtabmap/utilite/UCv2Qt.h>
00053 #include <rtabmap/utilite/UStl.h>
00054 #include <rtabmap/utilite/ULogger.h>
00055 #include <rtabmap/utilite/UTimer.h>
00056 
00057 namespace rtabmap {
00058 
00059 class NodeItem: public QGraphicsEllipseItem
00060 {
00061 public:
00062         // in meter
00063         NodeItem(int id, int mapId, const Transform & pose, float radius) :
00064                 QGraphicsEllipseItem(QRectF(-radius*100.0f,-radius*100.0f,radius*100.0f*2.0f,radius*100.0f*2.0f)),
00065                 _id(id),
00066                 _mapId(mapId),
00067                 _pose(pose),
00068                 _line(0)
00069         {
00070                 this->setPos(-pose.y()*100.0f,-pose.x()*100.0f);
00071                 this->setBrush(pen().color());
00072                 this->setAcceptHoverEvents(true);
00073                 float r,p,yaw;
00074                 pose.getEulerAngles(r, p, yaw);
00075                 radius*=100.0f;
00076                 _line = new QGraphicsLineItem(0,0,-radius*sin(yaw),-radius*cos(yaw), this);
00077         }
00078         virtual ~NodeItem() {}
00079 
00080         void setColor(const QColor & color)
00081         {
00082                 QPen p = this->pen();
00083                 p.setColor(color);
00084                 this->setPen(p);
00085                 QBrush b = this->brush();
00086                 b.setColor(color);
00087                 this->setBrush(b);
00088 
00089                 _line->setPen(QPen(QColor(255-color.red(), 255-color.green(), 255-color.blue())));
00090         }
00091 
00092         void setRadius(float radius)
00093         {
00094                 float r,p,yaw;
00095                 _pose.getEulerAngles(r, p, yaw);
00096                 radius*=100.0f;
00097                 this->setRect(-radius, -radius, radius*2.0f, radius*2.0f);
00098                 _line->setLine(0,0,-radius*sin(yaw),-radius*cos(yaw));
00099         }
00100 
00101         int id() const {return _id;};
00102         int mapId() const {return _mapId;}
00103         const Transform & pose() const {return _pose;}
00104         void setPose(const Transform & pose) {this->setPos(-pose.y()*100.0f,-pose.x()*100.0f); _pose=pose;}
00105 
00106 protected:
00107         virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
00108         {
00109                 this->setToolTip(QString("%1 [%2] %3").arg(_id).arg(_mapId).arg(_pose.prettyPrint().c_str()));
00110                 this->setScale(2);
00111                 QGraphicsEllipseItem::hoverEnterEvent(event);
00112         }
00113 
00114         virtual void hoverLeaveEvent ( QGraphicsSceneHoverEvent * event )
00115         {
00116                 this->setScale(1);
00117                 QGraphicsEllipseItem::hoverEnterEvent(event);
00118         }
00119 
00120 private:
00121         int _id;
00122         int _mapId;
00123         Transform _pose;
00124         QGraphicsLineItem * _line;
00125 };
00126 
00127 class NodeGPSItem: public NodeItem
00128 {
00129 public:
00130         NodeGPSItem(int id, int mapId, const Transform & pose, float radius, const GPS & gps) :
00131                 NodeItem(id, mapId, pose, radius),
00132                 _gps(gps)
00133         {
00134         }
00135         virtual ~NodeGPSItem() {}
00136 protected:
00137         virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
00138         {
00139                 this->setToolTip(QString("%1 [%2] %3\n"
00140                                 "longitude=%4 latitude=%5 altitude=%6m error=%7m bearing=%8deg")
00141                                 .arg(id()).arg(mapId()).arg(pose().prettyPrint().c_str())
00142                                 .arg(_gps.longitude()).arg(_gps.latitude()).arg(_gps.altitude()).arg(_gps.error()).arg(_gps.bearing()));
00143                 this->setScale(2);
00144                 QGraphicsEllipseItem::hoverEnterEvent(event);
00145         }
00146 private:
00147         GPS _gps;
00148 };
00149 
00150 class LinkItem: public QGraphicsLineItem
00151 {
00152 public:
00153         // in meter
00154         LinkItem(int from, int to, const Transform & poseA, const Transform & poseB, const Link & link, bool interSessionClosure) :
00155                 QGraphicsLineItem(-poseA.y()*100.0f, -poseA.x()*100.0f, -poseB.y()*100.0f, -poseB.x()*100.0f),
00156                 _from(from),
00157                 _to(to),
00158                 _poseA(poseA),
00159                 _poseB(poseB),
00160                 _link(link),
00161                 _interSession(interSessionClosure)
00162         {
00163                 this->setAcceptHoverEvents(true);
00164         }
00165         virtual ~LinkItem() {}
00166 
00167         void setColor(const QColor & color)
00168         {
00169                 QPen p = this->pen();
00170                 p.setColor(color);
00171                 this->setPen(p);
00172         }
00173 
00174         void setPoses(const Transform & poseA, const Transform & poseB)
00175         {
00176                 this->setLine(-poseA.y()*100.0f, -poseA.x()*100.0f, -poseB.y()*100.0f, -poseB.x()*100.0f);
00177                 _poseA = poseA;
00178                 _poseB = poseB;
00179         }
00180 
00181         const Transform & getPoseA() const
00182         {
00183                 return _poseA;
00184         }
00185         const Transform & getPoseB() const
00186         {
00187                 return _poseB;
00188         }
00189 
00190         Link::Type linkType() const {return _link.type();}
00191         bool isInterSession() const {return _interSession;}
00192         int from() const {return _from;}
00193         int to() const {return _to;}
00194 
00195 protected:
00196         virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
00197         {
00198                 QString str = QString("%1->%2 (%3 m)").arg(_from).arg(_to).arg(_poseA.getDistance(_poseB));
00199                 if(!_link.transform().isNull())
00200                 {
00201                         str.append(QString("\n%1\n%2 %3").arg(_link.transform().prettyPrint().c_str()).arg(_link.transVariance()).arg(_link.rotVariance()));
00202                 }
00203                 this->setToolTip(str);
00204                 QPen pen = this->pen();
00205                 pen.setWidthF(pen.widthF()+2);
00206                 this->setPen(pen);
00207                 QGraphicsLineItem::hoverEnterEvent(event);
00208         }
00209 
00210         virtual void hoverLeaveEvent ( QGraphicsSceneHoverEvent * event )
00211         {
00212                 QPen pen = this->pen();
00213                 pen.setWidthF(pen.widthF()-2);
00214                 this->setPen(pen);
00215                 QGraphicsLineItem::hoverEnterEvent(event);
00216         }
00217 
00218 private:
00219         int _from;
00220         int _to;
00221         Transform _poseA;
00222         Transform _poseB;
00223         Link _link;
00224         bool _interSession;
00225 };
00226 
00227 GraphViewer::GraphViewer(QWidget * parent) :
00228                 QGraphicsView(parent),
00229                 _nodeColor(Qt::blue),
00230                 _currentGoalColor(Qt::darkMagenta),
00231                 _neighborColor(Qt::blue),
00232                 _loopClosureColor(Qt::red),
00233                 _loopClosureLocalColor(Qt::yellow),
00234                 _loopClosureUserColor(Qt::red),
00235                 _loopClosureVirtualColor(Qt::magenta),
00236                 _neighborMergedColor(QColor(255,170,0)),
00237                 _loopClosureRejectedColor(Qt::black),
00238                 _localPathColor(Qt::cyan),
00239                 _globalPathColor(Qt::darkMagenta),
00240                 _gtPathColor(Qt::gray),
00241                 _gpsPathColor(Qt::darkCyan),
00242                 _loopIntraSessionColor(Qt::red),
00243                 _loopInterSessionColor(Qt::green),
00244                 _intraInterSessionColors(false),
00245                 _root(0),
00246                 _graphRoot(0),
00247                 _globalPathRoot(0),
00248                 _nodeVisible(true),
00249                 _nodeRadius(0.01f),
00250                 _linkWidth(0),
00251                 _gridMap(0),
00252                 _referential(0),
00253                 _originReferential(0),
00254                 _gridCellSize(0.0f),
00255                 _localRadius(0),
00256                 _loopClosureOutlierThr(0),
00257                 _maxLinkLength(0.02f),
00258                 _orientationENU(false)
00259 {
00260         this->setScene(new QGraphicsScene(this));
00261         this->setDragMode(QGraphicsView::ScrollHandDrag);
00262         _workingDirectory = QDir::homePath();
00263 
00264         this->scene()->clear();
00265         _root = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
00266 
00267         // add referential
00268         _originReferential = new QGraphicsItemGroup();
00269         this->scene()->addItem(_originReferential); // ownership transfered
00270         QGraphicsLineItem * item = this->scene()->addLine(0,0,0,-100, QPen(QBrush(Qt::red), _linkWidth));
00271         item->setZValue(100);
00272         item->setParentItem(_root);
00273         _originReferential->addToGroup(item);
00274         item = this->scene()->addLine(0,0,-100,0, QPen(QBrush(Qt::green), _linkWidth));
00275         item->setZValue(100);
00276         item->setParentItem(_root);
00277         _originReferential->addToGroup(item);
00278 
00279         // current pose
00280         _referential = new QGraphicsItemGroup();
00281         this->scene()->addItem(_referential); // ownership transfered
00282         item = this->scene()->addLine(0,0,0,-50, QPen(QBrush(Qt::red), _linkWidth));
00283         item->setZValue(100);
00284         item->setParentItem(_root);
00285         _referential->addToGroup(item);
00286         item = this->scene()->addLine(0,0,-50,0, QPen(QBrush(Qt::green), _linkWidth));
00287         item->setZValue(100);
00288         item->setParentItem(_root);
00289         _referential->addToGroup(item);
00290 
00291         _localRadius = this->scene()->addEllipse(-0.0001,-0.0001,0.0001,0.0001);
00292         _localRadius->setZValue(1);
00293         _localRadius->setParentItem(_root);
00294         _localRadius->setVisible(false);
00295         _localRadius->setPen(QPen(Qt::DashLine));
00296 
00297         _gridMap = this->scene()->addPixmap(QPixmap());
00298         _gridMap->setZValue(0);
00299         _gridMap->setParentItem(_root);
00300 
00301         _graphRoot = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
00302         _graphRoot->setZValue(4);
00303         _graphRoot->setParentItem(_root);
00304 
00305         _globalPathRoot = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
00306         _globalPathRoot->setZValue(8);
00307         _globalPathRoot->setParentItem(_root);
00308 
00309         _localPathRoot = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
00310         _localPathRoot->setZValue(9);
00311         _localPathRoot->setParentItem(_root);
00312 
00313         _gtGraphRoot = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
00314         _gtGraphRoot->setZValue(2);
00315         _gtGraphRoot->setParentItem(_root);
00316 
00317         _gpsGraphRoot = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
00318         _gpsGraphRoot->setZValue(3);
00319         _gpsGraphRoot->setParentItem(_root);
00320 
00321         this->restoreDefaults();
00322 
00323         this->fitInView(this->sceneRect(), Qt::KeepAspectRatio);
00324 }
00325 
00326 GraphViewer::~GraphViewer()
00327 {
00328 }
00329 
00330 void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
00331                                  const std::multimap<int, Link> & constraints,
00332                                  const std::map<int, int> & mapIds)
00333 {
00334         UTimer timer;
00335         bool wasVisible = _graphRoot->isVisible();
00336         _graphRoot->show();
00337 
00338         bool wasEmpty = _nodeItems.size() == 0 && _linkItems.size() == 0;
00339         UDEBUG("poses=%d constraints=%d", (int)poses.size(), (int)constraints.size());
00340         //Hide nodes and links
00341         for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
00342         {
00343                 iter.value()->hide();
00344                 iter.value()->setColor(_nodeColor); // reset color
00345         }
00346         for(QMultiMap<int, LinkItem*>::iterator iter = _linkItems.begin(); iter!=_linkItems.end(); ++iter)
00347         {
00348                 iter.value()->hide();
00349         }
00350 
00351         for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00352         {
00353                 if(!iter->second.isNull())
00354                 {
00355                         QMap<int, NodeItem*>::iterator itemIter = _nodeItems.find(iter->first);
00356                         if(itemIter != _nodeItems.end())
00357                         {
00358                                 itemIter.value()->setPose(iter->second);
00359                                 itemIter.value()->show();
00360                         }
00361                         else
00362                         {
00363                                 // create node item
00364                                 const Transform & pose = iter->second;
00365                                 NodeItem * item = new NodeItem(iter->first, uContains(mapIds, iter->first)?mapIds.at(iter->first):-1, pose, _nodeRadius);
00366                                 this->scene()->addItem(item);
00367                                 item->setZValue(20);
00368                                 item->setColor(_nodeColor);
00369                                 item->setParentItem(_graphRoot);
00370                                 item->setVisible(_nodeVisible);
00371                                 _nodeItems.insert(iter->first, item);
00372                         }
00373                 }
00374         }
00375 
00376         for(std::multimap<int, Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
00377         {
00378                 // make the first id the smallest one
00379                 int idFrom = iter->first<iter->second.to()?iter->first:iter->second.to();
00380                 int idTo = iter->first<iter->second.to()?iter->second.to():iter->first;
00381 
00382                 std::map<int, Transform>::const_iterator jterA = poses.find(idFrom);
00383                 std::map<int, Transform>::const_iterator jterB = poses.find(idTo);
00384                 LinkItem * linkItem = 0;
00385                 if(jterA != poses.end() && jterB != poses.end() &&
00386                    _nodeItems.contains(iter->first) && _nodeItems.contains(idTo))
00387                 {
00388                         const Transform & poseA = jterA->second;
00389                         const Transform & poseB = jterB->second;
00390 
00391                         QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.end();
00392                         if(_linkItems.contains(idFrom))
00393                         {
00394                                 itemIter = _linkItems.find(iter->first);
00395                                 while(itemIter.key() == idFrom && itemIter != _linkItems.end())
00396                                 {
00397                                         if(itemIter.value()->to() == idTo)
00398                                         {
00399                                                 itemIter.value()->setPoses(poseA, poseB);
00400                                                 itemIter.value()->show();
00401                                                 linkItem = itemIter.value();
00402                                                 break;
00403                                         }
00404                                         ++itemIter;
00405                                 }
00406                         }
00407 
00408                         bool interSessionClosure = false;
00409                         if(uContains(mapIds, jterA->first) && uContains(mapIds, jterB->first))
00410                         {
00411                                 interSessionClosure = mapIds.at(jterA->first) != mapIds.at(jterB->first);
00412                         }
00413 
00414                         if(poseA.getDistance(poseB) > _maxLinkLength)
00415                         {
00416                                 if(linkItem == 0)
00417                                 {
00418                                         //create a link item
00419                                         linkItem = new LinkItem(idFrom, idTo, poseA, poseB, iter->second, interSessionClosure);
00420                                         QPen p = linkItem->pen();
00421                                         p.setWidthF(_linkWidth*100.0f);
00422                                         linkItem->setPen(p);
00423                                         linkItem->setZValue(10);
00424                                         this->scene()->addItem(linkItem);
00425                                         linkItem->setParentItem(_graphRoot);
00426                                         _linkItems.insert(idFrom, linkItem);
00427                                 }
00428                         }
00429                         else if(linkItem && itemIter != _linkItems.end())
00430                         {
00431                                 // erase small links
00432                                 _linkItems.erase(itemIter);
00433                                 delete linkItem;
00434                                 linkItem = 0;
00435                         }
00436 
00437                         if(linkItem)
00438                         {
00439                                 //update color
00440                                 if(iter->second.type() == Link::kNeighbor)
00441                                 {
00442                                         linkItem->setColor(_neighborColor);
00443                                 }
00444                                 else if(iter->second.type() == Link::kVirtualClosure)
00445                                 {
00446                                         linkItem->setColor(_loopClosureVirtualColor);
00447                                 }
00448                                 else if(iter->second.type() == Link::kNeighborMerged)
00449                                 {
00450                                         linkItem->setColor(_neighborMergedColor);
00451                                 }
00452                                 else if(iter->second.type() == Link::kUserClosure)
00453                                 {
00454                                         linkItem->setColor(_loopClosureUserColor);
00455                                 }
00456                                 else if(iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure)
00457                                 {
00458                                         if(_intraInterSessionColors)
00459                                         {
00460                                                 linkItem->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor);
00461                                                 linkItem->setZValue(interSessionClosure?6:7);
00462                                         }
00463                                         else
00464                                         {
00465                                                 linkItem->setColor(_loopClosureLocalColor);
00466                                                 linkItem->setZValue(7);
00467                                         }
00468                                 }
00469                                 else
00470                                 {
00471                                         if(_intraInterSessionColors)
00472                                         {
00473                                                 linkItem->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor);
00474                                                 linkItem->setZValue(interSessionClosure?8:9);
00475                                         }
00476                                         else
00477                                         {
00478                                                 linkItem->setColor(_loopClosureColor);
00479                                                 linkItem->setZValue(9);
00480                                         }
00481                                 }
00482 
00483                                 //rejected loop closures
00484                                 if(_loopClosureOutlierThr > 0.0f)
00485                                 {
00486                                         Transform t = poseA.inverse()*poseB;
00487                                         if(iter->second.to() != idTo)
00488                                         {
00489                                                 t = t.inverse();
00490                                         }
00491                                         if(iter->second.type() != Link::kNeighbor &&
00492                                            iter->second.type() != Link::kNeighborMerged)
00493                                         {
00494                                                 float linearError = uMax3(
00495                                                                 fabs(iter->second.transform().x() - t.x()),
00496                                                                 fabs(iter->second.transform().y() - t.y()),
00497                                                                 fabs(iter->second.transform().z() - t.z()));
00498                                                 if(linearError > _loopClosureOutlierThr)
00499                                                 {
00500                                                         linkItem->setColor(_loopClosureRejectedColor);
00501                                                 }
00502                                         }
00503                                 }
00504                         }
00505                 }
00506         }
00507 
00508         //remove not used nodes and links
00509         for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end();)
00510         {
00511                 if(!iter.value()->isVisible())
00512                 {
00513                         delete iter.value();
00514                         iter = _nodeItems.erase(iter);
00515                 }
00516                 else
00517                 {
00518                         ++iter;
00519                 }
00520         }
00521         for(QMultiMap<int, LinkItem*>::iterator iter = _linkItems.begin(); iter!=_linkItems.end();)
00522         {
00523                 if(!iter.value()->isVisible())
00524                 {
00525                         delete iter.value();
00526                         iter = _linkItems.erase(iter);
00527                 }
00528                 else
00529                 {
00530                         ++iter;
00531                 }
00532         }
00533 
00534         if(_nodeItems.size())
00535         {
00536                 (--_nodeItems.end()).value()->setColor(Qt::green);
00537         }
00538 
00539         this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
00540 
00541         if(wasEmpty)
00542         {
00543                 QRectF rect = this->scene()->itemsBoundingRect();
00544                 this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio);
00545         }
00546 
00547         _graphRoot->setVisible(wasVisible);
00548 
00549         UDEBUG("_nodeItems=%d, _linkItems=%d, timer=%fs", _nodeItems.size(), _linkItems.size(), timer.ticks());
00550 }
00551 
00552 void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
00553 {
00554         UTimer timer;
00555         bool wasVisible = _gtGraphRoot->isVisible();
00556         _gtGraphRoot->show();
00557         bool wasEmpty = _gtNodeItems.size() == 0 && _gtLinkItems.size() == 0;
00558         UDEBUG("poses=%d", (int)poses.size());
00559         //Hide nodes and links
00560         for(QMap<int, NodeItem*>::iterator iter = _gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
00561         {
00562                 iter.value()->hide();
00563                 iter.value()->setColor(_gtPathColor); // reset color
00564         }
00565         for(QMultiMap<int, LinkItem*>::iterator iter = _gtLinkItems.begin(); iter!=_gtLinkItems.end(); ++iter)
00566         {
00567                 iter.value()->hide();
00568         }
00569 
00570         for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00571         {
00572                 if(!iter->second.isNull())
00573                 {
00574                         QMap<int, NodeItem*>::iterator itemIter = _gtNodeItems.find(iter->first);
00575                         if(itemIter != _gtNodeItems.end())
00576                         {
00577                                 itemIter.value()->setPose(iter->second);
00578                                 itemIter.value()->show();
00579                         }
00580                         else
00581                         {
00582                                 // create node item
00583                                 const Transform & pose = iter->second;
00584                                 NodeItem * item = new NodeItem(iter->first, -1, pose, _nodeRadius);
00585                                 this->scene()->addItem(item);
00586                                 item->setZValue(20);
00587                                 item->setColor(_gtPathColor);
00588                                 item->setParentItem(_gtGraphRoot);
00589                                 item->setVisible(_nodeVisible);
00590                                 _gtNodeItems.insert(iter->first, item);
00591                         }
00592 
00593                         if(iter!=poses.begin())
00594                         {
00595                                 std::map<int, Transform>::const_iterator iterPrevious = iter;
00596                                 --iterPrevious;
00597                                 Transform previousPose = iterPrevious->second;
00598                                 Transform currentPose = iter->second;
00599 
00600                                 LinkItem * linkItem = 0;
00601                                 QMultiMap<int, LinkItem*>::iterator linkIter = _gtLinkItems.end();
00602                                 if(_gtLinkItems.contains(iterPrevious->first))
00603                                 {
00604                                         linkIter = _gtLinkItems.find(iter->first);
00605                                         while(linkIter.key() == iterPrevious->first && linkIter != _gtLinkItems.end())
00606                                         {
00607                                                 if(linkIter.value()->to() == iter->first)
00608                                                 {
00609                                                         linkIter.value()->setPoses(previousPose, currentPose);
00610                                                         linkIter.value()->show();
00611                                                         linkItem = linkIter.value();
00612                                                         break;
00613                                                 }
00614                                                 ++linkIter;
00615                                         }
00616                                 }
00617                                 if(linkItem == 0)
00618                                 {
00619                                         //create a link item
00620                                         linkItem = new LinkItem(iterPrevious->first, iter->first, previousPose, currentPose, Link(), 1);
00621                                         QPen p = linkItem->pen();
00622                                         p.setWidthF(_linkWidth*100.0f);
00623                                         linkItem->setPen(p);
00624                                         linkItem->setZValue(10);
00625                                         this->scene()->addItem(linkItem);
00626                                         linkItem->setParentItem(_gtGraphRoot);
00627                                         _gtLinkItems.insert(iterPrevious->first, linkItem);
00628                                 }
00629                                 if(linkItem)
00630                                 {
00631                                         linkItem->setColor(_gtPathColor);
00632                                 }
00633                         }
00634                 }
00635         }
00636 
00637         //remove not used nodes and links
00638         for(QMap<int, NodeItem*>::iterator iter = _gtNodeItems.begin(); iter!=_gtNodeItems.end();)
00639         {
00640                 if(!iter.value()->isVisible())
00641                 {
00642                         delete iter.value();
00643                         iter = _gtNodeItems.erase(iter);
00644                 }
00645                 else
00646                 {
00647                         ++iter;
00648                 }
00649         }
00650         for(QMultiMap<int, LinkItem*>::iterator iter = _gtLinkItems.begin(); iter!=_gtLinkItems.end();)
00651         {
00652                 if(!iter.value()->isVisible())
00653                 {
00654                         delete iter.value();
00655                         iter = _gtLinkItems.erase(iter);
00656                 }
00657                 else
00658                 {
00659                         ++iter;
00660                 }
00661         }
00662 
00663         if(_gtNodeItems.size() || _gtLinkItems.size())
00664         {
00665                 this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
00666 
00667                 if(wasEmpty)
00668                 {
00669                         QRectF rect = this->scene()->itemsBoundingRect();
00670                         this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio);
00671                 }
00672         }
00673 
00674         _gtGraphRoot->setVisible(wasVisible);
00675 
00676         UDEBUG("_gtNodeItems=%d, _gtLinkItems=%d timer=%fs", _gtNodeItems.size(), _gtLinkItems.size(), timer.ticks());
00677 }
00678 
00679 void GraphViewer::updateGPSGraph(
00680                 const std::map<int, Transform> & poses,
00681                 const std::map<int, GPS> & gpsValues)
00682 {
00683         UTimer timer;
00684         bool wasVisible = _gpsGraphRoot->isVisible();
00685         _gpsGraphRoot->show();
00686         bool wasEmpty = _gpsNodeItems.size() == 0 && _gpsNodeItems.size() == 0;
00687         UDEBUG("poses=%d", (int)poses.size());
00688         //Hide nodes and links
00689         for(QMap<int, NodeItem*>::iterator iter = _gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter)
00690         {
00691                 iter.value()->hide();
00692                 iter.value()->setColor(_gpsPathColor); // reset color
00693         }
00694         for(QMultiMap<int, LinkItem*>::iterator iter = _gpsLinkItems.begin(); iter!=_gpsLinkItems.end(); ++iter)
00695         {
00696                 iter.value()->hide();
00697         }
00698 
00699         for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00700         {
00701                 if(!iter->second.isNull())
00702                 {
00703                         QMap<int, NodeItem*>::iterator itemIter = _gpsNodeItems.find(iter->first);
00704                         if(itemIter != _gpsNodeItems.end())
00705                         {
00706                                 itemIter.value()->setPose(iter->second);
00707                                 itemIter.value()->show();
00708                         }
00709                         else
00710                         {
00711                                 // create node item
00712                                 const Transform & pose = iter->second;
00713                                 UASSERT(gpsValues.find(iter->first) != gpsValues.end());
00714                                 NodeItem * item = new NodeGPSItem(iter->first, -1, pose, _nodeRadius, gpsValues.at(iter->first));
00715                                 this->scene()->addItem(item);
00716                                 item->setZValue(20);
00717                                 item->setColor(_gpsPathColor);
00718                                 item->setParentItem(_gpsGraphRoot);
00719                                 item->setVisible(_nodeVisible);
00720                                 _gpsNodeItems.insert(iter->first, item);
00721                         }
00722 
00723                         if(iter!=poses.begin())
00724                         {
00725                                 std::map<int, Transform>::const_iterator iterPrevious = iter;
00726                                 --iterPrevious;
00727                                 Transform previousPose = iterPrevious->second;
00728                                 Transform currentPose = iter->second;
00729 
00730                                 LinkItem * linkItem = 0;
00731                                 QMultiMap<int, LinkItem*>::iterator linkIter = _gpsLinkItems.end();
00732                                 if(_gpsLinkItems.contains(iterPrevious->first))
00733                                 {
00734                                         linkIter = _gpsLinkItems.find(iter->first);
00735                                         while(linkIter.key() == iterPrevious->first && linkIter != _gpsLinkItems.end())
00736                                         {
00737                                                 if(linkIter.value()->to() == iter->first)
00738                                                 {
00739                                                         linkIter.value()->setPoses(previousPose, currentPose);
00740                                                         linkIter.value()->show();
00741                                                         linkItem = linkIter.value();
00742                                                         break;
00743                                                 }
00744                                                 ++linkIter;
00745                                         }
00746                                 }
00747                                 if(linkItem == 0)
00748                                 {
00749                                         //create a link item
00750                                         linkItem = new LinkItem(iterPrevious->first, iter->first, previousPose, currentPose, Link(), 1);
00751                                         QPen p = linkItem->pen();
00752                                         p.setWidthF(_linkWidth*100.0f);
00753                                         linkItem->setPen(p);
00754                                         linkItem->setZValue(10);
00755                                         this->scene()->addItem(linkItem);
00756                                         linkItem->setParentItem(_gpsGraphRoot);
00757                                         _gpsLinkItems.insert(iterPrevious->first, linkItem);
00758                                 }
00759                                 if(linkItem)
00760                                 {
00761                                         linkItem->setColor(_gpsPathColor);
00762                                 }
00763                         }
00764                 }
00765         }
00766 
00767         //remove not used nodes and links
00768         for(QMap<int, NodeItem*>::iterator iter = _gpsNodeItems.begin(); iter!=_gpsNodeItems.end();)
00769         {
00770                 if(!iter.value()->isVisible())
00771                 {
00772                         delete iter.value();
00773                         iter = _gpsNodeItems.erase(iter);
00774                 }
00775                 else
00776                 {
00777                         ++iter;
00778                 }
00779         }
00780         for(QMultiMap<int, LinkItem*>::iterator iter = _gpsLinkItems.begin(); iter!=_gpsLinkItems.end();)
00781         {
00782                 if(!iter.value()->isVisible())
00783                 {
00784                         delete iter.value();
00785                         iter = _gpsLinkItems.erase(iter);
00786                 }
00787                 else
00788                 {
00789                         ++iter;
00790                 }
00791         }
00792 
00793         if(_gpsNodeItems.size() || _gpsLinkItems.size())
00794         {
00795                 this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
00796 
00797                 if(wasEmpty)
00798                 {
00799                         QRectF rect = this->scene()->itemsBoundingRect();
00800                         this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio);
00801                 }
00802         }
00803 
00804         _gpsGraphRoot->setVisible(wasVisible);
00805 
00806         UDEBUG("_gpsNodeItems=%d, _gpsLinkItems=%d timer=%fs", _gpsNodeItems.size(), _gpsLinkItems.size(), timer.ticks());
00807 }
00808 
00809 void GraphViewer::updateReferentialPosition(const Transform & t)
00810 {
00811         QTransform qt(t.r11(), t.r12(), t.r21(), t.r22(), -t.o24()*100.0f, -t.o14()*100.0f);
00812         _referential->setTransform(qt);
00813         _localRadius->setTransform(qt);
00814 
00815         this->ensureVisible(_referential);
00816         if(_localRadius->isVisible())
00817         {
00818                 this->ensureVisible(_localRadius, 0, 0);
00819         }
00820 }
00821 
00822 void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin)
00823 {
00824         UASSERT(map8U.empty() || (!map8U.empty() && resolution > 0.0f));
00825         if(!map8U.empty())
00826         {
00827                 _gridCellSize = resolution;
00828                 QImage image = uCvMat2QImage(map8U, false);
00829                 _gridMap->resetTransform();
00830                 _gridMap->setTransform(QTransform::fromScale(resolution*100.0f, -resolution*100.0f), true);
00831                 _gridMap->setRotation(90);
00832                 _gridMap->setPixmap(QPixmap::fromImage(image));
00833                 _gridMap->setPos(-yMin*100.0f, -xMin*100.0f);
00834                 this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
00835         }
00836         else
00837         {
00838                 this->clearMap();
00839         }
00840 }
00841 
00842 void GraphViewer::updatePosterior(const std::map<int, float> & posterior, float max)
00843 {
00844         //find max
00845         if(max <= 0.0f)
00846         {
00847                 for(std::map<int, float>::const_iterator iter = posterior.begin(); iter!=posterior.end(); ++iter)
00848                 {
00849                         if(iter->first > 0 && iter->second>max)
00850                         {
00851                                 max = iter->second;
00852                         }
00853                 }
00854         }
00855         if(max > 0.0f)
00856         {
00857                 for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
00858                 {
00859                         std::map<int,float>::const_iterator jter = posterior.find(iter.key());
00860                         if(jter != posterior.end())
00861                         {
00862                                 float v = jter->second>max?max:jter->second;
00863                                 iter.value()->setColor(QColor::fromHsvF((1-v/max)*240.0f/360.0f, 1, 1, 1)); //0=red 240=blue
00864                         }
00865                         else
00866                         {
00867                                 iter.value()->setColor(QColor::fromHsvF(240.0f/360.0f, 1, 1, 1)); // blue
00868                         }
00869                 }
00870         }
00871 }
00872 
00873 void GraphViewer::setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath)
00874 {
00875         UDEBUG("Set global path size=%d", (int)globalPath.size());
00876         qDeleteAll(_globalPathLinkItems);
00877         _globalPathLinkItems.clear();
00878 
00879         if(globalPath.size() >= 2)
00880         {
00881                 for(unsigned int i=0; i<globalPath.size()-1; ++i)
00882                 {
00883                         //create a link item
00884                         int idFrom = globalPath[i].first;
00885                         int idTo = globalPath[i+1].first;
00886                         LinkItem * item = new LinkItem(idFrom, idTo, globalPath[i].second, globalPath[i+1].second, Link(), false);
00887                         QPen p = item->pen();
00888                         p.setWidthF(_linkWidth*100.0f);
00889                         item->setPen(p);
00890                         item->setColor(_globalPathColor);
00891                         this->scene()->addItem(item);
00892                         item->setZValue(15);
00893                         item->setParentItem(_globalPathRoot);
00894                         _globalPathLinkItems.insert(idFrom, item);
00895                 }
00896         }
00897 }
00898 
00899 void GraphViewer::setCurrentGoalID(int id, const Transform & pose)
00900 {
00901         NodeItem * node = _nodeItems.value(id, 0);
00902         if(node)
00903         {
00904                 node->setColor(_currentGoalColor);
00905         }
00906         else
00907         {
00908                 UWARN("Current goal %d not found in the graph", id);
00909         }
00910 
00911         if(!pose.isNull() && _globalPathLinkItems.size() && _globalPathLinkItems.contains(id))
00912         {
00913                 // transform the global path in the goal referential
00914                 const LinkItem * oldPose = _globalPathLinkItems.value(id);
00915                 Transform t = pose * oldPose->getPoseA().inverse();
00916                 for(QMultiMap<int, LinkItem*>::iterator iter=_globalPathLinkItems.begin(); iter!=_globalPathLinkItems.end(); ++iter)
00917                 {
00918                         iter.value()->setPoses(t*iter.value()->getPoseA(), t*iter.value()->getPoseB());
00919                 }
00920         }
00921 }
00922 
00923 void GraphViewer::setLocalRadius(float radius)
00924 {
00925         _localRadius->setRect(-radius, -radius, radius*2, radius*2);
00926 }
00927 
00928 void GraphViewer::updateLocalPath(const std::vector<int> & localPath)
00929 {
00930         bool wasVisible = _localPathRoot->isVisible();
00931         _localPathRoot->show();
00932 
00933         for(QMultiMap<int, LinkItem*>::iterator iter = _localPathLinkItems.begin(); iter!=_localPathLinkItems.end(); ++iter)
00934         {
00935                 iter.value()->hide();
00936         }
00937 
00938         if(localPath.size() > 1)
00939         {
00940                 for(unsigned int i=0; i<localPath.size()-1; ++i)
00941                 {
00942                         int idFrom = localPath[i]<localPath[i+1]?localPath[i]:localPath[i+1];
00943                         int idTo = localPath[i]<localPath[i+1]?localPath[i+1]:localPath[i];
00944                         if(_nodeItems.contains(idFrom) && _nodeItems.contains(idTo))
00945                         {
00946                                 bool updated = false;
00947                                 if(_localPathLinkItems.contains(idFrom))
00948                                 {
00949                                         QMultiMap<int, LinkItem*>::iterator itemIter = _localPathLinkItems.find(idFrom);
00950                                         while(itemIter.key() == idFrom && itemIter != _localPathLinkItems.end())
00951                                         {
00952                                                 if(itemIter.value()->to() == idTo)
00953                                                 {
00954                                                         itemIter.value()->setPoses(_nodeItems.value(idFrom)->pose(), _nodeItems.value(idTo)->pose());
00955                                                         itemIter.value()->show();
00956                                                         updated = true;
00957                                                         break;
00958                                                 }
00959                                                 ++itemIter;
00960                                         }
00961                                 }
00962                                 if(!updated)
00963                                 {
00964                                         //create a link item
00965                                         LinkItem * item = new LinkItem(idFrom, idTo, _nodeItems.value(idFrom)->pose(), _nodeItems.value(idTo)->pose(), Link(), false);
00966                                         QPen p = item->pen();
00967                                         p.setWidthF(_linkWidth*100.0f);
00968                                         item->setPen(p);
00969                                         item->setColor(_localPathColor);
00970                                         this->scene()->addItem(item);
00971                                         item->setZValue(16); // just over the global path
00972                                         item->setParentItem(_localPathRoot);
00973                                         _localPathLinkItems.insert(idFrom, item);
00974                                 }
00975                         }
00976                 }
00977         }
00978 
00979         // remove not used links
00980         for(QMultiMap<int, LinkItem*>::iterator iter = _localPathLinkItems.begin(); iter!=_localPathLinkItems.end();)
00981         {
00982                 if(!iter.value()->isVisible())
00983                 {
00984                         delete iter.value();
00985                         iter = _localPathLinkItems.erase(iter);
00986                 }
00987                 else
00988                 {
00989                         ++iter;
00990                 }
00991         }
00992         _localPathRoot->setVisible(wasVisible);
00993 }
00994 
00995 void GraphViewer::clearGraph()
00996 {
00997         qDeleteAll(_nodeItems);
00998         _nodeItems.clear();
00999         qDeleteAll(_linkItems);
01000         _linkItems.clear();
01001         qDeleteAll(_localPathLinkItems);
01002         _localPathLinkItems.clear();
01003         qDeleteAll(_globalPathLinkItems);
01004         _globalPathLinkItems.clear();
01005         qDeleteAll(_gtNodeItems);
01006         _gtNodeItems.clear();
01007         qDeleteAll(_gtLinkItems);
01008         _gtLinkItems.clear();
01009         qDeleteAll(_gpsNodeItems);
01010         _gpsNodeItems.clear();
01011         qDeleteAll(_gpsLinkItems);
01012         _gpsLinkItems.clear();
01013 
01014         _referential->resetTransform();
01015         _localRadius->resetTransform();
01016         this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
01017 }
01018 
01019 void GraphViewer::clearMap()
01020 {
01021         _gridMap->setPixmap(QPixmap());
01022         _gridCellSize = 0.0f;
01023         this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
01024 }
01025 
01026 void GraphViewer::clearPosterior()
01027 {
01028         for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
01029         {
01030                 iter.value()->setColor(Qt::blue); // blue
01031         }
01032 }
01033 
01034 void GraphViewer::clearAll()
01035 {
01036         clearMap();
01037         clearGraph();
01038 }
01039 
01040 void GraphViewer::saveSettings(QSettings & settings, const QString & group) const
01041 {
01042         if(!group.isEmpty())
01043         {
01044                 settings.beginGroup(group);
01045         }
01046         settings.setValue("node_radius", (double)this->getNodeRadius());
01047         settings.setValue("link_width", (double)this->getLinkWidth());
01048         settings.setValue("node_color", this->getNodeColor());
01049         settings.setValue("current_goal_color", this->getCurrentGoalColor());
01050         settings.setValue("neighbor_color", this->getNeighborColor());
01051         settings.setValue("global_color", this->getGlobalLoopClosureColor());
01052         settings.setValue("local_color", this->getLocalLoopClosureColor());
01053         settings.setValue("user_color", this->getUserLoopClosureColor());
01054         settings.setValue("virtual_color", this->getVirtualLoopClosureColor());
01055         settings.setValue("neighbor_merged_color", this->getNeighborMergedColor());
01056         settings.setValue("rejected_color", this->getRejectedLoopClosureColor());
01057         settings.setValue("local_path_color", this->getLocalPathColor());
01058         settings.setValue("global_path_color", this->getGlobalPathColor());
01059         settings.setValue("gt_color", this->getGTColor());
01060         settings.setValue("gps_color", this->getGPSColor());
01061         settings.setValue("intra_session_color", this->getIntraSessionLoopColor());
01062         settings.setValue("inter_session_color", this->getInterSessionLoopColor());
01063         settings.setValue("intra_inter_session_colors_enabled", this->isIntraInterSessionColorsEnabled());
01064         settings.setValue("grid_visible", this->isGridMapVisible());
01065         settings.setValue("origin_visible", this->isOriginVisible());
01066         settings.setValue("referential_visible", this->isReferentialVisible());
01067         settings.setValue("local_radius_visible", this->isLocalRadiusVisible());
01068         settings.setValue("loop_closure_outlier_thr", this->getLoopClosureOutlierThr());
01069         settings.setValue("max_link_length", this->getMaxLinkLength());
01070         settings.setValue("graph_visible", this->isGraphVisible());
01071         settings.setValue("global_path_visible", this->isGlobalPathVisible());
01072         settings.setValue("local_path_visible", this->isLocalPathVisible());
01073         settings.setValue("gt_graph_visible", this->isGtGraphVisible());
01074         settings.setValue("gps_graph_visible", this->isGPSGraphVisible());
01075         settings.setValue("orientation_ENU", this->isOrientationENU());
01076         if(!group.isEmpty())
01077         {
01078                 settings.endGroup();
01079         }
01080 }
01081 
01082 void GraphViewer::loadSettings(QSettings & settings, const QString & group)
01083 {
01084         if(!group.isEmpty())
01085         {
01086                 settings.beginGroup(group);
01087         }
01088         this->setNodeRadius(settings.value("node_radius", this->getNodeRadius()).toDouble());
01089         this->setLinkWidth(settings.value("link_width", this->getLinkWidth()).toDouble());
01090         this->setNodeColor(settings.value("node_color", this->getNodeColor()).value<QColor>());
01091         this->setCurrentGoalColor(settings.value("current_goal_color", this->getCurrentGoalColor()).value<QColor>());
01092         this->setNeighborColor(settings.value("neighbor_color", this->getNeighborColor()).value<QColor>());
01093         this->setGlobalLoopClosureColor(settings.value("global_color", this->getGlobalLoopClosureColor()).value<QColor>());
01094         this->setLocalLoopClosureColor(settings.value("local_color", this->getLocalLoopClosureColor()).value<QColor>());
01095         this->setUserLoopClosureColor(settings.value("user_color", this->getUserLoopClosureColor()).value<QColor>());
01096         this->setVirtualLoopClosureColor(settings.value("virtual_color", this->getVirtualLoopClosureColor()).value<QColor>());
01097         this->setNeighborMergedColor(settings.value("neighbor_merged_color", this->getNeighborMergedColor()).value<QColor>());
01098         this->setRejectedLoopClosureColor(settings.value("rejected_color", this->getRejectedLoopClosureColor()).value<QColor>());
01099         this->setLocalPathColor(settings.value("local_path_color", this->getLocalPathColor()).value<QColor>());
01100         this->setGlobalPathColor(settings.value("global_path_color", this->getGlobalPathColor()).value<QColor>());
01101         this->setGTColor(settings.value("gt_color", this->getGTColor()).value<QColor>());
01102         this->setGPSColor(settings.value("gps_color", this->getGPSColor()).value<QColor>());
01103         this->setIntraSessionLoopColor(settings.value("intra_session_color", this->getIntraSessionLoopColor()).value<QColor>());
01104         this->setInterSessionLoopColor(settings.value("inter_session_color", this->getInterSessionLoopColor()).value<QColor>());
01105         this->setGridMapVisible(settings.value("grid_visible", this->isGridMapVisible()).toBool());
01106         this->setOriginVisible(settings.value("origin_visible", this->isOriginVisible()).toBool());
01107         this->setReferentialVisible(settings.value("referential_visible", this->isReferentialVisible()).toBool());
01108         this->setLocalRadiusVisible(settings.value("local_radius_visible", this->isLocalRadiusVisible()).toBool());
01109         this->setIntraInterSessionColorsEnabled(settings.value("intra_inter_session_colors_enabled", this->isIntraInterSessionColorsEnabled()).toBool());
01110         this->setLoopClosureOutlierThr(settings.value("loop_closure_outlier_thr", this->getLoopClosureOutlierThr()).toDouble());
01111         this->setMaxLinkLength(settings.value("max_link_length", this->getMaxLinkLength()).toDouble());
01112         this->setGraphVisible(settings.value("graph_visible", this->isGraphVisible()).toBool());
01113         this->setGlobalPathVisible(settings.value("global_path_visible", this->isGlobalPathVisible()).toBool());
01114         this->setLocalPathVisible(settings.value("local_path_visible", this->isLocalPathVisible()).toBool());
01115         this->setGtGraphVisible(settings.value("gt_graph_visible", this->isGtGraphVisible()).toBool());
01116         this->setGPSGraphVisible(settings.value("gps_graph_visible", this->isGPSGraphVisible()).toBool());
01117         this->setOrientationENU(settings.value("orientation_ENU", this->isOrientationENU()).toBool());
01118         if(!group.isEmpty())
01119         {
01120                 settings.endGroup();
01121         }
01122 }
01123 
01124 bool GraphViewer::isGridMapVisible() const
01125 {
01126         return _gridMap->isVisible();
01127 }
01128 bool GraphViewer::isOriginVisible() const
01129 {
01130         return _originReferential->isVisible();
01131 }
01132 bool GraphViewer::isReferentialVisible() const
01133 {
01134         return _referential->isVisible();
01135 }
01136 bool GraphViewer::isLocalRadiusVisible() const
01137 {
01138         return _localRadius->isVisible();
01139 }
01140 bool GraphViewer::isGraphVisible() const
01141 {
01142         return _graphRoot->isVisible();
01143 }
01144 bool GraphViewer::isGlobalPathVisible() const
01145 {
01146         return _globalPathRoot->isVisible();
01147 }
01148 bool GraphViewer::isLocalPathVisible() const
01149 {
01150         return _localPathRoot->isVisible();
01151 }
01152 bool GraphViewer::isGtGraphVisible() const
01153 {
01154         return _gtGraphRoot->isVisible();
01155 }
01156 bool GraphViewer::isGPSGraphVisible() const
01157 {
01158         return _gpsGraphRoot->isVisible();
01159 }
01160 bool GraphViewer::isOrientationENU() const
01161 {
01162         return _orientationENU;
01163 }
01164 
01165 void GraphViewer::setWorkingDirectory(const QString & path)
01166 {
01167         _workingDirectory = path;
01168 }
01169 void GraphViewer::setNodeVisible(bool visible)
01170 {
01171         _nodeVisible = visible;
01172         for(QMap<int, NodeItem*>::iterator iter=_nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
01173         {
01174                 iter.value()->setVisible(_nodeVisible);
01175         }
01176         for(QMap<int, NodeItem*>::iterator iter=_gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
01177         {
01178                 iter.value()->setVisible(_nodeVisible);
01179         }
01180         for(QMap<int, NodeItem*>::iterator iter=_gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter)
01181         {
01182                 iter.value()->setVisible(_nodeVisible);
01183         }
01184 }
01185 void GraphViewer::setNodeRadius(float radius)
01186 {
01187         _nodeRadius = radius;
01188         for(QMap<int, NodeItem*>::iterator iter=_nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
01189         {
01190                 iter.value()->setRadius(_nodeRadius);
01191         }
01192         for(QMap<int, NodeItem*>::iterator iter=_gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
01193         {
01194                 iter.value()->setRadius(_nodeRadius);
01195         }
01196         for(QMap<int, NodeItem*>::iterator iter=_gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter)
01197         {
01198                 iter.value()->setRadius(_nodeRadius);
01199         }
01200 }
01201 void GraphViewer::setLinkWidth(float width)
01202 {
01203         _linkWidth = width;
01204         QList<QGraphicsItem*> items = this->scene()->items();
01205         for(int i=0; i<items.size(); ++i)
01206         {
01207                 QGraphicsLineItem * line = qgraphicsitem_cast<QGraphicsLineItem *>(items[i]);
01208                 if(line)
01209                 {
01210                         QPen pen = line->pen();
01211                         pen.setWidthF(_linkWidth*100.0f);
01212                         line->setPen(pen);
01213                 }
01214         }
01215 }
01216 void GraphViewer::setNodeColor(const QColor & color)
01217 {
01218         _nodeColor = color;
01219         for(QMap<int, NodeItem*>::iterator iter=_nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
01220         {
01221                 iter.value()->setColor(_nodeColor);
01222         }
01223 }
01224 void GraphViewer::setCurrentGoalColor(const QColor & color)
01225 {
01226         _currentGoalColor = color;
01227 }
01228 void GraphViewer::setNeighborColor(const QColor & color)
01229 {
01230         _neighborColor = color;
01231         for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
01232         {
01233                 if(iter.value()->linkType() == Link::kNeighbor)
01234                 {
01235                         iter.value()->setColor(_neighborColor);
01236                 }
01237         }
01238 }
01239 void GraphViewer::setGlobalLoopClosureColor(const QColor & color)
01240 {
01241         _loopClosureColor = color;
01242         if(!_intraInterSessionColors)
01243         {
01244                 for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
01245                 {
01246                         if(iter.value()->linkType() == Link::kGlobalClosure)
01247                         {
01248                                 iter.value()->setColor(_loopClosureColor);
01249                                 iter.value()->setZValue(10);
01250                         }
01251                 }
01252         }
01253 }
01254 void GraphViewer::setLocalLoopClosureColor(const QColor & color)
01255 {
01256         _loopClosureLocalColor = color;
01257         if(!_intraInterSessionColors)
01258         {
01259                 for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
01260                 {
01261                         if(iter.value()->linkType() == Link::kLocalSpaceClosure ||
01262                            iter.value()->linkType() == Link::kLocalTimeClosure)
01263                         {
01264                                 iter.value()->setColor(_loopClosureLocalColor);
01265                                 iter.value()->setZValue(10);
01266                         }
01267                 }
01268         }
01269 }
01270 void GraphViewer::setUserLoopClosureColor(const QColor & color)
01271 {
01272         _loopClosureUserColor = color;
01273         for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
01274         {
01275                 if(iter.value()->linkType() == Link::kUserClosure)
01276                 {
01277                         iter.value()->setColor(_loopClosureUserColor);
01278                 }
01279         }
01280 }
01281 void GraphViewer::setVirtualLoopClosureColor(const QColor & color)
01282 {
01283         _loopClosureVirtualColor = color;
01284         for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
01285         {
01286                 if(iter.value()->linkType() == Link::kVirtualClosure)
01287                 {
01288                         iter.value()->setColor(_loopClosureVirtualColor);
01289                 }
01290         }
01291 }
01292 void GraphViewer::setNeighborMergedColor(const QColor & color)
01293 {
01294         _neighborMergedColor = color;
01295         for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
01296         {
01297                 if(iter.value()->linkType() == Link::kNeighborMerged)
01298                 {
01299                         iter.value()->setColor(_neighborMergedColor);
01300                 }
01301         }
01302 }
01303 void GraphViewer::setRejectedLoopClosureColor(const QColor & color)
01304 {
01305         _loopClosureRejectedColor = color;
01306 }
01307 void GraphViewer::setLocalPathColor(const QColor & color)
01308 {
01309         _localPathColor = color;
01310 }
01311 void GraphViewer::setGlobalPathColor(const QColor & color)
01312 {
01313         _globalPathColor = color;
01314 }
01315 void GraphViewer::setGTColor(const QColor & color)
01316 {
01317         _gtPathColor = color;
01318         for(QMap<int, NodeItem*>::iterator iter=_gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
01319         {
01320                 iter.value()->setColor(_gtPathColor);
01321         }
01322         for(QMultiMap<int, LinkItem*>::iterator iter=_gtLinkItems.begin(); iter!=_gtLinkItems.end(); ++iter)
01323         {
01324                 iter.value()->setColor(_gtPathColor);
01325         }
01326 }
01327 void GraphViewer::setGPSColor(const QColor & color)
01328 {
01329         _gpsPathColor = color;
01330         for(QMap<int, NodeItem*>::iterator iter=_gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter)
01331         {
01332                 iter.value()->setColor(_gpsPathColor);
01333         }
01334         for(QMultiMap<int, LinkItem*>::iterator iter=_gpsLinkItems.begin(); iter!=_gpsLinkItems.end(); ++iter)
01335         {
01336                 iter.value()->setColor(_gpsPathColor);
01337         }
01338 }
01339 void GraphViewer::setIntraSessionLoopColor(const QColor & color)
01340 {
01341         _loopIntraSessionColor = color;
01342         if(_intraInterSessionColors)
01343         {
01344                 for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
01345                 {
01346                         if((iter.value()->linkType() == Link::kGlobalClosure ||
01347                                 iter.value()->linkType() == Link::kLocalSpaceClosure ||
01348                                 iter.value()->linkType() == Link::kLocalTimeClosure) &&
01349                                 !iter.value()->isInterSession())
01350                         {
01351                                 iter.value()->setColor(_loopIntraSessionColor);
01352                                 iter.value()->setZValue(9);
01353                         }
01354                 }
01355         }
01356 }
01357 void GraphViewer::setInterSessionLoopColor(const QColor & color)
01358 {
01359         _loopInterSessionColor = color;
01360         if(_intraInterSessionColors)
01361         {
01362                 for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
01363                 {
01364                         if((iter.value()->linkType() == Link::kGlobalClosure ||
01365                                 iter.value()->linkType() == Link::kLocalSpaceClosure ||
01366                                 iter.value()->linkType() == Link::kLocalTimeClosure) &&
01367                                 iter.value()->isInterSession())
01368                         {
01369                                 iter.value()->setColor(_loopInterSessionColor);
01370                                 iter.value()->setZValue(8);
01371                         }
01372                 }
01373         }
01374 }
01375 
01376 void GraphViewer::setIntraInterSessionColorsEnabled(bool enabled)
01377 {
01378         _intraInterSessionColors = enabled;
01379         if(_intraInterSessionColors)
01380         {
01381                 this->setIntraSessionLoopColor(_loopIntraSessionColor);
01382                 this->setInterSessionLoopColor(_loopInterSessionColor);
01383         }
01384         else
01385         {
01386                 this->setGlobalLoopClosureColor(_loopClosureColor);
01387                 this->setLocalLoopClosureColor(_loopClosureLocalColor);
01388         }
01389 }
01390 
01391 void GraphViewer::setGridMapVisible(bool visible)
01392 {
01393         _gridMap->setVisible(visible);
01394 }
01395 void GraphViewer::setOriginVisible(bool visible)
01396 {
01397         _originReferential->setVisible(visible);
01398 }
01399 void GraphViewer::setReferentialVisible(bool visible)
01400 {
01401         _referential->setVisible(visible);
01402 }
01403 void GraphViewer::setLocalRadiusVisible(bool visible)
01404 {
01405         _localRadius->setVisible(visible);
01406 }
01407 void GraphViewer::setLoopClosureOutlierThr(float value)
01408 {
01409         _loopClosureOutlierThr = value;
01410 }
01411 void GraphViewer::setMaxLinkLength(float value)
01412 {
01413         _maxLinkLength = value;
01414 }
01415 void GraphViewer::setGraphVisible(bool visible)
01416 {
01417         _graphRoot->setVisible(visible);
01418 }
01419 void GraphViewer::setGlobalPathVisible(bool visible)
01420 {
01421         _globalPathRoot->setVisible(visible);
01422 }
01423 void GraphViewer::setLocalPathVisible(bool visible)
01424 {
01425         _localPathRoot->setVisible(visible);
01426 }
01427 void GraphViewer::setGtGraphVisible(bool visible)
01428 {
01429         _gtGraphRoot->setVisible(visible);
01430 }
01431 void GraphViewer::setGPSGraphVisible(bool visible)
01432 {
01433         _gpsGraphRoot->setVisible(visible);
01434 }
01435 void GraphViewer::setOrientationENU(bool enabled)
01436 {
01437         if(_orientationENU!=enabled)
01438         {
01439                 _orientationENU = enabled;
01440                 this->rotate(_orientationENU?90:270);
01441         }
01442 }
01443 
01444 void GraphViewer::restoreDefaults()
01445 {
01446         setNodeRadius(0.01f);
01447         setLinkWidth(0.0f);
01448         setNodeColor(Qt::blue);
01449         setNeighborColor(Qt::blue);
01450         setGlobalLoopClosureColor(Qt::red);
01451         setLocalLoopClosureColor(Qt::yellow);
01452         setUserLoopClosureColor(Qt::red);
01453         setVirtualLoopClosureColor(Qt::magenta);
01454         setNeighborMergedColor(QColor(255,170,0));
01455         setGridMapVisible(true);
01456         setGraphVisible(true);
01457         setGlobalPathVisible(true);
01458         setLocalPathVisible(true);
01459         setGtGraphVisible(true);
01460 }
01461 
01462 void GraphViewer::wheelEvent ( QWheelEvent * event )
01463 {
01464         if(event->delta() < 0)
01465         {
01466                 this->scale(0.95, 0.95);
01467         }
01468         else
01469         {
01470                 this->scale(1.05, 1.05);
01471         }
01472 }
01473 
01474 QIcon createIcon(const QColor & color)
01475 {
01476         QPixmap pixmap(50, 50);
01477         pixmap.fill(color);
01478         return QIcon(pixmap);
01479 }
01480 
01481 void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
01482 {
01483         QMenu menu;
01484         QAction * aScreenShotPNG = menu.addAction(tr("Take a screenshot (PNG)"));
01485         QAction * aScreenShotSVG = menu.addAction(tr("Take a screenshot (SVG)"));
01486 #ifndef QT_SVG_LIB
01487         aScreenShotSVG->setEnabled(false);
01488 #endif
01489         menu.addSeparator();
01490 
01491         QAction * aChangeNodeColor = menu.addAction(createIcon(_nodeColor), tr("Set node color..."));
01492         QAction * aChangeCurrentGoalColor = menu.addAction(createIcon(_currentGoalColor), tr("Set current goal color..."));
01493         aChangeNodeColor->setIconVisibleInMenu(true);
01494         aChangeCurrentGoalColor->setIconVisibleInMenu(true);
01495 
01496         // Links
01497         QMenu * menuLink = menu.addMenu(tr("Set link color..."));
01498         QAction * aChangeNeighborColor = menuLink->addAction(tr("Neighbor"));
01499         QAction * aChangeGlobalLoopColor = menuLink->addAction(tr("Global loop closure"));
01500         QAction * aChangeLocalLoopColor = menuLink->addAction(tr("Local loop closure"));
01501         QAction * aChangeUserLoopColor = menuLink->addAction(tr("User loop closure"));
01502         QAction * aChangeVirtualLoopColor = menuLink->addAction(tr("Virtual loop closure"));
01503         QAction * aChangeNeighborMergedColor = menuLink->addAction(tr("Neighbor merged"));
01504         QAction * aChangeRejectedLoopColor = menuLink->addAction(tr("Outlier loop closure"));
01505         QAction * aChangeRejectedLoopThr = menuLink->addAction(tr("Set outlier threshold..."));
01506         QAction * aChangeLocalPathColor = menuLink->addAction(tr("Local path"));
01507         QAction * aChangeGlobalPathColor = menuLink->addAction(tr("Global path"));
01508         QAction * aChangeGTColor = menuLink->addAction(tr("Ground truth"));
01509         QAction * aChangeGPSColor = menuLink->addAction(tr("GPS"));
01510         menuLink->addSeparator();
01511         QAction * aSetIntraInterSessionColors = menuLink->addAction(tr("Enable intra/inter-session colors"));
01512         QAction * aChangeIntraSessionLoopColor = menuLink->addAction(tr("Intra-session loop closure"));
01513         QAction * aChangeInterSessionLoopColor = menuLink->addAction(tr("Inter-session loop closure"));
01514         aChangeNeighborColor->setIcon(createIcon(_neighborColor));
01515         aChangeGlobalLoopColor->setIcon(createIcon(_loopClosureColor));
01516         aChangeLocalLoopColor->setIcon(createIcon(_loopClosureLocalColor));
01517         aChangeUserLoopColor->setIcon(createIcon(_loopClosureUserColor));
01518         aChangeVirtualLoopColor->setIcon(createIcon(_loopClosureVirtualColor));
01519         aChangeNeighborMergedColor->setIcon(createIcon(_neighborMergedColor));
01520         aChangeRejectedLoopColor->setIcon(createIcon(_loopClosureRejectedColor));
01521         aChangeLocalPathColor->setIcon(createIcon(_localPathColor));
01522         aChangeGlobalPathColor->setIcon(createIcon(_globalPathColor));
01523         aChangeGTColor->setIcon(createIcon(_gtPathColor));
01524         aChangeGPSColor->setIcon(createIcon(_gpsPathColor));
01525         aChangeIntraSessionLoopColor->setIcon(createIcon(_loopIntraSessionColor));
01526         aChangeInterSessionLoopColor->setIcon(createIcon(_loopInterSessionColor));
01527         aChangeNeighborColor->setIconVisibleInMenu(true);
01528         aChangeGlobalLoopColor->setIconVisibleInMenu(true);
01529         aChangeLocalLoopColor->setIconVisibleInMenu(true);
01530         aChangeUserLoopColor->setIconVisibleInMenu(true);
01531         aChangeVirtualLoopColor->setIconVisibleInMenu(true);
01532         aChangeNeighborMergedColor->setIconVisibleInMenu(true);
01533         aChangeRejectedLoopColor->setIconVisibleInMenu(true);
01534         aChangeLocalPathColor->setIconVisibleInMenu(true);
01535         aChangeGlobalPathColor->setIconVisibleInMenu(true);
01536         aChangeGTColor->setIconVisibleInMenu(true);
01537         aChangeGPSColor->setIconVisibleInMenu(true);
01538         aChangeIntraSessionLoopColor->setIconVisibleInMenu(true);
01539         aChangeInterSessionLoopColor->setIconVisibleInMenu(true);
01540         aSetIntraInterSessionColors->setCheckable(true);
01541         aSetIntraInterSessionColors->setChecked(_intraInterSessionColors);
01542 
01543         menu.addSeparator();
01544         QAction * aSetNodeSize = menu.addAction(tr("Set node radius..."));
01545         QAction * aSetLinkSize = menu.addAction(tr("Set link width..."));
01546         QAction * aChangeMaxLinkLength = menu.addAction(tr("Set maximum link length..."));
01547         menu.addSeparator();
01548         QAction * aShowHideGridMap;
01549         QAction * aShowHideGraph;
01550         QAction * aShowHideGraphNodes;
01551         QAction * aShowHideOrigin;
01552         QAction * aShowHideReferential;
01553         QAction * aShowHideLocalRadius;
01554         QAction * aShowHideGlobalPath;
01555         QAction * aShowHideLocalPath;
01556         QAction * aShowHideGtGraph;
01557         QAction * aShowHideGPSGraph;
01558         QAction * aOrientationENU;
01559         if(_gridMap->isVisible())
01560         {
01561                 aShowHideGridMap = menu.addAction(tr("Hide grid map"));
01562         }
01563         else
01564         {
01565                 aShowHideGridMap = menu.addAction(tr("Show grid map"));
01566         }
01567         if(_originReferential->isVisible())
01568         {
01569                 aShowHideOrigin = menu.addAction(tr("Hide origin referential"));
01570         }
01571         else
01572         {
01573                 aShowHideOrigin = menu.addAction(tr("Show origin referential"));
01574         }
01575         if(_referential->isVisible())
01576         {
01577                 aShowHideReferential = menu.addAction(tr("Hide current referential"));
01578         }
01579         else
01580         {
01581                 aShowHideReferential = menu.addAction(tr("Show current referential"));
01582         }
01583         if(_localRadius->isVisible())
01584         {
01585                 aShowHideLocalRadius = menu.addAction(tr("Hide local radius"));
01586         }
01587         else
01588         {
01589                 aShowHideLocalRadius = menu.addAction(tr("Show local radius"));
01590         }
01591         if(_graphRoot->isVisible())
01592         {
01593                 aShowHideGraph = menu.addAction(tr("Hide graph"));
01594         }
01595         else
01596         {
01597                 aShowHideGraph = menu.addAction(tr("Show graph"));
01598         }
01599         if(_nodeVisible)
01600         {
01601                 aShowHideGraphNodes = menu.addAction(tr("Hide graph nodes"));
01602         }
01603         else
01604         {
01605                 aShowHideGraphNodes = menu.addAction(tr("Show graph nodes"));
01606         }
01607         if(_globalPathRoot->isVisible())
01608         {
01609                 aShowHideGlobalPath = menu.addAction(tr("Hide global path"));
01610         }
01611         else
01612         {
01613                 aShowHideGlobalPath = menu.addAction(tr("Show global path"));
01614         }
01615         if(_localPathRoot->isVisible())
01616         {
01617                 aShowHideLocalPath = menu.addAction(tr("Hide local path"));
01618         }
01619         else
01620         {
01621                 aShowHideLocalPath = menu.addAction(tr("Show local path"));
01622         }
01623         if(_gtGraphRoot->isVisible())
01624         {
01625                 aShowHideGtGraph = menu.addAction(tr("Hide ground truth graph"));
01626         }
01627         else
01628         {
01629                 aShowHideGtGraph = menu.addAction(tr("Show ground truth graph"));
01630         }
01631         if(_gpsGraphRoot->isVisible())
01632         {
01633                 aShowHideGPSGraph = menu.addAction(tr("Hide GPS graph"));
01634         }
01635         else
01636         {
01637                 aShowHideGPSGraph = menu.addAction(tr("Show GPS graph"));
01638         }
01639         aOrientationENU = menu.addAction(tr("ENU Orientation"));
01640         aOrientationENU->setCheckable(true);
01641         aOrientationENU->setChecked(_orientationENU);
01642         aShowHideGraph->setEnabled(_nodeItems.size());
01643         aShowHideGraphNodes->setEnabled(_nodeItems.size() && _graphRoot->isVisible());
01644         aShowHideGlobalPath->setEnabled(_globalPathLinkItems.size());
01645         aShowHideLocalPath->setEnabled(_localPathLinkItems.size());
01646         aShowHideGtGraph->setEnabled(_gtNodeItems.size());
01647         aShowHideGPSGraph->setEnabled(_gpsNodeItems.size());
01648         menu.addSeparator();
01649         QAction * aRestoreDefaults = menu.addAction(tr("Restore defaults"));
01650 
01651         QAction * r = menu.exec(event->globalPos());
01652         if(r == aScreenShotPNG || r == aScreenShotSVG)
01653         {
01654                 if(_root)
01655                 {
01656                         QString targetDir = _workingDirectory + "/ScreensCaptured";
01657                         QDir dir;
01658                         if(!dir.exists(targetDir))
01659                         {
01660                                 dir.mkdir(targetDir);
01661                         }
01662                         targetDir += "/";
01663                         targetDir += "Graph_view";
01664                         if(!dir.exists(targetDir))
01665                         {
01666                                 dir.mkdir(targetDir);
01667                         }
01668                         targetDir += "/";
01669                         bool isPNG = r == aScreenShotPNG;
01670                         QString name = (QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + (isPNG?".png":".svg"));
01671 
01672                         if(_gridCellSize)
01673                         {
01674                                 _root->setScale(1.0f/(_gridCellSize*100.0f)); // grid map precision (for 5cm grid cell, x20 to have 1pix/5cm)
01675                         }
01676                         else
01677                         {
01678                                 _root->setScale(this->transform().m11()); // current view
01679                         }
01680 
01681                         this->scene()->clearSelection();                                  // Selections would also render to the file
01682                         this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
01683                         QSize sceneSize = this->scene()->sceneRect().size().toSize();
01684 
01685                         if(isPNG)
01686                         {
01687                                 QImage image(sceneSize, QImage::Format_ARGB32);  // Create the image with the exact size of the shrunk scene
01688                                 image.fill(Qt::transparent);                     // Start all pixels transparent
01689                                 QPainter painter(&image);
01690 
01691                                 this->scene()->render(&painter);
01692                                 if(!image.isNull())
01693                                 {
01694                                         image.save(targetDir + name);
01695                                 }
01696                                 else
01697                                 {
01698                                         QMessageBox::warning(this,
01699                                                         tr("Save PNG"),
01700                                                         tr("Could not export in PNG (the scene may be too large %1x%2), try saving in SVG.").arg(sceneSize.width()).arg(sceneSize.height()));
01701                                 }
01702                         }
01703                         else
01704                         {
01705 #ifdef QT_SVG_LIB
01706                                 QSvgGenerator svgGen;
01707 
01708                                 svgGen.setFileName( targetDir + name );
01709                                 svgGen.setSize(sceneSize);
01710                                 // add 1% border to make sure values are not cropped
01711                                 int borderH = sceneSize.width()/100;
01712                                 int borderV = sceneSize.height()/100;
01713                                 svgGen.setViewBox(QRect(-borderH, -borderV, sceneSize.width()+borderH*2, sceneSize.height()+borderV*2));
01714                                 svgGen.setTitle(tr("RTAB-Map graph"));
01715                                 svgGen.setDescription(tr("RTAB-Map map and graph"));
01716 
01717                                 QPainter painter( &svgGen );
01718 
01719                                 this->scene()->render(&painter);
01720 #else
01721                                 UERROR("RTAB-MAp is not built with Qt's SVG library, cannot save picture in svg format.");
01722 #endif
01723                         }
01724 
01725                         //reset scale
01726                         _root->setScale(1.0f);
01727                         this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
01728 
01729 
01730                         QDesktopServices::openUrl(QUrl::fromLocalFile(targetDir + name));
01731                 }
01732                 return; // without emitting configChanged
01733         }
01734         else if(r == aSetIntraInterSessionColors)
01735         {
01736                 setIntraInterSessionColorsEnabled(aSetIntraInterSessionColors->isChecked());
01737         }
01738         else if(r == aChangeRejectedLoopThr)
01739         {
01740                 bool ok;
01741                 double value = QInputDialog::getDouble(this, tr("Loop closure outlier threshold"), tr("Value (m)"), _loopClosureOutlierThr, 0.0, 1000.0, 2, &ok);
01742                 if(ok)
01743                 {
01744                         setLoopClosureOutlierThr(value);
01745                 }
01746         }
01747         else if(r == aChangeMaxLinkLength)
01748         {
01749                 bool ok;
01750                 double value = QInputDialog::getDouble(this, tr("Maximum link length to be shown"), tr("Value (m)"), _maxLinkLength, 0.0, 1000.0, 3, &ok);
01751                 if(ok)
01752                 {
01753                         setMaxLinkLength(value);
01754                 }
01755         }
01756         else if(r == aChangeNodeColor ||
01757                         r == aChangeCurrentGoalColor ||
01758                         r == aChangeNeighborColor ||
01759                         r == aChangeGlobalLoopColor ||
01760                         r == aChangeLocalLoopColor ||
01761                         r == aChangeUserLoopColor ||
01762                         r == aChangeVirtualLoopColor ||
01763                         r == aChangeNeighborMergedColor ||
01764                         r == aChangeRejectedLoopColor ||
01765                         r == aChangeLocalPathColor ||
01766                         r == aChangeGlobalPathColor ||
01767                         r == aChangeGTColor ||
01768                         r == aChangeGPSColor ||
01769                         r == aChangeIntraSessionLoopColor ||
01770                         r == aChangeInterSessionLoopColor)
01771         {
01772                 QColor color;
01773                 if(r == aChangeNodeColor)
01774                 {
01775                         color = _nodeColor;
01776                 }
01777                 else if(r == aChangeCurrentGoalColor)
01778                 {
01779                         color = _currentGoalColor;
01780                 }
01781                 else if(r == aChangeGlobalLoopColor)
01782                 {
01783                         color = _loopClosureColor;
01784                 }
01785                 else if(r == aChangeLocalLoopColor)
01786                 {
01787                         color = _loopClosureLocalColor;
01788                 }
01789                 else if(r == aChangeUserLoopColor)
01790                 {
01791                         color = _loopClosureUserColor;
01792                 }
01793                 else if(r == aChangeVirtualLoopColor)
01794                 {
01795                         color = _loopClosureVirtualColor;
01796                 }
01797                 else if(r == aChangeNeighborMergedColor)
01798                 {
01799                         color = _neighborMergedColor;
01800                 }
01801                 else if(r == aChangeRejectedLoopColor)
01802                 {
01803                         color = _loopClosureRejectedColor;
01804                 }
01805                 else if(r == aChangeLocalPathColor)
01806                 {
01807                         color = _localPathColor;
01808                 }
01809                 else if(r == aChangeGlobalPathColor)
01810                 {
01811                         color = _globalPathColor;
01812                 }
01813                 else if(r == aChangeGTColor)
01814                 {
01815                         color = _gtPathColor;
01816                 }
01817                 else if(r == aChangeGPSColor)
01818                 {
01819                         color = _gpsPathColor;
01820                 }
01821                 else if(r == aChangeIntraSessionLoopColor)
01822                 {
01823                         color = _loopIntraSessionColor;
01824                 }
01825                 else if(r == aChangeInterSessionLoopColor)
01826                 {
01827                         color = _loopInterSessionColor;
01828                 }
01829                 else //if(r == aChangeNeighborColor)
01830                 {
01831                         color = _neighborColor;
01832                 }
01833                 color = QColorDialog::getColor(color, this);
01834                 if(color.isValid())
01835                 {
01836 
01837                         if(r == aChangeNodeColor)
01838                         {
01839                                 this->setNodeColor(color);
01840                         }
01841                         else if(r == aChangeCurrentGoalColor)
01842                         {
01843                                 this->setCurrentGoalColor(color);
01844                         }
01845                         else if(r == aChangeGlobalLoopColor)
01846                         {
01847                                 this->setGlobalLoopClosureColor(color);
01848                         }
01849                         else if(r == aChangeLocalLoopColor)
01850                         {
01851                                 this->setLocalLoopClosureColor(color);
01852                         }
01853                         else if(r == aChangeUserLoopColor)
01854                         {
01855                                 this->setUserLoopClosureColor(color);
01856                         }
01857                         else if(r == aChangeVirtualLoopColor)
01858                         {
01859                                 this->setVirtualLoopClosureColor(color);
01860                         }
01861                         else if(r == aChangeNeighborMergedColor)
01862                         {
01863                                 this->setNeighborMergedColor(color);
01864                         }
01865                         else if(r == aChangeRejectedLoopColor)
01866                         {
01867                                 this->setRejectedLoopClosureColor(color);
01868                         }
01869                         else if(r == aChangeLocalPathColor)
01870                         {
01871                                 this->setLocalPathColor(color);
01872                         }
01873                         else if(r == aChangeGlobalPathColor)
01874                         {
01875                                 this->setGlobalPathColor(color);
01876                         }
01877                         else if(r == aChangeGTColor)
01878                         {
01879                                 this->setGTColor(color);
01880                         }
01881                         else if(r == aChangeGPSColor)
01882                         {
01883                                 this->setGPSColor(color);
01884                         }
01885                         else if(r == aChangeIntraSessionLoopColor)
01886                         {
01887                                 this->setIntraSessionLoopColor(color);
01888                         }
01889                         else if(r == aChangeInterSessionLoopColor)
01890                         {
01891                                 this->setInterSessionLoopColor(color);
01892                         }
01893                         else //if(r == aChangeNeighborColor)
01894                         {
01895                                 this->setNeighborColor(color);
01896                         }
01897                 }
01898                 else
01899                 {
01900                         return; // without emitting configChanged
01901                 }
01902         }
01903         else if(r == aSetNodeSize)
01904         {
01905                 bool ok;
01906                 double value = QInputDialog::getDouble(this, tr("Node radius"), tr("Radius (m)"), _nodeRadius, 0.001, 100, 3, &ok);
01907                 if(ok)
01908                 {
01909                         setNodeRadius(value);
01910                 }
01911         }
01912         else if(r == aSetLinkSize)
01913         {
01914                 bool ok;
01915                 double value = QInputDialog::getDouble(this, tr("Link width"), tr("Width (m)"), _linkWidth, 0, 100, 2, &ok);
01916                 if(ok)
01917                 {
01918                         setLinkWidth(value);
01919                 }
01920         }
01921         else if(r == aShowHideGridMap)
01922         {
01923                 this->setGridMapVisible(!this->isGridMapVisible());
01924                 if(_gridMap->isVisible())
01925                 {
01926                          mapShownRequested();
01927                 }
01928         }
01929         else if(r == aShowHideOrigin)
01930         {
01931                 this->setOriginVisible(!this->isOriginVisible());
01932         }
01933         else if(r == aShowHideReferential)
01934         {
01935                 this->setReferentialVisible(!this->isReferentialVisible());
01936         }
01937         else if(r == aShowHideLocalRadius)
01938         {
01939                 this->setLocalRadiusVisible(!this->isLocalRadiusVisible());
01940         }
01941         else if(r == aRestoreDefaults)
01942         {
01943                 this->restoreDefaults();
01944         }
01945         else if(r == aShowHideGraph)
01946         {
01947                 this->setGraphVisible(!this->isGraphVisible());
01948         }
01949         else if(r == aShowHideGraphNodes)
01950         {
01951                 this->setNodeVisible(!_nodeVisible);
01952         }
01953         else if(r == aShowHideGlobalPath)
01954         {
01955                 this->setGlobalPathVisible(!this->isGlobalPathVisible());
01956         }
01957         else if(r == aShowHideLocalPath)
01958         {
01959                 this->setLocalPathVisible(!this->isLocalPathVisible());
01960         }
01961         else if(r == aShowHideGtGraph)
01962         {
01963                 this->setGtGraphVisible(!this->isGtGraphVisible());
01964         }
01965         else if(r == aShowHideGPSGraph)
01966         {
01967                 this->setGPSGraphVisible(!this->isGPSGraphVisible());
01968         }
01969         else if(r == aOrientationENU)
01970         {
01971                 this->setOrientationENU(!this->isOrientationENU());
01972         }
01973 
01974         if(r)
01975         {
01976                 Q_EMIT configChanged();
01977         }
01978 }
01979 
01980 } /* namespace rtabmap */


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:20