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