From b4b7b6f455ee59db816e537f262472cf3f188307 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 15 Jul 2016 17:46:13 -0400 Subject: [PATCH] Added Octomap visualization and export options --- corelib/include/rtabmap/core/OctoMap.h | 6 +- corelib/src/OctoMap.cpp | 124 ++++- guilib/include/rtabmap/gui/CloudViewer.h | 7 + guilib/include/rtabmap/gui/MainWindow.h | 7 +- .../include/rtabmap/gui/PreferencesDialog.h | 2 + guilib/src/AboutDialog.cpp | 6 + guilib/src/CMakeLists.txt | 11 + guilib/src/CloudViewer.cpp | 120 ++++- guilib/src/MainWindow.cpp | 484 ++++++++++++------ guilib/src/PreferencesDialog.cpp | 32 ++ guilib/src/ui/aboutDialog.ui | 66 ++- guilib/src/ui/mainWindow.ui | 129 +---- guilib/src/ui/preferencesDialog.ui | 140 +++-- 13 files changed, 737 insertions(+), 397 deletions(-) diff --git a/corelib/include/rtabmap/core/OctoMap.h b/corelib/include/rtabmap/core/OctoMap.h index a0fee4d8..5e12e0c9 100644 --- a/corelib/include/rtabmap/core/OctoMap.h +++ b/corelib/include/rtabmap/core/OctoMap.h @@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include namespace rtabmap { @@ -67,8 +68,9 @@ public: const octomap::ColorOcTree * octree() const {return octree_;} pcl::PointCloud::Ptr createCloud( + unsigned int treeDepth = 0, std::vector * obstacleIndices = 0, - std::vector * groundIndices = 0) const; + std::vector * emptyIndices = 0) const; cv::Mat createProjectionMap( float & xMin, @@ -76,6 +78,8 @@ public: float & gridCellSize, float minGridSize); + bool writeBinary(const std::string & path); + virtual ~OctoMap(); void clear(); diff --git a/corelib/src/OctoMap.cpp b/corelib/src/OctoMap.cpp index 94195198..c901d09c 100644 --- a/corelib/src/OctoMap.cpp +++ b/corelib/src/OctoMap.cpp @@ -59,6 +59,7 @@ void OctoMap::addToCache(int nodeId, pcl::PointCloud::Ptr & ground, pcl::PointCloud::Ptr & obstacles) { + UDEBUG("nodeId=%d", nodeId); cache_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles))); } @@ -92,7 +93,7 @@ void OctoMap::update(const std::map & poses) } if(graphChanged) { - UWARN("Graph changed!"); + UINFO("Graph changed!"); octomap::ColorOcTree * newOcTree = new octomap::ColorOcTree(octree_->getResolution()); std::map newOccupiedCells; int copied=0; @@ -299,32 +300,105 @@ void OctoMap::update(const std::map & poses) cache_.clear(); } -pcl::PointCloud::Ptr OctoMap::createCloud( - std::vector * obstacleIndices, - std::vector * groundIndices) const +void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v ) { + int i; + float f, p, q, t; + if( s == 0 ) { + // achromatic (grey) + *r = *g = *b = v; + return; + } + h /= 60; // sector 0 to 5 + i = floor( h ); + f = h - i; // factorial part of h + p = v * ( 1 - s ); + q = v * ( 1 - s * f ); + t = v * ( 1 - s * ( 1 - f ) ); + switch( i ) { + case 0: + *r = v; + *g = t; + *b = p; + break; + case 1: + *r = q; + *g = v; + *b = p; + break; + case 2: + *r = p; + *g = v; + *b = t; + break; + case 3: + *r = p; + *g = q; + *b = v; + break; + case 4: + *r = t; + *g = p; + *b = v; + break; + default: // case 5: + *r = v; + *g = p; + *b = q; + break; + } +} + +pcl::PointCloud::Ptr OctoMap::createCloud( + unsigned int treeDepth, + std::vector * obstacleIndices, + std::vector * emptyIndices) const +{ + UASSERT(treeDepth <= octree_->getTreeDepth()); pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - UDEBUG("occupied cells = %d", (int)occupiedCells_.size()); - cloud->resize(occupiedCells_.size()); + UDEBUG("depth=%d (maxDepth=%d) octree = %d", + (int)treeDepth, (int)octree_->getTreeDepth(), (int)octree_->size()); + cloud->resize(octree_->size()); if(obstacleIndices) { - obstacleIndices->resize(occupiedCells_.size()); + obstacleIndices->resize(octree_->size()); } - if(groundIndices) + if(emptyIndices) { - groundIndices->resize(occupiedCells_.size()); + emptyIndices->resize(octree_->size()); } + + if(treeDepth == 0) + { + treeDepth = octree_->getTreeDepth(); + } + + double minX, minY, minZ, maxX, maxY, maxZ; + octree_->getMetricMin(minX, minY, minZ); + octree_->getMetricMax(maxX, maxY, maxZ); + int oi=0; int si=0; int gi=0; - for(std::map::const_iterator iter = occupiedCells_.begin(); - iter!=occupiedCells_.end(); - ++iter) + for (octomap::ColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it) { - if(iter->second.isObstacle_ && octree_->isNodeOccupied(iter->first)) + if(octree_->isNodeOccupied(*it)) { - octomap::point3d pt = octree_->keyToCoord(iter->second.key_); - (*cloud)[oi] = pcl::PointXYZRGB(iter->first->getColor().r, iter->first->getColor().g, iter->first->getColor().b); + octomap::point3d pt = octree_->keyToCoord(it.getKey()); + if(octree_->getTreeDepth() == it.getDepth()) + { + (*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b); + } + else + { + // Gradiant color on z axis + float H = (maxZ - pt.z())*299.0f/(maxZ-minZ); + float r,g,b; + HSVtoRGB(&r, &g, &b, H, 1, 1); + (*cloud)[oi].r = r*255.0f; + (*cloud)[oi].g = g*255.0f; + (*cloud)[oi].b = b*255.0f; + } (*cloud)[oi].x = pt.x(); (*cloud)[oi].y = pt.y(); (*cloud)[oi].z = pt.z(); @@ -334,28 +408,29 @@ pcl::PointCloud::Ptr OctoMap::createCloud( } ++oi; } - else if(!iter->second.isObstacle_) + else { - octomap::point3d pt = octree_->keyToCoord(iter->second.key_); - (*cloud)[oi] = pcl::PointXYZRGB(iter->first->getColor().r, iter->first->getColor().g, iter->first->getColor().b); + octomap::point3d pt = octree_->keyToCoord(it.getKey()); + (*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b); (*cloud)[oi].x = pt.x(); (*cloud)[oi].y = pt.y(); (*cloud)[oi].z = pt.z(); - if(groundIndices) + if(emptyIndices) { - groundIndices->at(gi++) = oi; + emptyIndices->at(gi++) = oi; } ++oi; } } + cloud->resize(oi); if(obstacleIndices) { obstacleIndices->resize(si); } - if(groundIndices) + if(emptyIndices) { - groundIndices->resize(gi); + emptyIndices->resize(gi); } UDEBUG(""); @@ -428,4 +503,9 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel false); } +bool OctoMap::writeBinary(const std::string & path) +{ + return octree_->writeBinary(path); +} + } /* namespace rtabmap */ diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index f8c13913..f70ae361 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -58,9 +58,12 @@ namespace pcl { } class QMenu; +class vtkProp; namespace rtabmap { +class OctoMap; + class RTABMAPGUI_EXP CloudViewer : public QVTKWidget { Q_OBJECT @@ -136,6 +139,9 @@ public: const pcl::TextureMesh::Ptr & textureMesh, const Transform & pose = Transform::getIdentity()); + bool addOctomap(const OctoMap * octomap, unsigned int treeDepth = 0, bool showEdges = true, bool lightingOn = false); + void removeOctomap(); + bool addOccupancyGridMap( const cv::Mat & map8U, float resolution, // cell size @@ -326,6 +332,7 @@ private: bool _backfaceCulling; bool _frontfaceCulling; double _renderingRate; + vtkProp * _octomapActor; }; } /* namespace rtabmap */ diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index b2296ce9..e7160004 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -69,6 +69,7 @@ class ExportCloudsDialog; class ExportScansDialog; class PostProcessingDialog; class DataRecorder; +class OctoMap; class RTABMAPGUI_EXP MainWindow : public QMainWindow, public UEventsHandler { @@ -137,6 +138,7 @@ private slots: void exportPosesTORO(); void exportPosesG2O(); void exportImages(); + void exportOctomap(); void postProcessing(); void deleteMemory(); void openWorkingDirectory(); @@ -239,7 +241,8 @@ private: const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, int nodeId, - const Transform & pose); + const Transform & pose, + bool updateOctomap = false); void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId); void createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId); Transform alignPosesToGroundTruth(std::map & poses, const std::map & groundTruth); @@ -297,6 +300,8 @@ private: std::map > _projectionLocalMaps; // std::map > _gridLocalMaps; // + rtabmap::OctoMap * _octomap; + std::map::Ptr> _createdFeatures; Transform _odometryCorrection; diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 4f693b4c..18b17a35 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -152,6 +152,8 @@ public: double getMapNoiseRadius() const; int getMapNoiseMinNeighbors() const; bool isCloudsShown(int index) const; // 0=map, 1=odom + bool isOctomapShown() const; + int getOctomapTreeDepth() const; int getCloudDecimation(int index) const; // 0=map, 1=odom double getCloudMaxDepth(int index) const; // 0=map, 1=odom double getCloudMinDepth(int index) const; // 0=map, 1=odom diff --git a/guilib/src/AboutDialog.cpp b/guilib/src/AboutDialog.cpp index dfb2abf7..0c47d074 100644 --- a/guilib/src/AboutDialog.cpp +++ b/guilib/src/AboutDialog.cpp @@ -54,11 +54,17 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_version->setText(version); _ui->label_opencv_version->setText(cv_version); _ui->label_pcl_version->setText(PCL_VERSION_PRETTY); +#ifdef RTABMAP_OCTOMAP + _ui->label_octomap->setText("Yes"); +#else + _ui->label_octomap->setText("No"); +#endif _ui->label_freenect->setText(CameraFreenect::available()?"Yes":"No"); _ui->label_openni2->setText(CameraOpenNI2::available()?"Yes":"No"); _ui->label_freenect2->setText(CameraFreenect2::available()?"Yes":"No"); _ui->label_dc1394->setText(CameraStereoDC1394::available()?"Yes":"No"); _ui->label_flycapture2->setText(CameraStereoFlyCapture2::available()?"Yes":"No"); + _ui->label_zed->setText(CameraStereoZed::available()?"Yes":"No"); _ui->label_g2o->setText(Optimizer::isAvailable(Optimizer::kTypeG2O)?"Yes":"No"); _ui->label_gtsam->setText(Optimizer::isAvailable(Optimizer::kTypeGTSAM)?"Yes":"No"); diff --git a/guilib/src/CMakeLists.txt b/guilib/src/CMakeLists.txt index f2d8f159..d53b33f0 100644 --- a/guilib/src/CMakeLists.txt +++ b/guilib/src/CMakeLists.txt @@ -116,6 +116,17 @@ SET(LIBRARIES ${PCL_LIBRARIES} ) +IF(OCTOMAP_FOUND) + SET(INCLUDE_DIRS + ${INCLUDE_DIRS} + ${OCTOMAP_INCLUDE_DIRS} + ) + SET(LIBRARIES + ${LIBRARIES} + ${OCTOMAP_LIBRARIES} + ) +ENDIF(OCTOMAP_FOUND) + IF(VTK_USE_QVTK) SET(INCLUDE_DIRS ${INCLUDE_DIRS} ${QVTK_INCLUDE_DIR}) SET(LIBRARIES ${LIBRARIES} ${QVTK_LIBRARY}) diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index 7652dc6a..2acdb1bc 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/gui/CloudViewer.h" +#include #include #include #include @@ -47,6 +48,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include +#include +#include +#include + +#ifdef RTABMAP_OCTOMAP +#include +#endif namespace rtabmap { @@ -123,7 +132,8 @@ CloudViewer::CloudViewer(QWidget *parent) : _currentBgColor(Qt::black), _backfaceCulling(false), _frontfaceCulling(false), - _renderingRate(5.0) + _renderingRate(5.0), + _octomapActor(0) { UDEBUG(""); this->setMinimumSize(200, 200); @@ -166,6 +176,7 @@ CloudViewer::~CloudViewer() UDEBUG(""); this->clear(); delete _visualizer; + UDEBUG(""); } void CloudViewer::clear() @@ -177,6 +188,8 @@ void CloudViewer::clear() this->removeAllFrustums(); this->removeAllTexts(); this->clearTrajectory(); + this->removeOccupancyGridMap(); + this->removeOctomap(); this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2); } @@ -639,6 +652,111 @@ bool CloudViewer::addCloudTextureMesh( return false; } +bool CloudViewer::addOctomap(const OctoMap * octomap, unsigned int treeDepth, bool showEdges, bool lightingOn) +{ + UDEBUG(""); +#ifdef RTABMAP_OCTOMAP + UASSERT(octomap!=0); + + pcl::IndicesPtr obstacles(new std::vector); + + if(treeDepth > octomap->octree()->getTreeDepth()) + { + UWARN("Tree depth requested (%d) is deeper than the " + "actual maximum tree depth of %d. Using maximum depth.", + (int)treeDepth, (int)octomap->octree()->getTreeDepth()); + } + + pcl::PointCloud::Ptr cloud = octomap->createCloud(treeDepth, obstacles.get()); + if(obstacles->size()) + { + //get the renderer of the visualizer object + vtkRenderer *renderer = _visualizer->getRenderWindow()->GetRenderers()->GetFirstRenderer(); + + if(_octomapActor) + { + renderer->RemoveActor(_octomapActor); + _octomapActor = 0; + } + + //vtkSmartPointer colors = vtkSmartPointer::New(); + //colors->SetName("colors"); + //colors->SetNumberOfComponents(3); + vtkSmartPointer colors = vtkSmartPointer::New(); + colors->SetName("colors"); + colors->SetNumberOfValues(obstacles->size()); + + vtkSmartPointer lut = vtkSmartPointer::New(); + lut->SetNumberOfTableValues(obstacles->size()); + lut->Build(); + + // Create points + vtkSmartPointer points = vtkSmartPointer::New(); + double s = octomap->octree()->getNodeSize(treeDepth) / 2.0; + for (unsigned int i = 0; i < obstacles->size(); i++) + { + points->InsertNextPoint( + cloud->at(obstacles->at(i)).x, + cloud->at(obstacles->at(i)).y, + cloud->at(obstacles->at(i)).z); + colors->InsertValue(i,i); + + lut->SetTableValue(i, + double(cloud->at(obstacles->at(i)).r) / 255.0, + double(cloud->at(obstacles->at(i)).g) / 255.0, + double(cloud->at(obstacles->at(i)).b) / 255.0); + } + + // Combine into a polydata + vtkSmartPointer polydata = vtkSmartPointer::New(); + polydata->SetPoints(points); + polydata->GetPointData()->SetScalars(colors); + + // Create anything you want here, we will use a cube for the demo. + vtkSmartPointer cubeSource = vtkSmartPointer::New(); + cubeSource->SetBounds(-s, s, -s, s, -s, s); + + vtkSmartPointer mapper = vtkSmartPointer::New(); + mapper->SetSourceConnection(cubeSource->GetOutputPort()); +#if VTK_MAJOR_VERSION <= 5 + mapper->SetInputConnection(polydata->GetProducerPort()); +#else + mapper->SetInputData(polydata); +#endif + mapper->SetScalarRange(0, obstacles->size() - 1); + mapper->SetLookupTable(lut); + mapper->ScalingOff(); + mapper->Update(); + + vtkSmartPointer octomapActor = vtkSmartPointer::New(); + octomapActor->SetMapper(mapper); + + octomapActor->GetProperty()->SetRepresentationToSurface(); + octomapActor->GetProperty()->SetEdgeVisibility(showEdges); + octomapActor->GetProperty()->SetLighting(lightingOn); + + renderer->AddActor(octomapActor); + _octomapActor = octomapActor.GetPointer(); + + return true; +#endif + } + return false; +} + +void CloudViewer::removeOctomap() +{ + UDEBUG(""); +#ifdef RTABMAP_OCTOMAP + if(_octomapActor) + { + vtkRenderer *renderer = _visualizer->getRenderWindow()->GetRenderers()->GetFirstRenderer(); + renderer->RemoveActor(_octomapActor); + _octomapActor = 0; + } +#endif +} + bool CloudViewer::addOccupancyGridMap( const cv::Mat & map8U, float resolution, // cell size diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 39798a2b..8491c605 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -109,6 +109,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#ifdef RTABMAP_OCTOMAP +#include +#endif + #define LOG_FILE_NAME "LogRtabmap.txt" #define SHARE_SHOW_LOG_FILE "share/rtabmap/showlogs.m" #define SHARE_GET_PRECISION_RECALL_FILE "share/rtabmap/getPrecisionRecall.m" @@ -146,6 +150,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : _waypointsIndex(0), _cachedMemoryUsage(0), _createdCloudsMemoryUsage(0), + _octomap(0), _odometryCorrection(Transform::getIdentity()), _processingOdometry(false), _oneSecondTimer(0), @@ -358,6 +363,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : connect(_ui->actionExport_images_RGB_jpg_Depth_png, SIGNAL(triggered()), this , SLOT(exportImages())); connect(_ui->actionExport_cameras_in_Bundle_format_out, SIGNAL(triggered()), SLOT(exportBundlerFormat())); connect(_ui->actionView_scans, SIGNAL(triggered()), this, SLOT(viewScans())); + connect(_ui->actionExport_octomap, SIGNAL(triggered()), this, SLOT(exportOctomap())); connect(_ui->actionView_high_res_point_cloud, SIGNAL(triggered()), this, SLOT(viewClouds())); connect(_ui->actionReset_Odometry, SIGNAL(triggered()), this, SLOT(resetOdometry())); connect(_ui->actionTrigger_a_new_map, SIGNAL(triggered()), this, SLOT(triggerNewMap())); @@ -1919,7 +1925,14 @@ void MainWindow::updateMapCloud( _ui->graphicsView_graphView->isGridMapVisible() && _preferencesDialog->isGridMapFrom3DCloud() && _projectionLocalMaps.find(iter->first) == _projectionLocalMaps.end(); - if(update3dCloud || updateProjMap) + bool updateOctomap = false; +#ifdef RTABMAP_OCTOMAP + updateOctomap = + _cloudViewer->isVisible() && + _preferencesDialog->isOctomapShown() && + _octomap->addedNodes().find(iter->first) == _octomap->addedNodes().end(); +#endif + if(update3dCloud || updateProjMap || updateOctomap) { // update cloud std::pair::Ptr, pcl::IndicesPtr> createdCloud; @@ -1942,23 +1955,23 @@ void MainWindow::updateMapCloud( else if(_cachedClouds.find(iter->first) == _cachedClouds.end() && _cachedSignatures.contains(iter->first)) { createdCloud = this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1)); - if(viewerClouds.contains(cloudName)) + if(_cloudViewer->getAddedClouds().contains(cloudName)) { _cloudViewer->setCloudVisibility(cloudName.c_str(), _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)); } } //Update projection map - if(updateProjMap) + if(updateProjMap || updateOctomap) { std::map::Ptr, pcl::IndicesPtr> >::iterator cloudIter = _cachedClouds.find(iter->first); if(cloudIter != _cachedClouds.end()) { - createAndAddProjectionMap(cloudIter->second.first, cloudIter->second.second, iter->first, iter->second); + createAndAddProjectionMap(cloudIter->second.first, cloudIter->second.second, iter->first, iter->second, updateOctomap); } - else if(createdCloud.first->size() && createdCloud.second->size()) + else if(createdCloud.first.get() && createdCloud.first->size() && createdCloud.second->size()) { - createAndAddProjectionMap(createdCloud.first, createdCloud.second, iter->first, iter->second); + createAndAddProjectionMap(createdCloud.first, createdCloud.second, iter->first, iter->second, updateOctomap); } } } @@ -2057,6 +2070,11 @@ void MainWindow::updateMapCloud( _ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty()); _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty()); _ui->actionView_scans->setEnabled(!_createdScans.empty()); +#ifdef RTABMAP_OCTOMAP + _ui->actionExport_octomap->setEnabled(_octomap && _octomap->octree()->size()); +#else + _ui->actionExport_octomap->setEnabled(false); +#endif } //remove not used clouds @@ -2200,6 +2218,18 @@ void MainWindow::updateMapCloud( _cloudViewer->removeOccupancyGridMap(); } +#ifdef RTABMAP_OCTOMAP + _cloudViewer->removeOctomap(); + if(_preferencesDialog->isOctomapShown() && _octomap) + { + UDEBUG(""); + UTimer time; + _octomap->update(poses); + _cloudViewer->addOctomap(_octomap, _preferencesDialog->getOctomapTreeDepth()); + UINFO("Octomap update time = %fs", time.ticks()); + } +#endif + if(viewerClouds.contains("cloudOdom")) { if(!_preferencesDialog->isCloudsShown(1)) @@ -2356,191 +2386,190 @@ std::pair::Ptr, pcl::IndicesPtr> MainWindow::c } pcl::PointCloud::Ptr cloudWithNormals(new pcl::PointCloud); - if(_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)) + if(_preferencesDialog->isSubtractFiltering() && + _preferencesDialog->getSubtractFilteringRadius() > 0.0) { - if(_preferencesDialog->isSubtractFiltering() && - _preferencesDialog->getSubtractFilteringRadius() > 0.0) + pcl::IndicesPtr beforeFiltering = indices; + if( cloud->size() && + _previousCloud.first>0 && + _previousCloud.second.first.first.get() != 0 && + _previousCloud.second.second.get() != 0 && + _previousCloud.second.second->size() && + _currentPosesMap.find(_previousCloud.first) != _currentPosesMap.end()) { - pcl::IndicesPtr beforeFiltering = indices; - if( cloud->size() && - _previousCloud.first>0 && - _previousCloud.second.first.first.get() != 0 && - _previousCloud.second.second.get() != 0 && - _previousCloud.second.second->size() && - _currentPosesMap.find(_previousCloud.first) != _currentPosesMap.end()) + UTimer time; + + rtabmap::Transform t = pose.inverse() * _currentPosesMap.at(_previousCloud.first); + + //UWARN("saved new.pcd and old.pcd"); + //pcl::io::savePCDFile("new.pcd", *cloud, *indices); + //pcl::io::savePCDFile("old.pcd", *previousCloud, *_previousCloud.second.second); + + if(_preferencesDialog->getSubtractFilteringAngle() > 0.0f) { - UTimer time; - - rtabmap::Transform t = pose.inverse() * _currentPosesMap.at(_previousCloud.first); - - //UWARN("saved new.pcd and old.pcd"); - //pcl::io::savePCDFile("new.pcd", *cloud, *indices); - //pcl::io::savePCDFile("old.pcd", *previousCloud, *_previousCloud.second.second); - - if(_preferencesDialog->getSubtractFilteringAngle() > 0.0f) - { - //normals required - if(_preferencesDialog->getNormalKSearch() > 0) - { - pcl::PointCloud::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), viewPoint); - pcl::concatenateFields(*cloud, *normals, *cloudWithNormals); - } - else - { - UWARN("Cloud subtraction with angle filtering is activated but " - "cloud normal K search is 0. Subtraction is done with angle."); - } - } - - if(cloudWithNormals->size() && - _previousCloud.second.first.second.get() && - _previousCloud.second.first.second->size()) - { - pcl::PointCloud::Ptr previousCloud = rtabmap::util3d::transformPointCloud(_previousCloud.second.first.second, t); - indices = rtabmap::util3d::subtractFiltering( - cloudWithNormals, - indices, - previousCloud, - _previousCloud.second.second, - _preferencesDialog->getSubtractFilteringRadius(), - _preferencesDialog->getSubtractFilteringAngle(), - _preferencesDialog->getSubtractFilteringMinPts()); - } - else - { - pcl::PointCloud::Ptr previousCloud = rtabmap::util3d::transformPointCloud(_previousCloud.second.first.first, t); - indices = rtabmap::util3d::subtractFiltering( - cloud, - indices, - previousCloud, - _previousCloud.second.second, - _preferencesDialog->getSubtractFilteringRadius(), - _preferencesDialog->getSubtractFilteringMinPts()); - } - - - UINFO("Time subtract filtering %d from %d -> %d (%fs)", - (int)_previousCloud.second.second->size(), - (int)beforeFiltering->size(), - (int)indices->size(), - time.ticks()); - } - // keep all indices for next subtraction - _previousCloud.first = nodeId; - _previousCloud.second.first.first = cloud; - _previousCloud.second.first.second = cloudWithNormals; - _previousCloud.second.second = beforeFiltering; - } - - if(indices->size()) - { - pcl::PointCloud::Ptr output; - bool added = false; - if(_preferencesDialog->isCloudMeshing() && cloud->isOrganized()) - { - // Fast organized mesh - // we need to extract indices as pcl::OrganizedFastMesh doesn't take indices - output = util3d::extractIndices(cloud, indices, false, true); - std::vector polygons = util3d::organizedFastMesh( - output, - _preferencesDialog->getCloudMeshingAngle(), - _preferencesDialog->isCloudMeshingQuad(), - _preferencesDialog->getCloudMeshingTriangleSize(), - viewPoint); - if(polygons.size()) - { - // remove unused vertices to save memory - pcl::PointCloud::Ptr outputFiltered(new pcl::PointCloud); - std::vector outputPolygons; - util3d::filterNotUsedVerticesFromMesh(*output, polygons, *outputFiltered, outputPolygons); - if(!_cloudViewer->addCloudMesh(cloudName, outputFiltered, outputPolygons, pose)) - { - UERROR("Adding mesh cloud %d to viewer failed!", nodeId); - } - else - { - added = true; - } - } - } - else - { - if(_preferencesDialog->isCloudMeshing()) - { - UWARN("Online meshing is activated but the generated cloud is " - "dense (voxel filtering is used or multiple cameras are used). Disable " - "online meshing in Preferences->3D Rendering to hide this warning."); - } - - if(_preferencesDialog->getNormalKSearch() > 0 && cloudWithNormals->size() == 0) + //normals required + if(_preferencesDialog->getNormalKSearch() > 0) { pcl::PointCloud::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), viewPoint); pcl::concatenateFields(*cloud, *normals, *cloudWithNormals); } - - QColor color = Qt::gray; - if(mapId >= 0) + else { - color = (Qt::GlobalColor)(mapId+3 % 12 + 7 ); + UWARN("Cloud subtraction with angle filtering is activated but " + "cloud normal K search is 0. Subtraction is done with angle."); } + } - output = util3d::extractIndices(cloud, indices, false, true); + if(cloudWithNormals->size() && + _previousCloud.second.first.second.get() && + _previousCloud.second.first.second->size()) + { + pcl::PointCloud::Ptr previousCloud = rtabmap::util3d::transformPointCloud(_previousCloud.second.first.second, t); + indices = rtabmap::util3d::subtractFiltering( + cloudWithNormals, + indices, + previousCloud, + _previousCloud.second.second, + _preferencesDialog->getSubtractFilteringRadius(), + _preferencesDialog->getSubtractFilteringAngle(), + _preferencesDialog->getSubtractFilteringMinPts()); + } + else + { + pcl::PointCloud::Ptr previousCloud = rtabmap::util3d::transformPointCloud(_previousCloud.second.first.first, t); + indices = rtabmap::util3d::subtractFiltering( + cloud, + indices, + previousCloud, + _previousCloud.second.second, + _preferencesDialog->getSubtractFilteringRadius(), + _preferencesDialog->getSubtractFilteringMinPts()); + } - if(cloudWithNormals->size()) + + UINFO("Time subtract filtering %d from %d -> %d (%fs)", + (int)_previousCloud.second.second->size(), + (int)beforeFiltering->size(), + (int)indices->size(), + time.ticks()); + } + // keep all indices for next subtraction + _previousCloud.first = nodeId; + _previousCloud.second.first.first = cloud; + _previousCloud.second.first.second = cloudWithNormals; + _previousCloud.second.second = beforeFiltering; + } + + if(indices->size()) + { + pcl::PointCloud::Ptr output; + bool added = false; + if(_preferencesDialog->isCloudMeshing() && cloud->isOrganized()) + { + // Fast organized mesh + // we need to extract indices as pcl::OrganizedFastMesh doesn't take indices + output = util3d::extractIndices(cloud, indices, false, true); + std::vector polygons = util3d::organizedFastMesh( + output, + _preferencesDialog->getCloudMeshingAngle(), + _preferencesDialog->isCloudMeshingQuad(), + _preferencesDialog->getCloudMeshingTriangleSize(), + viewPoint); + if(polygons.size()) + { + // remove unused vertices to save memory + pcl::PointCloud::Ptr outputFiltered(new pcl::PointCloud); + std::vector outputPolygons; + util3d::filterNotUsedVerticesFromMesh(*output, polygons, *outputFiltered, outputPolygons); + if(!_cloudViewer->addCloudMesh(cloudName, outputFiltered, outputPolygons, pose)) { - pcl::PointCloud::Ptr outputWithNormals; - outputWithNormals = util3d::extractIndices(cloudWithNormals, indices, false, false); - - if(!_cloudViewer->addCloud(cloudName, outputWithNormals, pose, color)) - { - UERROR("Adding cloud %d to viewer failed!", nodeId); - } - else - { - added = true; - } + UERROR("Adding mesh cloud %d to viewer failed!", nodeId); } else { - if(!_cloudViewer->addCloud(cloudName, output, pose, color)) - { - UERROR("Adding cloud %d to viewer failed!", nodeId); - } - else - { - added = true; - } - } - } - if(added) - { - outputPair.first = output; - outputPair.second = indices; - if(_preferencesDialog->isCloudsKept()) - { - _cachedClouds.insert(std::make_pair(nodeId, outputPair)); - _createdCloudsMemoryUsage += output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int); + added = true; } } } - _cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0)); - _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0)); + else + { + if(_preferencesDialog->isCloudMeshing()) + { + UWARN("Online meshing is activated but the generated cloud is " + "dense (voxel filtering is used or multiple cameras are used). Disable " + "online meshing in Preferences->3D Rendering to hide this warning."); + } + + if(_preferencesDialog->getNormalKSearch() > 0 && cloudWithNormals->size() == 0) + { + pcl::PointCloud::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), viewPoint); + pcl::concatenateFields(*cloud, *normals, *cloudWithNormals); + } + + QColor color = Qt::gray; + if(mapId >= 0) + { + color = (Qt::GlobalColor)(mapId+3 % 12 + 7 ); + } + + output = util3d::extractIndices(cloud, indices, false, true); + + if(cloudWithNormals->size()) + { + pcl::PointCloud::Ptr outputWithNormals; + outputWithNormals = util3d::extractIndices(cloudWithNormals, indices, false, false); + + if(!_cloudViewer->addCloud(cloudName, outputWithNormals, pose, color)) + { + UERROR("Adding cloud %d to viewer failed!", nodeId); + } + else + { + added = true; + } + } + else + { + if(!_cloudViewer->addCloud(cloudName, output, pose, color)) + { + UERROR("Adding cloud %d to viewer failed!", nodeId); + } + else + { + added = true; + } + } + } + if(added) + { + outputPair.first = output; + outputPair.second = indices; + if(_preferencesDialog->isCloudsKept()) + { + _cachedClouds.insert(std::make_pair(nodeId, outputPair)); + _createdCloudsMemoryUsage += output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int); + } + } } + _cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0)); + _cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0)); } - return outputPair; UDEBUG(""); + return outputPair; } void MainWindow::createAndAddProjectionMap( const pcl::PointCloud::Ptr & cloud, const pcl::IndicesPtr & indices, int nodeId, - const Transform & pose) + const Transform & pose, + bool updateOctomap) { + UDEBUG(""); UASSERT(!pose.isNull()); - if(_projectionLocalMaps.find(nodeId) != _projectionLocalMaps.end()) + if(_projectionLocalMaps.find(nodeId) != _projectionLocalMaps.end() && !updateOctomap) { UERROR("Projection map %d already added.", nodeId); return; @@ -2559,9 +2588,9 @@ void MainWindow::createAndAddProjectionMap( } // add pose rotation without yaw + float roll, pitch, yaw; if(_preferencesDialog->projMapFrame()) { - float roll, pitch, yaw; pose.getEulerAngles(roll, pitch, yaw); voxelCloud = util3d::transformPointCloud(voxelCloud, Transform(0,0, pose.z(), roll, pitch, 0)); } @@ -2571,19 +2600,61 @@ void MainWindow::createAndAddProjectionMap( voxelCloud = util3d::passThrough(voxelCloud, "z", std::numeric_limits::min(), _preferencesDialog->projMaxObstaclesHeight()); } - util3d::occupancy2DFromCloud3D( + pcl::IndicesPtr groundIndices, obstaclesIndices; + util3d::segmentObstaclesFromGround( voxelCloud, - ground, - obstacles, - _preferencesDialog->getGridMapResolution(), + groundIndices, + obstaclesIndices, + 20, _preferencesDialog->projMaxGroundAngle(), + _preferencesDialog->getGridMapResolution()*2.0f, _preferencesDialog->projMinClusterSize(), _preferencesDialog->projFlatObstaclesDetected(), _preferencesDialog->projMaxGroundHeight()); + pcl::PointCloud::Ptr groundCloud(new pcl::PointCloud); + pcl::PointCloud::Ptr obstaclesCloud(new pcl::PointCloud); + + if(groundIndices->size()) + { + pcl::copyPointCloud(*voxelCloud, *groundIndices, *groundCloud); + } + + if(obstaclesIndices->size()) + { + pcl::copyPointCloud(*voxelCloud, *obstaclesIndices, *obstaclesCloud); + } + + util3d::occupancy2DFromGroundObstacles( + groundCloud, + obstaclesCloud, + ground, + obstacles, + _preferencesDialog->getGridMapResolution()); + + if(updateOctomap) + { + // Update octomap +#ifdef RTABMAP_OCTOMAP + if(_octomap && + (_octomap->addedNodes().empty() || + nodeId > _octomap->addedNodes().rbegin()->first)) + { + if(_preferencesDialog->projMapFrame()) + { + Transform tinv = Transform(0,0,0, roll, pitch, 0).inverse(); + groundCloud = util3d::transformPointCloud(groundCloud, tinv); + obstaclesCloud = util3d::transformPointCloud(obstaclesCloud, tinv); + } + _octomap->addToCache(nodeId, groundCloud, obstaclesCloud); + } +#endif + } + _projectionLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles))); UDEBUG("time gridMapFrom3DCloud = %f s", timer.ticks()); } + UDEBUG(""); } void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int mapId) @@ -3965,6 +4036,15 @@ void MainWindow::startDetection() "progress will not be shown in the GUI.")); } +#ifdef RTABMAP_OCTOMAP + if(_octomap) + { + delete _octomap; + _octomap = 0; + } + _octomap = new OctoMap(_preferencesDialog->getGridMapResolution()); +#endif + emit stateChanged(kDetecting); } @@ -4948,6 +5028,7 @@ void MainWindow::clearTheCache() _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false); _ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(false); _ui->actionView_scans->setEnabled(false); + _ui->actionExport_octomap->setEnabled(false); _ui->actionView_high_res_point_cloud->setEnabled(false); _likelihoodCurve->clear(); _rawLikelihoodCurve->clear(); @@ -4968,6 +5049,12 @@ void MainWindow::clearTheCache() _ui->imageView_source->setBackgroundColor(Qt::black); _ui->imageView_loopClosure->setBackgroundColor(Qt::black); _ui->imageView_odometry->setBackgroundColor(Qt::black); +#ifdef RTABMAP_OCTOMAP + if(_octomap) + { + _octomap->clear(); + } +#endif } void MainWindow::updateElapsedTime() @@ -5350,6 +5437,44 @@ void MainWindow::viewClouds() } +void MainWindow::exportOctomap() +{ +#ifdef RTABMAP_OCTOMAP + if(_octomap && _octomap->octree()->size()) + { + QString path = QFileDialog::getSaveFileName( + this, + tr("Save File"), + this->getWorkingDirectory()+"/"+"octomap.bt", + tr("Octomap file (*.bt)")); + + if(!path.isEmpty()) + { + if(_octomap->writeBinary(path.toStdString())) + { + QMessageBox::information(this, + tr("Export octomap..."), + tr("Octomap successfully saved to \"%1\".") + .arg(path)); + } + else + { + QMessageBox::information(this, + tr("Export octomap..."), + tr("Failed to save octomap to \"%1\"!") + .arg(path)); + } + } + } + else + { + UERROR("Empty octomap."); + } +#else + UERROR("Cannot export octomap, RTAB-Map is not built with it."); +#endif +} + void MainWindow::exportImages() { if(_cachedSignatures.empty()) @@ -5727,7 +5852,7 @@ void MainWindow::changeState(MainWindow::State newState) } } actions = _ui->menuFile->actions(); - if(actions.size()==15) + if(actions.size()==16) { if(actions.at(2)->isSeparator()) { @@ -5737,9 +5862,9 @@ void MainWindow::changeState(MainWindow::State newState) { UWARN("Menu File separators have not the same order."); } - if(actions.at(11)->isSeparator()) + if(actions.at(12)->isSeparator()) { - actions.at(11)->setVisible(!monitoring); + actions.at(12)->setVisible(!monitoring); } else { @@ -5796,6 +5921,11 @@ void MainWindow::changeState(MainWindow::State newState) _ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty()); _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty()); _ui->actionView_scans->setEnabled(!_createdScans.empty()); +#ifdef RTABMAP_OCTOMAP + _ui->actionExport_octomap->setEnabled(_octomap && _octomap->octree()->size()); +#else + _ui->actionExport_octomap->setEnabled(false); +#endif _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty()); _ui->actionDownload_all_clouds->setEnabled(false); _ui->actionDownload_graph->setEnabled(false); @@ -5852,6 +5982,11 @@ void MainWindow::changeState(MainWindow::State newState) _ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty()); _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty()); _ui->actionView_scans->setEnabled(!_createdScans.empty()); +#ifdef RTABMAP_OCTOMAP + _ui->actionExport_octomap->setEnabled(_octomap && _octomap->octree()->size()); +#else + _ui->actionExport_octomap->setEnabled(false); +#endif _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty()); _ui->actionDownload_all_clouds->setEnabled(true); _ui->actionDownload_graph->setEnabled(true); @@ -5897,6 +6032,7 @@ void MainWindow::changeState(MainWindow::State newState) _ui->actionExport_2D_scans_ply_pcd->setEnabled(false); _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false); _ui->actionView_scans->setEnabled(false); + _ui->actionExport_octomap->setEnabled(false); _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false); _ui->actionDownload_all_clouds->setEnabled(false); _ui->actionDownload_graph->setEnabled(false); @@ -5939,6 +6075,7 @@ void MainWindow::changeState(MainWindow::State newState) _ui->actionExport_2D_scans_ply_pcd->setEnabled(false); _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false); _ui->actionView_scans->setEnabled(false); + _ui->actionExport_octomap->setEnabled(false); _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false); _ui->actionDownload_all_clouds->setEnabled(false); _ui->actionDownload_graph->setEnabled(false); @@ -5968,6 +6105,11 @@ void MainWindow::changeState(MainWindow::State newState) _ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty()); _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty()); _ui->actionView_scans->setEnabled(!_createdScans.empty()); +#ifdef RTABMAP_OCTOMAP + _ui->actionExport_octomap->setEnabled(_octomap && _octomap->octree()->size()); +#else + _ui->actionExport_octomap->setEnabled(false); +#endif _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty()); _ui->actionDownload_all_clouds->setEnabled(true); _ui->actionDownload_graph->setEnabled(true); @@ -5997,6 +6139,7 @@ void MainWindow::changeState(MainWindow::State newState) _ui->actionExport_2D_scans_ply_pcd->setEnabled(false); _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(false); _ui->actionView_scans->setEnabled(false); + _ui->actionExport_octomap->setEnabled(false); _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(false); _ui->actionDelete_memory->setEnabled(true); _ui->actionDownload_all_clouds->setEnabled(true); @@ -6027,6 +6170,11 @@ void MainWindow::changeState(MainWindow::State newState) _ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty()); _ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty()); _ui->actionView_scans->setEnabled(!_createdScans.empty()); +#ifdef RTABMAP_OCTOMAP + _ui->actionExport_octomap->setEnabled(_octomap && _octomap->octree()->size()); +#else + _ui->actionExport_octomap->setEnabled(false); +#endif _ui->actionExport_cameras_in_Bundle_format_out->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty()); _ui->actionDelete_memory->setEnabled(true); _ui->actionDownload_all_clouds->setEnabled(true); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index e6ff9e8d..371774ad 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -150,6 +150,16 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->checkBox_map_shown->setChecked(false); _ui->checkBox_map_shown->setEnabled(false); _ui->label_map_shown->setText(_ui->label_map_shown->text() + " (Disabled, PCL >=1.7.2 required)"); + _ui->label_map_shown->setEnabled(false); +#endif + +#ifndef RTABMAP_OCTOMAP + _ui->checkBox_octomap->setChecked(false); + _ui->checkBox_octomap->setEnabled(false); + _ui->label_octomap->setEnabled(false); + _ui->spinBox_octomap_treeDepth->setEnabled(false); + _ui->label_octomap_treeDepth->setEnabled(false); + #endif #ifndef RTABMAP_NONFREE @@ -378,6 +388,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : connect(_ui->doubleSpinBox_projMaxObstaclesHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->checkBox_projFlatObstaclesDetected, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_ui->checkBox_octomap, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_ui->spinBox_octomap_treeDepth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); + connect(_ui->groupBox_organized, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->doubleSpinBox_mesh_angleTolerance, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel())); connect(_ui->checkBox_mesh_quad, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel())); @@ -1213,6 +1226,9 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox) _ui->doubleSpinBox_projMaxObstaclesHeight->setValue(0); _ui->checkBox_projFlatObstaclesDetected->setChecked(true); + _ui->checkBox_octomap->setChecked(false); + _ui->spinBox_octomap_treeDepth->setValue(16); + _ui->doubleSpinBox_mesh_angleTolerance->setValue(15.0); #if PCL_VERSION_COMPARE(>=, 1, 7, 2) _ui->groupBox_organized->setChecked(false); @@ -1571,6 +1587,9 @@ void PreferencesDialog::readGuiSettings(const QString & filePath) _ui->doubleSpinBox_projMaxObstaclesHeight->setValue(settings.value("projMaxObstaclesHeight", _ui->doubleSpinBox_projMaxObstaclesHeight->value()).toDouble()); _ui->checkBox_projFlatObstaclesDetected->setChecked(settings.value("projFlatObstaclesDetected", _ui->checkBox_projFlatObstaclesDetected->isChecked()).toBool()); + _ui->checkBox_octomap->setChecked(settings.value("octomap", _ui->checkBox_octomap->isChecked()).toBool()); + _ui->spinBox_octomap_treeDepth->setValue(settings.value("octomap_depth", _ui->spinBox_octomap_treeDepth->value()).toInt()); + _ui->groupBox_organized->setChecked(settings.value("meshing", _ui->groupBox_organized->isChecked()).toBool()); _ui->doubleSpinBox_mesh_angleTolerance->setValue(settings.value("meshing_angle", _ui->doubleSpinBox_mesh_angleTolerance->value()).toDouble()); _ui->checkBox_mesh_quad->setChecked(settings.value("meshing_quad", _ui->checkBox_mesh_quad->isChecked()).toBool()); @@ -1978,6 +1997,8 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const settings.setValue("projMaxObstaclesHeight", _ui->doubleSpinBox_projMaxObstaclesHeight->value()); settings.setValue("projFlatObstaclesDetected", _ui->checkBox_projFlatObstaclesDetected->isChecked()); + settings.setValue("octomap", _ui->checkBox_octomap->isChecked()); + settings.setValue("octomap_depth", _ui->spinBox_octomap_treeDepth->value()); settings.setValue("meshing", _ui->groupBox_organized->isChecked()); settings.setValue("meshing_angle", _ui->doubleSpinBox_mesh_angleTolerance->value()); @@ -3631,6 +3652,17 @@ bool PreferencesDialog::isCloudsShown(int index) const UASSERT(index >= 0 && index <= 1); return _3dRenderingShowClouds[index]->isChecked(); } +bool PreferencesDialog::isOctomapShown() const +{ +#ifdef RTABMAP_OCTOMAP + return _ui->checkBox_octomap->isChecked(); +#endif + return false; +} +int PreferencesDialog::getOctomapTreeDepth() const +{ + return _ui->spinBox_octomap_treeDepth->value(); +} double PreferencesDialog::getMapVoxel() const { diff --git a/guilib/src/ui/aboutDialog.ui b/guilib/src/ui/aboutDialog.ui index 6859e0e9..9f69bd4c 100644 --- a/guilib/src/ui/aboutDialog.ui +++ b/guilib/src/ui/aboutDialog.ui @@ -82,6 +82,16 @@ p, li { white-space: pre-wrap; } + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + @@ -202,17 +212,7 @@ p, li { white-space: pre-wrap; } - - - - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - - + With g2o : @@ -239,7 +239,7 @@ p, li { white-space: pre-wrap; } - + @@ -293,14 +293,14 @@ p, li { white-space: pre-wrap; } - + With cvsba : - + @@ -310,14 +310,14 @@ p, li { white-space: pre-wrap; } - + With GTSAM : - + @@ -327,6 +327,40 @@ p, li { white-space: pre-wrap; } + + + + With Octomap : + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + With stereo Zed : + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index 5f3d875f..c0a40d9e 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -27,7 +27,7 @@ 0 0 1012 - 21 + 25 @@ -52,6 +52,7 @@ + @@ -297,16 +298,7 @@ 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -344,16 +336,7 @@ 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -596,16 +579,7 @@ 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -626,16 +600,7 @@ 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -656,16 +621,7 @@ 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -686,16 +642,7 @@ 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -713,16 +660,7 @@ 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -743,16 +681,7 @@ 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -773,16 +702,7 @@ 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -806,16 +726,7 @@ 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -885,16 +796,7 @@ 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -1419,6 +1321,11 @@ Stereo Usb Camera + + + Export octomap... + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 12d534df..e885fce4 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,25 +63,16 @@ 0 - 0 - 685 - 1826 + -464 + 686 + 2023 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -95,7 +86,7 @@ QFrame::Raised - 14 + 1 @@ -794,6 +785,19 @@ Show a yellow background when the number of odometry inliers goes under this thr + + + + Maximum obstacles height (0=disabled). + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + @@ -937,10 +941,20 @@ Show a yellow background when the number of odometry inliers goes under this thr - - + + - Maximum obstacles height (0=disabled). + + + + true + + + + + + + Octomap: 3D occupancy grid map. true @@ -950,6 +964,32 @@ Show a yellow background when the number of odometry inliers goes under this thr + + + + Octomap maximum tree depth (max 16). The highest depth means the smallest resolution of the map (cell size). At smallest resolution the octomap shows RGB colors. Other resolutions produce z-axis gradient colored octomap. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + 1 + + + 16 + + + 16 + + + @@ -3986,16 +4026,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Directory of images (optional settings) - - 0 - - - 0 - - - 0 - - + 0 @@ -9172,16 +9203,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - 0 - - - 0 - - - 0 - - + 0 @@ -9321,16 +9343,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - 0 - - - 0 - - - 0 - - + 0 @@ -9488,16 +9501,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -9577,16 +9581,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -9698,16 +9693,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - - 0 - - - 0 - - - 0 - - + 0