00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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
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
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
00268 _originReferential = new QGraphicsItemGroup();
00269 this->scene()->addItem(_originReferential);
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
00280 _referential = new QGraphicsItemGroup();
00281 this->scene()->addItem(_referential);
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
00341 for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
00342 {
00343 iter.value()->hide();
00344 iter.value()->setColor(_nodeColor);
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
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
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
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
00432 _linkItems.erase(itemIter);
00433 delete linkItem;
00434 linkItem = 0;
00435 }
00436
00437 if(linkItem)
00438 {
00439
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
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
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());
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
00560 for(QMap<int, NodeItem*>::iterator iter = _gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
00561 {
00562 iter.value()->hide();
00563 iter.value()->setColor(_gtPathColor);
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
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
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
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());
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
00689 for(QMap<int, NodeItem*>::iterator iter = _gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter)
00690 {
00691 iter.value()->hide();
00692 iter.value()->setColor(_gpsPathColor);
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
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
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
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());
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());
00835 }
00836 else
00837 {
00838 this->clearMap();
00839 }
00840 }
00841
00842 void GraphViewer::updatePosterior(const std::map<int, float> & posterior, float max)
00843 {
00844
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));
00864 }
00865 else
00866 {
00867 iter.value()->setColor(QColor::fromHsvF(240.0f/360.0f, 1, 1, 1));
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
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
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
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);
00972 item->setParentItem(_localPathRoot);
00973 _localPathLinkItems.insert(idFrom, item);
00974 }
00975 }
00976 }
00977 }
00978
00979
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());
01017 }
01018
01019 void GraphViewer::clearMap()
01020 {
01021 _gridMap->setPixmap(QPixmap());
01022 _gridCellSize = 0.0f;
01023 this->scene()->setSceneRect(this->scene()->itemsBoundingRect());
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);
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
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));
01675 }
01676 else
01677 {
01678 _root->setScale(this->transform().m11());
01679 }
01680
01681 this->scene()->clearSelection();
01682 this->scene()->setSceneRect(this->scene()->itemsBoundingRect());
01683 QSize sceneSize = this->scene()->sceneRect().size().toSize();
01684
01685 if(isPNG)
01686 {
01687 QImage image(sceneSize, QImage::Format_ARGB32);
01688 image.fill(Qt::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
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
01726 _root->setScale(1.0f);
01727 this->scene()->setSceneRect(this->scene()->itemsBoundingRect());
01728
01729
01730 QDesktopServices::openUrl(QUrl::fromLocalFile(targetDir + name));
01731 }
01732 return;
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
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
01894 {
01895 this->setNeighborColor(color);
01896 }
01897 }
01898 else
01899 {
01900 return;
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 }