Added Octomap visualization and export options

corelib/include/rtabmap/core/OctoMap.h

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Transform.h>
 
 #include <map>
+#include <string>
 
 namespace rtabmap {
 
@@ -67,8 +68,9 @@ public:
 	const octomap::ColorOcTree * octree() const {return octree_;}
 
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
+			unsigned int treeDepth = 0,
 			std::vector<int> * obstacleIndices = 0,
-			std::vector<int> * groundIndices = 0) const;
+			std::vector<int> * 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();
 

corelib/src/OctoMap.cpp

@@ -59,6 +59,7 @@ void OctoMap::addToCache(int nodeId,
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
 		pcl::PointCloud<pcl::PointXYZRGB>::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<int, Transform> & poses)
 	}
 	if(graphChanged)
 	{
-		UWARN("Graph changed!");
+		UINFO("Graph changed!");
 		octomap::ColorOcTree * newOcTree = new octomap::ColorOcTree(octree_->getResolution());
 		std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo > newOccupiedCells;
 		int copied=0;
@@ -299,32 +300,105 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 	cache_.clear();
 }
 
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
+void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
-		std::vector<int> * obstacleIndices,
-		std::vector<int> * groundIndices) const
 {
+	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<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
+		unsigned int treeDepth,
+		std::vector<int> * obstacleIndices,
+		std::vector<int> * emptyIndices) const
+{
+	UASSERT(treeDepth <= octree_->getTreeDepth());
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-	UDEBUG("occupied cells = %d", (int)occupiedCells_.size());
+	UDEBUG("depth=%d (maxDepth=%d) octree = %d",
-	cloud->resize(occupiedCells_.size());
+			(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<octomap::ColorOcTreeNode*, OcTreeNodeInfo>::const_iterator iter = occupiedCells_.begin();
+	for (octomap::ColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
-		iter!=occupiedCells_.end();
-		++iter)
 	{
-		if(iter->second.isObstacle_ && octree_->isNodeOccupied(iter->first))
+		if(octree_->isNodeOccupied(*it))
 		{
-			octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
+			octomap::point3d pt = octree_->keyToCoord(it.getKey());
-			(*cloud)[oi]  = pcl::PointXYZRGB(iter->first->getColor().r, iter->first->getColor().g, iter->first->getColor().b);
+			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<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 			}
 			++oi;
 		}
-		else if(!iter->second.isObstacle_)
+		else
 		{
-			octomap::point3d pt = octree_->keyToCoord(iter->second.key_);
+			octomap::point3d pt = octree_->keyToCoord(it.getKey());
-			(*cloud)[oi]  = pcl::PointXYZRGB(iter->first->getColor().r, iter->first->getColor().g, iter->first->getColor().b);
+			(*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 */

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 */

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<pcl::PointXYZRGB>::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<int, Transform> & poses, const std::map<int, Transform> & groundTruth);
@@ -297,6 +300,8 @@ private:
 	std::map<int, std::pair<cv::Mat, cv::Mat> > _projectionLocalMaps; // <ground, obstacles>
 	std::map<int, std::pair<cv::Mat, cv::Mat> > _gridLocalMaps; // <ground, obstacles>
 
+	rtabmap::OctoMap * _octomap;
+
 	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> _createdFeatures;
 
 	Transform _odometryCorrection;

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

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");

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})

guilib/src/CloudViewer.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/gui/CloudViewer.h"
 
+#include <rtabmap/core/Version.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UMath.h>
@@ -47,6 +48,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <vtkCamera.h>
 #include <vtkRenderWindow.h>
+#include <vtkCubeSource.h>
+#include <vtkGlyph3D.h>
+#include <vtkGlyph3DMapper.h>
+#include <vtkLookupTable.h>
+
+#ifdef RTABMAP_OCTOMAP
+#include <rtabmap/core/OctoMap.h>
+#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<int>);
+
+	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<pcl::PointXYZRGB>::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<vtkUnsignedCharArray> colors = vtkSmartPointer<vtkUnsignedCharArray>::New();
+		//colors->SetName("colors");
+		//colors->SetNumberOfComponents(3);
+		vtkSmartPointer<vtkFloatArray> colors = vtkSmartPointer<vtkFloatArray>::New();
+		colors->SetName("colors");
+		colors->SetNumberOfValues(obstacles->size());
+
+		vtkSmartPointer<vtkLookupTable> lut = vtkSmartPointer<vtkLookupTable>::New();
+		lut->SetNumberOfTableValues(obstacles->size());
+		lut->Build();
+
+		// Create points
+		vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::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<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
+		polydata->SetPoints(points);
+		polydata->GetPointData()->SetScalars(colors);
+
+		// Create anything you want here, we will use a cube for the demo.
+		vtkSmartPointer<vtkCubeSource> cubeSource = vtkSmartPointer<vtkCubeSource>::New();
+		cubeSource->SetBounds(-s, s, -s, s, -s, s);
+
+		vtkSmartPointer<vtkGlyph3DMapper> mapper = vtkSmartPointer<vtkGlyph3DMapper>::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<vtkActor> octomapActor = vtkSmartPointer<vtkActor>::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

guilib/src/MainWindow.cpp

@@ -109,6 +109,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/filters/filter.h>
 #include <pcl/search/kdtree.h>
 
+#ifdef RTABMAP_OCTOMAP
+#include <rtabmap/core/OctoMap.h>
+#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<pcl::PointCloud<pcl::PointXYZRGB>::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<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::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<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 		}
 
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-		if(_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0))
+		if(_preferencesDialog->isSubtractFiltering() &&
+		   _preferencesDialog->getSubtractFilteringRadius() > 0.0)
 		{
-			if(_preferencesDialog->isSubtractFiltering() &&
+			pcl::IndicesPtr beforeFiltering = indices;
-			   _preferencesDialog->getSubtractFilteringRadius() > 0.0)
+			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;
+				UTimer time;
-				if(	cloud->size() &&
+
-					_previousCloud.first>0 &&
+				rtabmap::Transform t = pose.inverse() * _currentPosesMap.at(_previousCloud.first);
-					_previousCloud.second.first.first.get() != 0 &&
+
-					_previousCloud.second.second.get() != 0 &&
+				//UWARN("saved new.pcd and old.pcd");
-					_previousCloud.second.second->size() &&
+				//pcl::io::savePCDFile("new.pcd", *cloud, *indices);
-					_currentPosesMap.find(_previousCloud.first) != _currentPosesMap.end())
+				//pcl::io::savePCDFile("old.pcd", *previousCloud, *_previousCloud.second.second);
+
+				if(_preferencesDialog->getSubtractFilteringAngle() > 0.0f)
 				{
-					UTimer time;
+					//normals required
-
+					if(_preferencesDialog->getNormalKSearch() > 0)
-					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<pcl::Normal>::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<pcl::PointXYZRGBNormal>::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<pcl::PointXYZRGB>::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<pcl::PointXYZRGB>::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<pcl::Vertices> polygons = util3d::organizedFastMesh(
-							output,
-							_preferencesDialog->getCloudMeshingAngle(),
-							_preferencesDialog->isCloudMeshingQuad(),
-							_preferencesDialog->getCloudMeshingTriangleSize(),
-							viewPoint);
-					if(polygons.size())
-					{
-						// remove unused vertices to save memory
-						pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputFiltered(new pcl::PointCloud<pcl::PointXYZRGB>);
-						std::vector<pcl::Vertices> 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)
 					{
 						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), viewPoint);
 						pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
 					}
-
+					else
-					QColor color = Qt::gray;
-					if(mapId >= 0)
 					{
-						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<pcl::PointXYZRGBNormal>::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<pcl::PointXYZRGB>::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<pcl::PointXYZRGB>::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<pcl::Vertices> polygons = util3d::organizedFastMesh(
+						output,
+						_preferencesDialog->getCloudMeshingAngle(),
+						_preferencesDialog->isCloudMeshingQuad(),
+						_preferencesDialog->getCloudMeshingTriangleSize(),
+						viewPoint);
+				if(polygons.size())
+				{
+					// remove unused vertices to save memory
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputFiltered(new pcl::PointCloud<pcl::PointXYZRGB>);
+					std::vector<pcl::Vertices> outputPolygons;
+					util3d::filterNotUsedVerticesFromMesh(*output, polygons, *outputFiltered, outputPolygons);
+					if(!_cloudViewer->addCloudMesh(cloudName, outputFiltered, outputPolygons, pose))
 					{
-						pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr outputWithNormals;
+						UERROR("Adding mesh cloud %d to viewer failed!", nodeId);
-						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))
+						added = true;
-						{
-							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));
+			else
-			_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
+			{
+				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<pcl::Normal>::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<pcl::PointXYZRGBNormal>::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<pcl::PointXYZRGB>::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<int>::min(), _preferencesDialog->projMaxObstaclesHeight());
 		}
 
-		util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(
+		pcl::IndicesPtr groundIndices, obstaclesIndices;
+		util3d::segmentObstaclesFromGround<pcl::PointXYZRGB>(
 				voxelCloud,
-				ground,
+				groundIndices,
-				obstacles,
+				obstaclesIndices,
-				_preferencesDialog->getGridMapResolution(),
+				20,
 				_preferencesDialog->projMaxGroundAngle(),
+				_preferencesDialog->getGridMapResolution()*2.0f,
 				_preferencesDialog->projMinClusterSize(),
 				_preferencesDialog->projFlatObstaclesDetected(),
 				_preferencesDialog->projMaxGroundHeight());
 
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+
+		if(groundIndices->size())
+		{
+			pcl::copyPointCloud(*voxelCloud, *groundIndices, *groundCloud);
+		}
+
+		if(obstaclesIndices->size())
+		{
+			pcl::copyPointCloud(*voxelCloud, *obstaclesIndices, *obstaclesCloud);
+		}
+
+		util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
+				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);

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
 {

guilib/src/ui/aboutDialog.ui

@@ -82,6 +82,16 @@ p, li { white-space: pre-wrap; }
         </item>
         <item>
          <layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
+          <item row="8" column="1">
+           <widget class="QLabel" name="label_opencv_version">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+            </property>
+           </widget>
+          </item>
           <item row="9" column="0">
            <widget class="QLabel" name="label_11">
             <property name="text">
@@ -202,17 +212,7 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="8" column="1">
+          <item row="16" column="0">
-           <widget class="QLabel" name="label_opencv_version">
-            <property name="text">
-             <string/>
-            </property>
-            <property name="alignment">
-             <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
-            </property>
-           </widget>
-          </item>
-          <item row="15" column="0">
            <widget class="QLabel" name="label_14">
             <property name="text">
              <string>With g2o :</string>
@@ -239,7 +239,7 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="15" column="1">
+          <item row="16" column="1">
            <widget class="QLabel" name="label_g2o">
             <property name="text">
              <string/>
@@ -293,14 +293,14 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="17" column="0">
+          <item row="18" column="0">
            <widget class="QLabel" name="label_18">
             <property name="text">
              <string>With cvsba :</string>
             </property>
            </widget>
           </item>
-          <item row="17" column="1">
+          <item row="18" column="1">
            <widget class="QLabel" name="label_cvsba">
             <property name="text">
              <string/>
@@ -310,14 +310,14 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
-          <item row="16" column="0">
+          <item row="17" column="0">
            <widget class="QLabel" name="label_19">
             <property name="text">
              <string>With GTSAM :</string>
             </property>
            </widget>
           </item>
-          <item row="16" column="1">
+          <item row="17" column="1">
            <widget class="QLabel" name="label_gtsam">
             <property name="text">
              <string/>
@@ -327,6 +327,40 @@ p, li { white-space: pre-wrap; }
             </property>
            </widget>
           </item>
+          <item row="19" column="0">
+           <widget class="QLabel" name="label_20">
+            <property name="text">
+             <string>With Octomap :</string>
+            </property>
+           </widget>
+          </item>
+          <item row="19" column="1">
+           <widget class="QLabel" name="label_octomap">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+            </property>
+           </widget>
+          </item>
+          <item row="15" column="0">
+           <widget class="QLabel" name="label_21">
+            <property name="text">
+             <string>With stereo Zed :</string>
+            </property>
+           </widget>
+          </item>
+          <item row="15" column="1">
+           <widget class="QLabel" name="label_zed">
+            <property name="text">
+             <string/>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+            </property>
+           </widget>
+          </item>
          </layout>
         </item>
         <item>

guilib/src/ui/mainWindow.ui

@@ -27,7 +27,7 @@
      <x>0</x>
      <y>0</y>
      <width>1012</width>
-     <height>21</height>
+     <height>25</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -52,6 +52,7 @@
     <addaction name="actionSave_point_cloud"/>
     <addaction name="actionExport_2D_scans_ply_pcd"/>
     <addaction name="actionExport_2D_Grid_map_bmp_png"/>
+    <addaction name="actionExport_octomap"/>
     <addaction name="menuExport_poses"/>
     <addaction name="actionExport_images_RGB_jpg_Depth_png"/>
     <addaction name="actionExport_cameras_in_Bundle_format_out"/>
@@ -297,16 +298,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -344,16 +336,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -596,16 +579,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -626,16 +600,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -656,16 +621,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -686,16 +642,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
     </layout>
@@ -713,16 +660,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
     </layout>
@@ -743,16 +681,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -773,16 +702,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -806,16 +726,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -885,16 +796,7 @@
      <property name="spacing">
       <number>0</number>
      </property>
-     <property name="leftMargin">
+     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="topMargin">
-      <number>0</number>
-     </property>
-     <property name="rightMargin">
-      <number>0</number>
-     </property>
-     <property name="bottomMargin">
       <number>0</number>
      </property>
      <item>
@@ -1419,6 +1321,11 @@
     <string>Stereo Usb Camera</string>
    </property>
   </action>
+  <action name="actionExport_octomap">
+   <property name="text">
+    <string>Export octomap...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -63,25 +63,16 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-464</y>
-            <width>685</width>
+            <width>686</width>
-            <height>1826</height>
+            <height>2023</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
            <property name="spacing">
             <number>0</number>
            </property>
-           <property name="leftMargin">
+           <property name="margin">
-            <number>0</number>
-           </property>
-           <property name="topMargin">
-            <number>0</number>
-           </property>
-           <property name="rightMargin">
-            <number>0</number>
-           </property>
-           <property name="bottomMargin">
             <number>0</number>
            </property>
            <item>
@@ -95,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>14</number>
+                <number>1</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -794,6 +785,19 @@ Show a yellow background when the number of odometry inliers goes under this thr
                           </item>
                           <item>
                            <layout class="QGridLayout" name="gridLayout_67" columnstretch="0,1">
+                            <item row="5" column="1">
+                             <widget class="QLabel" name="label_309">
+                              <property name="text">
+                               <string>Maximum obstacles height (0=disabled).</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
                             <item row="0" column="0">
                              <widget class="QCheckBox" name="checkBox_projMapFrame">
                               <property name="text">
@@ -937,10 +941,20 @@ Show a yellow background when the number of odometry inliers goes under this thr
                               </property>
                              </widget>
                             </item>
-                            <item row="5" column="1">
+                            <item row="6" column="0">
-                             <widget class="QLabel" name="label_309">
+                             <widget class="QCheckBox" name="checkBox_octomap">
                               <property name="text">
-                               <string>Maximum obstacles height (0=disabled).</string>
+                               <string/>
+                              </property>
+                              <property name="checked">
+                               <bool>true</bool>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="6" column="1">
+                             <widget class="QLabel" name="label_octomap">
+                              <property name="text">
+                               <string>Octomap: 3D occupancy grid map.</string>
                               </property>
                               <property name="wordWrap">
                                <bool>true</bool>
@@ -950,6 +964,32 @@ Show a yellow background when the number of odometry inliers goes under this thr
                               </property>
                              </widget>
                             </item>
+                            <item row="7" column="1">
+                             <widget class="QLabel" name="label_octomap_treeDepth">
+                              <property name="text">
+                               <string>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.</string>
+                              </property>
+                              <property name="wordWrap">
+                               <bool>true</bool>
+                              </property>
+                              <property name="textInteractionFlags">
+                               <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                              </property>
+                             </widget>
+                            </item>
+                            <item row="7" column="0">
+                             <widget class="QSpinBox" name="spinBox_octomap_treeDepth">
+                              <property name="minimum">
+                               <number>1</number>
+                              </property>
+                              <property name="maximum">
+                               <number>16</number>
+                              </property>
+                              <property name="value">
+                               <number>16</number>
+                              </property>
+                             </widget>
+                            </item>
                            </layout>
                           </item>
                          </layout>
@@ -3986,16 +4026,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <string>Directory of images (optional settings)</string>
                       </property>
                       <layout class="QVBoxLayout" name="verticalLayout_93">
-                       <property name="leftMargin">
+                       <property name="margin">
-                        <number>0</number>
-                       </property>
-                       <property name="topMargin">
-                        <number>0</number>
-                       </property>
-                       <property name="rightMargin">
-                        <number>0</number>
-                       </property>
-                       <property name="bottomMargin">
                         <number>0</number>
                        </property>
                        <item>
@@ -9172,16 +9203,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       </property>
                       <widget class="QWidget" name="page_54">
                        <layout class="QVBoxLayout" name="verticalLayout_85">
-                        <property name="leftMargin">
+                        <property name="margin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -9321,16 +9343,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       </widget>
                       <widget class="QWidget" name="page_55">
                        <layout class="QVBoxLayout" name="verticalLayout_86">
-                        <property name="leftMargin">
+                        <property name="margin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -9488,16 +9501,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="leftMargin">
+                        <property name="margin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -9577,16 +9581,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="leftMargin">
+                        <property name="margin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -9698,16 +9693,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="leftMargin">
+                        <property name="margin">
-                         <number>0</number>
-                        </property>
-                        <property name="topMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="rightMargin">
-                         <number>0</number>
-                        </property>
-                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>