OdometryViewer.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 
29 
32 #include "rtabmap/core/util3d.h"
34 #include "rtabmap/core/Odometry.h"
37 #include "rtabmap/utilite/UCv2Qt.h"
38 
39 #include "rtabmap/gui/ImageView.h"
41 
42 #include <QPushButton>
43 #include <QSpinBox>
44 #include <QDoubleSpinBox>
45 #include <QCheckBox>
46 #include <QLabel>
47 #include <QHBoxLayout>
48 #include <QVBoxLayout>
49 #include <QApplication>
50 
51 namespace rtabmap {
52 
54  int maxClouds,
55  int decimation,
56  float voxelSize,
57  float maxDepth,
58  int qualityWarningThr,
59  QWidget * parent,
60  const ParametersMap & parameters) :
61  QDialog(parent),
62  imageView_(new ImageView(this)),
63  cloudView_(new CloudViewer(this)),
64  processingData_(false),
65  odomImageShow_(true),
66  odomImageDepthShow_(true),
67  lastOdomPose_(Transform::getIdentity()),
68  qualityWarningThr_(qualityWarningThr),
69  id_(0),
70  validDecimationValue_(1),
71  parameters_(parameters)
72 {
73 
74  qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
75 
77  imageView_->setMinimumSize(320, 240);
78  imageView_->setAlpha(255);
79 
81  cloudView_->setGridShown(true);
83 
84  QLabel * maxCloudsLabel = new QLabel("Max clouds", this);
85  QLabel * voxelLabel = new QLabel("Voxel", this);
86  QLabel * maxDepthLabel = new QLabel("Max depth", this);
87  QLabel * decimationLabel = new QLabel("Decimation", this);
88  maxCloudsSpin_ = new QSpinBox(this);
89  maxCloudsSpin_->setMinimum(0);
90  maxCloudsSpin_->setMaximum(100);
91  maxCloudsSpin_->setValue(maxClouds);
92  voxelSpin_ = new QDoubleSpinBox(this);
93  voxelSpin_->setMinimum(0);
94  voxelSpin_->setMaximum(1);
95  voxelSpin_->setDecimals(3);
96  voxelSpin_->setSingleStep(0.01);
97  voxelSpin_->setSuffix(" m");
98  voxelSpin_->setValue(voxelSize);
99  maxDepthSpin_ = new QDoubleSpinBox(this);
100  maxDepthSpin_->setMinimum(0);
101  maxDepthSpin_->setMaximum(100);
102  maxDepthSpin_->setDecimals(0);
103  maxDepthSpin_->setSingleStep(1);
104  maxDepthSpin_->setSuffix(" m");
105  maxDepthSpin_->setValue(maxDepth);
106  decimationSpin_ = new QSpinBox(this);
107  decimationSpin_->setMinimum(1);
108  decimationSpin_->setMaximum(16);
109  decimationSpin_->setValue(decimation);
110  cloudShown_ = new QCheckBox(this);
111  cloudShown_->setText("Cloud");
112  cloudShown_->setChecked(true);
113  scanShown_ = new QCheckBox(this);
114  scanShown_->setText("Scan");
115  scanShown_->setChecked(true);
116  featuresShown_ = new QCheckBox(this);
117  featuresShown_->setText("Features");
118  featuresShown_->setChecked(true);
119  timeLabel_ = new QLabel(this);
120  QPushButton * resetButton = new QPushButton("reset", this);
121  QPushButton * clearButton = new QPushButton("clear", this);
122  QPushButton * closeButton = new QPushButton("close", this);
123  connect(resetButton, SIGNAL(clicked()), this, SLOT(reset()));
124  connect(clearButton, SIGNAL(clicked()), this, SLOT(clear()));
125  connect(closeButton, SIGNAL(clicked()), this, SLOT(reject()));
126 
127  //layout
128  QHBoxLayout * layout = new QHBoxLayout();
129  layout->setContentsMargins(0,0,0,0);
130  layout->setSpacing(0);
131  layout->addWidget(imageView_,1);
132  layout->addWidget(cloudView_,1);
133 
134  QHBoxLayout * hlayout2 = new QHBoxLayout();
135  hlayout2->setContentsMargins(0,0,0,0);
136  hlayout2->addWidget(maxCloudsLabel);
137  hlayout2->addWidget(maxCloudsSpin_);
138  hlayout2->addWidget(voxelLabel);
139  hlayout2->addWidget(voxelSpin_);
140  hlayout2->addWidget(maxDepthLabel);
141  hlayout2->addWidget(maxDepthSpin_);
142  hlayout2->addWidget(decimationLabel);
143  hlayout2->addWidget(decimationSpin_);
144  hlayout2->addWidget(cloudShown_);
145  hlayout2->addWidget(scanShown_);
146  hlayout2->addWidget(featuresShown_);
147  hlayout2->addWidget(timeLabel_);
148  hlayout2->addStretch(1);
149  hlayout2->addWidget(resetButton);
150  hlayout2->addWidget(clearButton);
151  hlayout2->addWidget(closeButton);
152 
153  QVBoxLayout * vlayout = new QVBoxLayout(this);
154  vlayout->setContentsMargins(0,0,0,0);
155  vlayout->setSpacing(0);
156  vlayout->addLayout(layout, 1);
157  vlayout->addLayout(hlayout2);
158 
159  this->setLayout(vlayout);
160 }
161 
163 {
165  this->clear();
166  UDEBUG("");
167 }
168 
170 {
171  this->post(new OdometryResetEvent());
172 }
173 
175 {
176  addedClouds_.clear();
177  cloudView_->clear();
178 }
179 
181 {
182  processingData_ = true;
183  int quality = odom.info().reg.inliers;
184 
185  bool lost = false;
186  bool lostStateChanged = false;
187 
188  if(odom.pose().isNull())
189  {
190  UDEBUG("odom lost"); // use last pose
191  lostStateChanged = imageView_->getBackgroundColor() != Qt::darkRed;
192  imageView_->setBackgroundColor(Qt::darkRed);
193  cloudView_->setBackgroundColor(Qt::darkRed);
194 
195  lost = true;
196  }
197  else if(odom.info().reg.inliers>0 &&
200  {
201  UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().reg.inliers, qualityWarningThr_);
202  lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
203  imageView_->setBackgroundColor(Qt::darkYellow);
204  cloudView_->setBackgroundColor(Qt::darkYellow);
205  }
206  else
207  {
208  UDEBUG("odom ok");
209  lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
211  cloudView_->setBackgroundColor(Qt::black);
212  }
213 
214  timeLabel_->setText(QString("%1 s").arg(odom.info().timeEstimation));
215 
216  if(cloudShown_->isChecked() &&
217  !odom.data().imageRaw().empty() &&
218  !odom.data().depthOrRightRaw().empty() &&
219  (odom.data().stereoCameraModels().size() || odom.data().cameraModels().size()))
220  {
221  UDEBUG("New pose = %s, quality=%d", odom.pose().prettyPrint().c_str(), quality);
222 
223  if(!odom.data().depthRaw().empty())
224  {
225  if(odom.data().imageRaw().cols % decimationSpin_->value() == 0 &&
226  odom.data().imageRaw().rows % decimationSpin_->value() == 0)
227  {
229  }
230  else
231  {
232  UWARN("Decimation (%d) must be a denominator of the width and height of "
233  "the image (%d/%d). Using last valid decimation value (%d).",
234  decimationSpin_->value(),
235  odom.data().imageRaw().cols,
236  odom.data().imageRaw().rows,
238  }
239  }
240  else
241  {
243  }
244 
245  // visualization: buffering the clouds
246  // Create the new cloud
247  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
248  pcl::IndicesPtr validIndices(new std::vector<int>);
250  odom.data(),
252  0,
253  0,
254  validIndices.get(),
255  parameters_);
256 
257  if(voxelSpin_->value())
258  {
259  cloud = util3d::voxelize(cloud, validIndices, voxelSpin_->value());
260  }
261 
262  if(cloud->size())
263  {
264  if(!odom.pose().isNull())
265  {
266  if(cloudView_->getAddedClouds().contains("cloudtmp"))
267  {
268  cloudView_->removeCloud("cloudtmp");
269  }
270 
271  while(maxCloudsSpin_->value()>0 && (int)addedClouds_.size() > maxCloudsSpin_->value())
272  {
274  addedClouds_.pop_front();
275  }
276 
277  odom.data().id()?id_=odom.data().id():++id_;
278  std::string cloudName = uFormat("cloud%d", id_);
279  addedClouds_.push_back(cloudName);
280  UASSERT(cloudView_->addCloud(cloudName, cloud, odom.pose()));
281  }
282  else
283  {
284  cloudView_->addCloud("cloudtmp", cloud, lastOdomPose_);
285  }
286  }
287  }
288  else if(!cloudShown_->isChecked())
289  {
290  while(!addedClouds_.empty())
291  {
293  addedClouds_.pop_front();
294  }
295  }
296 
297  if(!odom.pose().isNull())
298  {
299  lastOdomPose_ = odom.pose();
301 
302  if(odom.data().cameraModels().size() && !odom.data().cameraModels()[0].localTransform().isNull())
303  {
305  }
306  else if(odom.data().stereoCameraModels().size() && !odom.data().stereoCameraModels()[0].localTransform().isNull())
307  {
309  }
310  }
311 
312  if(scanShown_->isChecked())
313  {
314  // scan local map
315  if(!odom.info().localScanMap.isEmpty())
316  {
317  pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
319  if(!cloudView_->addCloud("scanMapOdom", cloud, Transform::getIdentity(), Qt::blue))
320  {
321  UERROR("Adding scanMapOdom to viewer failed!");
322  }
323  else
324  {
325  cloudView_->setCloudVisibility("scanMapOdom", true);
326  cloudView_->setCloudOpacity("scanMapOdom", 0.5);
327  }
328  }
329  // scan cloud
330  if(!odom.data().laserScanRaw().isEmpty())
331  {
332  LaserScan scan = odom.data().laserScanRaw();
333 
334  pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
335  cloud = util3d::laserScanToPointCloudNormal(scan, odom.pose() * scan.localTransform());
336 
337  if(!cloudView_->addCloud("scanOdom", cloud, Transform::getIdentity(), Qt::magenta))
338  {
339  UERROR("Adding scanOdom to viewer failed!");
340  }
341  else
342  {
343  cloudView_->setCloudVisibility("scanOdom", true);
344  cloudView_->setCloudOpacity("scanOdom", 0.5);
345  }
346  }
347  }
348  else
349  {
350  cloudView_->removeCloud("scanMapOdom");
351  cloudView_->removeCloud("scanOdom");
352  }
353 
354  // 3d features
355  if(featuresShown_->isChecked())
356  {
357  if(!odom.info().localMap.empty() && !odom.pose().isNull())
358  {
359  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
360  cloud->resize(odom.info().localMap.size());
361  int i=0;
362  for(std::map<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
363  {
364  // filter very far features from current location
365  if(uNormSquared(iter->second.x-odom.pose().x(), iter->second.y-odom.pose().y(), iter->second.z-odom.pose().z()) < 50*50)
366  {
367  (*cloud)[i].x = iter->second.x;
368  (*cloud)[i].y = iter->second.y;
369  (*cloud)[i].z = iter->second.z;
370 
371  // green = inlier, yellow = outliers
372  bool inlier = odom.info().words.find(iter->first) != odom.info().words.end();
373  (*cloud)[i].r = inlier?0:255;
374  (*cloud)[i].g = 255;
375  (*cloud)[i++].b = 0;
376  }
377  }
378  cloud->resize(i);
379 
380  if(!cloudView_->addCloud("featuresOdom", cloud))
381  {
382  UERROR("Adding featuresOdom to viewer failed!");
383  }
384  else
385  {
386  cloudView_->setCloudVisibility("featuresOdom", true);
387  cloudView_->setCloudPointSize("featuresOdom", 3);
388  }
389  }
390  }
391  else
392  {
393  cloudView_->removeCloud("featuresOdom");
394  }
395 
396  if(!odom.data().imageRaw().empty())
397  {
398  if(odom.info().type == (int)Odometry::kTypeF2M || odom.info().type == (int)Odometry::kTypeORBSLAM)
399  {
400  imageView_->setFeatures(odom.info().words, odom.data().depthRaw(), Qt::yellow);
401  }
402  else if(odom.info().type == (int)Odometry::kTypeF2F ||
403  odom.info().type == (int)Odometry::kTypeViso2 ||
404  odom.info().type == (int)Odometry::kTypeFovis ||
405  odom.info().type == (int)Odometry::kTypeMSCKF)
406  {
407  std::vector<cv::KeyPoint> kpts;
408  cv::KeyPoint::convert(odom.info().newCorners, kpts, 7);
409  imageView_->setFeatures(kpts, odom.data().depthRaw(), Qt::red);
410  }
411 
413  if(lost)
414  {
415  if(lostStateChanged)
416  {
417  // save state
420  }
422  imageView_->setImageShown(true);
424  }
425  else
426  {
427  if(lostStateChanged)
428  {
429  // restore state
432  }
433 
436  {
438  }
439 
440  if( odom.info().type == Odometry::kTypeF2M ||
441  odom.info().type == (int)Odometry::kTypeORBSLAM ||
442  odom.info().type == (int)Odometry::kTypeMSCKF)
443  {
445  {
446  for(unsigned int i=0; i<odom.info().reg.matchesIDs.size(); ++i)
447  {
448  imageView_->setFeatureColor(odom.info().reg.matchesIDs[i], Qt::red); // outliers
449  }
450  for(unsigned int i=0; i<odom.info().reg.inliersIDs.size(); ++i)
451  {
452  imageView_->setFeatureColor(odom.info().reg.inliersIDs[i], Qt::green); // inliers
453  }
454  }
455  }
456  }
457  if((odom.info().type == (int)Odometry::kTypeF2F ||
458  odom.info().type == (int)Odometry::kTypeViso2 ||
459  odom.info().type == (int)Odometry::kTypeFovis) && odom.info().cornerInliers.size())
460  {
462  {
463  //draw lines
464  UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
465  for(unsigned int i=0; i<odom.info().cornerInliers.size(); ++i)
466  {
468  {
469  imageView_->setFeatureColor(odom.info().cornerInliers[i], Qt::green); // inliers
470  }
471  if(imageView_->isLinesShown())
472  {
474  odom.info().newCorners[odom.info().cornerInliers[i]].x,
475  odom.info().newCorners[odom.info().cornerInliers[i]].y,
476  odom.info().refCorners[odom.info().cornerInliers[i]].x,
477  odom.info().refCorners[odom.info().cornerInliers[i]].y,
478  Qt::blue);
479  }
480  }
481  }
482  }
483 
484  if(!odom.data().imageRaw().empty())
485  {
486  imageView_->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows));
487  }
488  }
489 
490  imageView_->update();
491  cloudView_->update();
492  QApplication::processEvents();
493  processingData_ = false;
494 }
495 
497 {
498  if(!processingData_ && this->isVisible())
499  {
500  if(event->getClassName().compare("OdometryEvent") == 0)
501  {
502  rtabmap::OdometryEvent * odomEvent = (rtabmap::OdometryEvent*)event;
503  if(odomEvent->data().isValid())
504  {
505  processingData_ = true;
506  QMetaObject::invokeMethod(this, "processData",
507  Q_ARG(rtabmap::OdometryEvent, *odomEvent));
508  }
509  }
510  }
511  return false;
512 }
513 
514 } /* namespace rtabmap */
rtabmap::OdometryInfo::refCorners
std::vector< cv::Point2f > refCorners
Definition: OdometryInfo.h:133
rtabmap::OdometryViewer::odomImageDepthShow_
bool odomImageDepthShow_
Definition: OdometryViewer.h:78
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:127
rtabmap::OdometryEvent
Definition: OdometryEvent.h:39
rtabmap::CloudViewer::updateCameraTargetPosition
void updateCameraTargetPosition(const Transform &pose)
Definition: CloudViewer.cpp:2979
rtabmap::OdometryViewer::handleEvent
virtual bool handleEvent(UEvent *event)
Definition: OdometryViewer.cpp:496
rtabmap::OdometryViewer::processData
void processData(const rtabmap::OdometryEvent &odom)
Definition: OdometryViewer.cpp:180
ImageView.h
rtabmap::ImageView::setImageDepthShown
void setImageDepthShown(bool shown)
Definition: ImageView.cpp:466
rtabmap::OdometryEvent::pose
const Transform & pose() const
Definition: OdometryEvent.h:71
rtabmap::CloudViewer::setBackgroundColor
void setBackgroundColor(const QColor &color)
Definition: CloudViewer.cpp:3175
rtabmap::RegistrationInfo::inliersIDs
std::vector< int > inliersIDs
Definition: RegistrationInfo.h:84
rtabmap::CloudViewer::addCloud
bool addCloud(const std::string &id, const pcl::PCLPointCloud2Ptr &binaryCloud, const Transform &pose, bool rgb, bool hasNormals, bool hasIntensity, const QColor &color=QColor(), int viewport=1)
Definition: CloudViewer.cpp:745
rtabmap::OdometryInfo::localMap
std::map< int, cv::Point3f > localMap
Definition: OdometryInfo.h:129
rtabmap::OdometryViewer::id_
int id_
Definition: OdometryViewer.h:82
rtabmap::CloudViewer::setCloudOpacity
void setCloudOpacity(const std::string &id, double opacity=1.0)
Definition: CloudViewer.cpp:3249
rtabmap::OdometryInfo::timeEstimation
float timeEstimation
Definition: OdometryInfo.h:111
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
this
this
rtabmap::OdometryViewer::OdometryViewer
OdometryViewer(int maxClouds=10, int decimation=2, float voxelSize=0.0f, float maxDepth=0, int qualityWarningThr=0, QWidget *parent=0, const ParametersMap &parameters=ParametersMap())
Definition: OdometryViewer.cpp:53
rtabmap::ImageView::addLine
void addLine(float x1, float y1, float x2, float y2, QColor color, const QString &text=QString())
Definition: ImageView.cpp:1219
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:613
rtabmap::SensorData::isValid
bool isValid() const
Definition: SensorData.h:156
rtabmap::OdometryViewer::validDecimationValue_
int validDecimationValue_
Definition: OdometryViewer.h:93
rtabmap::ImageView::clearLines
void clearLines()
Definition: ImageView.cpp:1429
rtabmap::OdometryViewer::parameters_
ParametersMap parameters_
Definition: OdometryViewer.h:94
rtabmap::ImageView::setSceneRect
void setSceneRect(const QRectF &rect)
Definition: ImageView.cpp:1404
rtabmap::ImageView::setImageShown
void setImageShown(bool shown)
Definition: ImageView.cpp:451
rtabmap::Odometry::kTypeMSCKF
@ kTypeMSCKF
Definition: Odometry.h:55
true
#define true
Definition: ConvertUTF.c:57
util3d.h
rtabmap::OdometryViewer::voxelSpin_
QDoubleSpinBox * voxelSpin_
Definition: OdometryViewer.h:86
rtabmap::LaserScan
Definition: LaserScan.h:37
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:240
rtabmap::CloudViewer::getDefaultBackgroundColor
const QColor & getDefaultBackgroundColor() const
Definition: CloudViewer.cpp:3156
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:130
rtabmap::OdometryViewer::cloudView_
CloudViewer * cloudView_
Definition: OdometryViewer.h:75
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:241
rtabmap::OdometryViewer::scanShown_
QCheckBox * scanShown_
Definition: OdometryViewer.h:90
rtabmap::Odometry::kTypeViso2
@ kTypeViso2
Definition: Odometry.h:50
rtabmap::OdometryViewer::odomImageShow_
bool odomImageShow_
Definition: OdometryViewer.h:77
rtabmap::ImageView::setBackgroundColor
void setBackgroundColor(const QColor &color)
Definition: ImageView.cpp:705
UEvent
Definition: UEvent.h:57
Odometry.h
rtabmap::OdometryInfo::type
int type
Definition: OdometryInfo.h:125
rtabmap::CloudViewer
Definition: CloudViewer.h:79
rtabmap::OdometryViewer::qualityWarningThr_
int qualityWarningThr_
Definition: OdometryViewer.h:81
rtabmap::Transform::y
float & y()
Definition: Transform.h:93
rtabmap::Transform::z
float & z()
Definition: Transform.h:94
rtabmap::util3d::cloudRGBFromSensorData
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1268
rtabmap::ImageView::setImage
void setImage(const QImage &image, const std::vector< CameraModel > &models=std::vector< CameraModel >(), const Transform &pose=Transform())
Definition: ImageView.cpp:1235
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:326
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:185
rtabmap::OdometryInfo::cornerInliers
std::vector< int > cornerInliers
Definition: OdometryInfo.h:135
util3d_transforms.h
UEvent::getClassName
virtual std::string getClassName() const =0
UConversion.h
Some conversion functions.
rtabmap::OdometryViewer::decimationSpin_
QSpinBox * decimationSpin_
Definition: OdometryViewer.h:87
CloudViewer.h
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:174
util3d_filtering.h
rtabmap::Odometry::kTypeFovis
@ kTypeFovis
Definition: Odometry.h:49
rtabmap::OdometryViewer::imageView_
ImageView * imageView_
Definition: OdometryViewer.h:74
rtabmap::Transform::x
float & x()
Definition: Transform.h:92
arg
UCv2Qt.h
rtabmap::OdometryViewer::addedClouds_
QList< std::string > addedClouds_
Definition: OdometryViewer.h:83
UEventsHandler::unregisterFromEventsManager
void unregisterFromEventsManager()
Definition: UEventsHandler.cpp:33
rtabmap::RegistrationInfo::inliers
int inliers
Definition: RegistrationInfo.h:80
rtabmap::OdometryViewer::lastOdomPose_
Transform lastOdomPose_
Definition: OdometryViewer.h:80
rtabmap::OdometryInfo::localScanMap
LaserScan localScanMap
Definition: OdometryInfo.h:130
UASSERT
#define UASSERT(condition)
rtabmap::CloudViewer::setCameraTargetLocked
void setCameraTargetLocked(bool enabled=true)
Definition: CloudViewer.cpp:3288
rtabmap::OdometryInfo::words
std::multimap< int, cv::KeyPoint > words
Definition: OdometryInfo.h:128
rtabmap::CloudViewer::updateCameraFrustums
void updateCameraFrustums(const Transform &pose, const std::vector< CameraModel > &models)
Definition: CloudViewer.cpp:3115
rtabmap::OdometryEvent::info
const OdometryInfo & info() const
Definition: OdometryEvent.h:89
rtabmap::CloudViewer::setCloudVisibility
void setCloudVisibility(const std::string &id, bool isVisible)
Definition: CloudViewer.cpp:3181
OdometryEvent.h
rtabmap::OdometryViewer::featuresShown_
QCheckBox * featuresShown_
Definition: OdometryViewer.h:91
rtabmap::OdometryViewer::~OdometryViewer
virtual ~OdometryViewer()
Definition: OdometryViewer.cpp:162
rtabmap::OdometryViewer::reset
void reset()
Definition: OdometryViewer.cpp:169
UWARN
#define UWARN(...)
rtabmap::CloudViewer::removeCloud
bool removeCloud(const std::string &id)
Definition: CloudViewer.cpp:2588
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
OdometryViewer.h
rtabmap::CloudViewer::getAddedClouds
const QMap< std::string, Transform > & getAddedClouds() const
Definition: CloudViewer.h:338
rtabmap::OdometryViewer::processingData_
bool processingData_
Definition: OdometryViewer.h:76
rtabmap::ImageView::setImageDepth
void setImageDepth(const cv::Mat &imageDepth)
Definition: ImageView.cpp:1264
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
rtabmap::RegistrationInfo::matchesIDs
std::vector< int > matchesIDs
Definition: RegistrationInfo.h:86
rtabmap::CloudViewer::clear
virtual void clear()
Definition: CloudViewer.cpp:261
rtabmap::OdometryViewer::timeLabel_
QLabel * timeLabel_
Definition: OdometryViewer.h:92
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
Definition: SensorData.h:210
iter
iterator iter(handle obj)
rtabmap::OdometryViewer::clear
virtual void clear()
Definition: OdometryViewer.cpp:174
rtabmap::OdometryViewer::maxDepthSpin_
QDoubleSpinBox * maxDepthSpin_
Definition: OdometryViewer.h:88
rtabmap::OdometryViewer::maxCloudsSpin_
QSpinBox * maxCloudsSpin_
Definition: OdometryViewer.h:85
rtabmap::Odometry::kTypeORBSLAM
@ kTypeORBSLAM
Definition: Odometry.h:52
rtabmap::OdometryInfo::newCorners
std::vector< cv::Point2f > newCorners
Definition: OdometryInfo.h:134
rtabmap::ImageView::isImageDepthShown
bool isImageDepthShown() const
Definition: ImageView.cpp:361
UDEBUG
#define UDEBUG(...)
rtabmap::ImageView::isLinesShown
bool isLinesShown() const
Definition: ImageView.cpp:481
rtabmap::ImageView::isImageShown
bool isImageShown() const
Definition: ImageView.cpp:356
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:99
rtabmap::OdometryResetEvent
Definition: OdometryEvent.h:97
rtabmap::ImageView::isFeaturesShown
bool isFeaturesShown() const
Definition: ImageView.cpp:366
rtabmap::ImageView
Definition: ImageView.h:49
rtabmap::ImageView::setAlpha
void setAlpha(int alpha)
Definition: ImageView.cpp:1343
uNormSquared
T uNormSquared(const std::vector< T > &v)
Definition: UMath.h:558
rtabmap::OdometryEvent::data
SensorData & data()
Definition: OdometryEvent.h:69
false
#define false
Definition: ConvertUTF.c:56
uCvMat2QImage
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack, float depthMin=0, float depthMax=0)
Definition: UCv2Qt.h:47
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:184
rtabmap::CloudViewer::setFrustumShown
void setFrustumShown(bool shown)
Definition: CloudViewer.cpp:2486
rtabmap::ImageView::setFeatures
void setFeatures(const std::multimap< int, cv::KeyPoint > &refWords, const cv::Mat &depth=cv::Mat(), const QColor &color=Qt::yellow)
Definition: ImageView.cpp:1137
rtabmap::Odometry::kTypeF2F
@ kTypeF2F
Definition: Odometry.h:48
rtabmap::CloudViewer::setCloudPointSize
void setCloudPointSize(const std::string &id, int size)
Definition: CloudViewer.cpp:3276
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::ImageView::getBackgroundColor
const QColor & getBackgroundColor() const
Definition: ImageView.cpp:403
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2296
UERROR
#define UERROR(...)
rtabmap::Odometry::kTypeF2M
@ kTypeF2M
Definition: Odometry.h:47
i
int i
rtabmap::CloudViewer::setGridShown
void setGridShown(bool shown)
Definition: CloudViewer.cpp:3377
rtabmap::OdometryViewer::cloudShown_
QCheckBox * cloudShown_
Definition: OdometryViewer.h:89
rtabmap::ImageView::setFeatureColor
void setFeatureColor(int id, QColor color)
Definition: ImageView.cpp:1307
UEventsSender::post
void post(UEvent *event, bool async=true) const
Definition: UEventsSender.cpp:28


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