00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/gui/OdometryViewer.h"
00029
00030 #include "rtabmap/core/util3d_transforms.h"
00031 #include "rtabmap/core/util3d_filtering.h"
00032 #include "rtabmap/core/util3d.h"
00033 #include "rtabmap/core/OdometryEvent.h"
00034 #include "rtabmap/utilite/ULogger.h"
00035 #include "rtabmap/utilite/UConversion.h"
00036 #include "rtabmap/utilite/UCv2Qt.h"
00037
00038 #include "rtabmap/gui/ImageView.h"
00039 #include "rtabmap/gui/CloudViewer.h"
00040
00041 #include <QPushButton>
00042 #include <QSpinBox>
00043 #include <QDoubleSpinBox>
00044 #include <QCheckBox>
00045 #include <QLabel>
00046 #include <QHBoxLayout>
00047 #include <QVBoxLayout>
00048 #include <QApplication>
00049
00050 namespace rtabmap {
00051
00052 OdometryViewer::OdometryViewer(
00053 int maxClouds,
00054 int decimation,
00055 float voxelSize,
00056 float maxDepth,
00057 int qualityWarningThr,
00058 QWidget * parent,
00059 const ParametersMap & parameters) :
00060 QDialog(parent),
00061 imageView_(new ImageView(this)),
00062 cloudView_(new CloudViewer(this)),
00063 processingData_(false),
00064 odomImageShow_(true),
00065 odomImageDepthShow_(true),
00066 lastOdomPose_(Transform::getIdentity()),
00067 qualityWarningThr_(qualityWarningThr),
00068 id_(0),
00069 validDecimationValue_(1),
00070 parameters_(parameters)
00071 {
00072
00073 qRegisterMetaType<rtabmap::OdometryEvent>("rtabmap::OdometryEvent");
00074
00075 imageView_->setImageDepthShown(false);
00076 imageView_->setMinimumSize(320, 240);
00077 imageView_->setAlpha(255);
00078
00079 cloudView_->setCameraTargetLocked();
00080 cloudView_->setGridShown(true);
00081
00082 QLabel * maxCloudsLabel = new QLabel("Max clouds", this);
00083 QLabel * voxelLabel = new QLabel("Voxel", this);
00084 QLabel * maxDepthLabel = new QLabel("Max depth", this);
00085 QLabel * decimationLabel = new QLabel("Decimation", this);
00086 maxCloudsSpin_ = new QSpinBox(this);
00087 maxCloudsSpin_->setMinimum(0);
00088 maxCloudsSpin_->setMaximum(100);
00089 maxCloudsSpin_->setValue(maxClouds);
00090 voxelSpin_ = new QDoubleSpinBox(this);
00091 voxelSpin_->setMinimum(0);
00092 voxelSpin_->setMaximum(1);
00093 voxelSpin_->setDecimals(3);
00094 voxelSpin_->setSingleStep(0.01);
00095 voxelSpin_->setSuffix(" m");
00096 voxelSpin_->setValue(voxelSize);
00097 maxDepthSpin_ = new QDoubleSpinBox(this);
00098 maxDepthSpin_->setMinimum(0);
00099 maxDepthSpin_->setMaximum(100);
00100 maxDepthSpin_->setDecimals(0);
00101 maxDepthSpin_->setSingleStep(1);
00102 maxDepthSpin_->setSuffix(" m");
00103 maxDepthSpin_->setValue(maxDepth);
00104 decimationSpin_ = new QSpinBox(this);
00105 decimationSpin_->setMinimum(1);
00106 decimationSpin_->setMaximum(16);
00107 decimationSpin_->setValue(decimation);
00108 cloudShown_ = new QCheckBox(this);
00109 cloudShown_->setText("Cloud");
00110 cloudShown_->setChecked(true);
00111 scanShown_ = new QCheckBox(this);
00112 scanShown_->setText("Scan");
00113 scanShown_->setChecked(true);
00114 featuresShown_ = new QCheckBox(this);
00115 featuresShown_->setText("Features");
00116 featuresShown_->setChecked(true);
00117 timeLabel_ = new QLabel(this);
00118 QPushButton * resetButton = new QPushButton("reset", this);
00119 QPushButton * clearButton = new QPushButton("clear", this);
00120 QPushButton * closeButton = new QPushButton("close", this);
00121 connect(resetButton, SIGNAL(clicked()), this, SLOT(reset()));
00122 connect(clearButton, SIGNAL(clicked()), this, SLOT(clear()));
00123 connect(closeButton, SIGNAL(clicked()), this, SLOT(reject()));
00124
00125
00126 QHBoxLayout * layout = new QHBoxLayout();
00127 layout->setMargin(0);
00128 layout->setSpacing(0);
00129 layout->addWidget(imageView_,1);
00130 layout->addWidget(cloudView_,1);
00131
00132 QHBoxLayout * hlayout2 = new QHBoxLayout();
00133 hlayout2->setMargin(0);
00134 hlayout2->addWidget(maxCloudsLabel);
00135 hlayout2->addWidget(maxCloudsSpin_);
00136 hlayout2->addWidget(voxelLabel);
00137 hlayout2->addWidget(voxelSpin_);
00138 hlayout2->addWidget(maxDepthLabel);
00139 hlayout2->addWidget(maxDepthSpin_);
00140 hlayout2->addWidget(decimationLabel);
00141 hlayout2->addWidget(decimationSpin_);
00142 hlayout2->addWidget(cloudShown_);
00143 hlayout2->addWidget(scanShown_);
00144 hlayout2->addWidget(featuresShown_);
00145 hlayout2->addWidget(timeLabel_);
00146 hlayout2->addStretch(1);
00147 hlayout2->addWidget(resetButton);
00148 hlayout2->addWidget(clearButton);
00149 hlayout2->addWidget(closeButton);
00150
00151 QVBoxLayout * vlayout = new QVBoxLayout(this);
00152 vlayout->setMargin(0);
00153 vlayout->setSpacing(0);
00154 vlayout->addLayout(layout, 1);
00155 vlayout->addLayout(hlayout2);
00156
00157 this->setLayout(vlayout);
00158 }
00159
00160 OdometryViewer::~OdometryViewer()
00161 {
00162 this->unregisterFromEventsManager();
00163 this->clear();
00164 UDEBUG("");
00165 }
00166
00167 void OdometryViewer::reset()
00168 {
00169 this->post(new OdometryResetEvent());
00170 }
00171
00172 void OdometryViewer::clear()
00173 {
00174 addedClouds_.clear();
00175 cloudView_->clear();
00176 }
00177
00178 void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
00179 {
00180 processingData_ = true;
00181 int quality = odom.info().inliers;
00182
00183 bool lost = false;
00184 bool lostStateChanged = false;
00185
00186 if(odom.pose().isNull())
00187 {
00188 UDEBUG("odom lost");
00189 lostStateChanged = imageView_->getBackgroundColor() != Qt::darkRed;
00190 imageView_->setBackgroundColor(Qt::darkRed);
00191 cloudView_->setBackgroundColor(Qt::darkRed);
00192
00193 lost = true;
00194 }
00195 else if(odom.info().inliers>0 &&
00196 qualityWarningThr_ &&
00197 odom.info().inliers < qualityWarningThr_)
00198 {
00199 UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, qualityWarningThr_);
00200 lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
00201 imageView_->setBackgroundColor(Qt::darkYellow);
00202 cloudView_->setBackgroundColor(Qt::darkYellow);
00203 }
00204 else
00205 {
00206 UDEBUG("odom ok");
00207 lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
00208 imageView_->setBackgroundColor(cloudView_->getDefaultBackgroundColor());
00209 cloudView_->setBackgroundColor(Qt::black);
00210 }
00211
00212 timeLabel_->setText(QString("%1 s").arg(odom.info().timeEstimation));
00213
00214 if(cloudShown_->isChecked() &&
00215 !odom.data().imageRaw().empty() &&
00216 !odom.data().depthOrRightRaw().empty() &&
00217 (odom.data().stereoCameraModel().isValidForProjection() || odom.data().cameraModels().size()))
00218 {
00219 UDEBUG("New pose = %s, quality=%d", odom.pose().prettyPrint().c_str(), quality);
00220
00221 if(!odom.data().depthRaw().empty())
00222 {
00223 if(odom.data().imageRaw().cols % decimationSpin_->value() == 0 &&
00224 odom.data().imageRaw().rows % decimationSpin_->value() == 0)
00225 {
00226 validDecimationValue_ = decimationSpin_->value();
00227 }
00228 else
00229 {
00230 UWARN("Decimation (%d) must be a denominator of the width and height of "
00231 "the image (%d/%d). Using last valid decimation value (%d).",
00232 decimationSpin_->value(),
00233 odom.data().imageRaw().cols,
00234 odom.data().imageRaw().rows,
00235 validDecimationValue_);
00236 }
00237 }
00238 else
00239 {
00240 validDecimationValue_ = decimationSpin_->value();
00241 }
00242
00243
00244
00245 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
00246 pcl::IndicesPtr validIndices(new std::vector<int>);
00247 cloud = util3d::cloudRGBFromSensorData(
00248 odom.data(),
00249 validDecimationValue_,
00250 0,
00251 0,
00252 validIndices.get(),
00253 parameters_);
00254
00255 if(voxelSpin_->value())
00256 {
00257 cloud = util3d::voxelize(cloud, validIndices, voxelSpin_->value());
00258 }
00259
00260 if(cloud->size())
00261 {
00262 if(!odom.pose().isNull())
00263 {
00264 if(cloudView_->getAddedClouds().contains("cloudtmp"))
00265 {
00266 cloudView_->removeCloud("cloudtmp");
00267 }
00268
00269 while(maxCloudsSpin_->value()>0 && (int)addedClouds_.size() > maxCloudsSpin_->value())
00270 {
00271 UASSERT(cloudView_->removeCloud(addedClouds_.first()));
00272 addedClouds_.pop_front();
00273 }
00274
00275 odom.data().id()?id_=odom.data().id():++id_;
00276 std::string cloudName = uFormat("cloud%d", id_);
00277 addedClouds_.push_back(cloudName);
00278 UASSERT(cloudView_->addCloud(cloudName, cloud, odom.pose()));
00279 }
00280 else
00281 {
00282 cloudView_->addCloud("cloudtmp", cloud, lastOdomPose_);
00283 }
00284 }
00285 }
00286 else if(!cloudShown_->isChecked())
00287 {
00288 while(!addedClouds_.empty())
00289 {
00290 UASSERT(cloudView_->removeCloud(addedClouds_.first()));
00291 addedClouds_.pop_front();
00292 }
00293 }
00294
00295 if(!odom.pose().isNull())
00296 {
00297 lastOdomPose_ = odom.pose();
00298 cloudView_->updateCameraTargetPosition(odom.pose());
00299 }
00300
00301 if(scanShown_->isChecked())
00302 {
00303
00304 if(!odom.info().localScanMap.empty())
00305 {
00306 pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
00307 cloud = util3d::laserScanToPointCloudNormal(odom.info().localScanMap);
00308 if(!cloudView_->addCloud("scanMapOdom", cloud, Transform::getIdentity(), Qt::blue))
00309 {
00310 UERROR("Adding scanMapOdom to viewer failed!");
00311 }
00312 else
00313 {
00314 cloudView_->setCloudVisibility("scanMapOdom", true);
00315 cloudView_->setCloudOpacity("scanMapOdom", 0.5);
00316 }
00317 }
00318
00319 if(!odom.data().laserScanRaw().empty())
00320 {
00321 cv::Mat scan = odom.data().laserScanRaw();
00322
00323 pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
00324 cloud = util3d::laserScanToPointCloudNormal(scan, odom.pose());
00325
00326 if(!cloudView_->addCloud("scanOdom", cloud, Transform::getIdentity(), Qt::magenta))
00327 {
00328 UERROR("Adding scanOdom to viewer failed!");
00329 }
00330 else
00331 {
00332 cloudView_->setCloudVisibility("scanOdom", true);
00333 cloudView_->setCloudOpacity("scanOdom", 0.5);
00334 }
00335 }
00336 }
00337 else
00338 {
00339 cloudView_->removeCloud("scanMapOdom");
00340 cloudView_->removeCloud("scanOdom");
00341 }
00342
00343
00344 if(featuresShown_->isChecked())
00345 {
00346 if(!odom.info().localMap.empty())
00347 {
00348 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00349 cloud->resize(odom.info().localMap.size());
00350 int i=0;
00351 for(std::map<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
00352 {
00353 (*cloud)[i].x = iter->second.x;
00354 (*cloud)[i].y = iter->second.y;
00355 (*cloud)[i].z = iter->second.z;
00356
00357
00358 bool inlier = odom.info().words.find(iter->first) != odom.info().words.end();
00359 (*cloud)[i].r = inlier?0:255;
00360 (*cloud)[i].g = 255;
00361 (*cloud)[i++].b = 0;
00362 }
00363
00364 if(!cloudView_->addCloud("featuresOdom", cloud))
00365 {
00366 UERROR("Adding featuresOdom to viewer failed!");
00367 }
00368 else
00369 {
00370 cloudView_->setCloudVisibility("featuresOdom", true);
00371 cloudView_->setCloudPointSize("featuresOdom", 3);
00372 }
00373 }
00374 }
00375 else
00376 {
00377 cloudView_->removeCloud("featuresOdom");
00378 }
00379
00380 if(!odom.data().imageRaw().empty())
00381 {
00382 if(odom.info().type == 0)
00383 {
00384 imageView_->setFeatures(odom.info().words, odom.data().depthRaw(), Qt::yellow);
00385 }
00386 else if(odom.info().type == 1)
00387 {
00388 std::vector<cv::KeyPoint> kpts;
00389 cv::KeyPoint::convert(odom.info().refCorners, kpts);
00390 imageView_->setFeatures(kpts, odom.data().depthRaw(), Qt::red);
00391 }
00392
00393 imageView_->clearLines();
00394 if(lost)
00395 {
00396 if(lostStateChanged)
00397 {
00398
00399 odomImageShow_ = imageView_->isImageShown();
00400 odomImageDepthShow_ = imageView_->isImageDepthShown();
00401 }
00402 imageView_->setImageDepth(uCvMat2QImage(odom.data().imageRaw()));
00403 imageView_->setImageShown(true);
00404 imageView_->setImageDepthShown(true);
00405 }
00406 else
00407 {
00408 if(lostStateChanged)
00409 {
00410
00411 imageView_->setImageShown(odomImageShow_);
00412 imageView_->setImageDepthShown(odomImageDepthShow_);
00413 }
00414
00415 imageView_->setImage(uCvMat2QImage(odom.data().imageRaw()));
00416 if(imageView_->isImageDepthShown())
00417 {
00418 imageView_->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw()));
00419 }
00420
00421 if(odom.info().type == 0)
00422 {
00423 if(imageView_->isFeaturesShown())
00424 {
00425 for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
00426 {
00427 imageView_->setFeatureColor(odom.info().wordMatches[i], Qt::red);
00428 }
00429 for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
00430 {
00431 imageView_->setFeatureColor(odom.info().wordInliers[i], Qt::green);
00432 }
00433 }
00434 }
00435 }
00436 if(odom.info().type == 1 && odom.info().cornerInliers.size())
00437 {
00438 if(imageView_->isFeaturesShown() || imageView_->isLinesShown())
00439 {
00440
00441 UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
00442 for(unsigned int i=0; i<odom.info().cornerInliers.size(); ++i)
00443 {
00444 if(imageView_->isFeaturesShown())
00445 {
00446 imageView_->setFeatureColor(odom.info().cornerInliers[i], Qt::green);
00447 }
00448 if(imageView_->isLinesShown())
00449 {
00450 imageView_->addLine(
00451 odom.info().refCorners[odom.info().cornerInliers[i]].x,
00452 odom.info().refCorners[odom.info().cornerInliers[i]].y,
00453 odom.info().newCorners[odom.info().cornerInliers[i]].x,
00454 odom.info().newCorners[odom.info().cornerInliers[i]].y,
00455 Qt::blue);
00456 }
00457 }
00458 }
00459 }
00460
00461 if(!odom.data().imageRaw().empty())
00462 {
00463 imageView_->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows));
00464 }
00465 }
00466
00467 imageView_->update();
00468 cloudView_->update();
00469 QApplication::processEvents();
00470 processingData_ = false;
00471 }
00472
00473 void OdometryViewer::handleEvent(UEvent * event)
00474 {
00475 if(!processingData_ && this->isVisible())
00476 {
00477 if(event->getClassName().compare("OdometryEvent") == 0)
00478 {
00479 rtabmap::OdometryEvent * odomEvent = (rtabmap::OdometryEvent*)event;
00480 if(odomEvent->data().isValid())
00481 {
00482 processingData_ = true;
00483 QMetaObject::invokeMethod(this, "processData",
00484 Q_ARG(rtabmap::OdometryEvent, *odomEvent));
00485 }
00486 }
00487 }
00488 }
00489
00490 }