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->setMargin(0);
130  layout->setSpacing(0);
131  layout->addWidget(imageView_,1);
132  layout->addWidget(cloudView_,1);
133 
134  QHBoxLayout * hlayout2 = new QHBoxLayout();
135  hlayout2->setMargin(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->setMargin(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().stereoCameraModel().isValidForProjection() || 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().stereoCameraModel().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::kTypeORBSLAM2)
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::kTypeORBSLAM2 ||
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 */
void addLine(float x1, float y1, float x2, float y2, QColor color, const QString &text=QString())
Definition: ImageView.cpp:1084
bool isImageDepthShown() const
Definition: ImageView.cpp:335
SensorData & data()
Definition: OdometryEvent.h:69
void setAlpha(int alpha)
Definition: ImageView.cpp:1205
void setFrustumShown(bool shown)
std::vector< cv::Point2f > refCorners
Definition: OdometryInfo.h:128
bool isLinesShown() const
Definition: ImageView.cpp:445
QDoubleSpinBox * maxDepthSpin_
std::string prettyPrint() const
Definition: Transform.cpp:295
Definition: UEvent.h:57
void updateCameraFrustum(const Transform &pose, const StereoCameraModel &model)
void setGridShown(bool shown)
void setCloudVisibility(const std::string &id, bool isVisible)
bool isImageShown() const
Definition: ImageView.cpp:330
void setFeatureColor(int id, QColor color)
Definition: ImageView.cpp:1169
std::map< int, cv::Point3f > localMap
Definition: OdometryInfo.h:124
static Transform getIdentity()
Definition: Transform.cpp:380
bool removeCloud(const std::string &id)
CloudViewer * cloudView_
bool isEmpty() const
Definition: LaserScan.h:101
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
virtual void clear()
const QColor & getDefaultBackgroundColor() const
std::vector< int > inliersIDs
Some conversion functions.
void setImage(const QImage &image)
Definition: ImageView.cpp:1099
const LaserScan & laserScanRaw() const
Definition: SensorData.h:166
const cv::Mat & imageRaw() const
Definition: SensorData.h:164
void post(UEvent *event, bool async=true) const
virtual bool handleEvent(UEvent *event)
void processData(const rtabmap::OdometryEvent &odom)
#define UASSERT(condition)
bool isValid() const
Definition: SensorData.h:137
const Transform & pose() const
Definition: OdometryEvent.h:71
void setSceneRect(const QRectF &rect)
Definition: ImageView.cpp:1250
void setImageShown(bool shown)
Definition: ImageView.cpp:415
void setBackgroundColor(const QColor &color)
Definition: ImageView.cpp:669
#define true
Definition: ConvertUTF.c:57
bool isNull() const
Definition: Transform.cpp:107
std::vector< int > cornerInliers
Definition: OdometryInfo.h:130
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack)
Definition: UCv2Qt.h:44
virtual std::string getClassName() const =0
void setImageDepth(const cv::Mat &imageDepth)
Definition: ImageView.cpp:1126
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP 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:1031
void setCloudPointSize(const std::string &id, int size)
void updateCameraTargetPosition(const Transform &pose)
Transform localTransform() const
Definition: LaserScan.h:98
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:165
int id() const
Definition: SensorData.h:155
const OdometryInfo & info() const
Definition: OdometryEvent.h:89
QDoubleSpinBox * voxelSpin_
const QMap< std::string, Transform > & getAddedClouds() const
Definition: CloudViewer.h:314
void setBackgroundColor(const QColor &color)
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)
#define false
Definition: ConvertUTF.c:56
OdometryViewer(int maxClouds=10, int decimation=2, float voxelSize=0.0f, float maxDepth=0, int qualityWarningThr=0, QWidget *parent=0, const ParametersMap &parameters=ParametersMap())
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:215
const QColor & getBackgroundColor() const
Definition: ImageView.cpp:377
void setCameraTargetLocked(bool enabled=true)
#define UDEBUG(...)
std::multimap< int, cv::KeyPoint > words
Definition: OdometryInfo.h:123
T uNormSquared(const std::vector< T > &v)
Definition: UMath.h:560
ParametersMap parameters_
void updateCameraFrustums(const Transform &pose, const std::vector< CameraModel > &models)
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2400
#define UERROR(...)
ULogger class and convenient macros.
const StereoCameraModel & stereoCameraModel() const
Definition: SensorData.h:216
#define UWARN(...)
QList< std::string > addedClouds_
std::vector< int > matchesIDs
std::vector< cv::Point2f > newCorners
Definition: OdometryInfo.h:129
RegistrationInfo reg
Definition: OdometryInfo.h:96
bool isFeaturesShown() const
Definition: ImageView.cpp:340
void unregisterFromEventsManager()
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
const Transform & localTransform() const
std::string UTILITE_EXP uFormat(const char *fmt,...)
void setFeatures(const std::multimap< int, cv::KeyPoint > &refWords, const cv::Mat &depth=cv::Mat(), const QColor &color=Qt::yellow)
Definition: ImageView.cpp:1002
cv::Mat depthRaw() const
Definition: SensorData.h:189
void setCloudOpacity(const std::string &id, double opacity=1.0)
void setImageDepthShown(bool shown)
Definition: ImageView.cpp:430


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59