From 9e1d38fe096ecab4215617dce2a960069c33b956 Mon Sep 17 00:00:00 2001 From: Mathieu Labbe Date: Tue, 2 Dec 2014 17:01:24 -0500 Subject: [PATCH] added Odometry view to MainWindow, increased version to 0.7.3 --- CMakeLists.txt | 2 +- corelib/src/Features2d.cpp | 14 +-- guilib/include/rtabmap/gui/CloudViewer.h | 4 +- guilib/src/CloudViewer.cpp | 9 +- guilib/src/MainWindow.cpp | 144 +++++++++++++++-------- guilib/src/ui/mainWindow.ui | 21 ++++ package.xml | 2 +- 7 files changed, 135 insertions(+), 61 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 212d4662..32c0685f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,7 +16,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") ####################### SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MINOR_VERSION 7) -SET(RTABMAP_PATCH_VERSION 2) +SET(RTABMAP_PATCH_VERSION 3) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) diff --git a/corelib/src/Features2d.cpp b/corelib/src/Features2d.cpp index ea0a8f3f..2ed0f23d 100644 --- a/corelib/src/Features2d.cpp +++ b/corelib/src/Features2d.cpp @@ -481,7 +481,7 @@ void SURF::parseParameters(const ParametersMap & parameters) _surf = new cv::SURF(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_); } #else - UERROR("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!"); + UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!"); #endif } @@ -502,7 +502,7 @@ std::vector SURF::generateKeypointsImpl(const cv::Mat & image, con _surf->detect(imgRoi, keypoints); } #else - UERROR("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!"); + UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!"); #endif return keypoints; } @@ -533,7 +533,7 @@ cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vectorcompute(image, keypoints, descriptors); } #else - UERROR("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!"); + UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!"); #endif return descriptors; @@ -561,7 +561,7 @@ SIFT::~SIFT() delete _sift; } #else - UERROR("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!"); + UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!"); #endif } @@ -582,7 +582,7 @@ void SIFT::parseParameters(const ParametersMap & parameters) _sift = new cv::SIFT(nfeatures_, nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_); #else - UERROR("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!"); + UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!"); #endif } @@ -594,7 +594,7 @@ std::vector SIFT::generateKeypointsImpl(const cv::Mat & image, con cv::Mat imgRoi(image, roi); _sift->detect(imgRoi, keypoints); // Opencv keypoints #else - UERROR("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!"); + UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!"); #endif return keypoints; } @@ -606,7 +606,7 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vectorcompute(image, keypoints, descriptors); #else - UERROR("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!"); + UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!"); #endif return descriptors; } diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index a9a217b2..51432f8a 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -147,7 +147,8 @@ public: bool getPose(const std::string & id, Transform & pose); //including meshes bool getCloudVisibility(const std::string & id); - const QMap & getAddedClouds() {return _addedClouds;} //including meshes + const QMap & getAddedClouds() const {return _addedClouds;} //including meshes + const QColor & getBackgroundColor() const; void setCameraTargetLocked(bool enabled = true); void setCameraTargetFollow(bool enabled = true); @@ -197,6 +198,7 @@ private: std::list _gridLines; QSet _keysPressed; QString _workingDirectory; + QColor _backgroundColor; }; } /* namespace rtabmap */ diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index fbd827eb..fa53e9bd 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -74,7 +74,8 @@ CloudViewer::CloudViewer(QWidget *parent) : _menu(0), _trajectory(new pcl::PointCloud), _maxTrajectorySize(100), - _workingDirectory(".") + _workingDirectory("."), + _backgroundColor(Qt::black) { this->setMinimumSize(200, 200); @@ -657,8 +658,14 @@ void CloudViewer::render() this->GetRenderWindow()->Render(); } +const QColor & CloudViewer::getBackgroundColor() const +{ + return _backgroundColor; +} + void CloudViewer::setBackgroundColor(const QColor & color) { + _backgroundColor = color; _visualizer->setBackgroundColor(color.redF(), color.greenF(), color.blueF()); } diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 35d063d7..bd6510e7 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -161,6 +161,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _ui->dockWidget_loopClosureViewer->setVisible(false); _ui->dockWidget_mapVisibility->setVisible(false); _ui->dockWidget_graphViewer->setVisible(false); + _ui->dockWidget_odometry->setVisible(false); //_ui->dockWidget_cloudViewer->setVisible(false); //_ui->dockWidget_imageView->setVisible(false); } @@ -194,6 +195,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : //Graphics scenes _ui->imageView_source->setBackgroundBrush(QBrush(Qt::black)); _ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::black)); + _ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::black)); _posteriorCurve = new PdfPlotCurve("Posterior", &_cachedSignatures, this); _ui->posteriorPlot->addCurve(_posteriorCurve, false); @@ -240,6 +242,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _ui->menuShow_view->addAction(_ui->dockWidget_loopClosureViewer->toggleViewAction()); _ui->menuShow_view->addAction(_ui->dockWidget_mapVisibility->toggleViewAction()); _ui->menuShow_view->addAction(_ui->dockWidget_graphViewer->toggleViewAction()); + _ui->menuShow_view->addAction(_ui->dockWidget_odometry->toggleViewAction()); _ui->menuShow_view->addAction(_ui->toolBar->toggleViewAction()); _ui->toolBar->setWindowTitle(tr("Control toolbar")); QAction * a = _ui->menuShow_view->addAction("Progress dialog"); @@ -455,6 +458,7 @@ void MainWindow::closeEvent(QCloseEvent* event) _ui->dockWidget_loopClosureViewer->close(); _ui->dockWidget_mapVisibility->close(); _ui->dockWidget_graphViewer->close(); + _ui->dockWidget_odometry->close(); if(_camera) { @@ -561,7 +565,7 @@ void MainWindow::handleEvent(UEvent* anEvent) else if(anEvent->getClassName().compare("OdometryEvent") == 0) { OdometryEvent * odomEvent = (OdometryEvent*)anEvent; - if(_ui->dockWidget_cloudViewer->isVisible() && + if((_ui->dockWidget_cloudViewer->isVisible() || _ui->dockWidget_odometry->isVisible()) && _lastOdometryProcessed && !_processingStatistics) { @@ -592,12 +596,16 @@ void MainWindow::handleEvent(UEvent* anEvent) void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality, float time, int features, int localMapSize) { Transform pose = data.pose(); + bool lost = false; + _ui->imageView_odometry->resetTransform(); if(pose.isNull()) { UDEBUG("odom lost"); // use last pose _ui->widget_cloudViewer->setBackgroundColor(Qt::darkRed); + _ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::darkRed)); pose = _lastOdomPose; + lost = true; } else if(quality>=0 && _preferencesDialog->getOdomQualityWarnThr() && @@ -605,11 +613,13 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality, { UDEBUG("odom warn, quality=%d thr=%d", quality, _preferencesDialog->getOdomQualityWarnThr()); _ui->widget_cloudViewer->setBackgroundColor(Qt::darkYellow); + _ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::darkYellow)); } else { UDEBUG("odom ok"); _ui->widget_cloudViewer->setBackgroundColor(Qt::black); + _ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::black)); } if(quality >= 0) { @@ -627,65 +637,94 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality, { _ui->statsToolBox->updateStat("Odometry/LocalMapSize/", (float)data.id(), (float)localMapSize); } - if(!pose.isNull()) + + if(_ui->dockWidget_cloudViewer->isVisible()) { - _lastOdomPose = pose; - _odometryReceived = true; - - // 3d cloud - if(data.depth().cols == data.image().cols && - data.depth().rows == data.image().rows && - !data.depth().empty() && - data.fx() > 0.0f && - data.fy() > 0.0f && - _preferencesDialog->isCloudsShown(1)) + if(!pose.isNull()) { - pcl::PointCloud::Ptr cloud; - cloud = createCloud(0, - data.image(), - data.depth(), - data.fx(), - data.fy(), - data.cx(), - data.cy(), - data.localTransform(), - pose, - _preferencesDialog->getCloudVoxelSize(1), - _preferencesDialog->getCloudDecimation(1), - _preferencesDialog->getCloudMaxDepth(1)); + _lastOdomPose = pose; + _odometryReceived = true; - if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection)) + // 3d cloud + if(data.depth().cols == data.image().cols && + data.depth().rows == data.image().rows && + !data.depth().empty() && + data.fx() > 0.0f && + data.fy() > 0.0f && + _preferencesDialog->isCloudsShown(1)) { - UERROR("Adding cloudOdom to viewer failed!"); + pcl::PointCloud::Ptr cloud; + cloud = createCloud(0, + data.image(), + data.depth(), + data.fx(), + data.fy(), + data.cx(), + data.cy(), + data.localTransform(), + pose, + _preferencesDialog->getCloudVoxelSize(1), + _preferencesDialog->getCloudDecimation(1), + _preferencesDialog->getCloudMaxDepth(1)); + + if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection)) + { + UERROR("Adding cloudOdom to viewer failed!"); + } + _ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true); + _ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1)); + _ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1)); } - _ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true); - _ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1)); - _ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1)); + + // 2d cloud + if(!data.depth2d().empty() && + _preferencesDialog->isScansShown(1)) + { + pcl::PointCloud::Ptr cloud; + cloud = util3d::depth2DToPointCloud(data.depth2d()); + cloud = util3d::transformPointCloud(cloud, pose); + if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection)) + { + UERROR("Adding scanOdom to viewer failed!"); + } + _ui->widget_cloudViewer->setCloudVisibility("scanOdom", true); + _ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1)); + _ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1)); + } + + if(!data.pose().isNull()) + { + // update camera position + _ui->widget_cloudViewer->updateCameraPosition(_odometryCorrection*data.pose()); + } + } + _ui->widget_cloudViewer->render(); + } + + if(_ui->dockWidget_odometry->isVisible() && + !data.image().empty()) + { + if(lost) + { + _ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.image())); + _ui->imageView_odometry->setImageShown(true); + _ui->imageView_odometry->setImageDepthShown(true); + } + else + { + _ui->imageView_odometry->setImage(uCvMat2QImage(data.image())); + _ui->imageView_odometry->setImageShown(true); + _ui->imageView_odometry->setImageDepthShown(false); } - // 2d cloud - if(!data.depth2d().empty() && - _preferencesDialog->isScansShown(1)) + _ui->imageView_odometry->resetZoom(); + _ui->imageView_odometry->setSceneRect(_ui->imageView_odometry->scene()->itemsBoundingRect()); + _ui->imageView_odometry->fitInView(_ui->imageView_odometry->sceneRect(), Qt::KeepAspectRatio); + if(_preferencesDialog->isImageFlipped()) { - pcl::PointCloud::Ptr cloud; - cloud = util3d::depth2DToPointCloud(data.depth2d()); - cloud = util3d::transformPointCloud(cloud, pose); - if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection)) - { - UERROR("Adding scanOdom to viewer failed!"); - } - _ui->widget_cloudViewer->setCloudVisibility("scanOdom", true); - _ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1)); - _ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1)); - } - - if(!data.pose().isNull()) - { - // update camera position - _ui->widget_cloudViewer->updateCameraPosition(_odometryCorrection*data.pose()); + _ui->imageView_odometry->scale(-1.0, 1.0); } } - _ui->widget_cloudViewer->render(); _lastOdometryProcessed = true; @@ -1885,8 +1924,10 @@ void MainWindow::resizeEvent(QResizeEvent* anEvent) { _ui->imageView_source->fitInView(_ui->imageView_source->sceneRect(), Qt::KeepAspectRatio); _ui->imageView_loopClosure->fitInView(_ui->imageView_source->sceneRect(), Qt::KeepAspectRatio); + _ui->imageView_source->fitInView(_ui->imageView_odometry->sceneRect(), Qt::KeepAspectRatio); _ui->imageView_source->resetZoom(); _ui->imageView_loopClosure->resetZoom(); + _ui->imageView_odometry->resetZoom(); } void MainWindow::updateSelectSourceImageMenu(int type) @@ -3266,10 +3307,13 @@ void MainWindow::clearTheCache() _ui->graphicsView_graphView->clearAll(); _ui->imageView_source->clear(); _ui->imageView_loopClosure->clear(); + _ui->imageView_odometry->clear(); _ui->imageView_source->resetTransform(); _ui->imageView_loopClosure->resetTransform(); + _ui->imageView_odometry->resetTransform(); _ui->imageView_source->setBackgroundBrush(QBrush(Qt::black)); _ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::black)); + _ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::black)); } void MainWindow::updateElapsedTime() diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index a4bf6e67..d1c6448f 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -716,6 +716,27 @@ + + + Odometry + + + 1 + + + + + 0 + + + 0 + + + + + + + Exit diff --git a/package.xml b/package.xml index 6873ac9d..3d9acf52 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap - 0.7.2 + 0.7.3 RTAB-Map's standalone library. RTAB-Map is an RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe