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