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.h"
00031 #include "rtabmap/core/OdometryEvent.h"
00032 #include "rtabmap/utilite/ULogger.h"
00033 #include "rtabmap/utilite/UConversion.h"
00034 #include "rtabmap/gui/UCv2Qt.h"
00035
00036 #include "rtabmap/gui/ImageView.h"
00037 #include "rtabmap/gui/CloudViewer.h"
00038
00039 #include <QPushButton>
00040 #include <QSpinBox>
00041 #include <QDoubleSpinBox>
00042 #include <QLabel>
00043 #include <QHBoxLayout>
00044 #include <QVBoxLayout>
00045 #include <QApplication>
00046
00047 namespace rtabmap {
00048
00049 OdometryViewer::OdometryViewer(int maxClouds, int decimation, float voxelSize, int qualityWarningThr, QWidget * parent) :
00050 QDialog(parent),
00051 imageView_(new ImageView(this)),
00052 cloudView_(new CloudViewer(this)),
00053 processingData_(false),
00054 odomImageShow_(true),
00055 odomImageDepthShow_(true),
00056 lastOdomPose_(Transform::getIdentity()),
00057 qualityWarningThr_(qualityWarningThr),
00058 id_(0),
00059 validDecimationValue_(1)
00060 {
00061
00062 qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
00063 qRegisterMetaType<rtabmap::OdometryInfo>("rtabmap::OdometryInfo");
00064
00065 imageView_->setImageDepthShown(false);
00066 imageView_->setMinimumSize(320, 240);
00067
00068 cloudView_->setCameraFree();
00069 cloudView_->setGridShown(true);
00070
00071 QLabel * maxCloudsLabel = new QLabel("Max clouds", this);
00072 QLabel * voxelLabel = new QLabel("Voxel", this);
00073 QLabel * decimationLabel = new QLabel("Decimation", this);
00074 maxCloudsSpin_ = new QSpinBox(this);
00075 maxCloudsSpin_->setMinimum(0);
00076 maxCloudsSpin_->setMaximum(100);
00077 maxCloudsSpin_->setValue(maxClouds);
00078 voxelSpin_ = new QDoubleSpinBox(this);
00079 voxelSpin_->setMinimum(0);
00080 voxelSpin_->setMaximum(1);
00081 voxelSpin_->setDecimals(3);
00082 voxelSpin_->setSingleStep(0.01);
00083 voxelSpin_->setSuffix(" m");
00084 voxelSpin_->setValue(voxelSize);
00085 decimationSpin_ = new QSpinBox(this);
00086 decimationSpin_->setMinimum(1);
00087 decimationSpin_->setMaximum(16);
00088 decimationSpin_->setValue(decimation);
00089 timeLabel_ = new QLabel(this);
00090 QPushButton * clearButton = new QPushButton("clear", this);
00091 QPushButton * closeButton = new QPushButton("close", this);
00092 connect(clearButton, SIGNAL(clicked()), this, SLOT(clear()));
00093 connect(closeButton, SIGNAL(clicked()), this, SLOT(reject()));
00094
00095
00096 QHBoxLayout * layout = new QHBoxLayout();
00097 layout->setMargin(0);
00098 layout->setSpacing(0);
00099 layout->addWidget(imageView_,1);
00100 layout->addWidget(cloudView_,1);
00101
00102 QHBoxLayout * hlayout2 = new QHBoxLayout();
00103 hlayout2->setMargin(0);
00104 hlayout2->addWidget(maxCloudsLabel);
00105 hlayout2->addWidget(maxCloudsSpin_);
00106 hlayout2->addWidget(voxelLabel);
00107 hlayout2->addWidget(voxelSpin_);
00108 hlayout2->addWidget(decimationLabel);
00109 hlayout2->addWidget(decimationSpin_);
00110 hlayout2->addWidget(timeLabel_);
00111 hlayout2->addStretch(1);
00112 hlayout2->addWidget(clearButton);
00113 hlayout2->addWidget(closeButton);
00114
00115 QVBoxLayout * vlayout = new QVBoxLayout(this);
00116 vlayout->setMargin(0);
00117 vlayout->setSpacing(0);
00118 vlayout->addLayout(layout, 1);
00119 vlayout->addLayout(hlayout2);
00120
00121 this->setLayout(vlayout);
00122 }
00123
00124 OdometryViewer::~OdometryViewer()
00125 {
00126 this->unregisterFromEventsManager();
00127 this->clear();
00128 UDEBUG("");
00129 }
00130
00131 void OdometryViewer::clear()
00132 {
00133 addedClouds_.clear();
00134 cloudView_->clear();
00135 }
00136
00137 void OdometryViewer::processData(const rtabmap::SensorData & data, const rtabmap::OdometryInfo & info)
00138 {
00139 processingData_ = true;
00140 int quality = info.inliers;
00141
00142 bool lost = false;
00143 bool lostStateChanged = false;
00144
00145 if(data.pose().isNull())
00146 {
00147 UDEBUG("odom lost");
00148 lostStateChanged = imageView_->getBackgroundColor() != Qt::darkRed;
00149 imageView_->setBackgroundColor(Qt::darkRed);
00150 cloudView_->setBackgroundColor(Qt::darkRed);
00151
00152 lost = true;
00153 }
00154 else if(info.inliers>0 &&
00155 qualityWarningThr_ &&
00156 info.inliers < qualityWarningThr_)
00157 {
00158 UDEBUG("odom warn, quality(inliers)=%d thr=%d", info.inliers, qualityWarningThr_);
00159 lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
00160 imageView_->setBackgroundColor(Qt::darkYellow);
00161 cloudView_->setBackgroundColor(Qt::darkYellow);
00162 }
00163 else
00164 {
00165 UDEBUG("odom ok");
00166 lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
00167 imageView_->setBackgroundColor(cloudView_->getDefaultBackgroundColor());
00168 cloudView_->setBackgroundColor(Qt::black);
00169 }
00170
00171 timeLabel_->setText(QString("%1 s").arg(info.time));
00172
00173 if(!data.image().empty() && !data.depthOrRightImage().empty() && data.fx()>0.0f && data.fyOrBaseline()>0.0f)
00174 {
00175 UDEBUG("New pose = %s, quality=%d", data.pose().prettyPrint().c_str(), quality);
00176
00177 if(data.image().cols % decimationSpin_->value() == 0 &&
00178 data.image().rows % decimationSpin_->value() == 0)
00179 {
00180 validDecimationValue_ = decimationSpin_->value();
00181 }
00182 else
00183 {
00184 UWARN("Decimation (%d) must be a denominator of the width and height of "
00185 "the image (%d/%d). Using last valid decimation value (%d).",
00186 decimationSpin_->value(),
00187 data.image().cols,
00188 data.image().rows,
00189 validDecimationValue_);
00190 }
00191
00192
00193
00194
00195 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
00196 if(!data.depth().empty())
00197 {
00198 cloud = util3d::cloudFromDepthRGB(
00199 data.image(),
00200 data.depth(),
00201 data.cx(), data.cy(),
00202 data.fx(), data.fy(),
00203 validDecimationValue_);
00204 }
00205 else if(!data.rightImage().empty())
00206 {
00207 cloud = util3d::cloudFromStereoImages(
00208 data.image(),
00209 data.rightImage(),
00210 data.cx(), data.cy(),
00211 data.fx(), data.baseline(),
00212 validDecimationValue_);
00213 }
00214
00215 if(voxelSpin_->value() > 0.0f && cloud->size())
00216 {
00217 cloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, voxelSpin_->value());
00218 }
00219
00220 if(cloud->size())
00221 {
00222 cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, data.localTransform());
00223
00224 if(!data.pose().isNull())
00225 {
00226 if(cloudView_->getAddedClouds().contains("cloudtmp"))
00227 {
00228 cloudView_->removeCloud("cloudtmp");
00229 }
00230
00231 while(maxCloudsSpin_->value()>0 && (int)addedClouds_.size() > maxCloudsSpin_->value())
00232 {
00233 UASSERT(cloudView_->removeCloud(addedClouds_.first()));
00234 addedClouds_.pop_front();
00235 }
00236
00237 data.id()?id_=data.id():++id_;
00238 std::string cloudName = uFormat("cloud%d", id_);
00239 addedClouds_.push_back(cloudName);
00240 UASSERT(cloudView_->addCloud(cloudName, cloud, data.pose()));
00241 }
00242 else
00243 {
00244 cloudView_->addOrUpdateCloud("cloudtmp", cloud, lastOdomPose_);
00245 }
00246 }
00247 }
00248
00249 if(!data.pose().isNull())
00250 {
00251 lastOdomPose_ = data.pose();
00252 cloudView_->updateCameraTargetPosition(data.pose());
00253 }
00254
00255 if(info.localMap.size())
00256 {
00257 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
00258 cloud->resize(info.localMap.size());
00259 int i=0;
00260 for(std::multimap<int, cv::Point3f>::const_iterator iter=info.localMap.begin(); iter!=info.localMap.end(); ++iter)
00261 {
00262 (*cloud)[i].x = iter->second.x;
00263 (*cloud)[i].y = iter->second.y;
00264 (*cloud)[i++].z = iter->second.z;
00265 }
00266 cloudView_->addOrUpdateCloud("localmap", cloud);
00267 }
00268
00269 if(!data.image().empty())
00270 {
00271 if(info.type == 0)
00272 {
00273 imageView_->setFeatures(info.words, data.depth(), Qt::yellow);
00274 }
00275 else if(info.type == 1)
00276 {
00277 std::vector<cv::KeyPoint> kpts;
00278 cv::KeyPoint::convert(info.refCorners, kpts);
00279 imageView_->setFeatures(kpts, data.depth(), Qt::red);
00280 }
00281
00282 imageView_->clearLines();
00283 if(lost)
00284 {
00285 if(lostStateChanged)
00286 {
00287
00288 odomImageShow_ = imageView_->isImageShown();
00289 odomImageDepthShow_ = imageView_->isImageDepthShown();
00290 }
00291 imageView_->setImageDepth(uCvMat2QImage(data.image()));
00292 imageView_->setImageShown(true);
00293 imageView_->setImageDepthShown(true);
00294 }
00295 else
00296 {
00297 if(lostStateChanged)
00298 {
00299
00300 imageView_->setImageShown(odomImageShow_);
00301 imageView_->setImageDepthShown(odomImageDepthShow_);
00302 }
00303
00304 imageView_->setImage(uCvMat2QImage(data.image()));
00305 if(imageView_->isImageDepthShown())
00306 {
00307 imageView_->setImageDepth(uCvMat2QImage(data.depthOrRightImage()));
00308 }
00309
00310 if(info.type == 0)
00311 {
00312 if(imageView_->isFeaturesShown())
00313 {
00314 for(unsigned int i=0; i<info.wordMatches.size(); ++i)
00315 {
00316 imageView_->setFeatureColor(info.wordMatches[i], Qt::red);
00317 }
00318 for(unsigned int i=0; i<info.wordInliers.size(); ++i)
00319 {
00320 imageView_->setFeatureColor(info.wordInliers[i], Qt::green);
00321 }
00322 }
00323 }
00324 }
00325 if(info.type == 1 && info.cornerInliers.size())
00326 {
00327 if(imageView_->isFeaturesShown() || imageView_->isLinesShown())
00328 {
00329
00330 UASSERT(info.refCorners.size() == info.newCorners.size());
00331 for(unsigned int i=0; i<info.cornerInliers.size(); ++i)
00332 {
00333 if(imageView_->isFeaturesShown())
00334 {
00335 imageView_->setFeatureColor(info.cornerInliers[i], Qt::green);
00336 }
00337 if(imageView_->isLinesShown())
00338 {
00339 imageView_->addLine(
00340 info.refCorners[info.cornerInliers[i]].x,
00341 info.refCorners[info.cornerInliers[i]].y,
00342 info.newCorners[info.cornerInliers[i]].x,
00343 info.newCorners[info.cornerInliers[i]].y,
00344 Qt::blue);
00345 }
00346 }
00347 }
00348 }
00349
00350 if(!data.image().empty())
00351 {
00352 imageView_->setSceneRect(QRectF(0,0,(float)data.image().cols, (float)data.image().rows));
00353 }
00354 }
00355
00356 imageView_->update();
00357 cloudView_->update();
00358 QApplication::processEvents();
00359 processingData_ = false;
00360 }
00361
00362 void OdometryViewer::handleEvent(UEvent * event)
00363 {
00364 if(!processingData_ && this->isVisible())
00365 {
00366 if(event->getClassName().compare("OdometryEvent") == 0)
00367 {
00368 rtabmap::OdometryEvent * odomEvent = (rtabmap::OdometryEvent*)event;
00369 if(odomEvent->data().isValid())
00370 {
00371 processingData_ = true;
00372 QMetaObject::invokeMethod(this, "processData",
00373 Q_ARG(rtabmap::SensorData, odomEvent->data()),
00374 Q_ARG(rtabmap::OdometryInfo, odomEvent->info()));
00375 }
00376 }
00377 }
00378 }
00379
00380 }