GraphViewer.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "rtabmap/gui/GraphViewer.h"
29 
30 #include <QGraphicsView>
31 #include <QVBoxLayout>
32 #include <QGraphicsScene>
33 #include <QGraphicsEllipseItem>
34 #include <QGraphicsRectItem>
35 #include <QtGui/QWheelEvent>
36 #include <QGraphicsSceneHoverEvent>
37 #include <QMenu>
38 #include <QtGui/QDesktopServices>
39 #include <QtGui/QContextMenuEvent>
40 #include <QColorDialog>
41 #include <QPrinter>
42 #include <QFileDialog>
43 #ifdef QT_SVG_LIB
44 #include <QtSvg/QSvgGenerator>
45 #endif
46 #include <QInputDialog>
47 #include <QMessageBox>
48 #include <QToolTip>
49 
50 #include <QtCore/QDir>
51 #include <QtCore/QDateTime>
52 #include <QtCore/QUrl>
53 
54 #include <rtabmap/core/util3d.h>
55 #include <rtabmap/core/GeodeticCoords.h>
56 #include <rtabmap/utilite/UCv2Qt.h>
57 #include <rtabmap/utilite/UStl.h>
58 #include <rtabmap/utilite/ULogger.h>
59 #include <rtabmap/utilite/UTimer.h>
60 
61 #include <QtGlobal>
62 #if QT_VERSION >= 0x050000
63  #include <QStandardPaths>
64 #endif
65 
66 #include <fstream>
67 
68 namespace rtabmap {
69 
70 class NodeItem: public QGraphicsEllipseItem
71 {
72 public:
73  // in meter
74  NodeItem(int id, int mapId, const Transform & pose, float radius, int weight, GraphViewer::ViewPlane plane, float linkWidth) :
75  QGraphicsEllipseItem(QRectF(-radius*100.0f,-radius*100.0f,radius*100.0f*2.0f,radius*100.0f*2.0f)),
76  _id(id),
77  _mapId(mapId),
78  _weight(weight),
79  _pose(pose),
80  _line(0)
81  {
82  this->setPose(pose, plane);
83  this->setBrush(pen().color());
84  this->setAcceptHoverEvents(true);
85  float r,p,yaw;
86  pose.getEulerAngles(r, p, yaw);
87  radius*=100.0f;
88  _line = new QGraphicsLineItem(0,0,-radius*sin(yaw),-radius*cos(yaw), this);
89  QPen pen = _line->pen();
90  pen.setWidth(linkWidth*100.0f);
91  _line->setPen(pen);
92  }
93  virtual ~NodeItem() {}
94 
95  void setColor(const QColor & color)
96  {
97  QPen p = this->pen();
98  p.setColor(color);
99  this->setPen(p);
100  QBrush b = this->brush();
101  b.setColor(color);
102  this->setBrush(b);
103 
104  QPen pen = _line->pen();
105  pen.setColor(QColor(255-color.red(), 255-color.green(), 255-color.blue()));
106  _line->setPen(pen);
107  }
108 
109  void setRadius(float radius)
110  {
111  float r,p,yaw;
112  _pose.getEulerAngles(r, p, yaw);
113  radius*=100.0f;
114  this->setRect(-radius, -radius, radius*2.0f, radius*2.0f);
115  _line->setLine(0,0,-radius*sin(yaw),-radius*cos(yaw));
116  }
117 
118  int id() const {return _id;};
119  int mapId() const {return _mapId;}
120  const Transform & pose() const {return _pose;}
121  void setPose(const Transform & pose, GraphViewer::ViewPlane plane) {
122  switch(plane)
123  {
124  case GraphViewer::XZ:
125  this->setPos(pose.x()*100.0f,-pose.z()*100.0f);
126  break;
127  case GraphViewer::YZ:
128  this->setPos(pose.y()*100.0f,-pose.z()*100.0f);
129  break;
130  default: // XY
131  this->setPos(-pose.y()*100.0f,-pose.x()*100.0f);
132  break;
133  }
134  _pose=pose;
135  float r,p,yaw;
136  _pose.getEulerAngles(r, p, yaw);
137  if(_line)
138  {
139  float radius = this->rect().width()/2.0f;
140  _line->setLine(0,0,-radius*sin(yaw),-radius*cos(yaw));
141  }
142  }
143 
144 protected:
145  virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
146  {
147  if(_weight>=0)
148  {
149  this->setToolTip(QString("%1 [map=%2, w=%3] %4").arg(_id).arg(_mapId).arg(_weight).arg(_pose.prettyPrint().c_str()));
150  }
151  else
152  {
153  this->setToolTip(QString("%1 [map=%2] %3").arg(_id).arg(_mapId).arg(_pose.prettyPrint().c_str()));
154  }
155  this->setScale(2);
156  QGraphicsEllipseItem::hoverEnterEvent(event);
157  }
158 
159  virtual void hoverLeaveEvent ( QGraphicsSceneHoverEvent * event )
160  {
161  this->setScale(1);
162  QGraphicsEllipseItem::hoverEnterEvent(event);
163  }
164 
165 private:
166  int _id;
167  int _mapId;
168  int _weight;
169  Transform _pose;
170  QGraphicsLineItem * _line;
171 };
172 
173 class NodeGPSItem: public NodeItem
174 {
175 public:
176  NodeGPSItem(int id, int mapId, const Transform & pose, float radius, const GPS & gps, GraphViewer::ViewPlane plane, float linkWidth) :
177  NodeItem(id, mapId, pose, radius, -1, plane, linkWidth),
178  _gps(gps)
179  {
180  }
181  virtual ~NodeGPSItem() {}
182 protected:
183  virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
184  {
185  this->setToolTip(QString("%1 [%2] %3\n"
186  "longitude=%4 latitude=%5 altitude=%6m error=%7m bearing=%8deg")
187  .arg(id()).arg(mapId()).arg(pose().prettyPrint().c_str())
188  .arg(_gps.longitude()).arg(_gps.latitude()).arg(_gps.altitude()).arg(_gps.error()).arg(_gps.bearing()));
189  this->setScale(2);
190  QGraphicsEllipseItem::hoverEnterEvent(event);
191  }
192 private:
193  GPS _gps;
194 };
195 
196 class LinkItem: public QGraphicsLineItem
197 {
198 public:
199  // in meter
200  LinkItem(int from, int to, const Transform & poseA, const Transform & poseB, const Link & link, bool interSessionClosure, GraphViewer::ViewPlane plane) :
201  _from(from),
202  _to(to),
203  _poseA(poseA),
204  _poseB(poseB),
205  _link(link),
206  _interSession(interSessionClosure)
207  {
208  this->setPoses(poseA, poseB, plane);
209  this->setAcceptHoverEvents(true);
210  }
211  virtual ~LinkItem() {}
212 
213  void setColor(const QColor & color)
214  {
215  QPen p = this->pen();
216  p.setColor(color);
217  this->setPen(p);
218  }
219 
220  void setPoses(const Transform & poseA, const Transform & poseB, GraphViewer::ViewPlane plane)
221  {
222  switch(plane)
223  {
224  case GraphViewer::XZ:
225  this->setLine(poseA.x()*100.0f, -poseA.z()*100.0f, poseB.x()*100.0f, -poseB.z()*100.0f);
226  break;
227  case GraphViewer::YZ:
228  this->setLine(poseA.y()*100.0f, -poseA.z()*100.0f, poseB.y()*100.0f, -poseB.z()*100.0f);
229  break;
230  default: // XY
231  this->setLine(-poseA.y()*100.0f, -poseA.x()*100.0f, -poseB.y()*100.0f, -poseB.x()*100.0f);
232  break;
233  }
234  _poseA = poseA;
235  _poseB = poseB;
236  }
237 
238  const Transform & getPoseA() const
239  {
240  return _poseA;
241  }
242  const Transform & getPoseB() const
243  {
244  return _poseB;
245  }
246 
247  Link::Type linkType() const {return _link.type();}
248  bool isInterSession() const {return _interSession;}
249  int from() const {return _from;}
250  int to() const {return _to;}
251 
252 protected:
253  virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
254  {
255  QString str = QString("%1->%2 (type=%3 length=%4 m)").arg(_from).arg(_to).arg(_link.type()).arg(_poseA.getDistance(_poseB));
256  if(!_link.transform().isNull())
257  {
258  str.append(QString("\n%1\nvar= %2 %3").arg(_link.transform().prettyPrint().c_str()).arg(_link.transVariance()).arg(_link.rotVariance()));
259  }
260  this->setToolTip(str);
261  QPen pen = this->pen();
262  pen.setWidthF(pen.widthF()+2);
263  this->setPen(pen);
264  QGraphicsLineItem::hoverEnterEvent(event);
265  }
266 
267  virtual void hoverLeaveEvent ( QGraphicsSceneHoverEvent * event )
268  {
269  QPen pen = this->pen();
270  pen.setWidthF(pen.widthF()-2);
271  this->setPen(pen);
272  QGraphicsLineItem::hoverEnterEvent(event);
273  }
274 
275 private:
276  int _from;
277  int _to;
278  Transform _poseA;
279  Transform _poseB;
280  Link _link;
281  bool _interSession;
282 };
283 
284 GraphViewer::GraphViewer(QWidget * parent) :
285  QGraphicsView(parent),
286  _nodeColor(Qt::blue),
287  _nodeOdomCacheColor(Qt::darkGreen),
288  _currentGoalColor(Qt::darkMagenta),
289  _neighborColor(Qt::blue),
290  _loopClosureColor(Qt::red),
291  _loopClosureLocalColor(Qt::yellow),
292  _loopClosureUserColor(Qt::red),
293  _loopClosureVirtualColor(Qt::magenta),
294  _neighborMergedColor(QColor(255,170,0)),
295  _landmarkColor(Qt::darkGreen),
296  _loopClosureRejectedColor(Qt::black),
297  _localPathColor(Qt::cyan),
298  _globalPathColor(Qt::darkMagenta),
299  _gtPathColor(Qt::gray),
300  _gpsPathColor(Qt::darkCyan),
301  _loopIntraSessionColor(Qt::red),
302  _loopInterSessionColor(Qt::green),
303  _intraInterSessionColors(false),
304  _worldMapRotation(0.0f),
305  _world(0),
306  _root(0),
307  _graphRoot(0),
308  _globalPathRoot(0),
309  _nodeVisible(true),
310  _nodeRadius(0.01f),
311  _linkWidth(0),
312  _gridMap(0),
313  _referential(0),
314  _originReferential(0),
315  _gridCellSize(0.0f),
316  _localRadius(0),
317  _loopClosureOutlierThr(0),
318  _maxLinkLength(0.02f),
319  _orientationENU(false),
320  _mouseTracking(false),
321  _viewPlane(XY),
322  _ensureFrameVisible(true)
323 {
324  this->setScene(new QGraphicsScene(this));
325  this->setDragMode(QGraphicsView::ScrollHandDrag);
326  _workingDirectory = QDir::homePath();
327 
328  this->scene()->clear();
329  _world = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
330  _root = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
331  _root->setParentItem(_world);
332 
333  // add referential
334  QGraphicsLineItem * item;
335  _originReferential = new QGraphicsItemGroup();
336  this->scene()->addItem(_originReferential); // ownership transfered
337 
338  // XY referential
339  _originReferentialXY = new QGraphicsItemGroup();
340  this->scene()->addItem(_originReferentialXY); // ownership transfered
341  item = this->scene()->addLine(0,0,0,-100, QPen(QBrush(Qt::red), _linkWidth));
342  item->setZValue(1);
343  item->setParentItem(_root);
344  _originReferentialXY->addToGroup(item);
345  item = this->scene()->addLine(0,0,-100,0, QPen(QBrush(Qt::green), _linkWidth));
346  item->setZValue(1);
347  item->setParentItem(_root);
348  _originReferentialXY->addToGroup(item);
349  _originReferential->addToGroup(_originReferentialXY);
350 
351  // XZ referential
352  _originReferentialXZ = new QGraphicsItemGroup();
353  this->scene()->addItem(_originReferentialXZ); // ownership transfered
354  item = this->scene()->addLine(0,0,100,0, QPen(QBrush(Qt::red), _linkWidth));
355  item->setZValue(1);
356  item->setParentItem(_root);
357  _originReferentialXZ->addToGroup(item);
358  item = this->scene()->addLine(0,0,0,-100, QPen(QBrush(Qt::blue), _linkWidth));
359  item->setZValue(1);
360  item->setParentItem(_root);
361  _originReferentialXZ->addToGroup(item);
362  _originReferential->addToGroup(_originReferentialXZ);
363  _originReferentialXZ->setVisible(false);
364 
365  // YZ referential
366  _originReferentialYZ = new QGraphicsItemGroup();
367  this->scene()->addItem(_originReferentialYZ); // ownership transfered
368  item = this->scene()->addLine(0,0,100,0, QPen(QBrush(Qt::green), _linkWidth));
369  item->setZValue(1);
370  item->setParentItem(_root);
371  _originReferentialYZ->addToGroup(item);
372  item = this->scene()->addLine(0,0,0,-100, QPen(QBrush(Qt::blue), _linkWidth));
373  item->setZValue(1);
374  item->setParentItem(_root);
375  _originReferentialYZ->addToGroup(item);
376  _originReferential->addToGroup(_originReferentialYZ);
377  _originReferentialYZ->setVisible(false);
378 
379  _originReferential->setZValue(1);
380  _originReferential->setParentItem(_root);
381 
382  // current pose
383  _referential = new QGraphicsItemGroup();
384  this->scene()->addItem(_referential); // ownership transfered
385 
386  // XY
387  _referentialXY = new QGraphicsItemGroup();
388  this->scene()->addItem(_referentialXY); // ownership transfered
389  item = this->scene()->addLine(0,0,0,-50, QPen(QBrush(Qt::red), _linkWidth));
390  item->setZValue(100);
391  item->setParentItem(_root);
392  _referentialXY->addToGroup(item);
393  item = this->scene()->addLine(0,0,-50,0, QPen(QBrush(Qt::green), _linkWidth));
394  item->setZValue(100);
395  item->setParentItem(_root);
396  _referentialXY->addToGroup(item);
397  _referential->addToGroup(_referentialXY);
398 
399  // XZ
400  _referentialXZ = new QGraphicsItemGroup();
401  this->scene()->addItem(_referentialXZ); // ownership transfered
402  item = this->scene()->addLine(0,0,50,0, QPen(QBrush(Qt::red), _linkWidth));
403  item->setZValue(100);
404  item->setParentItem(_root);
405  _referentialXZ->addToGroup(item);
406  item = this->scene()->addLine(0,0,0,-50, QPen(QBrush(Qt::blue), _linkWidth));
407  item->setZValue(100);
408  item->setParentItem(_root);
409  _referentialXZ->addToGroup(item);
410  _referential->addToGroup(_referentialXZ);
411  _referentialXZ->setVisible(false);
412 
413  // XZ
414  _referentialYZ = new QGraphicsItemGroup();
415  this->scene()->addItem(_referentialYZ); // ownership transfered
416  item = this->scene()->addLine(0,0,50,0, QPen(QBrush(Qt::green), _linkWidth));
417  item->setZValue(100);
418  item->setParentItem(_root);
419  _referentialYZ->addToGroup(item);
420  item = this->scene()->addLine(0,0,0,-50, QPen(QBrush(Qt::blue), _linkWidth));
421  item->setZValue(100);
422  item->setParentItem(_root);
423  _referentialYZ->addToGroup(item);
424  _referential->addToGroup(_referentialYZ);
425  _referentialYZ->setVisible(false);
426 
427  _referential->setZValue(100);
428  _referential->setParentItem(_root);
429 
430  _localRadius = this->scene()->addEllipse(-0.0001,-0.0001,0.0001,0.0001);
431  _localRadius->setZValue(1);
432  _localRadius->setParentItem(_root);
433  _localRadius->setVisible(false);
434  _localRadius->setPen(QPen(Qt::DashLine));
435 
436  _gridMap = this->scene()->addPixmap(QPixmap());
437  _gridMap->setZValue(0);
438  _gridMap->setParentItem(_root);
439 
440  _graphRoot = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
441  _graphRoot->setZValue(4);
442  _graphRoot->setParentItem(_root);
443 
444  _globalPathRoot = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
445  _globalPathRoot->setZValue(8);
446  _globalPathRoot->setParentItem(_root);
447 
448  _localPathRoot = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
449  _localPathRoot->setZValue(9);
450  _localPathRoot->setParentItem(_root);
451 
452  _gtGraphRoot = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
453  _gtGraphRoot->setZValue(2);
454  _gtGraphRoot->setParentItem(_root);
455 
456  _gpsGraphRoot = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
457  _gpsGraphRoot->setZValue(3);
458  _gpsGraphRoot->setParentItem(_root);
459 
460  _odomCacheOverlay = this->scene()->addRect(0,0,0,0);
461  _odomCacheOverlay->setZValue(21); // just under odom cache nodes and links
462  _odomCacheOverlay->setParentItem(_graphRoot);
463  _odomCacheOverlay->setBrush(QBrush(QColor(255, 255, 255, 150)));
464  _odomCacheOverlay->setPen(QPen(Qt::NoPen));
465 
466  _highlightedNodes.push_back(QPair<QColor, NodeItem*>(Qt::magenta, nullptr));
467  _highlightedNodes.push_back(QPair<QColor, NodeItem*>(Qt::yellow, nullptr));
468 
469  this->restoreDefaults();
470 
471  this->fitInView(this->sceneRect(), Qt::KeepAspectRatio);
472 }
473 
474 GraphViewer::~GraphViewer()
475 {
476 }
477 
478 void GraphViewer::setWorldMapRotation(const float & theta)
479 {
480  _worldMapRotation = theta;
481  setOrientationENU(isOrientationENU());
482 }
483 
484 void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
485  const std::multimap<int, Link> & constraints,
486  const std::map<int, int> & mapIds,
487  const std::map<int, int> & weights,
488  const std::set<int> & odomCacheIds)
489 {
490  UTimer timer;
491  bool wasVisible = _graphRoot->isVisible();
492  _graphRoot->show();
493 
494  bool wasEmpty = _nodeItems.size() == 0 && _linkItems.size() == 0;
495  UDEBUG("poses=%d constraints=%d", (int)poses.size(), (int)constraints.size());
496  //Hide nodes and links
497  for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
498  {
499  QColor color = _nodeColor;
500  bool isOdomCache = odomCacheIds.find(iter.key()) != odomCacheIds.end();
501  if(iter.key()<0)
502  {
503  color = QColor(255-color.red(), 255-color.green(), 255-color.blue());
504  }
505  else if(isOdomCache)
506  {
507  color = _nodeOdomCacheColor;
508  }
509  iter.value()->hide();
510  iter.value()->setColor(color); // reset color
511  iter.value()->setZValue(iter.key()<0?21:20);
512  }
513  for(QMultiMap<int, LinkItem*>::iterator iter = _linkItems.begin(); iter!=_linkItems.end(); ++iter)
514  {
515  iter.value()->hide();
516  }
517 
518  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
519  {
520  if(!iter->second.isNull())
521  {
522  QMap<int, NodeItem*>::iterator itemIter = _nodeItems.find(iter->first);
523  if(itemIter != _nodeItems.end())
524  {
525  itemIter.value()->setPose(iter->second, _viewPlane);
526  itemIter.value()->show();
527  }
528  else
529  {
530  // create node item
531  QColor color = _nodeColor;
532  bool isOdomCache = odomCacheIds.find(iter->first) != odomCacheIds.end();
533  if(iter->first<0)
534  {
535  color = QColor(255-color.red(), 255-color.green(), 255-color.blue());
536  }
537  else if(isOdomCache)
538  {
539  color = _nodeOdomCacheColor;
540  }
541  const Transform & pose = iter->second;
542  NodeItem * item = new NodeItem(iter->first, uContains(mapIds, iter->first)?mapIds.at(iter->first):-1, pose, _nodeRadius, uContains(weights, iter->first)?weights.at(iter->first):-1, _viewPlane, _linkWidth);
543  this->scene()->addItem(item);
544  item->setZValue(iter->first<0?21:20);
545  item->setColor(color);
546  item->setParentItem(_graphRoot);
547  item->setVisible(_nodeVisible);
548  _nodeItems.insert(iter->first, item);
549  }
550  }
551  }
552 
553  for(std::multimap<int, Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
554  {
555  // make the first id the smallest one
556  int idFrom = iter->first<iter->second.to()?iter->first:iter->second.to();
557  int idTo = iter->first<iter->second.to()?iter->second.to():iter->first;
558 
559  std::map<int, Transform>::const_iterator jterA = poses.find(idFrom);
560  std::map<int, Transform>::const_iterator jterB = poses.find(idTo);
561  LinkItem * linkItem = 0;
562  if(jterA != poses.end() && jterB != poses.end() &&
563  _nodeItems.contains(iter->first) && _nodeItems.contains(idTo))
564  {
565  const Transform & poseA = jterA->second;
566  const Transform & poseB = jterB->second;
567 
568  QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.end();
569  if(_linkItems.contains(idFrom))
570  {
571  itemIter = _linkItems.find(iter->first);
572  while(itemIter.key() == idFrom && itemIter != _linkItems.end())
573  {
574  if(itemIter.value()->to() == idTo && itemIter.value()->type() == iter->second.type())
575  {
576  itemIter.value()->setPoses(poseA, poseB, _viewPlane);
577  itemIter.value()->show();
578  linkItem = itemIter.value();
579  break;
580  }
581  ++itemIter;
582  }
583  }
584 
585  bool interSessionClosure = false;
586  if(uContains(mapIds, jterA->first) && uContains(mapIds, jterB->first))
587  {
588  interSessionClosure = mapIds.at(jterA->first) != mapIds.at(jterB->first);
589  }
590 
591  bool isLinkedToOdomCachePoses =
592  odomCacheIds.find(idFrom)!=odomCacheIds.end() ||
593  odomCacheIds.find(idTo)!=odomCacheIds.end();
594 
595  if(isLinkedToOdomCachePoses)
596  {
597  _nodeItems.value(idFrom)->setZValue(odomCacheIds.find(idFrom)!=odomCacheIds.end()?24:23);
598  _nodeItems.value(idTo)->setZValue(odomCacheIds.find(idTo)!=odomCacheIds.end()?24:23);
599  }
600 
601  if(poseA.getDistance(poseB) > _maxLinkLength)
602  {
603  if(linkItem == 0)
604  {
605  //create a link item
606  linkItem = new LinkItem(idFrom, idTo, poseA, poseB, iter->second, interSessionClosure, _viewPlane);
607  QPen p = linkItem->pen();
608  p.setWidthF(_linkWidth*100.0f);
609  linkItem->setPen(p);
610  linkItem->setZValue(isLinkedToOdomCachePoses?22:10);
611  this->scene()->addItem(linkItem);
612  linkItem->setParentItem(_graphRoot);
613  _linkItems.insert(idFrom, linkItem);
614  }
615  }
616  else if(linkItem && itemIter != _linkItems.end())
617  {
618  // erase small links
619  _linkItems.erase(itemIter);
620  delete linkItem;
621  linkItem = 0;
622  }
623 
624  if(linkItem)
625  {
626  //update color
627  if(iter->second.type() == Link::kNeighbor)
628  {
629  linkItem->setColor(_neighborColor);
630  }
631  else if(iter->second.type() == Link::kVirtualClosure)
632  {
633  linkItem->setColor(_loopClosureVirtualColor);
634  }
635  else if(iter->second.type() == Link::kNeighborMerged)
636  {
637  linkItem->setColor(_neighborMergedColor);
638  }
639  else if(iter->second.type() == Link::kUserClosure)
640  {
641  if(_intraInterSessionColors)
642  {
643  linkItem->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor);
644  }
645  else
646  {
647  linkItem->setColor(_loopClosureUserColor);
648  }
649  }
650  else if(iter->second.type() == Link::kLandmark)
651  {
652  linkItem->setColor(_landmarkColor);
653  }
654  else if(iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure)
655  {
656  if(_intraInterSessionColors)
657  {
658  linkItem->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor);
659  linkItem->setZValue(isLinkedToOdomCachePoses?22:interSessionClosure?6:7);
660  }
661  else
662  {
663  linkItem->setColor(_loopClosureLocalColor);
664  linkItem->setZValue(isLinkedToOdomCachePoses?22:7);
665  }
666  }
667  else
668  {
669  if(_intraInterSessionColors)
670  {
671  linkItem->setColor(interSessionClosure?_loopInterSessionColor:_loopIntraSessionColor);
672  linkItem->setZValue(isLinkedToOdomCachePoses?22:interSessionClosure?8:9);
673  }
674  else
675  {
676  linkItem->setColor(_loopClosureColor);
677  linkItem->setZValue(isLinkedToOdomCachePoses?22:9);
678  }
679  }
680 
681  //rejected loop closures
682  if(_loopClosureOutlierThr > 0.0f)
683  {
684  Transform t = poseA.inverse()*poseB;
685  if(iter->second.to() != idTo)
686  {
687  t = t.inverse();
688  }
689  if(iter->second.type() != Link::kNeighbor &&
690  iter->second.type() != Link::kNeighborMerged)
691  {
692  float linearError = fabs(iter->second.transform().getNorm() - t.getNorm());
693  if(linearError > _loopClosureOutlierThr)
694  {
695  linkItem->setColor(_loopClosureRejectedColor);
696  }
697  }
698  }
699  }
700  }
701  }
702 
703  //remove not used nodes and links
704  for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end();)
705  {
706  if(!iter.value()->isVisible())
707  {
708  for(int i=0; i<_highlightedNodes.size(); ++i)
709  {
710  if(_highlightedNodes[i].second && _highlightedNodes[i].second == iter.value())
711  {
712  _highlightedNodes[i].second = nullptr;
713  }
714  }
715 
716  delete iter.value();
717  iter = _nodeItems.erase(iter);
718  }
719  else
720  {
721  ++iter;
722  }
723  }
724  for(QMultiMap<int, LinkItem*>::iterator iter = _linkItems.begin(); iter!=_linkItems.end();)
725  {
726  if(!iter.value()->isVisible())
727  {
728  delete iter.value();
729  iter = _linkItems.erase(iter);
730  }
731  else
732  {
733  ++iter;
734  }
735  }
736 
737  if(_nodeItems.size())
738  {
739  (--_nodeItems.end()).value()->setColor(_nodeOdomCacheColor);
740  }
741 
742  this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
743 
744  if(!odomCacheIds.empty())
745  _odomCacheOverlay->setRect(this->scene()->itemsBoundingRect());
746  else
747  _odomCacheOverlay->setRect(0, 0, 0, 0);
748 
749  if(wasEmpty)
750  {
751  QRectF rect = this->scene()->itemsBoundingRect();
752  this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio);
753  }
754 
755  _graphRoot->setVisible(wasVisible);
756 
757  UDEBUG("_nodeItems=%d, _linkItems=%d, timer=%fs", _nodeItems.size(), _linkItems.size(), timer.ticks());
758 }
759 
760 void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
761 {
762  UTimer timer;
763  bool wasVisible = _gtGraphRoot->isVisible();
764  _gtGraphRoot->show();
765  bool wasEmpty = _gtNodeItems.size() == 0 && _gtLinkItems.size() == 0;
766  UDEBUG("poses=%d", (int)poses.size());
767  //Hide nodes and links
768  for(QMap<int, NodeItem*>::iterator iter = _gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
769  {
770  iter.value()->hide();
771  iter.value()->setColor(_gtPathColor); // reset color
772  }
773  for(QMultiMap<int, LinkItem*>::iterator iter = _gtLinkItems.begin(); iter!=_gtLinkItems.end(); ++iter)
774  {
775  iter.value()->hide();
776  }
777 
778  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
779  {
780  if(!iter->second.isNull())
781  {
782  QMap<int, NodeItem*>::iterator itemIter = _gtNodeItems.find(iter->first);
783  if(itemIter != _gtNodeItems.end())
784  {
785  itemIter.value()->setPose(iter->second, _viewPlane);
786  itemIter.value()->show();
787  }
788  else
789  {
790  // create node item
791  const Transform & pose = iter->second;
792  NodeItem * item = new NodeItem(iter->first, -1, pose, _nodeRadius, -1, _viewPlane, _linkWidth);
793  this->scene()->addItem(item);
794  item->setZValue(20);
795  item->setColor(_gtPathColor);
796  item->setParentItem(_gtGraphRoot);
797  item->setVisible(_nodeVisible);
798  _gtNodeItems.insert(iter->first, item);
799  }
800 
801  if(iter!=poses.begin())
802  {
803  std::map<int, Transform>::const_iterator iterPrevious = iter;
804  --iterPrevious;
805  Transform previousPose = iterPrevious->second;
806  Transform currentPose = iter->second;
807 
808  LinkItem * linkItem = 0;
809  QMultiMap<int, LinkItem*>::iterator linkIter = _gtLinkItems.end();
810  if(_gtLinkItems.contains(iterPrevious->first))
811  {
812  linkIter = _gtLinkItems.find(iter->first);
813  while(linkIter.key() == iterPrevious->first && linkIter != _gtLinkItems.end())
814  {
815  if(linkIter.value()->to() == iter->first)
816  {
817  linkIter.value()->setPoses(previousPose, currentPose, _viewPlane);
818  linkIter.value()->show();
819  linkItem = linkIter.value();
820  break;
821  }
822  ++linkIter;
823  }
824  }
825  if(linkItem == 0)
826  {
827  bool linkFound = iter->first - iterPrevious->first == 1; // if consecutive, add link
828  for(QMultiMap<int, LinkItem*>::iterator kter = _linkItems.find(iterPrevious->first);
829  kter!=_linkItems.end() && kter.key()==iterPrevious->first && !linkFound;
830  ++kter)
831  {
832  if(kter.value()->from() == iterPrevious->first && kter.value()->to() == iter->first)
833  {
834  linkFound = true;
835  }
836  }
837 
838  if(linkFound)
839  {
840  //create a link item
841  linkItem = new LinkItem(iterPrevious->first, iter->first, previousPose, currentPose, Link(), 1, _viewPlane);
842  QPen p = linkItem->pen();
843  p.setWidthF(_linkWidth*100.0f);
844  linkItem->setPen(p);
845  linkItem->setZValue(10);
846  this->scene()->addItem(linkItem);
847  linkItem->setParentItem(_gtGraphRoot);
848  _gtLinkItems.insert(iterPrevious->first, linkItem);
849  }
850  }
851  if(linkItem)
852  {
853  linkItem->setColor(_gtPathColor);
854  }
855  }
856  }
857  }
858 
859  //remove not used nodes and links
860  for(QMap<int, NodeItem*>::iterator iter = _gtNodeItems.begin(); iter!=_gtNodeItems.end();)
861  {
862  if(!iter.value()->isVisible())
863  {
864  delete iter.value();
865  iter = _gtNodeItems.erase(iter);
866  }
867  else
868  {
869  ++iter;
870  }
871  }
872  for(QMultiMap<int, LinkItem*>::iterator iter = _gtLinkItems.begin(); iter!=_gtLinkItems.end();)
873  {
874  if(!iter.value()->isVisible())
875  {
876  delete iter.value();
877  iter = _gtLinkItems.erase(iter);
878  }
879  else
880  {
881  ++iter;
882  }
883  }
884 
885  if(_gtNodeItems.size() || _gtLinkItems.size())
886  {
887  this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
888 
889  if(wasEmpty)
890  {
891  QRectF rect = this->scene()->itemsBoundingRect();
892  this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio);
893  }
894  }
895 
896  _gtGraphRoot->setVisible(wasVisible);
897 
898  UDEBUG("_gtNodeItems=%d, _gtLinkItems=%d timer=%fs", _gtNodeItems.size(), _gtLinkItems.size(), timer.ticks());
899 }
900 
901 void GraphViewer::updateGPSGraph(
902  const std::map<int, Transform> & poses,
903  const std::map<int, GPS> & gpsValues)
904 {
905  UTimer timer;
906  bool wasVisible = _gpsGraphRoot->isVisible();
907  _gpsGraphRoot->show();
908  bool wasEmpty = _gpsNodeItems.size() == 0 && _gpsNodeItems.size() == 0;
909  UDEBUG("poses=%d", (int)poses.size());
910  //Hide nodes and links
911  for(QMap<int, NodeItem*>::iterator iter = _gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter)
912  {
913  iter.value()->hide();
914  iter.value()->setColor(_gpsPathColor); // reset color
915  }
916  for(QMultiMap<int, LinkItem*>::iterator iter = _gpsLinkItems.begin(); iter!=_gpsLinkItems.end(); ++iter)
917  {
918  iter.value()->hide();
919  }
920 
921  for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
922  {
923  if(!iter->second.isNull())
924  {
925  QMap<int, NodeItem*>::iterator itemIter = _gpsNodeItems.find(iter->first);
926  if(itemIter != _gpsNodeItems.end())
927  {
928  itemIter.value()->setPose(iter->second, _viewPlane);
929  itemIter.value()->show();
930  }
931  else
932  {
933  // create node item
934  const Transform & pose = iter->second;
935  UASSERT(gpsValues.find(iter->first) != gpsValues.end());
936  NodeItem * item = new NodeGPSItem(iter->first, -1, pose, _nodeRadius, gpsValues.at(iter->first), _viewPlane, _linkWidth);
937  this->scene()->addItem(item);
938  item->setZValue(20);
939  item->setColor(_gpsPathColor);
940  item->setParentItem(_gpsGraphRoot);
941  item->setVisible(_nodeVisible);
942  _gpsNodeItems.insert(iter->first, item);
943  }
944 
945  if(iter!=poses.begin())
946  {
947  std::map<int, Transform>::const_iterator iterPrevious = iter;
948  --iterPrevious;
949  Transform previousPose = iterPrevious->second;
950  Transform currentPose = iter->second;
951 
952  LinkItem * linkItem = 0;
953  QMultiMap<int, LinkItem*>::iterator linkIter = _gpsLinkItems.end();
954  if(_gpsLinkItems.contains(iterPrevious->first))
955  {
956  linkIter = _gpsLinkItems.find(iter->first);
957  while(linkIter.key() == iterPrevious->first && linkIter != _gpsLinkItems.end())
958  {
959  if(linkIter.value()->to() == iter->first)
960  {
961  linkIter.value()->setPoses(previousPose, currentPose, _viewPlane);
962  linkIter.value()->show();
963  linkItem = linkIter.value();
964  break;
965  }
966  ++linkIter;
967  }
968  }
969  if(linkItem == 0)
970  {
971  //create a link item
972  linkItem = new LinkItem(iterPrevious->first, iter->first, previousPose, currentPose, Link(), 1, _viewPlane);
973  QPen p = linkItem->pen();
974  p.setWidthF(_linkWidth*100.0f);
975  linkItem->setPen(p);
976  linkItem->setZValue(10);
977  this->scene()->addItem(linkItem);
978  linkItem->setParentItem(_gpsGraphRoot);
979  _gpsLinkItems.insert(iterPrevious->first, linkItem);
980  }
981  if(linkItem)
982  {
983  linkItem->setColor(_gpsPathColor);
984  }
985  }
986  }
987  }
988 
989  //remove not used nodes and links
990  for(QMap<int, NodeItem*>::iterator iter = _gpsNodeItems.begin(); iter!=_gpsNodeItems.end();)
991  {
992  if(!iter.value()->isVisible())
993  {
994  delete iter.value();
995  iter = _gpsNodeItems.erase(iter);
996  }
997  else
998  {
999  ++iter;
1000  }
1001  }
1002  for(QMultiMap<int, LinkItem*>::iterator iter = _gpsLinkItems.begin(); iter!=_gpsLinkItems.end();)
1003  {
1004  if(!iter.value()->isVisible())
1005  {
1006  delete iter.value();
1007  iter = _gpsLinkItems.erase(iter);
1008  }
1009  else
1010  {
1011  ++iter;
1012  }
1013  }
1014 
1015  if(_gpsNodeItems.size() || _gpsLinkItems.size())
1016  {
1017  this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
1018 
1019  if(wasEmpty)
1020  {
1021  QRectF rect = this->scene()->itemsBoundingRect();
1022  this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio);
1023  }
1024  }
1025 
1026  _gpsGraphRoot->setVisible(wasVisible);
1027 
1028  UDEBUG("_gpsNodeItems=%d, _gpsLinkItems=%d timer=%fs", _gpsNodeItems.size(), _gpsLinkItems.size(), timer.ticks());
1029 }
1030 
1031 void GraphViewer::updateReferentialPosition(const Transform & t)
1032 {
1033  QTransform qt;
1034  qt.translate(-t.o24()*100.0f, -t.o14()*100.0f);
1035  if(_viewPlane == XY)
1036  qt.rotateRadians(-t.theta());
1037 
1038  _referential->setTransform(qt);
1039  _localRadius->setTransform(qt);
1040 
1041  if(_ensureFrameVisible)
1042  {
1043  this->ensureVisible(_referential);
1044  if(_localRadius->isVisible())
1045  {
1046  this->ensureVisible(_localRadius, 0, 0);
1047  }
1048  }
1049 }
1050 
1051 void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin)
1052 {
1053  UASSERT(map8U.empty() || (!map8U.empty() && resolution > 0.0f));
1054  if(!map8U.empty())
1055  {
1056  _gridCellSize = resolution;
1057  QImage image = uCvMat2QImage(map8U, false);
1058  _gridMap->resetTransform();
1059  _gridMap->setTransform(QTransform::fromScale(resolution*100.0f, -resolution*100.0f), true);
1060  _gridMap->setRotation(90);
1061  _gridMap->setPixmap(QPixmap::fromImage(image));
1062  _gridMap->setPos(-yMin*100.0f, -xMin*100.0f);
1063  // Re-shrink the scene to it's bounding contents
1064  this->scene()->setSceneRect(this->scene()->itemsBoundingRect());
1065  }
1066  else
1067  {
1068  this->clearMap();
1069  }
1070 }
1071 
1072 void GraphViewer::updatePosterior(const std::map<int, float> & posterior, float max, int zValueOffset)
1073 {
1074  //find max
1075  if(max <= 0.0f)
1076  {
1077  for(std::map<int, float>::const_iterator iter = posterior.begin(); iter!=posterior.end(); ++iter)
1078  {
1079  if(iter->first > 0 && iter->second>max)
1080  {
1081  max = iter->second;
1082  }
1083  }
1084  }
1085  if(max > 0.0f)
1086  {
1087  for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
1088  {
1089  std::map<int,float>::const_iterator jter = posterior.find(iter.key());
1090  if(jter != posterior.end())
1091  {
1092  float v = jter->second>max?max:jter->second;
1093  iter.value()->setColor(QColor::fromHsvF((1-v/max)*240.0f/360.0f, 1, 1, 1)); //0=red 240=blue
1094  iter.value()->setZValue(iter.value()->zValue()+zValueOffset);
1095  }
1096  }
1097  }
1098 }
1099 
1100 void GraphViewer::setGlobalPath(const std::vector<std::pair<int, Transform> > & globalPath)
1101 {
1102  UDEBUG("Set global path size=%d", (int)globalPath.size());
1103  qDeleteAll(_globalPathLinkItems);
1104  _globalPathLinkItems.clear();
1105 
1106  if(globalPath.size() >= 2)
1107  {
1108  for(unsigned int i=0; i<globalPath.size()-1; ++i)
1109  {
1110  //create a link item
1111  int idFrom = globalPath[i].first;
1112  int idTo = globalPath[i+1].first;
1113  LinkItem * item = new LinkItem(idFrom, idTo, globalPath[i].second, globalPath[i+1].second, Link(), false, _viewPlane);
1114  QPen p = item->pen();
1115  p.setWidthF(_linkWidth*100.0f);
1116  item->setPen(p);
1117  item->setColor(_globalPathColor);
1118  this->scene()->addItem(item);
1119  item->setZValue(15);
1120  item->setParentItem(_globalPathRoot);
1121  _globalPathLinkItems.insert(idFrom, item);
1122  }
1123  }
1124 }
1125 
1126 void GraphViewer::setCurrentGoalID(int id, const Transform & pose)
1127 {
1128  NodeItem * node = _nodeItems.value(id, 0);
1129  if(node)
1130  {
1131  node->setColor(_currentGoalColor);
1132  }
1133  else
1134  {
1135  UWARN("Current goal %d not found in the graph", id);
1136  }
1137 
1138  if(!pose.isNull() && _globalPathLinkItems.size() && _globalPathLinkItems.contains(id))
1139  {
1140  // transform the global path in the goal referential
1141  const LinkItem * oldPose = _globalPathLinkItems.value(id);
1142  Transform t = pose * oldPose->getPoseA().inverse();
1143  for(QMultiMap<int, LinkItem*>::iterator iter=_globalPathLinkItems.begin(); iter!=_globalPathLinkItems.end(); ++iter)
1144  {
1145  iter.value()->setPoses(t*iter.value()->getPoseA(), t*iter.value()->getPoseB(), _viewPlane);
1146  }
1147  }
1148 }
1149 
1150 void GraphViewer::setLocalRadius(float radius)
1151 {
1152  _localRadius->setRect(-radius*100, -radius*100, radius*200, radius*200);
1153 }
1154 
1155 void GraphViewer::updateLocalPath(const std::vector<int> & localPath)
1156 {
1157  bool wasVisible = _localPathRoot->isVisible();
1158  _localPathRoot->show();
1159 
1160  for(QMultiMap<int, LinkItem*>::iterator iter = _localPathLinkItems.begin(); iter!=_localPathLinkItems.end(); ++iter)
1161  {
1162  iter.value()->hide();
1163  }
1164 
1165  if(localPath.size() > 1)
1166  {
1167  for(unsigned int i=0; i<localPath.size()-1; ++i)
1168  {
1169  int idFrom = localPath[i]<localPath[i+1]?localPath[i]:localPath[i+1];
1170  int idTo = localPath[i]<localPath[i+1]?localPath[i+1]:localPath[i];
1171  if(_nodeItems.contains(idFrom) && _nodeItems.contains(idTo))
1172  {
1173  bool updated = false;
1174  if(_localPathLinkItems.contains(idFrom))
1175  {
1176  QMultiMap<int, LinkItem*>::iterator itemIter = _localPathLinkItems.find(idFrom);
1177  while(itemIter.key() == idFrom && itemIter != _localPathLinkItems.end())
1178  {
1179  if(itemIter.value()->to() == idTo)
1180  {
1181  itemIter.value()->setPoses(_nodeItems.value(idFrom)->pose(), _nodeItems.value(idTo)->pose(), _viewPlane);
1182  itemIter.value()->show();
1183  updated = true;
1184  break;
1185  }
1186  ++itemIter;
1187  }
1188  }
1189  if(!updated)
1190  {
1191  //create a link item
1192  LinkItem * item = new LinkItem(idFrom, idTo, _nodeItems.value(idFrom)->pose(), _nodeItems.value(idTo)->pose(), Link(), false, _viewPlane);
1193  QPen p = item->pen();
1194  p.setWidthF(_linkWidth*100.0f);
1195  item->setPen(p);
1196  item->setColor(_localPathColor);
1197  this->scene()->addItem(item);
1198  item->setZValue(16); // just over the global path
1199  item->setParentItem(_localPathRoot);
1200  _localPathLinkItems.insert(idFrom, item);
1201  }
1202  }
1203  }
1204  }
1205 
1206  // remove not used links
1207  for(QMultiMap<int, LinkItem*>::iterator iter = _localPathLinkItems.begin(); iter!=_localPathLinkItems.end();)
1208  {
1209  if(!iter.value()->isVisible())
1210  {
1211  delete iter.value();
1212  iter = _localPathLinkItems.erase(iter);
1213  }
1214  else
1215  {
1216  ++iter;
1217  }
1218  }
1219  _localPathRoot->setVisible(wasVisible);
1220 }
1221 
1222 
1223 void GraphViewer::highlightNode(int nodeId, int highlightIndex)
1224 {
1225  if(highlightIndex<0 || highlightIndex>_highlightedNodes.size())
1226  {
1227  UERROR("Unsupported highlight color index %d", highlightIndex);
1228  return;
1229  }
1230  for(int i=0; i<_highlightedNodes.size(); ++i)
1231  {
1232  if(_highlightedNodes[i].second &&
1233  (_highlightedNodes[i].second->id() == nodeId || i == highlightIndex))
1234  {
1235  // reset to normal color
1236  _highlightedNodes[i].second->setColor(_nodeColor);
1237  _highlightedNodes[i].second = nullptr;
1238  }
1239  }
1240 
1241  QMap<int, NodeItem*>::iterator iter = _nodeItems.find(nodeId);
1242  if(iter != _nodeItems.end())
1243  {
1244  iter.value()->setColor(_highlightedNodes[highlightIndex].first);
1245  _highlightedNodes[highlightIndex].second = iter.value();
1246  }
1247 }
1248 
1249 void GraphViewer::clearGraph()
1250 {
1251  qDeleteAll(_nodeItems);
1252  _nodeItems.clear();
1253  qDeleteAll(_linkItems);
1254  _linkItems.clear();
1255  qDeleteAll(_localPathLinkItems);
1256  _localPathLinkItems.clear();
1257  qDeleteAll(_globalPathLinkItems);
1258  _globalPathLinkItems.clear();
1259  qDeleteAll(_gtNodeItems);
1260  _gtNodeItems.clear();
1261  qDeleteAll(_gtLinkItems);
1262  _gtLinkItems.clear();
1263  qDeleteAll(_gpsNodeItems);
1264  _gpsNodeItems.clear();
1265  qDeleteAll(_gpsLinkItems);
1266  _gpsLinkItems.clear();
1267 
1268  for(int i=0; i<_highlightedNodes.size(); ++i)
1269  {
1270  _highlightedNodes[i].second=nullptr;
1271  }
1272 
1273  _root->resetTransform();
1274  _worldMapRotation = 0.0f;
1275  _referential->resetTransform();
1276  _localRadius->resetTransform();
1277  this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
1278 }
1279 
1280 void GraphViewer::clearMap()
1281 {
1282  _gridMap->setPixmap(QPixmap());
1283  _gridCellSize = 0.0f;
1284  this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
1285 }
1286 
1287 void GraphViewer::clearPosterior()
1288 {
1289  for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
1290  {
1291  iter.value()->setColor(Qt::blue); // blue
1292  }
1293 }
1294 
1295 void GraphViewer::clearAll()
1296 {
1297  clearMap();
1298  clearGraph();
1299 }
1300 
1301 void GraphViewer::saveSettings(QSettings & settings, const QString & group) const
1302 {
1303  if(!group.isEmpty())
1304  {
1305  settings.beginGroup(group);
1306  }
1307  settings.setValue("node_radius", (double)this->getNodeRadius());
1308  settings.setValue("link_width", (double)this->getLinkWidth());
1309  settings.setValue("node_color", this->getNodeColor());
1310  settings.setValue("node_odom_cache_color", this->getNodeOdomCacheColor());
1311  settings.setValue("current_goal_color", this->getCurrentGoalColor());
1312  for(int i=0; i<_highlightedNodes.size(); ++i)
1313  {
1314  settings.setValue(QString("highlighting_color_%1").arg(i), _highlightedNodes[i].first);
1315  }
1316  settings.setValue("neighbor_color", this->getNeighborColor());
1317  settings.setValue("global_color", this->getGlobalLoopClosureColor());
1318  settings.setValue("local_color", this->getLocalLoopClosureColor());
1319  settings.setValue("user_color", this->getUserLoopClosureColor());
1320  settings.setValue("virtual_color", this->getVirtualLoopClosureColor());
1321  settings.setValue("neighbor_merged_color", this->getNeighborMergedColor());
1322  settings.setValue("rejected_color", this->getRejectedLoopClosureColor());
1323  settings.setValue("local_path_color", this->getLocalPathColor());
1324  settings.setValue("global_path_color", this->getGlobalPathColor());
1325  settings.setValue("gt_color", this->getGTColor());
1326  settings.setValue("gps_color", this->getGPSColor());
1327  settings.setValue("intra_session_color", this->getIntraSessionLoopColor());
1328  settings.setValue("inter_session_color", this->getInterSessionLoopColor());
1329  settings.setValue("intra_inter_session_colors_enabled", this->isIntraInterSessionColorsEnabled());
1330  settings.setValue("grid_visible", this->isGridMapVisible());
1331  settings.setValue("origin_visible", this->isOriginVisible());
1332  settings.setValue("referential_visible", this->isReferentialVisible());
1333  settings.setValue("local_radius_visible", this->isLocalRadiusVisible());
1334  settings.setValue("loop_closure_outlier_thr", this->getLoopClosureOutlierThr());
1335  settings.setValue("max_link_length", this->getMaxLinkLength());
1336  settings.setValue("graph_visible", this->isGraphVisible());
1337  settings.setValue("global_path_visible", this->isGlobalPathVisible());
1338  settings.setValue("local_path_visible", this->isLocalPathVisible());
1339  settings.setValue("gt_graph_visible", this->isGtGraphVisible());
1340  settings.setValue("gps_graph_visible", this->isGPSGraphVisible());
1341  settings.setValue("odom_cache_overlay", this->isOdomCacheOverlayVisible());
1342  settings.setValue("orientation_ENU", this->isOrientationENU());
1343  settings.setValue("view_plane", (int)this->getViewPlane());
1344  settings.setValue("ensure_frame_visible", (int)this->isEnsureFrameVisible());
1345  if(!group.isEmpty())
1346  {
1347  settings.endGroup();
1348  }
1349 }
1350 
1351 void GraphViewer::loadSettings(QSettings & settings, const QString & group)
1352 {
1353  if(!group.isEmpty())
1354  {
1355  settings.beginGroup(group);
1356  }
1357  this->setNodeRadius(settings.value("node_radius", this->getNodeRadius()).toDouble());
1358  this->setLinkWidth(settings.value("link_width", this->getLinkWidth()).toDouble());
1359  this->setNodeColor(settings.value("node_color", this->getNodeColor()).value<QColor>());
1360  this->setNodeOdomCacheColor(settings.value("node_odom_cache_color", this->getNodeOdomCacheColor()).value<QColor>());
1361  this->setCurrentGoalColor(settings.value("current_goal_color", this->getCurrentGoalColor()).value<QColor>());
1362  for(int i=0; i<_highlightedNodes.size(); ++i)
1363  {
1364  this->setHighlightColor(settings.value(QString("highlighting_color_%1").arg(i), _highlightedNodes[i].first).value<QColor>(), i);
1365  }
1366  this->setNeighborColor(settings.value("neighbor_color", this->getNeighborColor()).value<QColor>());
1367  this->setGlobalLoopClosureColor(settings.value("global_color", this->getGlobalLoopClosureColor()).value<QColor>());
1368  this->setLocalLoopClosureColor(settings.value("local_color", this->getLocalLoopClosureColor()).value<QColor>());
1369  this->setUserLoopClosureColor(settings.value("user_color", this->getUserLoopClosureColor()).value<QColor>());
1370  this->setVirtualLoopClosureColor(settings.value("virtual_color", this->getVirtualLoopClosureColor()).value<QColor>());
1371  this->setNeighborMergedColor(settings.value("neighbor_merged_color", this->getNeighborMergedColor()).value<QColor>());
1372  this->setRejectedLoopClosureColor(settings.value("rejected_color", this->getRejectedLoopClosureColor()).value<QColor>());
1373  this->setLocalPathColor(settings.value("local_path_color", this->getLocalPathColor()).value<QColor>());
1374  this->setGlobalPathColor(settings.value("global_path_color", this->getGlobalPathColor()).value<QColor>());
1375  this->setGTColor(settings.value("gt_color", this->getGTColor()).value<QColor>());
1376  this->setGPSColor(settings.value("gps_color", this->getGPSColor()).value<QColor>());
1377  this->setIntraSessionLoopColor(settings.value("intra_session_color", this->getIntraSessionLoopColor()).value<QColor>());
1378  this->setInterSessionLoopColor(settings.value("inter_session_color", this->getInterSessionLoopColor()).value<QColor>());
1379  this->setGridMapVisible(settings.value("grid_visible", this->isGridMapVisible()).toBool());
1380  this->setOriginVisible(settings.value("origin_visible", this->isOriginVisible()).toBool());
1381  this->setReferentialVisible(settings.value("referential_visible", this->isReferentialVisible()).toBool());
1382  this->setLocalRadiusVisible(settings.value("local_radius_visible", this->isLocalRadiusVisible()).toBool());
1383  this->setIntraInterSessionColorsEnabled(settings.value("intra_inter_session_colors_enabled", this->isIntraInterSessionColorsEnabled()).toBool());
1384  this->setLoopClosureOutlierThr(settings.value("loop_closure_outlier_thr", this->getLoopClosureOutlierThr()).toDouble());
1385  this->setMaxLinkLength(settings.value("max_link_length", this->getMaxLinkLength()).toDouble());
1386  this->setGraphVisible(settings.value("graph_visible", this->isGraphVisible()).toBool());
1387  this->setGlobalPathVisible(settings.value("global_path_visible", this->isGlobalPathVisible()).toBool());
1388  this->setLocalPathVisible(settings.value("local_path_visible", this->isLocalPathVisible()).toBool());
1389  this->setGtGraphVisible(settings.value("gt_graph_visible", this->isGtGraphVisible()).toBool());
1390  this->setGPSGraphVisible(settings.value("gps_graph_visible", this->isGPSGraphVisible()).toBool());
1391  this->setOdomCacheOverlayVisible(settings.value("odom_cache_overlay", this->isOdomCacheOverlayVisible()).toBool());
1392  this->setOrientationENU(settings.value("orientation_ENU", this->isOrientationENU()).toBool());
1393  this->setViewPlane((ViewPlane)settings.value("view_plane", (int)this->getViewPlane()).toInt());
1394  this->setEnsureFrameVisible(settings.value("ensure_frame_visible", this->isEnsureFrameVisible()).toBool());
1395  if(!group.isEmpty())
1396  {
1397  settings.endGroup();
1398  }
1399 }
1400 
1401 bool GraphViewer::isGridMapVisible() const
1402 {
1403  return _gridMap->isVisible();
1404 }
1405 bool GraphViewer::isOriginVisible() const
1406 {
1407  return _originReferential->isVisible();
1408 }
1409 bool GraphViewer::isReferentialVisible() const
1410 {
1411  return _referential->isVisible();
1412 }
1413 bool GraphViewer::isLocalRadiusVisible() const
1414 {
1415  return _localRadius->isVisible();
1416 }
1417 bool GraphViewer::isGraphVisible() const
1418 {
1419  return _graphRoot->isVisible();
1420 }
1421 bool GraphViewer::isGlobalPathVisible() const
1422 {
1423  return _globalPathRoot->isVisible();
1424 }
1425 bool GraphViewer::isLocalPathVisible() const
1426 {
1427  return _localPathRoot->isVisible();
1428 }
1429 bool GraphViewer::isGtGraphVisible() const
1430 {
1431  return _gtGraphRoot->isVisible();
1432 }
1433 bool GraphViewer::isGPSGraphVisible() const
1434 {
1435  return _gpsGraphRoot->isVisible();
1436 }
1437 bool GraphViewer::isOdomCacheOverlayVisible() const
1438 {
1439  return _odomCacheOverlay->isVisible();
1440 }
1441 bool GraphViewer::isOrientationENU() const
1442 {
1443  return _orientationENU;
1444 }
1445 GraphViewer::ViewPlane GraphViewer::getViewPlane() const
1446 {
1447  return _viewPlane;
1448 }
1449 bool GraphViewer::isEnsureFrameVisible() const
1450 {
1451  return _ensureFrameVisible;
1452 }
1453 
1454 void GraphViewer::setWorkingDirectory(const QString & path)
1455 {
1456  _workingDirectory = path;
1457 }
1458 void GraphViewer::setNodeVisible(bool visible)
1459 {
1460  _nodeVisible = visible;
1461  for(QMap<int, NodeItem*>::iterator iter=_nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
1462  {
1463  iter.value()->setVisible(_nodeVisible);
1464  }
1465  for(QMap<int, NodeItem*>::iterator iter=_gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
1466  {
1467  iter.value()->setVisible(_nodeVisible);
1468  }
1469  for(QMap<int, NodeItem*>::iterator iter=_gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter)
1470  {
1471  iter.value()->setVisible(_nodeVisible);
1472  }
1473 }
1474 void GraphViewer::setNodeRadius(float radius)
1475 {
1476  _nodeRadius = radius;
1477  for(QMap<int, NodeItem*>::iterator iter=_nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
1478  {
1479  iter.value()->setRadius(_nodeRadius);
1480  }
1481  for(QMap<int, NodeItem*>::iterator iter=_gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
1482  {
1483  iter.value()->setRadius(_nodeRadius);
1484  }
1485  for(QMap<int, NodeItem*>::iterator iter=_gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter)
1486  {
1487  iter.value()->setRadius(_nodeRadius);
1488  }
1489 }
1490 void GraphViewer::setLinkWidth(float width)
1491 {
1492  _linkWidth = width;
1493  QList<QGraphicsItem*> items = this->scene()->items();
1494  for(int i=0; i<items.size(); ++i)
1495  {
1496  QGraphicsLineItem * line = qgraphicsitem_cast<QGraphicsLineItem *>(items[i]);
1497  if(line)
1498  {
1499  QPen pen = line->pen();
1500  pen.setWidthF(_linkWidth*100.0f);
1501  line->setPen(pen);
1502  }
1503  }
1504 }
1505 void GraphViewer::setNodeColor(const QColor & color)
1506 {
1507  _nodeColor = color;
1508  for(QMap<int, NodeItem*>::iterator iter=_nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
1509  {
1510  iter.value()->setColor(_nodeColor);
1511  }
1512 }
1513 void GraphViewer::setNodeOdomCacheColor(const QColor & color)
1514 {
1515  _nodeOdomCacheColor = color;
1516  for(QMap<int, NodeItem*>::iterator iter=_nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
1517  {
1518  if(iter.value()->zValue() == 24)
1519  {
1520  iter.value()->setColor(_nodeOdomCacheColor);
1521  }
1522  }
1523 }
1524 void GraphViewer::setCurrentGoalColor(const QColor & color)
1525 {
1526  _currentGoalColor = color;
1527 }
1528 void GraphViewer::setNeighborColor(const QColor & color)
1529 {
1530  _neighborColor = color;
1531  for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
1532  {
1533  if(iter.value()->linkType() == Link::kNeighbor)
1534  {
1535  iter.value()->setColor(_neighborColor);
1536  }
1537  }
1538 }
1539 void GraphViewer::setGlobalLoopClosureColor(const QColor & color)
1540 {
1541  _loopClosureColor = color;
1542  if(!_intraInterSessionColors)
1543  {
1544  for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
1545  {
1546  if(iter.value()->linkType() == Link::kGlobalClosure)
1547  {
1548  iter.value()->setColor(_loopClosureColor);
1549  iter.value()->setZValue(10);
1550  }
1551  }
1552  }
1553 }
1554 void GraphViewer::setLocalLoopClosureColor(const QColor & color)
1555 {
1556  _loopClosureLocalColor = color;
1557  if(!_intraInterSessionColors)
1558  {
1559  for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
1560  {
1561  if(iter.value()->linkType() == Link::kLocalSpaceClosure ||
1562  iter.value()->linkType() == Link::kLocalTimeClosure)
1563  {
1564  iter.value()->setColor(_loopClosureLocalColor);
1565  iter.value()->setZValue(10);
1566  }
1567  }
1568  }
1569 }
1570 void GraphViewer::setUserLoopClosureColor(const QColor & color)
1571 {
1572  _loopClosureUserColor = color;
1573  for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
1574  {
1575  if(iter.value()->linkType() == Link::kUserClosure)
1576  {
1577  iter.value()->setColor(_loopClosureUserColor);
1578  }
1579  }
1580 }
1581 void GraphViewer::setVirtualLoopClosureColor(const QColor & color)
1582 {
1583  _loopClosureVirtualColor = color;
1584  for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
1585  {
1586  if(iter.value()->linkType() == Link::kVirtualClosure)
1587  {
1588  iter.value()->setColor(_loopClosureVirtualColor);
1589  }
1590  }
1591 }
1592 void GraphViewer::setNeighborMergedColor(const QColor & color)
1593 {
1594  _neighborMergedColor = color;
1595  for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
1596  {
1597  if(iter.value()->linkType() == Link::kNeighborMerged)
1598  {
1599  iter.value()->setColor(_neighborMergedColor);
1600  }
1601  }
1602 }
1603 void GraphViewer::setLandmarkColor(const QColor & color)
1604 {
1605  _landmarkColor = color;
1606  for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
1607  {
1608  if(iter.value()->linkType() == Link::kLandmark)
1609  {
1610  iter.value()->setColor(_landmarkColor);
1611  }
1612  }
1613 }
1614 void GraphViewer::setRejectedLoopClosureColor(const QColor & color)
1615 {
1616  _loopClosureRejectedColor = color;
1617 }
1618 void GraphViewer::setLocalPathColor(const QColor & color)
1619 {
1620  _localPathColor = color;
1621 }
1622 void GraphViewer::setGlobalPathColor(const QColor & color)
1623 {
1624  _globalPathColor = color;
1625 }
1626 void GraphViewer::setGTColor(const QColor & color)
1627 {
1628  _gtPathColor = color;
1629  for(QMap<int, NodeItem*>::iterator iter=_gtNodeItems.begin(); iter!=_gtNodeItems.end(); ++iter)
1630  {
1631  iter.value()->setColor(_gtPathColor);
1632  }
1633  for(QMultiMap<int, LinkItem*>::iterator iter=_gtLinkItems.begin(); iter!=_gtLinkItems.end(); ++iter)
1634  {
1635  iter.value()->setColor(_gtPathColor);
1636  }
1637 }
1638 void GraphViewer::setGPSColor(const QColor & color)
1639 {
1640  _gpsPathColor = color;
1641  for(QMap<int, NodeItem*>::iterator iter=_gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter)
1642  {
1643  iter.value()->setColor(_gpsPathColor);
1644  }
1645  for(QMultiMap<int, LinkItem*>::iterator iter=_gpsLinkItems.begin(); iter!=_gpsLinkItems.end(); ++iter)
1646  {
1647  iter.value()->setColor(_gpsPathColor);
1648  }
1649 }
1650 void GraphViewer::setHighlightColor(const QColor & color, int index)
1651 {
1652  if(index<0 || index > _highlightedNodes.size())
1653  {
1654  UERROR("Unsupported highlight color index %d", index);
1655  return;
1656  }
1657  _highlightedNodes[index].first = color;
1658  NodeItem * node = _highlightedNodes[index].second;
1659  if(node != nullptr)
1660  {
1661  node->setColor(color);
1662  }
1663 }
1664 void GraphViewer::setIntraSessionLoopColor(const QColor & color)
1665 {
1666  _loopIntraSessionColor = color;
1667  if(_intraInterSessionColors)
1668  {
1669  for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
1670  {
1671  if((iter.value()->linkType() == Link::kGlobalClosure ||
1672  iter.value()->linkType() == Link::kLocalSpaceClosure ||
1673  iter.value()->linkType() == Link::kLocalTimeClosure ||
1674  iter.value()->linkType() == Link::kUserClosure) &&
1675  !iter.value()->isInterSession())
1676  {
1677  iter.value()->setColor(_loopIntraSessionColor);
1678  iter.value()->setZValue(9);
1679  }
1680  }
1681  }
1682 }
1683 void GraphViewer::setInterSessionLoopColor(const QColor & color)
1684 {
1685  _loopInterSessionColor = color;
1686  if(_intraInterSessionColors)
1687  {
1688  for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
1689  {
1690  if((iter.value()->linkType() == Link::kGlobalClosure ||
1691  iter.value()->linkType() == Link::kLocalSpaceClosure ||
1692  iter.value()->linkType() == Link::kLocalTimeClosure ||
1693  iter.value()->linkType() == Link::kUserClosure) &&
1694  iter.value()->isInterSession())
1695  {
1696  iter.value()->setColor(_loopInterSessionColor);
1697  iter.value()->setZValue(8);
1698  }
1699  }
1700  }
1701 }
1702 
1703 void GraphViewer::setIntraInterSessionColorsEnabled(bool enabled)
1704 {
1705  _intraInterSessionColors = enabled;
1706  if(_intraInterSessionColors)
1707  {
1708  this->setIntraSessionLoopColor(_loopIntraSessionColor);
1709  this->setInterSessionLoopColor(_loopInterSessionColor);
1710  }
1711  else
1712  {
1713  this->setGlobalLoopClosureColor(_loopClosureColor);
1714  this->setLocalLoopClosureColor(_loopClosureLocalColor);
1715  this->setUserLoopClosureColor(_loopClosureUserColor);
1716  }
1717 }
1718 
1719 void GraphViewer::setGridMapVisible(bool visible)
1720 {
1721  if(visible && _viewPlane!=XY)
1722  {
1723  UWARN("Grid map can be shown only with view plane is XY.");
1724  }
1725  _gridMap->setVisible(_viewPlane==XY && visible);
1726 }
1727 void GraphViewer::setOriginVisible(bool visible)
1728 {
1729  _originReferential->setVisible(visible);
1730 }
1731 void GraphViewer::setReferentialVisible(bool visible)
1732 {
1733  _referential->setVisible(visible);
1734 }
1735 void GraphViewer::setLocalRadiusVisible(bool visible)
1736 {
1737  _localRadius->setVisible(visible);
1738 }
1739 void GraphViewer::setLoopClosureOutlierThr(float value)
1740 {
1741  _loopClosureOutlierThr = value;
1742 }
1743 void GraphViewer::setMaxLinkLength(float value)
1744 {
1745  _maxLinkLength = value;
1746 }
1747 void GraphViewer::setGraphVisible(bool visible)
1748 {
1749  _graphRoot->setVisible(visible);
1750 }
1751 void GraphViewer::setGlobalPathVisible(bool visible)
1752 {
1753  _globalPathRoot->setVisible(visible);
1754 }
1755 void GraphViewer::setLocalPathVisible(bool visible)
1756 {
1757  _localPathRoot->setVisible(visible);
1758 }
1759 void GraphViewer::setGtGraphVisible(bool visible)
1760 {
1761  _gtGraphRoot->setVisible(visible);
1762 }
1763 void GraphViewer::setGPSGraphVisible(bool visible)
1764 {
1765  _gpsGraphRoot->setVisible(visible);
1766 }
1767 void GraphViewer::setOdomCacheOverlayVisible(bool visible)
1768 {
1769  _odomCacheOverlay->setVisible(visible);
1770 }
1771 void GraphViewer::setOrientationENU(bool enabled)
1772 {
1773  if(enabled && _viewPlane!=XY)
1774  {
1775  UWARN("ENU orientation can be set only with view plane is XY.");
1776  }
1777  enabled = _viewPlane==XY && enabled;
1778  if(_orientationENU!=enabled)
1779  {
1780  _orientationENU = enabled;
1781  this->rotate(_orientationENU?90:270);
1782  }
1783  QTransform t;
1784  t.rotateRadians(_worldMapRotation);
1785  _root->setTransform(t);
1786  if(_nodeItems.size() || _linkItems.size())
1787  {
1788  this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
1789  }
1790 }
1791 
1792 void GraphViewer::setViewPlane(ViewPlane plane)
1793 {
1794  if(plane != XY)
1795  {
1796  setOrientationENU(false);
1797  setGridMapVisible(false);
1798  }
1799  _viewPlane = plane;
1800 
1801  for(QMap<int, NodeItem*>::iterator iter=_nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
1802  {
1803  iter.value()->setPose(iter.value()->pose(), _viewPlane);
1804  }
1805  for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
1806  {
1807  iter.value()->setPoses(iter.value()->getPoseA(), iter.value()->getPoseB(), _viewPlane);
1808  }
1809 
1810  _originReferentialXY->setVisible(plane==XY);
1811  _originReferentialXZ->setVisible(plane==XZ);
1812  _originReferentialYZ->setVisible(plane==YZ);
1813  _referentialXY->setVisible(plane==XY);
1814  _referentialXZ->setVisible(plane==XZ);
1815  _referentialYZ->setVisible(plane==YZ);
1816 
1817  if(_nodeItems.size() || _linkItems.size())
1818  {
1819  this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
1820  }
1821 }
1822 void GraphViewer::setEnsureFrameVisible(bool visible)
1823 {
1824  _ensureFrameVisible = visible;
1825 }
1826 
1827 
1828 void GraphViewer::restoreDefaults()
1829 {
1830  setNodeRadius(0.01f);
1831  setLinkWidth(0.0f);
1832  setNodeColor(Qt::blue);
1833  setNodeOdomCacheColor(Qt::darkGreen);
1834  setCurrentGoalColor(Qt::darkMagenta);
1835  setHighlightColor(Qt::magenta, 0);
1836  setHighlightColor(Qt::yellow, 1);
1837  setNeighborColor(Qt::blue);
1838  setGlobalLoopClosureColor(Qt::red);
1839  setLocalLoopClosureColor(Qt::yellow);
1840  setUserLoopClosureColor(Qt::red);
1841  setVirtualLoopClosureColor(Qt::magenta);
1842  setNeighborMergedColor(QColor(255,170,0));
1843  setRejectedLoopClosureColor(Qt::black);
1844  setLandmarkColor(Qt::darkGreen);
1845  setLocalPathColor(Qt::cyan);
1846  setGlobalPathColor(Qt::darkMagenta);
1847  setGTColor(Qt::gray);
1848  setGPSColor(Qt::darkCyan);
1849  setIntraSessionLoopColor(Qt::red);
1850  setInterSessionLoopColor(Qt::green);
1851  setIntraInterSessionColorsEnabled(false);
1852  setGridMapVisible(true);
1853  setGraphVisible(true);
1854  setGlobalPathVisible(true);
1855  setLocalPathVisible(true);
1856  setGtGraphVisible(true);
1857 }
1858 
1859 void GraphViewer::wheelEvent ( QWheelEvent * event )
1860 {
1861  if(event->angleDelta().y() < 0)
1862  {
1863  this->scale(0.95, 0.95);
1864  }
1865  else
1866  {
1867  this->scale(1.05, 1.05);
1868  }
1869 }
1870 
1871 void GraphViewer::mouseMoveEvent(QMouseEvent * event)
1872 {
1873  QPointF scenePoint = mapToScene(event->pos());
1874  if(_mouseTracking && _viewPlane==XY && this->sceneRect().contains(scenePoint))
1875  {
1876 #if QT_VERSION >= QT_VERSION_CHECK(6, 0, 0)
1877  QToolTip::showText(event->globalPosition().toPoint(), QString("%1m %2m").arg(-scenePoint.y()/100.0).arg(-scenePoint.x()/100.0));
1878 #else
1879  QToolTip::showText(event->globalPos(), QString("%1m %2m").arg(-scenePoint.y()/100.0).arg(-scenePoint.x()/100.0));
1880 #endif
1881  }
1882  else
1883  {
1884  QToolTip::hideText();
1885  }
1886  QGraphicsView::mouseMoveEvent(event);
1887 }
1888 
1889 void GraphViewer::mousePressEvent(QMouseEvent * event)
1890 {
1891  QGraphicsItem *item = this->scene()->itemAt(mapToScene(event->pos()), QTransform());
1892  if(item)
1893  {
1894  NodeItem *nodeItem = qgraphicsitem_cast<NodeItem*>(item);
1895  LinkItem *linkItem = qgraphicsitem_cast<LinkItem*>(item);
1896  if(nodeItem && nodeItem->parentItem() == _graphRoot && nodeItem->id() != 0)
1897  {
1898  Q_EMIT nodeSelected(nodeItem->id());
1899  }
1900  else if(linkItem && linkItem->parentItem() == _graphRoot && linkItem->from() != 0 && linkItem->to() != 0)
1901  {
1902  Q_EMIT linkSelected(linkItem->from(), linkItem->to());
1903  }
1904  else
1905  {
1906  QGraphicsView::mousePressEvent(event);
1907  }
1908  }
1909  else
1910  {
1911  QGraphicsView::mousePressEvent(event);
1912  }
1913 }
1914 
1915 QIcon createIcon(const QColor & color)
1916 {
1917  QPixmap pixmap(50, 50);
1918  pixmap.fill(color);
1919  return QIcon(pixmap);
1920 }
1921 
1922 void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
1923 {
1924  QMenu menu;
1925  QAction * aScreenShot = menu.addAction(tr("Take a screenshot..."));
1926  QAction * aExportGridMap = menu.addAction(tr("Export grid map..."));
1927  aExportGridMap->setEnabled(!_gridMap->pixmap().isNull());
1928  menu.addSeparator();
1929 
1930  QAction * aChangeNodeColor = menu.addAction(createIcon(_nodeColor), tr("Set node color..."));
1931  QAction * aChangeNodeOdomCacheColor = menu.addAction(createIcon(_nodeOdomCacheColor), tr("Set node odom cache color..."));
1932  QAction * aChangeCurrentGoalColor = menu.addAction(createIcon(_currentGoalColor), tr("Set current goal color..."));
1933  QMenu * menuChangeHighlightingColors = menu.addMenu(tr("Set node highlighting colors..."));
1934  QVector<QAction*> aChangeHighlightingColors(_highlightedNodes.size());
1935  for(int i=0; i<_highlightedNodes.size(); ++i) {
1936  aChangeHighlightingColors[i] = menuChangeHighlightingColors->addAction(createIcon(_highlightedNodes[i].first), tr("Color %1...").arg(i+1));
1937  aChangeHighlightingColors[i]->setIconVisibleInMenu(true);
1938  }
1939  aChangeNodeColor->setIconVisibleInMenu(true);
1940  aChangeNodeOdomCacheColor->setIconVisibleInMenu(true);
1941  aChangeCurrentGoalColor->setIconVisibleInMenu(true);
1942 
1943  // Links
1944  QMenu * menuLink = menu.addMenu(tr("Set link color..."));
1945  QAction * aChangeNeighborColor = menuLink->addAction(tr("Neighbor"));
1946  QAction * aChangeGlobalLoopColor = menuLink->addAction(tr("Global loop closure"));
1947  QAction * aChangeLocalLoopColor = menuLink->addAction(tr("Local loop closure"));
1948  QAction * aChangeUserLoopColor = menuLink->addAction(tr("User loop closure"));
1949  QAction * aChangeVirtualLoopColor = menuLink->addAction(tr("Virtual loop closure"));
1950  QAction * aChangeNeighborMergedColor = menuLink->addAction(tr("Neighbor merged"));
1951  QAction * aChangeLandmarkColor = menuLink->addAction(tr("Landmark"));
1952  QAction * aChangeRejectedLoopColor = menuLink->addAction(tr("Outlier loop closure"));
1953  QAction * aChangeRejectedLoopThr = menuLink->addAction(tr("Set outlier threshold..."));
1954  QAction * aChangeLocalPathColor = menuLink->addAction(tr("Local path"));
1955  QAction * aChangeGlobalPathColor = menuLink->addAction(tr("Global path"));
1956  QAction * aChangeGTColor = menuLink->addAction(tr("Ground truth"));
1957  QAction * aChangeGPSColor = menuLink->addAction(tr("GPS"));
1958  menuLink->addSeparator();
1959  QAction * aSetIntraInterSessionColors = menuLink->addAction(tr("Enable intra/inter-session colors"));
1960  QAction * aChangeIntraSessionLoopColor = menuLink->addAction(tr("Intra-session loop closure"));
1961  QAction * aChangeInterSessionLoopColor = menuLink->addAction(tr("Inter-session loop closure"));
1962  aChangeNeighborColor->setIcon(createIcon(_neighborColor));
1963  aChangeGlobalLoopColor->setIcon(createIcon(_loopClosureColor));
1964  aChangeLocalLoopColor->setIcon(createIcon(_loopClosureLocalColor));
1965  aChangeUserLoopColor->setIcon(createIcon(_loopClosureUserColor));
1966  aChangeVirtualLoopColor->setIcon(createIcon(_loopClosureVirtualColor));
1967  aChangeNeighborMergedColor->setIcon(createIcon(_neighborMergedColor));
1968  aChangeLandmarkColor->setIcon(createIcon(_landmarkColor));
1969  aChangeRejectedLoopColor->setIcon(createIcon(_loopClosureRejectedColor));
1970  aChangeLocalPathColor->setIcon(createIcon(_localPathColor));
1971  aChangeGlobalPathColor->setIcon(createIcon(_globalPathColor));
1972  aChangeGTColor->setIcon(createIcon(_gtPathColor));
1973  aChangeGPSColor->setIcon(createIcon(_gpsPathColor));;
1974  aChangeIntraSessionLoopColor->setIcon(createIcon(_loopIntraSessionColor));
1975  aChangeInterSessionLoopColor->setIcon(createIcon(_loopInterSessionColor));
1976  aChangeNeighborColor->setIconVisibleInMenu(true);
1977  aChangeGlobalLoopColor->setIconVisibleInMenu(true);
1978  aChangeLocalLoopColor->setIconVisibleInMenu(true);
1979  aChangeUserLoopColor->setIconVisibleInMenu(true);
1980  aChangeVirtualLoopColor->setIconVisibleInMenu(true);
1981  aChangeNeighborMergedColor->setIconVisibleInMenu(true);
1982  aChangeRejectedLoopColor->setIconVisibleInMenu(true);
1983  aChangeLocalPathColor->setIconVisibleInMenu(true);
1984  aChangeGlobalPathColor->setIconVisibleInMenu(true);
1985  aChangeGTColor->setIconVisibleInMenu(true);
1986  aChangeGPSColor->setIconVisibleInMenu(true);
1987  aChangeIntraSessionLoopColor->setIconVisibleInMenu(true);
1988  aChangeInterSessionLoopColor->setIconVisibleInMenu(true);
1989  aSetIntraInterSessionColors->setCheckable(true);
1990  aSetIntraInterSessionColors->setChecked(_intraInterSessionColors);
1991 
1992  menu.addSeparator();
1993  QAction * aSetNodeSize = menu.addAction(tr("Set node radius..."));
1994  QAction * aSetLinkSize = menu.addAction(tr("Set link width..."));
1995  QAction * aChangeMaxLinkLength = menu.addAction(tr("Set maximum link length..."));
1996  menu.addSeparator();
1997  QAction * aEnsureFrameVisible;
1998  QAction * aShowHideGridMap;
1999  QAction * aShowHideGraph;
2000  QAction * aShowHideGraphNodes;
2001  QAction * aShowHideOrigin;
2002  QAction * aShowHideReferential;
2003  QAction * aShowHideLocalRadius;
2004  QAction * aShowHideGlobalPath;
2005  QAction * aShowHideLocalPath;
2006  QAction * aShowHideGtGraph;
2007  QAction * aShowHideGPSGraph;
2008  QAction * aShowHideOdomCacheOverlay;
2009  QAction * aOrientationENU;
2010  QAction * aMouseTracking;
2011  QAction * aViewPlaneXY;
2012  QAction * aViewPlaneXZ;
2013  QAction * aViewPlaneYZ;
2014  aEnsureFrameVisible = menu.addAction(tr("Ensure Frame Visible"));
2015  aEnsureFrameVisible->setCheckable(true);
2016  aEnsureFrameVisible->setChecked(_ensureFrameVisible);
2017  if(_gridMap->isVisible())
2018  {
2019  aShowHideGridMap = menu.addAction(tr("Hide grid map"));
2020  }
2021  else
2022  {
2023  aShowHideGridMap = menu.addAction(tr("Show grid map"));
2024  }
2025  aShowHideGridMap->setEnabled(_viewPlane == XY);
2026  if(_originReferential->isVisible())
2027  {
2028  aShowHideOrigin = menu.addAction(tr("Hide origin referential"));
2029  }
2030  else
2031  {
2032  aShowHideOrigin = menu.addAction(tr("Show origin referential"));
2033  }
2034  if(_referential->isVisible())
2035  {
2036  aShowHideReferential = menu.addAction(tr("Hide current referential"));
2037  }
2038  else
2039  {
2040  aShowHideReferential = menu.addAction(tr("Show current referential"));
2041  }
2042  if(_localRadius->isVisible())
2043  {
2044  aShowHideLocalRadius = menu.addAction(tr("Hide local radius"));
2045  }
2046  else
2047  {
2048  aShowHideLocalRadius = menu.addAction(tr("Show local radius"));
2049  }
2050  if(_graphRoot->isVisible())
2051  {
2052  aShowHideGraph = menu.addAction(tr("Hide graph"));
2053  }
2054  else
2055  {
2056  aShowHideGraph = menu.addAction(tr("Show graph"));
2057  }
2058  if(_nodeVisible)
2059  {
2060  aShowHideGraphNodes = menu.addAction(tr("Hide graph nodes"));
2061  }
2062  else
2063  {
2064  aShowHideGraphNodes = menu.addAction(tr("Show graph nodes"));
2065  }
2066  if(_globalPathRoot->isVisible())
2067  {
2068  aShowHideGlobalPath = menu.addAction(tr("Hide global path"));
2069  }
2070  else
2071  {
2072  aShowHideGlobalPath = menu.addAction(tr("Show global path"));
2073  }
2074  if(_localPathRoot->isVisible())
2075  {
2076  aShowHideLocalPath = menu.addAction(tr("Hide local path"));
2077  }
2078  else
2079  {
2080  aShowHideLocalPath = menu.addAction(tr("Show local path"));
2081  }
2082  if(_gtGraphRoot->isVisible())
2083  {
2084  aShowHideGtGraph = menu.addAction(tr("Hide ground truth graph"));
2085  }
2086  else
2087  {
2088  aShowHideGtGraph = menu.addAction(tr("Show ground truth graph"));
2089  }
2090  if(_gpsGraphRoot->isVisible())
2091  {
2092  aShowHideGPSGraph = menu.addAction(tr("Hide GPS graph"));
2093  }
2094  else
2095  {
2096  aShowHideGPSGraph = menu.addAction(tr("Show GPS graph"));
2097  }
2098  if(_odomCacheOverlay->isVisible())
2099  {
2100  aShowHideOdomCacheOverlay = menu.addAction(tr("Hide odom cache overlay"));
2101  }
2102  else
2103  {
2104  aShowHideOdomCacheOverlay = menu.addAction(tr("Show odom cache overlay"));
2105  }
2106  aOrientationENU = menu.addAction(tr("ENU Orientation"));
2107  aOrientationENU->setCheckable(true);
2108  aOrientationENU->setChecked(_orientationENU);
2109  aMouseTracking = menu.addAction(tr("Show mouse cursor position (m)"));
2110  aMouseTracking->setCheckable(true);
2111  aMouseTracking->setChecked(_mouseTracking);
2112  aMouseTracking->setEnabled(_viewPlane == XY);
2113  aShowHideGraph->setEnabled(_nodeItems.size() && _viewPlane == XY);
2114  aShowHideGraphNodes->setEnabled(_nodeItems.size() && _graphRoot->isVisible());
2115  aShowHideGlobalPath->setEnabled(_globalPathLinkItems.size());
2116  aShowHideLocalPath->setEnabled(_localPathLinkItems.size());
2117  aShowHideGtGraph->setEnabled(_gtNodeItems.size());
2118  aShowHideGPSGraph->setEnabled(_gpsNodeItems.size());
2119  aShowHideOdomCacheOverlay->setEnabled(_odomCacheOverlay->rect().width()>0);
2120 
2121  QMenu * viewPlaneMenu = menu.addMenu("View Plane...");
2122  aViewPlaneXY = viewPlaneMenu->addAction("XY");
2123  aViewPlaneXY->setCheckable(true);
2124  aViewPlaneXY->setChecked(_viewPlane == XY);
2125  aViewPlaneXZ = viewPlaneMenu->addAction("XZ");
2126  aViewPlaneXZ->setCheckable(true);
2127  aViewPlaneXZ->setChecked(_viewPlane == XZ);
2128  aViewPlaneYZ = viewPlaneMenu->addAction("YZ");
2129  aViewPlaneYZ->setCheckable(true);
2130  aViewPlaneYZ->setChecked(_viewPlane == YZ);
2131 
2132  menu.addSeparator();
2133  QAction * aRestoreDefaults = menu.addAction(tr("Restore defaults"));
2134 
2135  QAction * r = menu.exec(event->globalPos());
2136  int aChangeHighlightingColorIndex = 0;
2137  for(int i=1; i<aChangeHighlightingColors.size(); ++i)
2138  {
2139  if(r == aChangeHighlightingColors[i]) {
2140  aChangeHighlightingColorIndex = i;
2141  }
2142  }
2143  if(r == aScreenShot)
2144  {
2145  if(_root)
2146  {
2147  QString filePath;
2148 #if QT_VERSION >= 0x050000
2149  filePath = QStandardPaths::writableLocation(QStandardPaths::PicturesLocation);
2150 #endif
2151  QDir dir;
2152  if(!dir.exists(filePath))
2153  {
2154  filePath = QDir::homePath();
2155  }
2156  filePath += "/graph.png";
2157 
2158 #ifdef QT_SVG_LIB
2159  filePath = QFileDialog::getSaveFileName(this, tr("Save figure to ..."), filePath, "*.png *.xpm *.jpg *.pdf *.svg");
2160 #else
2161  filePath = QFileDialog::getSaveFileName(this, tr("Save figure to ..."), filePath, "*.png *.xpm *.jpg *.pdf");
2162 #endif
2163  if(!filePath.isEmpty())
2164  {
2165  if(QFileInfo(filePath).suffix() == "")
2166  {
2167  //use png by default
2168  filePath += ".png";
2169  }
2170 
2171  if(_gridCellSize)
2172  {
2173  _root->setScale(1.0f/(_gridCellSize*100.0f)); // grid map precision (for 5cm grid cell, x20 to have 1pix/5cm)
2174  }
2175  else
2176  {
2177  _root->setScale(this->transform().m11()); // current view
2178  }
2179 
2180  this->scene()->clearSelection(); // Selections would also render to the file
2181  this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
2182  QSize sceneSize = this->scene()->sceneRect().size().toSize();
2183 
2184  if(QFileInfo(filePath).suffix().compare("pdf") == 0)
2185  {
2186  QPrinter printer(QPrinter::HighResolution);
2187  printer.setPageOrientation(QPageLayout::Portrait);
2188  printer.setOutputFileName( filePath );
2189  QPainter p(&printer);
2190  scene()->render(&p);
2191  p.end();
2192  }
2193  else if(QFileInfo(filePath).suffix().compare("svg") == 0)
2194  {
2195 #ifdef QT_SVG_LIB
2196  QSvgGenerator svgGen;
2197 
2198  svgGen.setFileName( filePath );
2199  svgGen.setSize(sceneSize);
2200  // add 1% border to make sure values are not cropped
2201  int borderH = sceneSize.width()/100;
2202  int borderV = sceneSize.height()/100;
2203  svgGen.setViewBox(QRect(-borderH, -borderV, sceneSize.width()+borderH*2, sceneSize.height()+borderV*2));
2204  svgGen.setTitle(tr("RTAB-Map graph"));
2205  svgGen.setDescription(tr("RTAB-Map map and graph"));
2206 
2207  QPainter painter( &svgGen );
2208 
2209  this->scene()->render(&painter);
2210 #else
2211  UERROR("RTAB-MAp is not built with Qt's SVG library, cannot save picture in svg format.");
2212 #endif
2213  }
2214  else
2215  {
2216  QImage image(sceneSize, QImage::Format_ARGB32); // Create the image with the exact size of the shrunk scene
2217  image.fill(Qt::transparent); // Start all pixels transparent
2218  QPainter painter(&image);
2219 
2220  this->scene()->render(&painter);
2221  if(!image.isNull())
2222  {
2223  image.save(filePath);
2224  }
2225  else
2226  {
2227  QMessageBox::warning(this,
2228  tr("Save PNG"),
2229  tr("Could not export in PNG (the scene may be too large %1x%2), try saving in SVG.").arg(sceneSize.width()).arg(sceneSize.height()));
2230  }
2231  }
2232 
2233  //reset scale
2234  _root->setScale(1.0f);
2235  this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
2236 
2237 
2238  QDesktopServices::openUrl(QUrl::fromLocalFile(filePath));
2239  }
2240  }
2241  return; // without emitting configChanged
2242  }
2243  else if(r == aExportGridMap)
2244  {
2245  float xMin, yMin, cellSize;
2246 
2247  cellSize = _gridCellSize;
2248  xMin = -_gridMap->y()/100.0f;
2249  yMin = -_gridMap->x()/100.0f;
2250 
2251  QString path = QFileDialog::getSaveFileName(
2252  this,
2253  tr("Save File"),
2254  "map.pgm",
2255  tr("Map (*.pgm)"));
2256 
2257  if(!path.isEmpty())
2258  {
2259  if(QFileInfo(path).suffix() == "")
2260  {
2261  path += ".pgm";
2262  }
2263  _gridMap->pixmap().save(path);
2264 
2265  QFileInfo info(path);
2266  QString yaml = info.absolutePath() + "/" + info.baseName() + ".yaml";
2267 
2268  float occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
2269 
2270  std::ofstream file;
2271  file.open (yaml.toStdString());
2272  file << "image: " << info.baseName().toStdString() << ".pgm" << std::endl;
2273  file << "resolution: " << cellSize << std::endl;
2274  file << "origin: [" << xMin << ", " << yMin << ", 0.0]" << std::endl;
2275  file << "negate: 0" << std::endl;
2276  file << "occupied_thresh: " << occupancyThr << std::endl;
2277  file << "free_thresh: 0.196" << std::endl;
2278  file << std::endl;
2279  file.close();
2280 
2281 
2282  QMessageBox::information(this, tr("Export 2D map"), tr("Exported %1 and %2!").arg(path).arg(yaml));
2283  }
2284  }
2285  else if(r == aSetIntraInterSessionColors)
2286  {
2287  setIntraInterSessionColorsEnabled(aSetIntraInterSessionColors->isChecked());
2288  }
2289  else if(r == aChangeRejectedLoopThr)
2290  {
2291  bool ok;
2292  double value = QInputDialog::getDouble(this, tr("Loop closure outlier threshold"), tr("Value (m)"), _loopClosureOutlierThr, 0.0, 1000.0, 2, &ok);
2293  if(ok)
2294  {
2295  setLoopClosureOutlierThr(value);
2296  }
2297  }
2298  else if(r == aChangeMaxLinkLength)
2299  {
2300  bool ok;
2301  double value = QInputDialog::getDouble(this, tr("Maximum link length to be shown"), tr("Value (m)"), _maxLinkLength, 0.0, 1000.0, 3, &ok);
2302  if(ok)
2303  {
2304  setMaxLinkLength(value);
2305  }
2306  }
2307  else if(r == aChangeNodeColor ||
2308  r == aChangeNodeOdomCacheColor ||
2309  r == aChangeCurrentGoalColor ||
2310  r == aChangeHighlightingColors[aChangeHighlightingColorIndex] ||
2311  r == aChangeNeighborColor ||
2312  r == aChangeGlobalLoopColor ||
2313  r == aChangeLocalLoopColor ||
2314  r == aChangeUserLoopColor ||
2315  r == aChangeVirtualLoopColor ||
2316  r == aChangeNeighborMergedColor ||
2317  r == aChangeRejectedLoopColor ||
2318  r == aChangeLocalPathColor ||
2319  r == aChangeGlobalPathColor ||
2320  r == aChangeGTColor ||
2321  r == aChangeGPSColor ||
2322  r == aChangeIntraSessionLoopColor ||
2323  r == aChangeInterSessionLoopColor)
2324  {
2325  QColor color;
2326  if(r == aChangeNodeColor)
2327  {
2328  color = _nodeColor;
2329  }
2330  else if(r == aChangeNodeOdomCacheColor)
2331  {
2332  color = _nodeOdomCacheColor;
2333  }
2334  else if(r == aChangeCurrentGoalColor)
2335  {
2336  color = _currentGoalColor;
2337  }
2338  else if(r == aChangeHighlightingColors[aChangeHighlightingColorIndex])
2339  {
2340  color = _highlightedNodes[aChangeHighlightingColorIndex].first;
2341  }
2342  else if(r == aChangeGlobalLoopColor)
2343  {
2344  color = _loopClosureColor;
2345  }
2346  else if(r == aChangeLocalLoopColor)
2347  {
2348  color = _loopClosureLocalColor;
2349  }
2350  else if(r == aChangeUserLoopColor)
2351  {
2352  color = _loopClosureUserColor;
2353  }
2354  else if(r == aChangeVirtualLoopColor)
2355  {
2356  color = _loopClosureVirtualColor;
2357  }
2358  else if(r == aChangeNeighborMergedColor)
2359  {
2360  color = _neighborMergedColor;
2361  }
2362  else if(r == aChangeLandmarkColor)
2363  {
2364  color = _landmarkColor;
2365  }
2366  else if(r == aChangeRejectedLoopColor)
2367  {
2368  color = _loopClosureRejectedColor;
2369  }
2370  else if(r == aChangeLocalPathColor)
2371  {
2372  color = _localPathColor;
2373  }
2374  else if(r == aChangeGlobalPathColor)
2375  {
2376  color = _globalPathColor;
2377  }
2378  else if(r == aChangeGTColor)
2379  {
2380  color = _gtPathColor;
2381  }
2382  else if(r == aChangeGPSColor)
2383  {
2384  color = _gpsPathColor;
2385  }
2386  else if(r == aChangeIntraSessionLoopColor)
2387  {
2388  color = _loopIntraSessionColor;
2389  }
2390  else if(r == aChangeInterSessionLoopColor)
2391  {
2392  color = _loopInterSessionColor;
2393  }
2394  else //if(r == aChangeNeighborColor)
2395  {
2396  color = _neighborColor;
2397  }
2398  color = QColorDialog::getColor(color, this);
2399  if(color.isValid())
2400  {
2401 
2402  if(r == aChangeNodeColor)
2403  {
2404  this->setNodeColor(color);
2405  }
2406  else if(r == aChangeNodeOdomCacheColor)
2407  {
2408  this->setNodeOdomCacheColor(color);
2409  }
2410  else if(r == aChangeCurrentGoalColor)
2411  {
2412  this->setCurrentGoalColor(color);
2413  }
2414  else if(r == aChangeHighlightingColors[aChangeHighlightingColorIndex])
2415  {
2416  this->setHighlightColor(color, aChangeHighlightingColorIndex);
2417  }
2418  else if(r == aChangeGlobalLoopColor)
2419  {
2420  this->setGlobalLoopClosureColor(color);
2421  }
2422  else if(r == aChangeLocalLoopColor)
2423  {
2424  this->setLocalLoopClosureColor(color);
2425  }
2426  else if(r == aChangeUserLoopColor)
2427  {
2428  this->setUserLoopClosureColor(color);
2429  }
2430  else if(r == aChangeVirtualLoopColor)
2431  {
2432  this->setVirtualLoopClosureColor(color);
2433  }
2434  else if(r == aChangeNeighborMergedColor)
2435  {
2436  this->setNeighborMergedColor(color);
2437  }
2438  else if(r == aChangeLandmarkColor)
2439  {
2440  this->setLandmarkColor(color);
2441  }
2442  else if(r == aChangeRejectedLoopColor)
2443  {
2444  this->setRejectedLoopClosureColor(color);
2445  }
2446  else if(r == aChangeLocalPathColor)
2447  {
2448  this->setLocalPathColor(color);
2449  }
2450  else if(r == aChangeGlobalPathColor)
2451  {
2452  this->setGlobalPathColor(color);
2453  }
2454  else if(r == aChangeGTColor)
2455  {
2456  this->setGTColor(color);
2457  }
2458  else if(r == aChangeGPSColor)
2459  {
2460  this->setGPSColor(color);
2461  }
2462  else if(r == aChangeIntraSessionLoopColor)
2463  {
2464  this->setIntraSessionLoopColor(color);
2465  }
2466  else if(r == aChangeInterSessionLoopColor)
2467  {
2468  this->setInterSessionLoopColor(color);
2469  }
2470  else //if(r == aChangeNeighborColor)
2471  {
2472  this->setNeighborColor(color);
2473  }
2474  }
2475  else
2476  {
2477  return; // without emitting configChanged
2478  }
2479  }
2480  else if(r == aSetNodeSize)
2481  {
2482  bool ok;
2483  double value = QInputDialog::getDouble(this, tr("Node radius"), tr("Radius (m)"), _nodeRadius, 0.001, 100, 3, &ok);
2484  if(ok)
2485  {
2486  setNodeRadius(value);
2487  }
2488  }
2489  else if(r == aSetLinkSize)
2490  {
2491  bool ok;
2492  double value = QInputDialog::getDouble(this, tr("Link width"), tr("Width (m)"), _linkWidth, 0, 100, 2, &ok);
2493  if(ok)
2494  {
2495  setLinkWidth(value);
2496  }
2497  }
2498  else if(r == aEnsureFrameVisible)
2499  {
2500  this->setEnsureFrameVisible(!this->isEnsureFrameVisible());
2501  }
2502  else if(r == aShowHideGridMap)
2503  {
2504  this->setGridMapVisible(!this->isGridMapVisible());
2505  if(_gridMap->isVisible())
2506  {
2507  mapShownRequested();
2508  }
2509  }
2510  else if(r == aShowHideOrigin)
2511  {
2512  this->setOriginVisible(!this->isOriginVisible());
2513  }
2514  else if(r == aShowHideReferential)
2515  {
2516  this->setReferentialVisible(!this->isReferentialVisible());
2517  }
2518  else if(r == aShowHideLocalRadius)
2519  {
2520  this->setLocalRadiusVisible(!this->isLocalRadiusVisible());
2521  }
2522  else if(r == aRestoreDefaults)
2523  {
2524  this->restoreDefaults();
2525  }
2526  else if(r == aShowHideGraph)
2527  {
2528  this->setGraphVisible(!this->isGraphVisible());
2529  }
2530  else if(r == aShowHideGraphNodes)
2531  {
2532  this->setNodeVisible(!_nodeVisible);
2533  }
2534  else if(r == aShowHideGlobalPath)
2535  {
2536  this->setGlobalPathVisible(!this->isGlobalPathVisible());
2537  }
2538  else if(r == aShowHideLocalPath)
2539  {
2540  this->setLocalPathVisible(!this->isLocalPathVisible());
2541  }
2542  else if(r == aShowHideGtGraph)
2543  {
2544  this->setGtGraphVisible(!this->isGtGraphVisible());
2545  }
2546  else if(r == aShowHideGPSGraph)
2547  {
2548  this->setGPSGraphVisible(!this->isGPSGraphVisible());
2549  }
2550  else if(r == aShowHideOdomCacheOverlay)
2551  {
2552  this->setOdomCacheOverlayVisible(!this->isOdomCacheOverlayVisible());
2553  }
2554  else if(r == aOrientationENU)
2555  {
2556  this->setOrientationENU(!this->isOrientationENU());
2557  }
2558  else if(r == aMouseTracking)
2559  {
2560  _mouseTracking = aMouseTracking->isChecked();
2561  }
2562  else if(r == aViewPlaneXY)
2563  {
2564  this->setViewPlane(XY);
2565  }
2566  else if(r == aViewPlaneXZ)
2567  {
2568  this->setViewPlane(XZ);
2569  }
2570  else if(r == aViewPlaneYZ)
2571  {
2572  this->setViewPlane(YZ);
2573  }
2574 
2575  if(r)
2576  {
2577  Q_EMIT configChanged();
2578  }
2579 }
2580 
2581 } /* namespace rtabmap */
rtabmap::GraphViewer::getMaxLinkLength
float getMaxLinkLength() const
Definition: GraphViewer.h:117
rtabmap::GraphViewer::clearGraph
void clearGraph()
Definition: GraphViewer.cpp:1249
rtabmap::GraphViewer::_gridMap
QGraphicsPixmapItem * _gridMap
Definition: GraphViewer.h:223
rtabmap::GraphViewer::setGTColor
void setGTColor(const QColor &color)
Definition: GraphViewer.cpp:1626
compare
bool compare
rtabmap::GraphViewer::setGPSGraphVisible
void setGPSGraphVisible(bool visible)
Definition: GraphViewer.cpp:1763
rtabmap::GraphViewer::_worldMapRotation
float _worldMapRotation
Definition: GraphViewer.h:203
rtabmap::GraphViewer::setOdomCacheOverlayVisible
void setOdomCacheOverlayVisible(bool visible)
Definition: GraphViewer.cpp:1767
rtabmap::NodeItem::_line
QGraphicsLineItem * _line
Definition: GraphViewer.cpp:170
rtabmap::GraphViewer::_loopClosureUserColor
QColor _loopClosureUserColor
Definition: GraphViewer.h:191
rtabmap::GraphViewer::updateLocalPath
void updateLocalPath(const std::vector< int > &localPath)
Definition: GraphViewer.cpp:1155
rtabmap::GraphViewer::_loopInterSessionColor
QColor _loopInterSessionColor
Definition: GraphViewer.h:201
rtabmap::GraphViewer::setLocalPathColor
void setLocalPathColor(const QColor &color)
Definition: GraphViewer.cpp:1618
rtabmap::GraphViewer::setNodeColor
void setNodeColor(const QColor &color)
Definition: GraphViewer.cpp:1505
rtabmap::GraphViewer::_gpsLinkItems
QMultiMap< int, LinkItem * > _gpsLinkItems
Definition: GraphViewer.h:216
rtabmap::GraphViewer::setGraphVisible
void setGraphVisible(bool visible)
Definition: GraphViewer.cpp:1747
rtabmap::GraphViewer::getRejectedLoopClosureColor
const QColor & getRejectedLoopClosureColor() const
Definition: GraphViewer.h:104
rtabmap::GraphViewer::setGlobalPath
void setGlobalPath(const std::vector< std::pair< int, Transform > > &globalPath)
Definition: GraphViewer.cpp:1100
rtabmap::GraphViewer::_gpsNodeItems
QMap< int, NodeItem * > _gpsNodeItems
Definition: GraphViewer.h:214
rtabmap::GraphViewer::_root
QGraphicsItem * _root
Definition: GraphViewer.h:205
rtabmap::LinkItem::getPoseB
const Transform & getPoseB() const
Definition: GraphViewer.cpp:242
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
rtabmap::GraphViewer::_graphRoot
QGraphicsItem * _graphRoot
Definition: GraphViewer.h:206
rtabmap::GraphViewer::_world
QGraphicsItem * _world
Definition: GraphViewer.h:204
rtabmap::GraphViewer::setWorldMapRotation
void setWorldMapRotation(const float &theta)
Definition: GraphViewer.cpp:478
arg::arg
constexpr arg(const char *name=nullptr)
rtabmap::GraphViewer::_loopIntraSessionColor
QColor _loopIntraSessionColor
Definition: GraphViewer.h:200
rtabmap::GraphViewer::getNodeOdomCacheColor
const QColor & getNodeOdomCacheColor() const
Definition: GraphViewer.h:96
timer
rtabmap::LinkItem::hoverEnterEvent
virtual void hoverEnterEvent(QGraphicsSceneHoverEvent *event)
Definition: GraphViewer.cpp:253
rtabmap::NodeItem::setColor
void setColor(const QColor &color)
Definition: GraphViewer.cpp:95
b
Array< int, 3, 1 > b
rtabmap::NodeItem::~NodeItem
virtual ~NodeItem()
Definition: GraphViewer.cpp:93
rtabmap::GraphViewer::_workingDirectory
QString _workingDirectory
Definition: GraphViewer.h:184
rtabmap::GraphViewer::_nodeItems
QMap< int, NodeItem * > _nodeItems
Definition: GraphViewer.h:211
rtabmap::GraphViewer::_currentGoalColor
QColor _currentGoalColor
Definition: GraphViewer.h:187
rtabmap::GraphViewer::_originReferentialYZ
QGraphicsItemGroup * _originReferentialYZ
Definition: GraphViewer.h:231
this
this
glm::rotate
GLM_FUNC_DECL detail::tmat4x4< T, P > rotate(detail::tmat4x4< T, P > const &m, T const &angle, detail::tvec3< T, P > const &axis)
rtabmap::GraphViewer::_orientationENU
bool _orientationENU
Definition: GraphViewer.h:237
rtabmap::GraphViewer::_landmarkColor
QColor _landmarkColor
Definition: GraphViewer.h:194
rtabmap::GraphViewer::_globalPathLinkItems
QMultiMap< int, LinkItem * > _globalPathLinkItems
Definition: GraphViewer.h:218
rtabmap::NodeItem::id
int id() const
Definition: GraphViewer.cpp:118
rtabmap::GraphViewer::getViewPlane
ViewPlane getViewPlane() const
Definition: GraphViewer.cpp:1445
updated
updated
GeodeticCoords.h
rtabmap::GraphViewer::_originReferentialXY
QGraphicsItemGroup * _originReferentialXY
Definition: GraphViewer.h:229
rtabmap::GPS::altitude
const double & altitude() const
Definition: GPS.h:62
rtabmap::GPS
Definition: GPS.h:35
rtabmap::GraphViewer::isEnsureFrameVisible
bool isEnsureFrameVisible() const
Definition: GraphViewer.cpp:1449
rtabmap::GraphViewer::_globalPathColor
QColor _globalPathColor
Definition: GraphViewer.h:197
rtabmap::GraphViewer::isIntraInterSessionColorsEnabled
bool isIntraInterSessionColorsEnabled() const
Definition: GraphViewer.h:111
rtabmap::GraphViewer::_originReferentialXZ
QGraphicsItemGroup * _originReferentialXZ
Definition: GraphViewer.h:230
rtabmap::GraphViewer::getGTColor
const QColor & getGTColor() const
Definition: GraphViewer.h:107
rtabmap::GraphViewer::contextMenuEvent
virtual void contextMenuEvent(QContextMenuEvent *event)
Definition: GraphViewer.cpp:1922
rtabmap::GPS::error
const double & error() const
Definition: GPS.h:63
rtabmap::LinkItem::getPoseA
const Transform & getPoseA() const
Definition: GraphViewer.cpp:238
rtabmap::NodeItem
Definition: GraphViewer.cpp:70
rtabmap::NodeItem::mapId
int mapId() const
Definition: GraphViewer.cpp:119
rtabmap::NodeItem::_pose
Transform _pose
Definition: GraphViewer.cpp:169
true
#define true
Definition: ConvertUTF.c:57
rtabmap::GraphViewer::getVirtualLoopClosureColor
const QColor & getVirtualLoopClosureColor() const
Definition: GraphViewer.h:102
rtabmap::NodeGPSItem::NodeGPSItem
NodeGPSItem(int id, int mapId, const Transform &pose, float radius, const GPS &gps, GraphViewer::ViewPlane plane, float linkWidth)
Definition: GraphViewer.cpp:176
util3d.h
rtabmap::GraphViewer::XY
@ XY
Definition: GraphViewer.h:57
rtabmap::GraphViewer::_gtGraphRoot
QGraphicsItem * _gtGraphRoot
Definition: GraphViewer.h:209
rtabmap::LinkItem
Definition: GraphViewer.cpp:196
rtabmap::GraphViewer::GraphViewer
GraphViewer(QWidget *parent=0)
Definition: GraphViewer.cpp:284
UTimer.h
rtabmap::GraphViewer::_referentialYZ
QGraphicsItemGroup * _referentialYZ
Definition: GraphViewer.h:227
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::GraphViewer::mousePressEvent
virtual void mousePressEvent(QMouseEvent *event)
Definition: GraphViewer.cpp:1889
GraphViewer.h
rtabmap::NodeGPSItem
Definition: GraphViewer.cpp:173
rtabmap::GraphViewer::_localPathLinkItems
QMultiMap< int, LinkItem * > _localPathLinkItems
Definition: GraphViewer.h:217
rtabmap::GraphViewer::setLinkWidth
void setLinkWidth(float width)
Definition: GraphViewer.cpp:1490
fabs
Real fabs(const Real &a)
rtabmap::GraphViewer::setCurrentGoalID
void setCurrentGoalID(int id, const Transform &pose=Transform())
Definition: GraphViewer.cpp:1126
rtabmap::GraphViewer::_loopClosureOutlierThr
float _loopClosureOutlierThr
Definition: GraphViewer.h:235
rtabmap::GraphViewer::setVirtualLoopClosureColor
void setVirtualLoopClosureColor(const QColor &color)
Definition: GraphViewer.cpp:1581
rtabmap::GraphViewer::setLoopClosureOutlierThr
void setLoopClosureOutlierThr(float value)
Definition: GraphViewer.cpp:1739
rtabmap::GraphViewer::isGPSGraphVisible
bool isGPSGraphVisible() const
Definition: GraphViewer.cpp:1433
rtabmap::NodeItem::hoverLeaveEvent
virtual void hoverLeaveEvent(QGraphicsSceneHoverEvent *event)
Definition: GraphViewer.cpp:159
rtabmap::GraphViewer::setMaxLinkLength
void setMaxLinkLength(float value)
Definition: GraphViewer.cpp:1743
rtabmap::GraphViewer::updateMap
void updateMap(const cv::Mat &map8U, float resolution, float xMin, float yMin)
Definition: GraphViewer.cpp:1051
rtabmap::GraphViewer::_gpsGraphRoot
QGraphicsItem * _gpsGraphRoot
Definition: GraphViewer.h:210
rtabmap::GraphViewer::_mouseTracking
bool _mouseTracking
Definition: GraphViewer.h:238
rtabmap::GraphViewer::setIntraSessionLoopColor
void setIntraSessionLoopColor(const QColor &color)
Definition: GraphViewer.cpp:1664
rtabmap::GraphViewer::setNodeRadius
void setNodeRadius(float radius)
Definition: GraphViewer.cpp:1474
rtabmap::Transform::y
float & y()
Definition: Transform.h:93
rtabmap::GraphViewer::~GraphViewer
virtual ~GraphViewer()
Definition: GraphViewer.cpp:474
rtabmap::GraphViewer::_loopClosureLocalColor
QColor _loopClosureLocalColor
Definition: GraphViewer.h:190
rtabmap::GraphViewer::_linkWidth
float _linkWidth
Definition: GraphViewer.h:222
rtabmap::GraphViewer::_nodeVisible
bool _nodeVisible
Definition: GraphViewer.h:220
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:407
rtabmap::Transform::z
float & z()
Definition: Transform.h:94
rtabmap::LinkItem::~LinkItem
virtual ~LinkItem()
Definition: GraphViewer.cpp:211
rtabmap::NodeItem::_id
int _id
Definition: GraphViewer.cpp:166
rtabmap::LinkItem::_poseB
Transform _poseB
Definition: GraphViewer.cpp:279
rtabmap::LinkItem::_poseA
Transform _poseA
Definition: GraphViewer.cpp:278
rtabmap::GraphViewer::updateGTGraph
void updateGTGraph(const std::map< int, Transform > &poses)
Definition: GraphViewer.cpp:760
rtabmap::Transform::getEulerAngles
void getEulerAngles(float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:263
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:326
rtabmap::NodeItem::pose
const Transform & pose() const
Definition: GraphViewer.cpp:120
rtabmap::LinkItem::_to
int _to
Definition: GraphViewer.cpp:277
rtabmap::GraphViewer::setHighlightColor
void setHighlightColor(const QColor &color, int index)
Definition: GraphViewer.cpp:1650
scale
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics scale
rtabmap::GraphViewer::_globalPathRoot
QGraphicsItem * _globalPathRoot
Definition: GraphViewer.h:207
rtabmap::GraphViewer::_linkItems
QMultiMap< int, LinkItem * > _linkItems
Definition: GraphViewer.h:212
rtabmap::GraphViewer::isLocalPathVisible
bool isLocalPathVisible() const
Definition: GraphViewer.cpp:1425
rtabmap::GraphViewer::updateReferentialPosition
void updateReferentialPosition(const Transform &t)
Definition: GraphViewer.cpp:1031
rtabmap::GPS::bearing
const double & bearing() const
Definition: GPS.h:64
rtabmap::NodeItem::setRadius
void setRadius(float radius)
Definition: GraphViewer.cpp:109
rtabmap::GraphViewer::_localRadius
QGraphicsEllipseItem * _localRadius
Definition: GraphViewer.h:233
rtabmap::LinkItem::linkType
Link::Type linkType() const
Definition: GraphViewer.cpp:247
rtabmap::GPS::latitude
const double & latitude() const
Definition: GPS.h:61
rtabmap::GPS::longitude
const double & longitude() const
Definition: GPS.h:60
rtabmap::Transform::x
float & x()
Definition: Transform.h:92
first
constexpr int first(int i)
rtabmap::GraphViewer::_neighborColor
QColor _neighborColor
Definition: GraphViewer.h:188
arg
rtabmap::GraphViewer::mapShownRequested
void mapShownRequested()
rtabmap::GraphViewer::setLocalRadius
void setLocalRadius(float radius)
Definition: GraphViewer.cpp:1150
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
UCv2Qt.h
rtabmap::GraphViewer::getGlobalLoopClosureColor
const QColor & getGlobalLoopClosureColor() const
Definition: GraphViewer.h:99
rtabmap::GraphViewer::YZ
@ YZ
Definition: GraphViewer.h:57
rtabmap::GraphViewer::setWorkingDirectory
void setWorkingDirectory(const QString &path)
Definition: GraphViewer.cpp:1454
rtabmap::NodeItem::hoverEnterEvent
virtual void hoverEnterEvent(QGraphicsSceneHoverEvent *event)
Definition: GraphViewer.cpp:145
rtabmap::GraphViewer::_intraInterSessionColors
bool _intraInterSessionColors
Definition: GraphViewer.h:202
rtabmap::GraphViewer::_gridCellSize
float _gridCellSize
Definition: GraphViewer.h:232
rtabmap::GraphViewer::setNodeVisible
void setNodeVisible(bool visible)
Definition: GraphViewer.cpp:1458
info
else if n * info
rtabmap::GraphViewer::highlightNode
void highlightNode(int nodeId, int highlightIndex)
Definition: GraphViewer.cpp:1223
contains
bool contains(const M &m, const typename M::key_type &k)
rtabmap::GraphViewer::_referentialXZ
QGraphicsItemGroup * _referentialXZ
Definition: GraphViewer.h:226
rtabmap::NodeItem::_weight
int _weight
Definition: GraphViewer.cpp:168
rtabmap::GraphViewer::isGraphVisible
bool isGraphVisible() const
Definition: GraphViewer.cpp:1417
UASSERT
#define UASSERT(condition)
rtabmap::LinkItem::LinkItem
LinkItem(int from, int to, const Transform &poseA, const Transform &poseB, const Link &link, bool interSessionClosure, GraphViewer::ViewPlane plane)
Definition: GraphViewer.cpp:200
rtabmap::GraphViewer::loadSettings
void loadSettings(QSettings &settings, const QString &group="")
Definition: GraphViewer.cpp:1351
rtabmap::GraphViewer::isOrientationENU
bool isOrientationENU() const
Definition: GraphViewer.cpp:1441
rtabmap::GraphViewer::updateGPSGraph
void updateGPSGraph(const std::map< int, Transform > &gpsMapPoses, const std::map< int, GPS > &gpsValues)
Definition: GraphViewer.cpp:901
iterator::value
object value
p
Point3_ p(2)
rtabmap::NodeGPSItem::hoverEnterEvent
virtual void hoverEnterEvent(QGraphicsSceneHoverEvent *event)
Definition: GraphViewer.cpp:183
rtabmap::GraphViewer::updatePosterior
void updatePosterior(const std::map< int, float > &posterior, float fixedMax=0.0f, int zValueOffset=0)
Definition: GraphViewer.cpp:1072
rtabmap::GraphViewer::setNeighborColor
void setNeighborColor(const QColor &color)
Definition: GraphViewer.cpp:1528
rtabmap::Transform::getDistance
float getDistance(const Transform &t) const
Definition: Transform.cpp:293
rtabmap::GraphViewer::getCurrentGoalColor
const QColor & getCurrentGoalColor() const
Definition: GraphViewer.h:97
rtabmap::GraphViewer::saveSettings
void saveSettings(QSettings &settings, const QString &group="") const
Definition: GraphViewer.cpp:1301
rtabmap::GraphViewer::setCurrentGoalColor
void setCurrentGoalColor(const QColor &color)
Definition: GraphViewer.cpp:1524
rtabmap::GraphViewer::setLandmarkColor
void setLandmarkColor(const QColor &color)
Definition: GraphViewer.cpp:1603
rtabmap::GraphViewer::setLocalRadiusVisible
void setLocalRadiusVisible(bool visible)
Definition: GraphViewer.cpp:1735
rtabmap::GraphViewer::isGridMapVisible
bool isGridMapVisible() const
Definition: GraphViewer.cpp:1401
rtabmap::GraphViewer::isReferentialVisible
bool isReferentialVisible() const
Definition: GraphViewer.cpp:1409
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::GraphViewer::getInterSessionLoopColor
const QColor & getInterSessionLoopColor() const
Definition: GraphViewer.h:110
rtabmap::GraphViewer::_referentialXY
QGraphicsItemGroup * _referentialXY
Definition: GraphViewer.h:225
rtabmap::GraphViewer::setRejectedLoopClosureColor
void setRejectedLoopClosureColor(const QColor &color)
Definition: GraphViewer.cpp:1614
path
path
str
UWARN
#define UWARN(...)
rtabmap::GraphViewer::getNodeRadius
float getNodeRadius() const
Definition: GraphViewer.h:93
rtabmap::GraphViewer::_gtNodeItems
QMap< int, NodeItem * > _gtNodeItems
Definition: GraphViewer.h:213
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::GraphViewer::linkSelected
void linkSelected(int, int)
rtabmap::GraphViewer::_odomCacheOverlay
QGraphicsRectItem * _odomCacheOverlay
Definition: GraphViewer.h:234
rtabmap::GraphViewer::_nodeRadius
float _nodeRadius
Definition: GraphViewer.h:221
rtabmap::LinkItem::_interSession
bool _interSession
Definition: GraphViewer.cpp:281
rtabmap::GraphViewer::_loopClosureRejectedColor
QColor _loopClosureRejectedColor
Definition: GraphViewer.h:195
ULogger.h
ULogger class and convenient macros.
rtabmap::GraphViewer::setLocalPathVisible
void setLocalPathVisible(bool visible)
Definition: GraphViewer.cpp:1755
rtabmap::GraphViewer::_viewPlane
ViewPlane _viewPlane
Definition: GraphViewer.h:239
rtabmap::GraphViewer::_loopClosureColor
QColor _loopClosureColor
Definition: GraphViewer.h:189
rtabmap::GraphViewer::getLocalPathColor
const QColor & getLocalPathColor() const
Definition: GraphViewer.h:105
rtabmap::GraphViewer::getLoopClosureOutlierThr
float getLoopClosureOutlierThr() const
Definition: GraphViewer.h:116
rtabmap::Transform
Definition: Transform.h:41
rtabmap::GraphViewer::_loopClosureVirtualColor
QColor _loopClosureVirtualColor
Definition: GraphViewer.h:192
rtabmap::createIcon
QIcon createIcon(const QColor &color)
Definition: GraphViewer.cpp:1915
rtabmap::GraphViewer::isOdomCacheOverlayVisible
bool isOdomCacheOverlayVisible() const
Definition: GraphViewer.cpp:1437
rtabmap::GraphViewer::setOriginVisible
void setOriginVisible(bool visible)
Definition: GraphViewer.cpp:1727
rtabmap::GraphViewer::ViewPlane
ViewPlane
Definition: GraphViewer.h:57
rtabmap::GraphViewer::setGlobalPathColor
void setGlobalPathColor(const QColor &color)
Definition: GraphViewer.cpp:1622
rtabmap::GraphViewer::getGlobalPathColor
const QColor & getGlobalPathColor() const
Definition: GraphViewer.h:106
rtabmap::GraphViewer::clearMap
void clearMap()
Definition: GraphViewer.cpp:1280
rtabmap::GraphViewer::setGridMapVisible
void setGridMapVisible(bool visible)
Definition: GraphViewer.cpp:1719
rtabmap::GraphViewer::_ensureFrameVisible
bool _ensureFrameVisible
Definition: GraphViewer.h:240
rtabmap::GraphViewer::getLinkWidth
float getLinkWidth() const
Definition: GraphViewer.h:94
rtabmap::GraphViewer::isLocalRadiusVisible
bool isLocalRadiusVisible() const
Definition: GraphViewer.cpp:1413
rtabmap::GraphViewer::setNeighborMergedColor
void setNeighborMergedColor(const QColor &color)
Definition: GraphViewer.cpp:1592
rtabmap::GraphViewer::_neighborMergedColor
QColor _neighborMergedColor
Definition: GraphViewer.h:193
rtabmap::GraphViewer::isGtGraphVisible
bool isGtGraphVisible() const
Definition: GraphViewer.cpp:1429
iter
iterator iter(handle obj)
rtabmap::GraphViewer::_originReferential
QGraphicsItemGroup * _originReferential
Definition: GraphViewer.h:228
rtabmap::GraphViewer::_maxLinkLength
float _maxLinkLength
Definition: GraphViewer.h:236
rtabmap::GraphViewer::setViewPlane
void setViewPlane(ViewPlane plane)
Definition: GraphViewer.cpp:1792
rtabmap::GraphViewer::getUserLoopClosureColor
const QColor & getUserLoopClosureColor() const
Definition: GraphViewer.h:101
rtabmap::GraphViewer::getNeighborMergedColor
const QColor & getNeighborMergedColor() const
Definition: GraphViewer.h:103
glm::cos
GLM_FUNC_DECL genType cos(genType const &angle)
rtabmap::GraphViewer::_gtPathColor
QColor _gtPathColor
Definition: GraphViewer.h:198
c_str
const char * c_str(Args &&...args)
UStl.h
Wrappers of STL for convenient functions.
v
Array< int, Dynamic, 1 > v
UDEBUG
#define UDEBUG(...)
rtabmap::GraphViewer::setGtGraphVisible
void setGtGraphVisible(bool visible)
Definition: GraphViewer.cpp:1759
rtabmap::NodeItem::_mapId
int _mapId
Definition: GraphViewer.cpp:167
rtabmap::GraphViewer::XZ
@ XZ
Definition: GraphViewer.h:57
rtabmap::GraphViewer::updateGraph
void updateGraph(const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints, const std::map< int, int > &mapIds, const std::map< int, int > &weights=std::map< int, int >(), const std::set< int > &odomCacheIds=std::set< int >())
Definition: GraphViewer.cpp:484
UTimer
Definition: UTimer.h:46
rtabmap::LinkItem::from
int from() const
Definition: GraphViewer.cpp:249
rtabmap::GraphViewer::_highlightedNodes
QVector< QPair< QColor, NodeItem * > > _highlightedNodes
Definition: GraphViewer.h:219
rtabmap::GraphViewer::isOriginVisible
bool isOriginVisible() const
Definition: GraphViewer.cpp:1405
rtabmap::GraphViewer::setIntraInterSessionColorsEnabled
void setIntraInterSessionColorsEnabled(bool enabled)
Definition: GraphViewer.cpp:1703
rtabmap::LinkItem::isInterSession
bool isInterSession() const
Definition: GraphViewer.cpp:248
rtabmap::GraphViewer::setNodeOdomCacheColor
void setNodeOdomCacheColor(const QColor &color)
Definition: GraphViewer.cpp:1513
scan_step::second
@ second
rtabmap::GraphViewer::_localPathRoot
QGraphicsItem * _localPathRoot
Definition: GraphViewer.h:208
rtabmap::GraphViewer::getIntraSessionLoopColor
const QColor & getIntraSessionLoopColor() const
Definition: GraphViewer.h:109
rtabmap::LinkItem::setColor
void setColor(const QColor &color)
Definition: GraphViewer.cpp:213
rtabmap::GraphViewer::_gpsPathColor
QColor _gpsPathColor
Definition: GraphViewer.h:199
rtabmap::GraphViewer::setInterSessionLoopColor
void setInterSessionLoopColor(const QColor &color)
Definition: GraphViewer.cpp:1683
rtabmap::GraphViewer::_referential
QGraphicsItemGroup * _referential
Definition: GraphViewer.h:224
false
#define false
Definition: ConvertUTF.c:56
transform
EIGEN_DONT_INLINE void transform(const Quaternion< Scalar > &t, Data &data)
rtabmap::NodeGPSItem::_gps
GPS _gps
Definition: GraphViewer.cpp:193
uCvMat2QImage
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack, float depthMin=0, float depthMax=0)
Definition: UCv2Qt.h:47
plane
plane
rtabmap::LinkItem::to
int to() const
Definition: GraphViewer.cpp:250
rtabmap::GraphViewer::_nodeColor
QColor _nodeColor
Definition: GraphViewer.h:185
rtabmap::NodeGPSItem::~NodeGPSItem
virtual ~NodeGPSItem()
Definition: GraphViewer.cpp:181
rtabmap::GraphViewer::setLocalLoopClosureColor
void setLocalLoopClosureColor(const QColor &color)
Definition: GraphViewer.cpp:1554
rtabmap::GraphViewer::getNeighborColor
const QColor & getNeighborColor() const
Definition: GraphViewer.h:98
rtabmap::GraphViewer::getNodeColor
const QColor & getNodeColor() const
Definition: GraphViewer.h:95
rtabmap::GraphViewer::isGlobalPathVisible
bool isGlobalPathVisible() const
Definition: GraphViewer.cpp:1421
t
Point2 t(10, 10)
rtabmap::GraphViewer::clearPosterior
void clearPosterior()
Definition: GraphViewer.cpp:1287
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::GraphViewer::setEnsureFrameVisible
void setEnsureFrameVisible(bool visible)
Definition: GraphViewer.cpp:1822
rtabmap::GraphViewer::restoreDefaults
void restoreDefaults()
Definition: GraphViewer.cpp:1828
rtabmap::GraphViewer::wheelEvent
virtual void wheelEvent(QWheelEvent *event)
Definition: GraphViewer.cpp:1859
rtabmap::GraphViewer::_gtLinkItems
QMultiMap< int, LinkItem * > _gtLinkItems
Definition: GraphViewer.h:215
rtabmap::GraphViewer::mouseMoveEvent
virtual void mouseMoveEvent(QMouseEvent *event)
Definition: GraphViewer.cpp:1871
rtabmap::GraphViewer::setGlobalLoopClosureColor
void setGlobalLoopClosureColor(const QColor &color)
Definition: GraphViewer.cpp:1539
UERROR
#define UERROR(...)
rtabmap::GraphViewer::configChanged
void configChanged()
rtabmap::GraphViewer::_nodeOdomCacheColor
QColor _nodeOdomCacheColor
Definition: GraphViewer.h:186
rtabmap::GraphViewer::setReferentialVisible
void setReferentialVisible(bool visible)
Definition: GraphViewer.cpp:1731
rtabmap::GraphViewer::_localPathColor
QColor _localPathColor
Definition: GraphViewer.h:196
rtabmap::LinkItem::_link
Link _link
Definition: GraphViewer.cpp:280
file
file
rtabmap::LinkItem::hoverLeaveEvent
virtual void hoverLeaveEvent(QGraphicsSceneHoverEvent *event)
Definition: GraphViewer.cpp:267
value
value
rtabmap::NodeItem::NodeItem
NodeItem(int id, int mapId, const Transform &pose, float radius, int weight, GraphViewer::ViewPlane plane, float linkWidth)
Definition: GraphViewer.cpp:74
i
int i
rtabmap::LinkItem::_from
int _from
Definition: GraphViewer.cpp:276
rtabmap::GraphViewer::setGPSColor
void setGPSColor(const QColor &color)
Definition: GraphViewer.cpp:1638
rtabmap::LinkItem::setPoses
void setPoses(const Transform &poseA, const Transform &poseB, GraphViewer::ViewPlane plane)
Definition: GraphViewer.cpp:220
rtabmap::GraphViewer::setGlobalPathVisible
void setGlobalPathVisible(bool visible)
Definition: GraphViewer.cpp:1751
rtabmap::GraphViewer::getLocalLoopClosureColor
const QColor & getLocalLoopClosureColor() const
Definition: GraphViewer.h:100
rtabmap::GraphViewer::setUserLoopClosureColor
void setUserLoopClosureColor(const QColor &color)
Definition: GraphViewer.cpp:1570
glm::sin
GLM_FUNC_DECL genType sin(genType const &angle)
rtabmap::GraphViewer::nodeSelected
void nodeSelected(int)
rtabmap::GraphViewer::getGPSColor
const QColor & getGPSColor() const
Definition: GraphViewer.h:108
rtabmap::GraphViewer::setOrientationENU
void setOrientationENU(bool enabled)
Definition: GraphViewer.cpp:1771
rtabmap::GraphViewer::clearAll
void clearAll()
Definition: GraphViewer.cpp:1295
rtabmap::NodeItem::setPose
void setPose(const Transform &pose, GraphViewer::ViewPlane plane)
Definition: GraphViewer.cpp:121

rtabmap
Author(s): Mathieu Labbe
autogenerated on Sun Dec 1 2024 03:42:46