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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:10