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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:45:54