From 7b0a34b150c5cff9ece9dfff12bb5e62404f42a4 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 23 Jun 2014 20:43:08 +0000 Subject: [PATCH] Save/view high-res point clouds depending on the visible clouds in Map view (only those checked in MapVisibility view) git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1437 f169173b-cf89-36c8-b27e-44dbe73f0c83 --- corelib/include/rtabmap/core/Parameters.h | 2 +- guilib/include/rtabmap/gui/CloudViewer.h | 6 +- guilib/include/rtabmap/gui/DatabaseViewer.h | 2 +- guilib/include/rtabmap/gui/MainWindow.h | 12 +- guilib/src/CloudViewer.cpp | 19 +- guilib/src/DatabaseViewer.cpp | 2 +- guilib/src/GraphViewer.cpp | 27 ++- guilib/src/GraphViewer.h | 2 +- guilib/src/MainWindow.cpp | 254 ++++++++++++-------- guilib/src/MapVisibilityWidget.cpp | 20 +- guilib/src/MapVisibilityWidget.h | 4 +- guilib/src/PreferencesDialog.cpp | 21 +- guilib/src/ui/preferencesDialog.ui | 154 ++++++------ 13 files changed, 310 insertions(+), 215 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index dd9009ef..b6d447fe 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -166,7 +166,7 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM_STR(Kp, DictionaryPath, "", "Path of the pre-computed dictionary"); //Database - RTABMAP_PARAM(DbSqlite3, InMemory, bool, true, "Using database in the memory instead of a file on the hard disk."); + RTABMAP_PARAM(DbSqlite3, InMemory, bool, false, "Using database in the memory instead of a file on the hard disk."); RTABMAP_PARAM(DbSqlite3, CacheSize, unsigned int, 10000, "Sqlite cache size (default is 2000)."); RTABMAP_PARAM(DbSqlite3, JournalMode, int, 3, "0=DELETE, 1=TRUNCATE, 2=PERSIST, 3=MEMORY, 4=OFF (see sqlite3 doc : \"PRAGMA journal_mode\")"); RTABMAP_PARAM(DbSqlite3, Synchronous, int, 0, "0=OFF, 1=NORMAL, 2=FULL (see sqlite3 doc : \"PRAGMA synchronous\")"); diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index a89892d9..4989ca3b 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -106,8 +106,10 @@ public: const QMap & getAddedClouds() {return _addedClouds;} //including meshes - void setCameraTargetLocked(); - void setCameraTargetFollow(); + void setCameraTargetLocked(bool enabled = true); + void setCameraTargetFollow(bool enabled = true); + void setCameraFree(); + void setCameraLockZ(bool enabled = true); public slots: void render(); diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/guilib/include/rtabmap/gui/DatabaseViewer.h index 806a8a90..d8847c8e 100644 --- a/guilib/include/rtabmap/gui/DatabaseViewer.h +++ b/guilib/include/rtabmap/gui/DatabaseViewer.h @@ -92,7 +92,7 @@ private: std::list > graphes_; std::map poses_; std::multimap links_; - std::map > scans_; + QMap > scans_; }; #endif /* DATABASEVIEWER_H_ */ diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 06088b4e..fa510644 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -176,6 +176,8 @@ signals: private: void update3DMapVisibility(bool cloudsShown, bool scansShown); void updateMapCloud(const std::map & poses, const Transform & pose); + void createAndAddCloudToMap(int nodeId, const Transform & pose); + void createAndAddScanToMap(int nodeId, const Transform & pose); std::map radiusPosesFiltering(const std::map & poses) const; void drawKeypoints(const std::multimap & refWords, const std::multimap & loopWords); void setupMainLayout(bool vertical); @@ -183,7 +185,7 @@ private: void updateSelectSourceDatabase(bool used); void updateSelectSourceRGBDMenu(bool used, PreferencesDialog::Src src); - pcl::PointCloud::Ptr createAssembledCloud(); + pcl::PointCloud::Ptr createAssembledCloud(const std::map & poses) const; pcl::PointCloud::Ptr createCloud( int id, const cv::Mat & rgb, @@ -193,9 +195,9 @@ private: const Transform & pose, float voxelSize, int decimation, - float maxDepth); - std::map::Ptr > createPointClouds(); - std::map createMeshes(); + float maxDepth) const; + std::map::Ptr > createPointClouds(const std::map & poses) const; + std::map createMeshes(const std::map & poses) const; void savePointClouds(const std::map::Ptr> & clouds); void saveMeshes(const std::map & meshes); @@ -222,7 +224,7 @@ private: QMap > _imagesMap; QMap > _depthsMap; - std::map > _depths2DMap; + QMap > _depths2DMap; QMap _depthConstantsMap; QMap _localTransformsMap; std::map _currentPosesMap; diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index 670662cc..2b5eddb2 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -536,14 +536,25 @@ void CloudViewer::setCloudPointSize(const std::string & id, int size) } } -void CloudViewer::setCameraTargetLocked() +void CloudViewer::setCameraTargetLocked(bool enabled) { - _aLockCamera->setChecked(true); + _aLockCamera->setChecked(enabled); } -void CloudViewer::setCameraTargetFollow() +void CloudViewer::setCameraTargetFollow(bool enabled) { - _aFollowCamera->setChecked(true); + _aFollowCamera->setChecked(enabled); +} + +void CloudViewer::setCameraFree() +{ + _aLockCamera->setChecked(false); + _aFollowCamera->setChecked(false); +} + +void CloudViewer::setCameraLockZ(bool enabled) +{ + _aLockViewZ->setChecked(enabled); } Eigen::Vector3f rotatePointAroundAxe( diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index df634e17..799184f0 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -904,7 +904,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) memory_->getImageDepth(ids_.at(i), imageBytesA, depthBytesA, depth2dBytesA, depthConstantA, localTransformA); if(depth2dBytesA.size()) { - scans_.insert(std::make_pair(ids_.at(i), depth2dBytesA)); + scans_.insert(ids_.at(i), depth2dBytesA); } } UINFO("Update scans list... done"); diff --git a/guilib/src/GraphViewer.cpp b/guilib/src/GraphViewer.cpp index 17d22062..83eea193 100644 --- a/guilib/src/GraphViewer.cpp +++ b/guilib/src/GraphViewer.cpp @@ -132,7 +132,7 @@ GraphViewer::GraphViewer(QWidget * parent) : _neighborColor(Qt::blue), _loopClosureColor(Qt::red), _root(0), - _nodeRadius(0.1), + _nodeRadius(0.01), _linkWidth(0), _gridMap(0), _gridCellSize(0.05f) @@ -167,7 +167,7 @@ GraphViewer::~GraphViewer() void GraphViewer::updateGraph(const std::map & poses, const std::multimap & constraints, - const std::map > & scans) + const QMap > & scans) { //Hide nodes and links for(QMap::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter) @@ -191,9 +191,9 @@ void GraphViewer::updateGraph(const std::map & poses, if(itemIter != _nodeItems.end()) { itemIter.value()->setPose(iter->second); - if(itemIter.value()->getScan().empty() && uContains(scans, iter->first)) + if(itemIter.value()->getScan().empty() && scans.contains(iter->first)) { - cv::Mat depth2d = util3d::uncompressData(scans.at(iter->first)); + cv::Mat depth2d = util3d::uncompressData(scans.value(iter->first)); itemIter.value()->setScan(depth2d); } if(!itemIter.value()->getScan().empty() && _gridMap->isVisible()) @@ -205,11 +205,11 @@ void GraphViewer::updateGraph(const std::map & poses, else { // create node item - std::map >::const_iterator jter = scans.find(iter->first); + QMap >::const_iterator jter = scans.find(iter->first); cv::Mat depth2d; if(jter != scans.end()) { - depth2d = util3d::uncompressData(jter->second); + depth2d = util3d::uncompressData(jter.value()); if(_gridMap->isVisible()) { scanClouds.insert(std::make_pair(iter->first, util3d::depth2DToPointCloud(depth2d))); @@ -319,19 +319,20 @@ void GraphViewer::updateGraph(const std::map & poses, { for (int j = 0; j < map8S.cols; ++j) { - unsigned char gray = map8S.at(i, j); - if(gray == -1) - { - gray = 89; - } - else if(gray == 0) + char v = map8S.at(i, j); + unsigned char gray; + if(v == 0) { gray = 178; } - else if(gray == 100) + else if(v == 100) { gray = 0; } + else // -1 + { + gray = 89; + } map8U.at(i, j) = gray; } } diff --git a/guilib/src/GraphViewer.h b/guilib/src/GraphViewer.h index 09368bab..8b604a79 100644 --- a/guilib/src/GraphViewer.h +++ b/guilib/src/GraphViewer.h @@ -28,7 +28,7 @@ public: void updateGraph(const std::map & poses, const std::multimap & constraints, - const std::map > & scans); + const QMap > & scans); void clearGraph(); void setWorkingDirectory(const QString & path) {_workingDirectory = path;} diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index a7cd18ac..ebdabc1d 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -704,7 +704,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) { if(!iter->second.empty()) { - _depths2DMap.insert(std::make_pair(iter->first, iter->second)); + _depths2DMap.insert(iter->first, iter->second); } } } @@ -941,7 +941,6 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) updateMapCloud(stat.poses(), _odometryReceived?Transform():stat.currentPose()); // update some widgets - _ui->widget_mapVisibility->setMap(stat.poses()); if(_ui->graphicsView_graphView->isVisible()) { _ui->graphicsView_graphView->updateGraph(stat.poses(), stat.constraints(), _depths2DMap); @@ -974,7 +973,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) std::multimap(), std::multimap(), Transform(), - uValue(_depths2DMap, loopOldId, std::vector()), + _depths2DMap.value(loopOldId, std::vector()), _imagesMap.value(loopOldId, std::vector()), _depthsMap.value(loopOldId, std::vector()), _depthConstantsMap.value(loopOldId, 0.0f), @@ -986,7 +985,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat) std::multimap(), std::multimap(), loopClosureTransform, - uValue(_depths2DMap, loopNewId, std::vector()), + _depths2DMap.value(loopNewId, std::vector()), _imagesMap.value(loopNewId, std::vector()), _depthsMap.value(loopNewId, std::vector()), _depthConstantsMap.value(loopNewId, 0.0f), @@ -1046,6 +1045,12 @@ void MainWindow::updateMapCloud(const std::map & posesIn, const { poses = posesIn; } + std::map posesMask; + for(std::map::const_iterator iter = posesIn.begin(); iter!=posesIn.end(); ++iter) + { + posesMask.insert(posesMask.end(), std::make_pair(iter->first, poses.find(iter->first) != poses.end())); + } + _ui->widget_mapVisibility->setMap(posesIn, posesMask); // Map updated! regenerate the assembled cloud, last pose is the new one UDEBUG("Update map with %d locations (currentPose=%s)", poses.size(), currentPose.prettyPrint().c_str()); @@ -1077,62 +1082,7 @@ void MainWindow::updateMapCloud(const std::map & posesIn, const } else if(_imagesMap.contains(iter->first) && _depthsMap.contains(iter->first)) { - pcl::PointCloud::Ptr cloud; - cloud = createCloud(iter->first, - util3d::uncompressImage(_imagesMap.value(iter->first)), - util3d::uncompressImage(_depthsMap.value(iter->first)), - _depthConstantsMap.value(iter->first), - _localTransformsMap.value(iter->first), - Transform::getIdentity(), - _preferencesDialog->getCloudVoxelSize(0), - _preferencesDialog->getCloudDecimation(0), - _preferencesDialog->getCloudMaxDepth(0)); - - if(_preferencesDialog->isCloudMeshing(0)) - { - pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh); - if(cloud->size()) - { - pcl::PointCloud::Ptr cloudWithNormals; - if(_preferencesDialog->getMeshSmoothing(0)) - { - cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(0)); - } - else - { - cloudWithNormals = util3d::computeNormals(cloud, _preferencesDialog->getMeshNormalKSearch(0)); - } - mesh = util3d::createMesh(cloudWithNormals, _preferencesDialog->getMeshGP3Radius(0)); - } - - if(mesh->polygons.size()) - { - pcl::PointCloud::Ptr tmp(new pcl::PointCloud); - pcl::fromPCLPointCloud2(mesh->cloud, *tmp); - if(!_ui->widget_cloudViewer->addCloudMesh(cloudName, tmp, mesh->polygons, iter->second)) - { - UERROR("Adding mesh cloud %d to viewer failed!", iter->first); - } - - } - } - else - { - if(_preferencesDialog->getMeshSmoothing(0)) - { - pcl::PointCloud::Ptr cloudWithNormals; - cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(0)); - cloud->clear(); - pcl::copyPointCloud(*cloudWithNormals, *cloud); - } - if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, iter->second)) - { - UERROR("Adding cloud %d to viewer failed!", iter->first); - } - } - - _ui->widget_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0)); - _ui->widget_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0)); + this->createAndAddCloudToMap(iter->first, iter->second); } } else if(viewerClouds.contains(cloudName)) @@ -1161,17 +1111,9 @@ void MainWindow::updateMapCloud(const std::map & posesIn, const _ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0)); _ui->widget_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0)); } - else if(uContains(_depths2DMap, iter->first)) + else if(_depths2DMap.contains(iter->first)) { - pcl::PointCloud::Ptr cloud; - cv::Mat depth2d = util3d::uncompressData(_depths2DMap.at(iter->first)); - cloud = util3d::depth2DToPointCloud(depth2d); - if(!_ui->widget_cloudViewer->addOrUpdateCloud(scanName, cloud, iter->second)) - { - UERROR("Adding cloud %d to viewer failed!", iter->first); - } - _ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0)); - _ui->widget_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0)); + this->createAndAddScanToMap(iter->first, iter->second); } } else if(viewerClouds.contains(scanName)) @@ -1238,20 +1180,120 @@ void MainWindow::updateMapCloud(const std::map & posesIn, const _ui->widget_cloudViewer->render(); } +void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose) +{ + std::string cloudName = uFormat("cloud%d", nodeId); + if(_ui->widget_cloudViewer->getAddedClouds().contains(cloudName)) + { + UERROR("Cloud %d already added to map.", nodeId); + return; + } + pcl::PointCloud::Ptr cloud; + cloud = createCloud(nodeId, + util3d::uncompressImage(_imagesMap.value(nodeId)), + util3d::uncompressImage(_depthsMap.value(nodeId)), + _depthConstantsMap.value(nodeId), + _localTransformsMap.value(nodeId), + Transform::getIdentity(), + _preferencesDialog->getCloudVoxelSize(0), + _preferencesDialog->getCloudDecimation(0), + _preferencesDialog->getCloudMaxDepth(0)); + + if(_preferencesDialog->isCloudMeshing(0)) + { + pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh); + if(cloud->size()) + { + pcl::PointCloud::Ptr cloudWithNormals; + if(_preferencesDialog->getMeshSmoothing(0)) + { + cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(0)); + } + else + { + cloudWithNormals = util3d::computeNormals(cloud, _preferencesDialog->getMeshNormalKSearch(0)); + } + mesh = util3d::createMesh(cloudWithNormals, _preferencesDialog->getMeshGP3Radius(0)); + } + + if(mesh->polygons.size()) + { + pcl::PointCloud::Ptr tmp(new pcl::PointCloud); + pcl::fromPCLPointCloud2(mesh->cloud, *tmp); + if(!_ui->widget_cloudViewer->addCloudMesh(cloudName, tmp, mesh->polygons, pose)) + { + UERROR("Adding mesh cloud %d to viewer failed!", nodeId); + } + + } + } + else + { + if(_preferencesDialog->getMeshSmoothing(0)) + { + pcl::PointCloud::Ptr cloudWithNormals; + cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(0)); + cloud->clear(); + pcl::copyPointCloud(*cloudWithNormals, *cloud); + } + if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, pose)) + { + UERROR("Adding cloud %d to viewer failed!", nodeId); + } + } + + _ui->widget_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0)); + _ui->widget_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0)); +} + +void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose) +{ + std::string scanName = uFormat("scan%d", nodeId); + if(_ui->widget_cloudViewer->getAddedClouds().contains(scanName)) + { + UERROR("Scan %d already added to map.", nodeId); + return; + } + pcl::PointCloud::Ptr cloud; + cv::Mat depth2d = util3d::uncompressData(_depths2DMap.value(nodeId)); + cloud = util3d::depth2DToPointCloud(depth2d); + if(!_ui->widget_cloudViewer->addOrUpdateCloud(scanName, cloud, pose)) + { + UERROR("Adding cloud %d to viewer failed!", nodeId); + } + _ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0)); + _ui->widget_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0)); +} + void MainWindow::updateNodeVisibility(int nodeId, bool visible) { if(_currentPosesMap.find(nodeId) != _currentPosesMap.end()) { - if(_preferencesDialog->isCloudsShown(0)) + QMap viewerClouds = _ui->widget_cloudViewer->getAddedClouds(); + if(_preferencesDialog->isCloudsShown(0) && _depthsMap.contains(nodeId)) { std::string cloudName = uFormat("cloud%d", nodeId); - _ui->widget_cloudViewer->setCloudVisibility(cloudName, visible); + if(visible && !viewerClouds.contains(cloudName)) + { + createAndAddCloudToMap(nodeId, _currentPosesMap.find(nodeId)->second); + } + else if(viewerClouds.contains(cloudName)) + { + _ui->widget_cloudViewer->setCloudVisibility(cloudName, visible); + } } - if(_preferencesDialog->isScansShown(0)) + if(_preferencesDialog->isScansShown(0) && _depths2DMap.contains(nodeId)) { std::string scanName = uFormat("scan%d", nodeId); - _ui->widget_cloudViewer->setCloudVisibility(scanName, visible); + if(visible && !viewerClouds.contains(scanName)) + { + createAndAddScanToMap(nodeId, _currentPosesMap.find(nodeId)->second); + } + else if(viewerClouds.contains(scanName)) + { + _ui->widget_cloudViewer->setCloudVisibility(scanName, visible); + } } } _ui->widget_cloudViewer->render(); @@ -1425,7 +1467,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve iter!=event.getDepths2d().end(); ++iter) { - _depths2DMap.insert(std::make_pair(iter->first, iter->second)); + _depths2DMap.insert(iter->first, iter->second); } _initProgressDialog->appendText(tr("Inserted %1 laser scans.").arg(_depths2DMap.size())); _initProgressDialog->incrementStep(); @@ -1458,7 +1500,7 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve _ui->graphicsView_graphView->updateGraph( event.getPoses(), event.getConstraints(), - event.getDepths2d().size()?event.getDepths2d():_depths2DMap); + _depths2DMap); _initProgressDialog->appendText("Updating the graph view... done."); } } @@ -2825,15 +2867,17 @@ void MainWindow::savePointClouds() if(button == QMessageBox::Yes || button == QMessageBox::No) { + std::map poses = _ui->widget_mapVisibility->getVisiblePoses(); + _initProgressDialog->setAutoClose(true, 1); _initProgressDialog->resetProgress(); _initProgressDialog->show(); - _initProgressDialog->setMaximumSteps(int(_currentPosesMap.size())*2+1); + _initProgressDialog->setMaximumSteps(int(poses.size())*2+1); std::map::Ptr> clouds; if(button == QMessageBox::Yes) { - pcl::PointCloud::Ptr cloud = this->createAssembledCloud(); + pcl::PointCloud::Ptr cloud = this->createAssembledCloud(poses); if(_preferencesDialog->getMeshSmoothing(1)) { pcl::PointCloud::Ptr cloudWithNormals; @@ -2849,7 +2893,7 @@ void MainWindow::savePointClouds() } else { - clouds = this->createPointClouds(); + clouds = this->createPointClouds(poses); } savePointClouds(clouds); _initProgressDialog->setValue(_initProgressDialog->maximumSteps()); @@ -2867,15 +2911,17 @@ void MainWindow::saveMeshes() if(button == QMessageBox::Yes || button == QMessageBox::No) { + std::map poses = _ui->widget_mapVisibility->getVisiblePoses(); + _initProgressDialog->setAutoClose(true, 1); _initProgressDialog->resetProgress(); _initProgressDialog->show(); - _initProgressDialog->setMaximumSteps(int(_currentPosesMap.size())*2+1); + _initProgressDialog->setMaximumSteps(int(poses.size())*2+1); std::map meshes; if(button == QMessageBox::Yes) { - pcl::PointCloud::Ptr cloud = this->createAssembledCloud(); + pcl::PointCloud::Ptr cloud = this->createAssembledCloud(poses); _initProgressDialog->appendText(tr("Meshing the assembled cloud (%1 points)...").arg(cloud->size())); _initProgressDialog->incrementStep(); QApplication::processEvents(); @@ -2907,7 +2953,7 @@ void MainWindow::saveMeshes() } else { - meshes = this->createMeshes(); + meshes = this->createMeshes(poses); } saveMeshes(meshes); _initProgressDialog->setValue(_initProgressDialog->maximumSteps()); @@ -2925,15 +2971,17 @@ void MainWindow::viewPointClouds() if(button == QMessageBox::Yes || button == QMessageBox::No) { + std::map poses = _ui->widget_mapVisibility->getVisiblePoses(); + _initProgressDialog->setAutoClose(true, 1); _initProgressDialog->resetProgress(); _initProgressDialog->show(); - _initProgressDialog->setMaximumSteps(int(_currentPosesMap.size())+1); + _initProgressDialog->setMaximumSteps(int(poses.size())+1); std::map::Ptr> clouds; if(button == QMessageBox::Yes) { - pcl::PointCloud::Ptr cloud = this->createAssembledCloud(); + pcl::PointCloud::Ptr cloud = this->createAssembledCloud(poses); if(_preferencesDialog->getMeshSmoothing(1)) { @@ -2951,7 +2999,7 @@ void MainWindow::viewPointClouds() } else { - clouds = this->createPointClouds(); + clouds = this->createPointClouds(poses); } if(clouds.size()) @@ -2964,6 +3012,7 @@ void MainWindow::viewPointClouds() window->setMinimumHeight(600); CloudViewer * viewer = new CloudViewer(window); + viewer->setCameraLockZ(false); QVBoxLayout *layout = new QVBoxLayout(); layout->addWidget(viewer); @@ -3001,15 +3050,17 @@ void MainWindow::viewMeshes() if(button == QMessageBox::Yes || button == QMessageBox::No) { + std::map poses = _ui->widget_mapVisibility->getVisiblePoses(); + _initProgressDialog->setAutoClose(true, 1); _initProgressDialog->resetProgress(); _initProgressDialog->show(); - _initProgressDialog->setMaximumSteps(int(_currentPosesMap.size())+1); + _initProgressDialog->setMaximumSteps(int(poses.size())+1); std::map meshes; if(button == QMessageBox::Yes) { - pcl::PointCloud::Ptr cloud = this->createAssembledCloud(); + pcl::PointCloud::Ptr cloud = this->createAssembledCloud(poses); _initProgressDialog->appendText(tr("Meshing the assembled cloud (%1 points)...").arg(cloud->size())); _initProgressDialog->incrementStep(); QApplication::processEvents(); @@ -3041,7 +3092,7 @@ void MainWindow::viewMeshes() } else { - meshes = this->createMeshes(); + meshes = this->createMeshes(poses); } if(meshes.size()) @@ -3053,6 +3104,7 @@ void MainWindow::viewMeshes() window->setMinimumHeight(600); CloudViewer * viewer = new CloudViewer(window); + viewer->setCameraLockZ(false); QVBoxLayout *layout = new QVBoxLayout(); layout->addWidget(viewer); @@ -3375,7 +3427,7 @@ pcl::PointCloud::Ptr MainWindow::createCloud( const Transform & pose, float voxelSize, int decimation, - float maxDepth) + float maxDepth) const { UTimer timer; pcl::PointCloud::Ptr cloud = util3d::cloudFromDepthRGB( @@ -3416,12 +3468,12 @@ pcl::PointCloud::Ptr MainWindow::createCloud( return cloud; } -pcl::PointCloud::Ptr MainWindow::createAssembledCloud() +pcl::PointCloud::Ptr MainWindow::createAssembledCloud(const std::map & poses) const { pcl::PointCloud::Ptr assembledCloud(new pcl::PointCloud); int i=0; int count = 0; - for(std::map::const_iterator iter = _currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter) + for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { bool inserted = false; if(!iter->second.isNull()) @@ -3465,7 +3517,7 @@ pcl::PointCloud::Ptr MainWindow::createAssembledCloud() if(inserted) { - _initProgressDialog->appendText(tr("Generated cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(_currentPosesMap.size())); + _initProgressDialog->appendText(tr("Generated cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size())); if(count % 100 == 0) { @@ -3477,7 +3529,7 @@ pcl::PointCloud::Ptr MainWindow::createAssembledCloud() } else { - _initProgressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(_currentPosesMap.size())); + _initProgressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size())); } _initProgressDialog->incrementStep(); QApplication::processEvents(); @@ -3491,11 +3543,11 @@ pcl::PointCloud::Ptr MainWindow::createAssembledCloud() return assembledCloud; } -std::map::Ptr > MainWindow::createPointClouds() +std::map::Ptr > MainWindow::createPointClouds(const std::map & poses) const { std::map::Ptr> clouds; int i=0; - for(std::map::const_iterator iter = _currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter) + for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { bool inserted = false; if(!iter->second.isNull()) @@ -3545,11 +3597,11 @@ std::map::Ptr > MainWindow::createPointCl if(inserted) { - _initProgressDialog->appendText(tr("Generated cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(_currentPosesMap.size())); + _initProgressDialog->appendText(tr("Generated cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size())); } else { - _initProgressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(_currentPosesMap.size())); + _initProgressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size())); } _initProgressDialog->incrementStep(); QApplication::processEvents(); @@ -3558,11 +3610,11 @@ std::map::Ptr > MainWindow::createPointCl return clouds; } -std::map MainWindow::createMeshes() +std::map MainWindow::createMeshes(const std::map & poses) const { std::map meshes; int i=0; - for(std::map::const_iterator iter = _currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter) + for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { bool inserted = false; if(!iter->second.isNull()) @@ -3619,11 +3671,11 @@ std::map MainWindow::createMeshes() if(inserted) { - _initProgressDialog->appendText(tr("Generated mesh %1 (%2/%3).").arg(iter->first).arg(++i).arg(_currentPosesMap.size())); + _initProgressDialog->appendText(tr("Generated mesh %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size())); } else { - _initProgressDialog->appendText(tr("Ignored mesh %1 (%2/%3).").arg(iter->first).arg(++i).arg(_currentPosesMap.size())); + _initProgressDialog->appendText(tr("Ignored mesh %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size())); } _initProgressDialog->incrementStep(); QApplication::processEvents(); diff --git a/guilib/src/MapVisibilityWidget.cpp b/guilib/src/MapVisibilityWidget.cpp index 853b17f8..80f2af0d 100644 --- a/guilib/src/MapVisibilityWidget.cpp +++ b/guilib/src/MapVisibilityWidget.cpp @@ -57,7 +57,7 @@ void MapVisibilityWidget::updateCheckBoxes() added = true; } checkboxes[i]->setText(QString("%1 (%2)").arg(iter->first).arg(iter->second.prettyPrint().c_str())); - checkboxes[i]->setChecked(true); + checkboxes[i]->setChecked(_mask.at(iter->first)); if(added) { connect(checkboxes[i], SIGNAL(stateChanged(int)), this, SLOT(signalVisibility())); @@ -68,18 +68,34 @@ void MapVisibilityWidget::updateCheckBoxes() } } -void MapVisibilityWidget::setMap(const std::map & poses) +void MapVisibilityWidget::setMap(const std::map & poses, const std::map & mask) { + UASSERT(poses.size() == mask.size()); _poses = poses; + _mask = mask; if(this->isVisible()) { updateCheckBoxes(); } } +std::map MapVisibilityWidget::getVisiblePoses() const +{ + std::map poses; + for(std::map::const_iterator iter=_poses.begin(); iter!=_poses.end(); ++iter) + { + if(_mask.at(iter->first)) + { + poses.insert(*iter); + } + } + return poses; +} + void MapVisibilityWidget::signalVisibility() { QCheckBox * check = qobject_cast(sender()); + _mask.at(check->text().split('(').first().toInt()) = check->isChecked(); emit visibilityChanged(check->text().split('(').first().toInt(), check->isChecked()); } diff --git a/guilib/src/MapVisibilityWidget.h b/guilib/src/MapVisibilityWidget.h index 62720fa4..9268baf6 100644 --- a/guilib/src/MapVisibilityWidget.h +++ b/guilib/src/MapVisibilityWidget.h @@ -20,7 +20,8 @@ public: MapVisibilityWidget(QWidget * parent = 0); virtual ~MapVisibilityWidget(); - void setMap(const std::map & poses); + void setMap(const std::map & poses, const std::map & mask); + std::map getVisiblePoses() const; protected: virtual void showEvent(QShowEvent * event); @@ -36,6 +37,7 @@ signals: private: std::map _poses; + std::map _mask; }; } /* namespace rtabmap */ diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index de810fcc..26b3a3a0 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -2324,12 +2324,21 @@ void PreferencesDialog::updateKpROI() void PreferencesDialog::changeDatabasePath() { - QString path = QFileDialog::getSaveFileName( - this, - tr("Select database file..."), - _ui->lineEdit_databasePath->text(), - tr("RTAB-Map database files (*.db)"), - 0); + QFileDialog fileDialog; + fileDialog.setFileMode(QFileDialog::AnyFile); + fileDialog.setNameFilter(tr("RTAB-Map database files (*.db)")); + fileDialog.setDefaultSuffix("db"); + fileDialog.setDirectory(QFileInfo(_ui->lineEdit_databasePath->text()).dir()); + fileDialog.selectFile (_ui->lineEdit_databasePath->text()); + QStringList fileNames; + QString path; + if(fileDialog.exec()) + { + if(!fileDialog.selectedFiles().empty()) + { + path = fileDialog.selectedFiles().first(); + } + } if(!path.isEmpty()) { diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index d58e033b..98a05697 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -6,8 +6,8 @@ 0 0 - 1007 - 494 + 876 + 526 @@ -64,8 +64,8 @@ 0 0 - 709 - 752 + 577 + 632 @@ -86,7 +86,7 @@ QFrame::Raised - 9 + 0 @@ -338,6 +338,78 @@ Show a yellow background when the number of odometry inliers goes under this thr 3D Rendering + + + + Cloud filtering + + + true + + + false + + + + + + For visualization, superposed clouds are not shown. By comparing poses in the same area, only one cloud in a fixed radius and angle is kept. + + + true + + + + + + + + + m + + + 0.010000000000000 + + + 0.500000000000000 + + + + + + + Radius + + + + + + + Angle + + + + + + + degrees + + + 0 + + + 180.000000000000000 + + + 30.000000000000000 + + + + + + + + @@ -979,78 +1051,6 @@ High-res view - - - - Cloud filtering - - - true - - - false - - - - - - For visualization, superposed clouds are not shown. By comparing poses in the same area, only one cloud in a fixed radius and angle is kept. - - - true - - - - - - - - - m - - - 0.010000000000000 - - - 0.500000000000000 - - - - - - - Radius - - - - - - - Angle - - - - - - - degrees - - - 0 - - - 180.000000000000000 - - - 30.000000000000000 - - - - - - - -