3D Rendering: Added option to show graph's frustums, added option to filter floor and/or ceiling.

corelib/src/RegistrationVis.cpp

@@ -1208,8 +1208,8 @@ Transform RegistrationVis::computeTransformationImpl(
 					matches[dir] = matchesV;
 					if(transforms[dir].isNull())
 					{
-						msg = uFormat("Not enough inliers %d/%d between %d and %d",
+						msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
-								(int)inliers[dir].size(), _minInliers, signatureA->id(), signatureB->id());
+								(int)inliers[dir].size(), _minInliers, (int)matches[dir].size(), signatureA->id(), signatureB->id());
 						UINFO(msg.c_str());
 					}
 				}

corelib/src/util3d_motion_estimation.cpp

@@ -227,9 +227,9 @@ Transform estimateMotion3DTo3D(
 		*varianceOut = 1.0;
 	}
 
+	std::vector<int> inliers;
 	if((int)inliers1.size() >= minInliers)
 	{
-		std::vector<int> inliers;
 		pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1cloud(new pcl::PointCloud<pcl::PointXYZ>);
 		pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2cloud(new pcl::PointCloud<pcl::PointXYZ>);
 		inliers1cloud->resize(inliers1.size());
@@ -257,12 +257,12 @@ Transform estimateMotion3DTo3D(
 		{
 			transform = t;
 		}
+	}
 
 	if(matchesOut)
 	{
 		*matchesOut = matches;
 	}
-
 	if(inliersOut)
 	{
 		inliersOut->resize(inliers.size());
@@ -271,7 +271,7 @@ Transform estimateMotion3DTo3D(
 			inliersOut->at(i) = matches[inliers[i]];
 		}
 	}
-	}
+
 	return transform;
 }
 

guilib/include/rtabmap/gui/CloudViewer.h

@@ -196,13 +196,15 @@ public:
 	void addOrUpdateFrustum(
 			const std::string & id,
 			const Transform & transform,
+			const Transform & localTransform,
 			double scale,
 			const QColor & color = QColor());
 	bool updateFrustumPose(
 			const std::string & id,
 			const Transform & pose);
 	void removeFrustum(const std::string & id);
-	void removeAllFrustums();
+	void removeAllFrustums(bool exceptCameraReference = false);
+	const QMap<std::string, Transform> & getAddedFrustums() const {return _frustums;}
 
 	void addOrUpdateGraph(
 			const std::string & id,
@@ -327,7 +329,7 @@ private:
     std::set<std::string> _coordinates;
     std::set<std::string> _texts;
     std::set<std::string> _lines;
-    std::set<std::string> _frustums;
+    QMap<std::string, Transform> _frustums;
     pcl::PointCloud<pcl::PointXYZ>::Ptr _trajectory;
     unsigned int _maxTrajectorySize;
     float _frustumScale;

guilib/include/rtabmap/gui/MainWindow.h

@@ -296,6 +296,7 @@ private:
 	std::map<int, std::string> _currentLabels; // <nodeId, label>
 	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> > _cachedClouds;
 	long _createdCloudsMemoryUsage;
+	std::set<int> _cachedEmptyClouds;
 	std::pair<int, std::pair<std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr>, pcl::IndicesPtr> > _previousCloud; // used for subtraction
 
 	std::map<int, cv::Mat> _createdScans;

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -155,10 +155,14 @@ public:
 	bool isGroundTruthAligned() const;
 
 	bool isGraphsShown() const;
+	bool isFrustumsShown() const;
 	bool isLabelsShown() const;
-	double getMapVoxel() const;
+	double getVoxel() const;
-	double getMapNoiseRadius() const;
+	double getNoiseRadius() const;
-	int getMapNoiseMinNeighbors() const;
+	int getNoiseMinNeighbors() const;
+	double getCeilingFilteringHeight() const;
+	double getFloorFilteringHeight() const;
+	int getNormalKSearch() const;
 	bool isCloudsShown(int index) const;      // 0=map, 1=odom
 	bool isOctomapUpdated() const;
 	bool isOctomapShown() const;
@@ -190,8 +194,6 @@ public:
 	int getSubtractFilteringMinPts() const;
 	double getSubtractFilteringRadius() const;
 	double getSubtractFilteringAngle() const;
-	int getNormalKSearch() const;
-	bool gainCompensation() const;
 
 	bool getGridMapShown() const;
 	double getGridMapResolution() const;;

guilib/src/CloudViewer.cpp

@@ -1237,6 +1237,7 @@ static const int frustum_indices[] = {
 void CloudViewer::addOrUpdateFrustum(
 			const std::string & id,
 			const Transform & transform,
+			const Transform & localTransform,
 			double scale,
 			const QColor & color)
 {
@@ -1246,11 +1247,11 @@ void CloudViewer::addOrUpdateFrustum(
 		return;
 	}
 
-	removeFrustum(id);
-
 	if(!transform.isNull())
 	{
-		_frustums.insert(id);
+		if(_frustums.find(id)==_frustums.end())
+		{
+			_frustums.insert(id, Transform());
 
 			int frustumSize = sizeof(frustum_vertices)/sizeof(float);
 			UASSERT(frustumSize>0 && frustumSize % 3 == 0);
@@ -1265,7 +1266,8 @@ void CloudViewer::addOrUpdateFrustum(
 			{
 				c = color;
 			}
-		Eigen::Affine3f t = transform.toEigen3f();
+			Transform opticalRotInv(0, -1, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0);
+			Eigen::Affine3f t = (localTransform*opticalRotInv).toEigen3f();
 			for(int i=0; i<frustumSize; ++i)
 			{
 				frustumPoints[i].x = frustum_vertices[i*3]*scaleX;
@@ -1285,7 +1287,15 @@ void CloudViewer::addOrUpdateFrustum(
 			mesh.polygons.push_back(vertices);
 			_visualizer->addPolylineFromPolygonMesh(mesh, id);
 			_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, c.redF(), c.greenF(), c.blueF(), id);
-
+		}
+		if(!this->updateFrustumPose(id, transform))
+		{
+			UERROR("Failed updating pose of frustum %s!?", id.c_str());
+		}
+	}
+	else
+	{
+		removeFrustum(id);
 	}
 }
 
@@ -1293,10 +1303,37 @@ bool CloudViewer::updateFrustumPose(
 		const std::string & id,
 		const Transform & pose)
 {
-	if(_frustums.find(id) != _frustums.end() && !pose.isNull())
+	QMap<std::string, Transform>::iterator iter=_frustums.find(id);
+	if(iter != _frustums.end() && !pose.isNull())
 	{
-		UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
+		if(iter.value() == pose)
-		return _visualizer->updateShapePose(id, pose.toEigen3f());
+		{
+			// same pose, just return
+			return true;
+		}
+
+		pcl::visualization::ShapeActorMap::iterator am_it = _visualizer->getShapeActorMap()->find (id);
+
+		vtkActor* actor;
+
+		if (am_it == _visualizer->getShapeActorMap()->end ())
+			return (false);
+		else
+			actor = vtkActor::SafeDownCast (am_it->second);
+
+		if (!actor)
+			return (false);
+
+		vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
+
+		pcl::visualization::PCLVisualizer::convertToVtkMatrix (pose.toEigen3f().matrix (), matrix);
+
+		actor->SetUserMatrix (matrix);
+		actor->Modified ();
+
+		iter.value() = pose;
+
+		return true;
 	}
 	return false;
 }
@@ -1312,18 +1349,21 @@ void CloudViewer::removeFrustum(const std::string & id)
 	if(_frustums.find(id) != _frustums.end())
 	{
 		_visualizer->removeShape(id);
-		_frustums.erase(id);
+		_frustums.remove(id);
 	}
 }
 
-void CloudViewer::removeAllFrustums()
+void CloudViewer::removeAllFrustums(bool exceptCameraReference)
 {
-	std::set<std::string> frustums = _frustums;
+	QMap<std::string, Transform> frustums = _frustums;
-	for(std::set<std::string>::iterator iter = frustums.begin(); iter!=frustums.end(); ++iter)
+	for(QMap<std::string, Transform>::iterator iter = frustums.begin(); iter!=frustums.end(); ++iter)
 	{
-		this->removeFrustum(*iter);
+		if(!exceptCameraReference || !uStrContains(iter.key(), "reference_frustum"))
+		{
+			this->removeFrustum(iter.key());
 		}
-	UASSERT(_frustums.empty());
+	}
+	UASSERT(exceptCameraReference || _frustums.empty());
 }
 
 void CloudViewer::addOrUpdateGraph(
@@ -1486,12 +1526,12 @@ void CloudViewer::setFrustumShown(bool shown)
 {
 	if(!shown)
 	{
-		std::set<std::string> frustumsCopy = _frustums;
+		QMap<std::string, Transform> frustumsCopy = _frustums;
-		for(std::set<std::string>::iterator iter=frustumsCopy.begin(); iter!=frustumsCopy.end(); ++iter)
+		for(QMap<std::string, Transform>::iterator iter=frustumsCopy.begin(); iter!=frustumsCopy.end(); ++iter)
 		{
-			if(uStrContains(*iter, "reference_frustum"))
+			if(uStrContains(iter.key(), "reference_frustum"))
 			{
-				this->removeFrustum(*iter);
+				this->removeFrustum(iter.key());
 			}
 		}
 		std::set<std::string> linesCopy = _lines;
@@ -1518,12 +1558,9 @@ void CloudViewer::setFrustumColor(QColor value)
 	{
 		value = Qt::gray;
 	}
-	for(std::set<std::string>::iterator iter=_frustums.begin(); iter!=_frustums.end(); ++iter)
+	for(QMap<std::string, Transform>::iterator iter=_frustums.begin(); iter!=_frustums.end(); ++iter)
 	{
-		if(uStrContains(*iter, "reference_frustum"))
+		_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, value.redF(), value.greenF(), value.blueF(), iter.key());
-		{
-			_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, value.redF(), value.greenF(), value.blueF(), *iter);
-		}
 	}
 	this->update();
 	_frustumColor = value;
@@ -1819,24 +1856,19 @@ void CloudViewer::updateCameraFrustums(const Transform & pose, const std::vector
 {
 	if(!pose.isNull())
 	{
-		// commented: update pose is crashing...
+		if(_aShowFrustum->isChecked())
-		/*if(_frustums.find("reference_frustum") != _frustums.end())
-		{
-			this->updateFrustumPose("reference_frustum", pose);
-		}
-		else */ if(_aShowFrustum->isChecked())
 		{
 			Transform baseToCamera;
-			Transform opticalRot(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
-
 			for(unsigned int i=0; i<models.size(); ++i)
 			{
 				baseToCamera = Transform::getIdentity();
 				if(!models[i].localTransform().isNull() && !models[i].localTransform().isIdentity())
 				{
-					baseToCamera = models[i].localTransform()*opticalRot.inverse();
+					baseToCamera = models[i].localTransform();
 				}
-				this->addOrUpdateFrustum(uFormat("reference_frustum_%d", i), pose * baseToCamera, _frustumScale, _frustumColor);
+				std::string id = uFormat("reference_frustum_%d", i);
+				this->removeFrustum(id);
+				this->addOrUpdateFrustum(id, pose, baseToCamera, _frustumScale, _frustumColor);
 				if(!baseToCamera.isIdentity())
 				{
 					this->addOrUpdateLine(uFormat("reference_frustum_line_%d", i), pose, pose * baseToCamera, _frustumColor);

guilib/src/DepthCalibrationDialog.cpp

@@ -355,11 +355,10 @@ void DepthCalibrationDialog::calibrate(
 				_progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)...").arg(map->size()).arg(sequence.size()));
 				_progressDialog->incrementStep();
 				viewer->addCloud("map", map);
-				Transform opticalRot(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
 				for(std::map<int, SensorData>::iterator iter=sequence.begin(); iter!=sequence.end(); ++iter)
 				{
-					Transform baseToCamera = iter->second.cameraModels()[0].localTransform()*opticalRot.inverse();
+					Transform baseToCamera = iter->second.cameraModels()[0].localTransform();
-					viewer->addOrUpdateFrustum(uFormat("frustum%d",iter->first), poses.at(iter->first) * baseToCamera, 0.2);
+					viewer->addOrUpdateFrustum(uFormat("frustum%d",iter->first), poses.at(iter->first), baseToCamera, 0.2);
 				}
 				_progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)... done.").arg(map->size()).arg(sequence.size()));
 

guilib/src/MainWindow.cpp

@@ -1935,7 +1935,6 @@ void MainWindow::updateMapCloud(
 			if(update3dCloud)
 			{
 				// update cloud
-				std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createdCloud;
 				if(viewerClouds.contains(cloudName))
 				{
 					// Update only if the pose has changed
@@ -1952,9 +1951,11 @@ void MainWindow::updateMapCloud(
 					_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
 					_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
 				}
-				else if(_cachedClouds.find(iter->first) == _cachedClouds.end() && _cachedSignatures.contains(iter->first))
+				else if(_cachedEmptyClouds.find(iter->first) == _cachedEmptyClouds.end() &&
+						_cachedClouds.find(iter->first) == _cachedClouds.end() &&
+						_cachedSignatures.contains(iter->first))
 				{
-					createdCloud = this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
+					std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createdCloud = this->createAndAddCloudToMap(iter->first, iter->second, uValue(mapIds, iter->first, -1));
 					if(_cloudViewer->getAddedClouds().contains(cloudName))
 					{
 						_cloudViewer->setCloudVisibility(cloudName.c_str(), _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0));
@@ -2145,14 +2146,21 @@ void MainWindow::updateMapCloud(
 	// update 3D graphes (show all poses)
 	_cloudViewer->removeAllGraphs();
 	_cloudViewer->removeCloud("graph_nodes");
-	if(_preferencesDialog->isGraphsShown() && _currentPosesMap.size())
+	if(!_preferencesDialog->isFrustumsShown())
 	{
+		_cloudViewer->removeAllFrustums(true);
+	}
+	if((_preferencesDialog->isGraphsShown() || _preferencesDialog->isFrustumsShown()) && _currentPosesMap.size())
+	{
+		UTimer timerGraph;
 		// Find all graphs
 		std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > graphs;
 		for(std::map<int, Transform>::iterator iter=_currentPosesMap.begin(); iter!=_currentPosesMap.end(); ++iter)
 		{
 			int mapId = uValue(_currentMapIds, iter->first, -1);
 
+			if(_preferencesDialog->isGraphsShown())
+			{
 				//edges
 				std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
 				if(kter == graphs.end())
@@ -2163,10 +2171,38 @@ void MainWindow::updateMapCloud(
 				kter->second->push_back(pt);
 			}
 
+			// get local transforms for frustums on the graph
+			if(_preferencesDialog->isFrustumsShown())
+			{
+				std::string frustumId = uFormat("f_%d_%d", mapId, iter->first);
+				if(_cloudViewer->getAddedFrustums().contains(frustumId))
+				{
+					_cloudViewer->updateFrustumPose(frustumId, iter->second);
+				}
+				else if(_cachedSignatures.contains(iter->first))
+				{
+					const Signature & s = _cachedSignatures.value(iter->first);
+					// Supporting only one frustum per node
+					if(s.sensorData().cameraModels().size() == 1 || s.sensorData().stereoCameraModel().isValidForProjection())
+					{
+						Transform t = s.sensorData().stereoCameraModel().isValidForProjection()?s.sensorData().stereoCameraModel().localTransform():s.sensorData().cameraModels()[0].localTransform();
+						if(!t.isNull())
+						{
+							QColor color = (Qt::GlobalColor)((mapId+3) % 12 + 7 );
+							_cloudViewer->addOrUpdateFrustum(frustumId, iter->second, t, _cloudViewer->getFrustumScale(), color);
+						}
+					}
+				}
+			}
+		}
+
 		//Ground truth graph?
 		for(std::map<int, Transform>::iterator iter=_currentGTPosesMap.begin(); iter!=_currentGTPosesMap.end(); ++iter)
 		{
 			int mapId = -100;
+
+			if(_preferencesDialog->isGraphsShown())
+			{
 				//edges
 				std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
 				if(kter == graphs.end())
@@ -2177,6 +2213,35 @@ void MainWindow::updateMapCloud(
 				kter->second->push_back(pt);
 			}
 
+			// get local transforms for frustums on the graph
+			if(_preferencesDialog->isFrustumsShown() && _cachedSignatures.contains(iter->first))
+			{
+				std::string frustumId = uFormat("f_gt_%d", iter->first);
+				QMap<std::string, Transform>::const_iterator jter=_cloudViewer->getAddedFrustums().find(frustumId);
+				if(jter != _cloudViewer->getAddedFrustums().end())
+				{
+					if(jter.value() != iter->second)
+					{
+						_cloudViewer->updateFrustumPose(frustumId, iter->second);
+					}
+				}
+				else if(_cachedSignatures.contains(iter->first))
+				{
+					const Signature & s = _cachedSignatures.value(iter->first);
+					// Supporting only one frustum per node
+					if(s.sensorData().cameraModels().size() == 1 || s.sensorData().stereoCameraModel().isValidForProjection())
+					{
+						Transform t = s.sensorData().stereoCameraModel().isValidForProjection()?s.sensorData().stereoCameraModel().localTransform():s.sensorData().cameraModels()[0].localTransform();
+						if(!t.isNull())
+						{
+							QColor color = Qt::gray;
+							_cloudViewer->addOrUpdateFrustum(frustumId, iter->second, t, _cloudViewer->getFrustumScale(), color);
+						}
+					}
+				}
+			}
+		}
+
 		// add graphs
 		for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter=graphs.begin(); iter!=graphs.end(); ++iter)
 		{
@@ -2187,6 +2252,8 @@ void MainWindow::updateMapCloud(
 			}
 			_cloudViewer->addOrUpdateGraph(uFormat("graph_%d", iter->first), iter->second, color);
 		}
+
+		UDEBUG("timerGraph=%fs", timerGraph.ticks());
 	}
 
 	UDEBUG("labels.size()=%d", (int)labels.size());
@@ -2435,9 +2502,9 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 		}
 
 		// filtering pipeline
-		if(indices->size() && _preferencesDialog->getMapVoxel() > 0.0)
+		if(indices->size() && _preferencesDialog->getVoxel() > 0.0)
 		{
-			cloud = util3d::voxelize(cloud, indices, _preferencesDialog->getMapVoxel());
+			cloud = util3d::voxelize(cloud, indices, _preferencesDialog->getVoxel());
 			//generate indices for all points (they are all valid)
 			indices->resize(cloud->size());
 			for(unsigned int i=0; i<cloud->size(); ++i)
@@ -2446,23 +2513,36 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 			}
 		}
 
+		// Do ceiling/floor filtering
+		if(indices->size() &&
+		   (_preferencesDialog->getFloorFilteringHeight() != 0.0 ||
+		   _preferencesDialog->getCeilingFilteringHeight() != 0.0))
+		{
+			// perform in /map frame
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose);
+			indices = rtabmap::util3d::passThrough(
+					cloudTransformed,
+					indices,
+					"z",
+					_preferencesDialog->getFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getFloorFilteringHeight(),
+					_preferencesDialog->getCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getCeilingFilteringHeight());
+		}
+
 		// Do radius filtering after voxel filtering ( a lot faster)
 		if(indices->size() &&
-		   _preferencesDialog->getMapNoiseRadius() > 0.0 &&
+		   _preferencesDialog->getNoiseRadius() > 0.0 &&
-		   _preferencesDialog->getMapNoiseMinNeighbors() > 0)
+		   _preferencesDialog->getNoiseMinNeighbors() > 0)
 		{
 			indices = rtabmap::util3d::radiusFiltering(
 					cloud,
 					indices,
-					_preferencesDialog->getMapNoiseRadius(),
+					_preferencesDialog->getNoiseRadius(),
-					_preferencesDialog->getMapNoiseMinNeighbors());
+					_preferencesDialog->getNoiseMinNeighbors());
 		}
 
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-		GainCompensator compensator;
+		if(_preferencesDialog->isSubtractFiltering() &&
-		if((_preferencesDialog->isSubtractFiltering() &&
+		   _preferencesDialog->getSubtractFilteringRadius() > 0.0)
-		   _preferencesDialog->getSubtractFilteringRadius() > 0.0) ||
-		   _preferencesDialog->gainCompensation())
 		{
 			pcl::IndicesPtr beforeFiltering = indices;
 			if(	cloud->size() &&
@@ -2480,13 +2560,6 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 				//pcl::io::savePCDFile("new.pcd", *cloud, *indices);
 				//pcl::io::savePCDFile("old.pcd", *previousCloud, *_previousCloud.second.second);
 
-				if(_preferencesDialog->gainCompensation())
-				{
-					compensator.feed(cloud, indices, _previousCloud.second.first.first, _previousCloud.second.second, t);
-					compensator.apply(0, cloud, indices);
-					UINFO("Time gain compensation = %fs", time.ticks());
-				}
-
 				if(_preferencesDialog->isSubtractFiltering())
 				{
 					if(_preferencesDialog->getSubtractFilteringAngle() > 0.0f)
@@ -2598,10 +2671,6 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 						tex_name >> mesh_material.tex_name;
 
 						mesh_material.tex_file = "";
-						if(_preferencesDialog->gainCompensation() && compensator.getIndex(0) >= 0)
-						{
-							compensator.apply(0, image);
-						}
 
 						textureMesh->tex_materials.push_back(mesh_material);
 
@@ -2682,11 +2751,19 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 					_cachedClouds.insert(std::make_pair(nodeId, outputPair));
 					_createdCloudsMemoryUsage += (long)(output->size() * sizeof(pcl::PointXYZRGB) + indices->size()*sizeof(int));
 				}
-			}
-		}
 				_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
 				_cloudViewer->setCloudPointSize(cloudName, _preferencesDialog->getCloudPointSize(0));
 			}
+			else
+			{
+				_cachedEmptyClouds.insert(nodeId);
+			}
+		}
+		else
+		{
+			_cachedEmptyClouds.insert(nodeId);
+		}
+	}
 
 	UDEBUG("");
 	return outputPair;
@@ -2747,6 +2824,9 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 					scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform());
 				}
 				_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
+
+				_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
+				_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
 			}
 		}
 		else
@@ -2778,12 +2858,13 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 					scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform());
 				}
 				_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
-			}
+
-		}
 				_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
 				_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
 			}
 		}
+	}
+}
 
 void MainWindow::createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId)
 {
@@ -5298,6 +5379,7 @@ void MainWindow::clearTheCache()
 	_cachedMemoryUsage = 0;
 	_cachedClouds.clear();
 	_createdCloudsMemoryUsage = 0;
+	_cachedEmptyClouds.clear();
 	_previousCloud.first = 0;
 	_previousCloud.second.first.first.reset();
 	_previousCloud.second.first.second.reset();
@@ -5725,8 +5807,27 @@ void MainWindow::exportClouds()
 		return;
 	}
 
+	std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
+
+	// Use ground truth poses if current clouds are using them
+	if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
+	{
+		for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
+			if(gtIter!=_currentGTPosesMap.end())
+			{
+				iter->second = gtIter->second;
+			}
+			else
+			{
+				UWARN("Not found ground truth pose for node %d", iter->first);
+			}
+		}
+	}
+
 	_exportCloudsDialog->exportClouds(
-			_ui->widget_mapVisibility->getVisiblePoses(),
+			poses,
 			_currentLinksMap,
 			_currentMapIds,
 			_cachedSignatures,
@@ -5742,8 +5843,27 @@ void MainWindow::viewClouds()
 		return;
 	}
 
+	std::map<int, Transform> poses = _ui->widget_mapVisibility->getVisiblePoses();
+
+	// Use ground truth poses if current clouds are using them
+	if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
+	{
+		for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
+			if(gtIter!=_currentGTPosesMap.end())
+			{
+				iter->second = gtIter->second;
+			}
+			else
+			{
+				UWARN("Not found ground truth pose for node %d", iter->first);
+			}
+		}
+	}
+
 	_exportCloudsDialog->viewClouds(
-			_ui->widget_mapVisibility->getVisiblePoses(),
+			poses,
 			_currentLinksMap,
 			_currentMapIds,
 			_cachedSignatures,
@@ -5962,6 +6082,23 @@ void MainWindow::exportBundlerFormat()
 {
 	std::map<int, Transform> posesIn = _ui->widget_mapVisibility->getVisiblePoses();
 
+	// Use ground truth poses if current clouds are using them
+	if(_currentGTPosesMap.size() && _ui->actionAnchor_clouds_to_ground_truth->isChecked())
+	{
+		for(std::map<int, Transform>::iterator iter = posesIn.begin(); iter!=posesIn.end(); ++iter)
+		{
+			std::map<int, Transform>::iterator gtIter = _currentGTPosesMap.find(iter->first);
+			if(gtIter!=_currentGTPosesMap.end())
+			{
+				iter->second = gtIter->second;
+			}
+			else
+			{
+				UWARN("Not found ground truth pose for node %d", iter->first);
+			}
+		}
+	}
+
 	std::map<int, Transform> poses;
 	for(std::map<int, Transform>::iterator iter=posesIn.begin(); iter!=posesIn.end(); ++iter)
 	{

guilib/src/PreferencesDialog.cpp

@@ -379,8 +379,12 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->doubleSpinBox_voxel, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_noiseRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->spinBox_noiseMinNeighbors, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_ceilingFilterHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_floorFilterHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	connect(_ui->checkBox_showGraphs, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->checkBox_showFrustums, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_showLabels, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	connect(_ui->radioButton_noFiltering, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -391,8 +395,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->spinBox_subtractFilteringMinPts, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_subtractFilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_subtractFilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->checkBox_gainCompensation, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	connect(_ui->checkBox_map_shown, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_map_resolution, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -1257,11 +1259,14 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->doubleSpinBox_noiseRadius->setValue(0);
 		_ui->spinBox_noiseMinNeighbors->setValue(5);
 
+		_ui->doubleSpinBox_ceilingFilterHeight->setValue(0);
+		_ui->doubleSpinBox_floorFilterHeight->setValue(0);
+
 		_ui->checkBox_showGraphs->setChecked(true);
+		_ui->checkBox_showFrustums->setChecked(false);
 		_ui->checkBox_showLabels->setChecked(false);
 
 		_ui->spinBox_normalKSearch->setValue(10);
-		_ui->checkBox_gainCompensation->setChecked(false);
 
 		_ui->doubleSpinBox_mesh_angleTolerance->setValue(15.0);
 		_ui->groupBox_organized->setChecked(false);
@@ -1647,8 +1652,12 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->doubleSpinBox_voxel->setValue(settings.value("cloudVoxel", _ui->doubleSpinBox_voxel->value()).toDouble());
 	_ui->doubleSpinBox_noiseRadius->setValue(settings.value("cloudNoiseRadius", _ui->doubleSpinBox_noiseRadius->value()).toDouble());
 	_ui->spinBox_noiseMinNeighbors->setValue(settings.value("cloudNoiseMinNeighbors", _ui->spinBox_noiseMinNeighbors->value()).toInt());
+	_ui->doubleSpinBox_ceilingFilterHeight->setValue(settings.value("cloudCeilingHeight", _ui->doubleSpinBox_ceilingFilterHeight->value()).toDouble());
+	_ui->doubleSpinBox_floorFilterHeight->setValue(settings.value("cloudFloorHeight", _ui->doubleSpinBox_floorFilterHeight->value()).toDouble());
+	_ui->spinBox_normalKSearch->setValue(settings.value("normalKSearch", _ui->spinBox_normalKSearch->value()).toInt());
 
 	_ui->checkBox_showGraphs->setChecked(settings.value("showGraphs", _ui->checkBox_showGraphs->isChecked()).toBool());
+	_ui->checkBox_showFrustums->setChecked(settings.value("showFrustums", _ui->checkBox_showFrustums->isChecked()).toBool());
 	_ui->checkBox_showLabels->setChecked(settings.value("showLabels", _ui->checkBox_showLabels->isChecked()).toBool());
 
 	_ui->radioButton_noFiltering->setChecked(settings.value("noFiltering", _ui->radioButton_noFiltering->isChecked()).toBool());
@@ -1659,8 +1668,6 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->spinBox_subtractFilteringMinPts->setValue(settings.value("subtractFilteringMinPts", _ui->spinBox_subtractFilteringMinPts->value()).toInt());
 	_ui->doubleSpinBox_subtractFilteringRadius->setValue(settings.value("subtractFilteringRadius", _ui->doubleSpinBox_subtractFilteringRadius->value()).toDouble());
 	_ui->doubleSpinBox_subtractFilteringAngle->setValue(settings.value("subtractFilteringAngle", _ui->doubleSpinBox_subtractFilteringAngle->value()).toDouble());
-	_ui->spinBox_normalKSearch->setValue(settings.value("normalKSearch", _ui->spinBox_normalKSearch->value()).toInt());
-	_ui->checkBox_gainCompensation->setChecked(settings.value("gainCompensation", _ui->checkBox_gainCompensation->isChecked()).toBool());
 
 	_ui->checkBox_map_shown->setChecked(settings.value("gridMapShown", _ui->checkBox_map_shown->isChecked()).toBool());
 	_ui->doubleSpinBox_map_resolution->setValue(settings.value("gridMapResolution", _ui->doubleSpinBox_map_resolution->value()).toDouble());
@@ -2030,8 +2037,12 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("cloudVoxel",             _ui->doubleSpinBox_voxel->value());
 	settings.setValue("cloudNoiseRadius",       _ui->doubleSpinBox_noiseRadius->value());
 	settings.setValue("cloudNoiseMinNeighbors", _ui->spinBox_noiseMinNeighbors->value());
+	settings.setValue("cloudCeilingHeight",     _ui->doubleSpinBox_ceilingFilterHeight->value());
+	settings.setValue("cloudFloorHeight",       _ui->doubleSpinBox_floorFilterHeight->value());
+	settings.setValue("normalKSearch",           _ui->spinBox_normalKSearch->value());
 
 	settings.setValue("showGraphs", _ui->checkBox_showGraphs->isChecked());
+	settings.setValue("showFrustums", _ui->checkBox_showFrustums->isChecked());
 	settings.setValue("showLabels", _ui->checkBox_showLabels->isChecked());
 
 	settings.setValue("noFiltering",             _ui->radioButton_noFiltering->isChecked());
@@ -2042,8 +2053,6 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("subtractFilteringMinPts", _ui->spinBox_subtractFilteringMinPts->value());
 	settings.setValue("subtractFilteringRadius", _ui->doubleSpinBox_subtractFilteringRadius->value());
 	settings.setValue("subtractFilteringAngle",  _ui->doubleSpinBox_subtractFilteringAngle->value());
-	settings.setValue("normalKSearch",           _ui->spinBox_normalKSearch->value());
-	settings.setValue("gainCompensation",        _ui->checkBox_gainCompensation->isChecked());
 
 	settings.setValue("gridMapShown",                _ui->checkBox_map_shown->isChecked());
 	settings.setValue("gridMapResolution",           _ui->doubleSpinBox_map_resolution->value());
@@ -3938,23 +3947,39 @@ double PreferencesDialog::getOctomapOccupancyThr() const
 	return _ui->doubleSpinBox_octomap_occupancyThr->value();
 }
 
-double PreferencesDialog::getMapVoxel() const
+double PreferencesDialog::getVoxel() const
 {
 	return _ui->doubleSpinBox_voxel->value();
 }
-double PreferencesDialog::getMapNoiseRadius() const
+double PreferencesDialog::getNoiseRadius() const
 {
 	return _ui->doubleSpinBox_noiseRadius->value();
 }
-int PreferencesDialog::getMapNoiseMinNeighbors() const
+int PreferencesDialog::getNoiseMinNeighbors() const
 {
 	return _ui->spinBox_noiseMinNeighbors->value();
 }
+double PreferencesDialog::getCeilingFilteringHeight() const
+{
+	return _ui->doubleSpinBox_ceilingFilterHeight->value();
+}
+double PreferencesDialog::getFloorFilteringHeight() const
+{
+	return _ui->doubleSpinBox_floorFilterHeight->value();
+}
+int PreferencesDialog::getNormalKSearch() const
+{
+	return _ui->spinBox_normalKSearch->value();
+}
 
 bool PreferencesDialog::isGraphsShown() const
 {
 	return _ui->checkBox_showGraphs->isChecked();
 }
+bool PreferencesDialog::isFrustumsShown() const
+{
+	return _ui->checkBox_showFrustums->isChecked();
+}
 bool PreferencesDialog::isLabelsShown() const
 {
 	return _ui->checkBox_showLabels->isChecked();
@@ -4088,14 +4113,6 @@ double PreferencesDialog::getSubtractFilteringAngle() const
 {
 	return _ui->doubleSpinBox_subtractFilteringAngle->value()*M_PI/180.0;
 }
-int PreferencesDialog::getNormalKSearch() const
-{
-	return _ui->spinBox_normalKSearch->value();
-}
-bool PreferencesDialog::gainCompensation() const
-{
-	return _ui->checkBox_gainCompensation->isChecked();
-}
 bool PreferencesDialog::getGridMapShown() const
 {
 	return _ui->checkBox_map_shown->isChecked();