Adding util3d::cloudsFromSensorData to be able to show in DBViewer individual clouds for each camera (#1182)

* splitting multicam generated clouds

* make sure returned cloud is valid (can be empty)

corelib/include/rtabmap/core/util3d.h

@@ -144,6 +144,43 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_CORE_EXPORT cloudFromStereoImages
 		std::vector<int> * validIndices = 0,
 		const ParametersMap & parameters = ParametersMap());
 
+/**
+ * Create a XYZ cloud from the images contained in SensorData, one for each camera
+ *
+ * @param sensorData, the sensor data.
+ * @param decimation, images are decimated by this factor before projecting points to 3D. The factor
+ * should be a factor of the image width and height.
+ * @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud).
+ * @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud).
+ * @param validIndices, the indices of valid points in the cloud
+ * @param stereoParameters, stereo optional parameters (in case it is stereo data)
+ * @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected.
+ * @return XYZ cloud(s), one per camera
+ */
+std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> RTABMAP_CORE_EXPORT cloudsFromSensorData(
+		const SensorData & sensorData,
+		int decimation = 1,
+		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
+		std::vector<pcl::IndicesPtr> * validIndices = 0,
+		const ParametersMap & stereoParameters = ParametersMap(),
+		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
+
+/**
+ * Create a XYZ cloud from the images contained in SensorData. If there is only one camera,
+ * the returned cloud is organized. Otherwise, all NaN
+ * points are removed and the cloud will be dense.
+ *
+ * @param sensorData, the sensor data.
+ * @param decimation, images are decimated by this factor before projecting points to 3D. The factor
+ * should be a factor of the image width and height.
+ * @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud).
+ * @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud).
+ * @param validIndices, the indices of valid points in the cloud
+ * @param stereoParameters, stereo optional parameters (in case it is stereo data)
+ * @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected.
+ * @return a XYZ cloud.
+ */
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(
 		const SensorData & sensorData,
 		int decimation = 1,
@@ -153,6 +190,28 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(
 		const ParametersMap & stereoParameters = ParametersMap(),
 		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
 
+/**
+ * Create an RGB cloud from the images contained in SensorData, one for each camera
+ *
+ * @param sensorData, the sensor data.
+ * @param decimation, images are decimated by this factor before projecting points to 3D. The factor
+ * should be a factor of the image width and height.
+ * @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud).
+ * @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud).
+ * @param validIndices, the indices of valid points in the cloud
+ * @param stereoParameters, stereo optional parameters (in case it is stereo data)
+ * @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected.
+ * @return RGB cloud(s), one per camera
+ */
+std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> RTABMAP_CORE_EXPORT cloudsRGBFromSensorData(
+		const SensorData & sensorData,
+		int decimation = 1,
+		float maxDepth = 0.0f,
+		float minDepth = 0.0f,
+		std::vector<pcl::IndicesPtr > * validIndices = 0,
+		const ParametersMap & stereoParameters = ParametersMap(),
+		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
+
 /**
  * Create an RGB cloud from the images contained in SensorData. If there is only one camera,
  * the returned cloud is organized. Otherwise, all NaN
@@ -164,6 +223,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(
  * @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud).
  * @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud).
  * @param validIndices, the indices of valid points in the cloud
+ * @param stereoParameters, stereo optional parameters (in case it is stereo data)
  * @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected.
  * @return a RGB cloud.
  */

corelib/src/util3d.cpp

@@ -850,12 +850,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 			validIndices);
 }
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
+std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudsFromSensorData(
 		const SensorData & sensorData,
 		int decimation,
 		float maxDepth,
 		float minDepth,
-		std::vector<int> * validIndices,
+		std::vector<pcl::IndicesPtr> * validIndices,
 		const ParametersMap & stereoParameters,
 		const std::vector<float> & roiRatios)
 {
@@ -864,7 +864,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
 		decimation = 1;
 	}
 
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
 
 	if(!sensorData.depthRaw().empty() && sensorData.cameraModels().size())
 	{
@@ -873,6 +873,11 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
 		int subImageWidth = sensorData.depthRaw().cols/sensorData.cameraModels().size();
 		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
 		{
+			clouds.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
+			if(validIndices)
+			{
+				validIndices->push_back(pcl::IndicesPtr(new std::vector<int>()));
+			}
 			if(sensorData.cameraModels()[i].isValidForProjection())
 			{
 				cv::Mat depth = cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows));
@@ -928,7 +933,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
 						decimation,
 						maxDepth,
 						minDepth,
-						sensorData.cameraModels().size()==1?validIndices:0);
+						validIndices?validIndices->back().get():0);
 
 				if(tmp->size())
 				{
@@ -936,16 +941,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
 					{
 						tmp = util3d::transformPointCloud(tmp, model.localTransform());
 					}
-
+					clouds.back() = tmp;
-					if(sensorData.cameraModels().size() > 1)
-					{
-						tmp = util3d::removeNaNFromPointCloud(tmp);
-						*cloud += *tmp;
-					}
-					else
-					{
-						cloud = tmp;
-					}
 				}
 			}
 			else
@@ -974,6 +970,11 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
 		int subImageWidth = sensorData.rightRaw().cols/sensorData.stereoCameraModels().size();
 		for(unsigned int i=0; i<sensorData.stereoCameraModels().size(); ++i)
 		{
+			clouds.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
+			if(validIndices)
+			{
+				validIndices->push_back(pcl::IndicesPtr(new std::vector<int>()));
+			}
 			if(sensorData.stereoCameraModels()[i].isValidForProjection())
 			{
 				cv::Mat left(leftMono, cv::Rect(subImageWidth*i, 0, subImageWidth, leftMono.rows));
@@ -1014,7 +1015,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
 						decimation,
 						maxDepth,
 						minDepth,
-						validIndices);
+						validIndices?validIndices->back().get():0);
 
 				if(tmp->size())
 				{
@@ -1022,16 +1023,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
 					{
 						tmp = util3d::transformPointCloud(tmp, model.localTransform());
 					}
-
+					clouds.back() = tmp;
-					if(sensorData.stereoCameraModels().size() > 1)
-					{
-						tmp = util3d::removeNaNFromPointCloud(tmp);
-						*cloud += *tmp;
-					}
-					else
-					{
-						cloud = tmp;
-					}
 				}
 			}
 			else
@@ -1041,19 +1033,10 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
 		}
 	}
 
-	if(!cloud->empty() && cloud->is_dense && validIndices)
+	return clouds;
-	{
-		//generate indices for all points (they are all valid)
-		validIndices->resize(cloud->size());
-		for(unsigned int i=0; i<cloud->size(); ++i)
-		{
-			validIndices->at(i) = i;
-		}
-	}
-	return cloud;
 }
 
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
+pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromSensorData(
 		const SensorData & sensorData,
 		int decimation,
 		float maxDepth,
@@ -1061,13 +1044,66 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
 		std::vector<int> * validIndices,
 		const ParametersMap & stereoParameters,
 		const std::vector<float> & roiRatios)
+{
+	std::vector<pcl::IndicesPtr> validIndicesV;
+	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds = cloudsFromSensorData(
+		sensorData,
+		decimation,
+		maxDepth,
+		minDepth,
+		validIndices?&validIndicesV:0,
+		stereoParameters,
+		roiRatios);
+
+	if(validIndices)
+	{
+		UASSERT(validIndicesV.size() == clouds.size());
+	}
+	
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+	if(clouds.size() == 1)
+	{
+		cloud = clouds[0];
+		if(validIndices)
+		{
+			*validIndices = *validIndicesV[0];
+		}
+	}
+	else
+	{
+		for(size_t i=0; i<clouds.size(); ++i)
+		{
+			*cloud += *util3d::removeNaNFromPointCloud(clouds[i]);
+		}
+		if(validIndices)
+		{
+			//generate indices for all points (they are all valid)
+			validIndices->resize(cloud->size());
+			for(size_t i=0; i<cloud->size(); ++i)
+			{
+				validIndices->at(i) = i;
+			}
+		}
+	}
+	return cloud;
+}
+
+std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> cloudsRGBFromSensorData(
+		const SensorData & sensorData,
+		int decimation,
+		float maxDepth,
+		float minDepth,
+		std::vector<pcl::IndicesPtr> * validIndices,
+		const ParametersMap & stereoParameters,
+		const std::vector<float> & roiRatios)
 {
 	if(decimation == 0)
 	{
 		decimation = 1;
 	}
 
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
 
 	if(!sensorData.imageRaw().empty() && !sensorData.depthRaw().empty() && sensorData.cameraModels().size())
 	{
@@ -1082,6 +1118,11 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
 
 		for(unsigned int i=0; i<sensorData.cameraModels().size(); ++i)
 		{
+			clouds.push_back(pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>));
+			if(validIndices)
+			{
+				validIndices->push_back(pcl::IndicesPtr(new std::vector<int>()));
+			}
 			if(sensorData.cameraModels()[i].isValidForProjection())
 			{
 				cv::Mat rgb(sensorData.imageRaw(), cv::Rect(subRGBWidth*i, 0, subRGBWidth, sensorData.imageRaw().rows));
@@ -1128,7 +1169,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
 						decimation,
 						maxDepth,
 						minDepth,
-						sensorData.cameraModels().size() == 1?validIndices:0);
+						validIndices?validIndices->back().get():0);
 
 				if(tmp->size())
 				{
@@ -1136,16 +1177,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
 					{
 						tmp = util3d::transformPointCloud(tmp, model.localTransform());
 					}
-
+					clouds.back() = tmp;
-					if(sensorData.cameraModels().size() > 1)
-					{
-						tmp = util3d::removeNaNFromPointCloud(tmp);
-						*cloud += *tmp;
-					}
-					else
-					{
-						cloud = tmp;
-					}
 				}
 			}
 			else
@@ -1164,6 +1196,11 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
 		int subImageWidth = sensorData.rightRaw().cols/sensorData.stereoCameraModels().size();
 		for(unsigned int i=0; i<sensorData.stereoCameraModels().size(); ++i)
 		{
+			clouds.push_back(pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>));
+			if(validIndices)
+			{
+				validIndices->push_back(pcl::IndicesPtr(new std::vector<int>()));
+			}
 			if(sensorData.stereoCameraModels()[i].isValidForProjection())
 			{
 				cv::Mat left(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows));
@@ -1205,7 +1242,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
 						decimation,
 						maxDepth,
 						minDepth,
-						validIndices,
+						validIndices?validIndices->back().get():0,
 						stereoParameters);
 
 				if(tmp->size())
@@ -1214,16 +1251,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
 					{
 						tmp = util3d::transformPointCloud(tmp, model.localTransform());
 					}
-
+					clouds.back() = tmp;
-					if(sensorData.stereoCameraModels().size() > 1)
-					{
-						tmp = util3d::removeNaNFromPointCloud(tmp);
-						*cloud += *tmp;
-					}
-					else
-					{
-						cloud = tmp;
-					}
 				}
 			}
 			else
@@ -1233,16 +1261,59 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
 		}
 	}
 
-	if(cloud->is_dense && validIndices)
+	return clouds;
+}
+
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBFromSensorData(
+		const SensorData & sensorData,
+		int decimation,
+		float maxDepth,
+		float minDepth,
+		std::vector<int> * validIndices,
+		const ParametersMap & stereoParameters,
+		const std::vector<float> & roiRatios)
+{
+	std::vector<pcl::IndicesPtr> validIndicesV;
+	std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds = cloudsRGBFromSensorData(
+		sensorData,
+		decimation,
+		maxDepth,
+		minDepth,
+		validIndices?&validIndicesV:0,
+		stereoParameters,
+		roiRatios);
+
+	if(validIndices)
 	{
-		//generate indices for all points (they are all valid)
+		UASSERT(validIndicesV.size() == clouds.size());
-		validIndices->resize(cloud->size());
+	}
-		for(unsigned int i=0; i<cloud->size(); ++i)
+	
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+
+	if(clouds.size() == 1)
+	{
+		cloud = clouds[0];
+		if(validIndices)
 		{
-			validIndices->at(i) = i;
+			*validIndices = *validIndicesV[0];
+		}
+	}
+	else
+	{
+		for(size_t i=0; i<clouds.size(); ++i)
+		{
+			*cloud += *util3d::removeNaNFromPointCloud(clouds[i]);
+		}
+		if(validIndices)
+		{
+			//generate indices for all points (they are all valid)
+			validIndices->resize(cloud->size());
+			for(size_t i=0; i<cloud->size(); ++i)
+			{
+				validIndices->at(i) = i;
+			}
 		}
 	}
-
 	return cloud;
 }
 

guilib/src/DatabaseViewer.cpp

@@ -4876,14 +4876,8 @@ void DatabaseViewer::update(int value,
 				{
 					cloudViewer_->removeAllLines();
 					cloudViewer_->removeAllFrustums();
-					cloudViewer_->removeCloud("mesh");
-					cloudViewer_->removeCloud("cloud");
-					cloudViewer_->removeCloud("scan");
 					cloudViewer_->removeCloud("map");
-					cloudViewer_->removeCloud("ground");
+					cloudViewer_->removeAllClouds();
-					cloudViewer_->removeCloud("obstacles");
-					cloudViewer_->removeCloud("empty_cells");
-					cloudViewer_->removeCloud("words");
 					cloudViewer_->removeOctomap();
 
 					Transform pose = Transform::getIdentity();
@@ -5047,8 +5041,8 @@ void DatabaseViewer::update(int value,
 						{
 							if(!data.imageRaw().empty())
 							{
-								pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+								std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
-								pcl::IndicesPtr indices(new std::vector<int>);
+								std::vector<pcl::IndicesPtr> allIndices;
 								if(!data.depthRaw().empty() && data.cameraModels().size()==1)
 								{
 									cv::Mat depth = data.depthRaw();
@@ -5056,96 +5050,110 @@ void DatabaseViewer::update(int value,
 									{
 										depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
 									}
-									cloud = util3d::cloudFromDepthRGB(
+									pcl::IndicesPtr indices(new std::vector<int>);
+									pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
 											data.imageRaw(),
 											depth,
 											data.cameraModels()[0],
 											ui_->spinBox_decimation->value(),0,0,indices.get());
 									if(indices->size())
 									{
-										cloud = util3d::transformPointCloud(cloud, data.cameraModels()[0].localTransform());
+										clouds.push_back(util3d::transformPointCloud(cloud, data.cameraModels()[0].localTransform()));
+										allIndices.push_back(indices);
 									}
-
 								}
 								else
 								{
-									cloud = util3d::cloudRGBFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, indices.get(), ui_->parameters_toolbox->getParameters());
+									clouds = util3d::cloudsRGBFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, &allIndices, ui_->parameters_toolbox->getParameters());
 								}
-								if(indices->size())
+								UASSERT(clouds.size() == allIndices.size());
+								for(size_t i=0; i<allIndices.size(); ++i)
 								{
-									if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
+									if(allIndices[i]->size())
 									{
-										cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
+										pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = clouds[i];
-									}
+										pcl::IndicesPtr indices = allIndices[i];
-
+										if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
-									if(ui_->checkBox_showMesh->isChecked() && !cloud->is_dense)
-									{
-										Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
-										if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
 										{
-											viewpoint[0] = data.cameraModels()[0].localTransform().x();
+											cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
-											viewpoint[1] = data.cameraModels()[0].localTransform().y();
-											viewpoint[2] = data.cameraModels()[0].localTransform().z();
 										}
-										else if(data.stereoCameraModels().size() && !data.stereoCameraModels()[0].localTransform().isNull())
-										{
-											viewpoint[0] = data.stereoCameraModels()[0].localTransform().x();
-											viewpoint[1] = data.stereoCameraModels()[0].localTransform().y();
-											viewpoint[2] = data.stereoCameraModels()[0].localTransform().z();
-										}
-										std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
-												cloud,
-												float(ui_->spinBox_mesh_angleTolerance->value())*M_PI/180.0f,
-												ui_->checkBox_mesh_quad->isChecked(),
-												ui_->spinBox_mesh_triangleSize->value(),
-												viewpoint);
 
-										if(ui_->spinBox_mesh_minClusterSize->value())
+										if(ui_->checkBox_showMesh->isChecked() && !cloud->is_dense)
 										{
-											// filter polygons
+											Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
-											std::vector<std::set<int> > neighbors;
+											if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
-											std::vector<std::set<int> > vertexToPolygons;
-											util3d::createPolygonIndexes(polygons,
-													cloud->size(),
-													neighbors,
-													vertexToPolygons);
-											std::list<std::list<int> > clusters = util3d::clusterPolygons(
-													neighbors,
-													ui_->spinBox_mesh_minClusterSize->value());
-											std::vector<pcl::Vertices> filteredPolygons(polygons.size());
-											int oi=0;
-											for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
 											{
-												for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
+												viewpoint[0] = data.cameraModels()[0].localTransform().x();
-												{
+												viewpoint[1] = data.cameraModels()[0].localTransform().y();
-													filteredPolygons[oi++] = polygons.at(*jter);
+												viewpoint[2] = data.cameraModels()[0].localTransform().z();
-												}
 											}
-											filteredPolygons.resize(oi);
+											else if(data.stereoCameraModels().size() && !data.stereoCameraModels()[0].localTransform().isNull())
-											polygons = filteredPolygons;
+											{
-										}
+												viewpoint[0] = data.stereoCameraModels()[0].localTransform().x();
+												viewpoint[1] = data.stereoCameraModels()[0].localTransform().y();
+												viewpoint[2] = data.stereoCameraModels()[0].localTransform().z();
+											}
+											std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
+													cloud,
+													float(ui_->spinBox_mesh_angleTolerance->value())*M_PI/180.0f,
+													ui_->checkBox_mesh_quad->isChecked(),
+													ui_->spinBox_mesh_triangleSize->value(),
+													viewpoint);
 
-										cloudViewer_->addCloudMesh("mesh", cloud, polygons, pose);
+											if(ui_->spinBox_mesh_minClusterSize->value())
-									}
+											{
-									if(ui_->checkBox_showCloud->isChecked())
+												// filter polygons
-									{
+												std::vector<std::set<int> > neighbors;
-										cloudViewer_->addCloud("cloud", cloud, pose);
+												std::vector<std::set<int> > vertexToPolygons;
+												util3d::createPolygonIndexes(polygons,
+														cloud->size(),
+														neighbors,
+														vertexToPolygons);
+												std::list<std::list<int> > clusters = util3d::clusterPolygons(
+														neighbors,
+														ui_->spinBox_mesh_minClusterSize->value());
+												std::vector<pcl::Vertices> filteredPolygons(polygons.size());
+												int oi=0;
+												for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
+												{
+													for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
+													{
+														filteredPolygons[oi++] = polygons.at(*jter);
+													}
+												}
+												filteredPolygons.resize(oi);
+												polygons = filteredPolygons;
+											}
+
+											cloudViewer_->addCloudMesh(uFormat("mesh_%d", i), cloud, polygons, pose);
+										}
+										if(ui_->checkBox_showCloud->isChecked())
+										{
+											cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose);
+										}
 									}
 								}
 							}
 							else if(ui_->checkBox_showCloud->isChecked())
 							{
-								pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+								std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
-								pcl::IndicesPtr indices(new std::vector<int>);
+								std::vector<pcl::IndicesPtr> allIndices;
-								cloud = util3d::cloudFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, indices.get(), ui_->parameters_toolbox->getParameters());
-								if(indices->size())
-								{
-									if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
-									{
-										cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
-									}
 
-									cloudViewer_->addCloud("cloud", cloud, pose);
+								clouds = util3d::cloudsFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, &allIndices, ui_->parameters_toolbox->getParameters());
+								UASSERT(clouds.size() == allIndices.size());
+								for(size_t i=0; i<allIndices.size(); ++i)
+								{
+									if(allIndices[i]->size())
+									{
+										pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = clouds[i];
+										pcl::IndicesPtr indices = allIndices[i];
+										if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
+										{
+											cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
+										}
+
+										cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose);
+									}
 								}
 							}
 						}