Mesh reconstruction update: fixed normals computation (with and without MLS)

corelib/include/rtabmap/core/util3d_filtering.h

@@ -46,6 +46,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP voxelize(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float voxelSize);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP voxelize(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		float voxelSize);
 
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP sampling(
@@ -141,6 +144,32 @@ pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
 		float radiusSearch,
 		int minNeighborsInRadius = 0);
 
+/**
+ * For convenience.
+ */
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
+		float radiusSearch,
+		int minNeighborsInRadius = 0);
+
+/**
+ * Subtract a cloud from another one using radius filtering.
+ * @param cloud the input cloud.
+ * @param indices the input indices of the cloud to check, if empty, all points in the cloud are checked.
+ * @param cloud the input cloud to subtract.
+ * @param indices the input indices of the subtracted cloud to check, if empty, all points in the cloud are checked.
+ * @param radiusSearch the radius in meter.
+ * @return the indices of the points satisfying the parameters.
+ */
+pcl::IndicesPtr RTABMAP_EXP subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
+		const pcl::IndicesPtr & substractIndices,
+		float radiusSearch,
+		int minNeighborsInRadius = 0);
+
 
 /**
  * For convenience.

corelib/include/rtabmap/core/util3d_surface.h

@@ -30,7 +30,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <rtabmap/core/RtabmapExp.h>
 
-
 #include <pcl/PolygonMesh.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -49,7 +48,7 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(
 		float gp3MaximumSurfaceAngle = M_PI/4,
 		float gp3MinimumAngle = M_PI/18,
 		float gp3MaximumAngle = 2*M_PI/3,
-		bool gp3NormalConsistency = false);
+		bool gp3NormalConsistency = true);
 
 pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -62,7 +61,42 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormals(
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP computeNormalsSmoothed(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float smoothingSearchRadius = 0.025,
-		bool smoothingPolynomialFit = true);
+		bool smoothingPolynomialFit = true,
+		float voxelSize = 0.0f);
+
+void RTABMAP_EXP adjustNormalsToViewPoints(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & viewpoints,
+		pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud);
+
+template<typename pointT>
+std::vector<pcl::Vertices> normalizePolygonsSide(
+		const pcl::PointCloud<pointT> & cloud,
+		const std::vector<pcl::Vertices> & polygons,
+		const pcl::PointXYZ & viewPoint = pcl::PointXYZ(0,0,0))
+{
+	std::vector<pcl::Vertices> output(polygons.size());
+	for(unsigned int i=0; i<polygons.size(); ++i)
+	{
+		pcl::Vertices polygon = polygons[i];
+		Eigen::Vector3f v1 = cloud.at(polygon.vertices[1]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
+		Eigen::Vector3f v2 = cloud.at(polygon.vertices[2]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
+		Eigen::Vector3f n = (v1.cross(v2)).normalized();
+
+		Eigen::Vector3f p = Eigen::Vector3f(viewPoint.x, viewPoint.y, viewPoint.z) - cloud.at(polygon.vertices[1]).getVector3fMap();
+
+		float result = n.dot(p);
+		if(result < 0)
+		{
+			//reverse vertices order
+			int tmp = polygon.vertices[0];
+			polygon.vertices[0] = polygon.vertices[2];
+			polygon.vertices[2] = tmp;
+		}
+
+		output[i] = polygon;
+	}
+	return output;
+}
 
 } // namespace util3d
 } // namespace rtabmap

corelib/include/rtabmap/core/util3d_transforms.h

@@ -46,6 +46,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const Transform & transform);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const Transform & transform);
 
 pcl::PointXYZ RTABMAP_EXP transformPoint(
 		const pcl::PointXYZ & pt,

corelib/src/util3d_filtering.cpp

@@ -72,6 +72,18 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
 	filter.filter(*output);
 	return output;
 }
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr voxelize(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		float voxelSize)
+{
+	UASSERT(voxelSize > 0.0f);
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	pcl::VoxelGrid<pcl::PointXYZRGBNormal> filter;
+	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
+	filter.setInputCloud(cloud);
+	filter.filter(*output);
+	return output;
+}
 
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr sampling(
@@ -357,6 +369,82 @@ pcl::IndicesPtr subtractFiltering(
 	}
 }
 
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
+		float radiusSearch,
+		int minNeighborsInRadius)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, minNeighborsInRadius);
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	pcl::copyPointCloud(*cloud, *indicesOut, *out);
+	return out;
+}
+
+
+pcl::IndicesPtr subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
+		const pcl::IndicesPtr & substractIndices,
+		float radiusSearch,
+		int minNeighborsInRadius)
+{
+	pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>(false));
+
+	if(indices->size())
+	{
+		pcl::IndicesPtr output(new std::vector<int>(indices->size()));
+		int oi = 0; // output iterator
+		if(substractIndices->size())
+		{
+			tree->setInputCloud(substractCloud, substractIndices);
+		}
+		else
+		{
+			tree->setInputCloud(substractCloud);
+		}
+		for(unsigned int i=0; i<indices->size(); ++i)
+		{
+			std::vector<int> kIndices;
+			std::vector<float> kDistances;
+			int k = tree->radiusSearch(cloud->at(indices->at(i)), radiusSearch, kIndices, kDistances);
+			if(k <= minNeighborsInRadius)
+			{
+				output->at(oi++) = indices->at(i);
+			}
+		}
+		output->resize(oi);
+		return output;
+	}
+	else
+	{
+		pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
+		int oi = 0; // output iterator
+		if(substractIndices->size())
+		{
+			tree->setInputCloud(substractCloud, substractIndices);
+		}
+		else
+		{
+			tree->setInputCloud(substractCloud);
+		}
+		for(unsigned int i=0; i<cloud->size(); ++i)
+		{
+			std::vector<int> kIndices;
+			std::vector<float> kDistances;
+			int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
+			if(k <= minNeighborsInRadius)
+			{
+				output->at(oi++) = i;
+			}
+		}
+		output->resize(oi);
+		return output;
+	}
+}
+
 
 pcl::IndicesPtr normalFiltering(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

corelib/src/util3d_surface.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/util3d_surface.h"
 #include "rtabmap/core/util3d_filtering.h"
+#include "rtabmap/utilite/ULogger.h"
 #include <pcl/search/kdtree.h>
 #include <pcl/surface/gp3.h>
 #include <pcl/features/normal_3d_omp.h>
@@ -68,12 +69,16 @@ pcl::PolygonMesh::Ptr createMesh(
 	gp3.setMinimumAngle(gp3MinimumAngle); // 10 degrees
 	gp3.setMaximumAngle(gp3MaximumAngle); // 120 degrees
 	gp3.setNormalConsistency(gp3NormalConsistency);
+	gp3.setConsistentVertexOrdering(gp3NormalConsistency);
 
 	// Get result
 	gp3.setInputCloud (cloudWithNormalsNoNaN);
 	gp3.setSearchMethod (tree2);
 	gp3.reconstruct (*mesh);
 
+	//UASSERT(mesh->cloud.data.size()/mesh->cloud.point_step == cloudWithNormalsNoNaN->size());
+	//mesh->polygons = normalizePolygonsSide(*cloudWithNormalsNoNaN, mesh->polygons);
+
 	return mesh;
 }
 
@@ -128,7 +133,8 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormals(
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormalsSmoothed(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float smoothingSearchRadius,
-		bool smoothingPolynomialFit)
+		bool smoothingPolynomialFit,
+		float voxelSize)
 {
 	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
@@ -144,6 +150,11 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormalsSmoothed(
 	mls.setPolynomialFit (smoothingPolynomialFit);
 	mls.setSearchMethod (tree);
 	mls.setSearchRadius (smoothingSearchRadius);
+	if(voxelSize > 0.0f)
+	{
+		mls.setUpsamplingMethod(pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGBNormal>::VOXEL_GRID_DILATION);
+		mls.setDilationVoxelSize(voxelSize);
+	}
 
 	// Reconstruct
 	mls.process (*cloud_with_normals);
@@ -151,6 +162,37 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr computeNormalsSmoothed(
 	return cloud_with_normals;
 }
 
+void adjustNormalsToViewPoints(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & viewpoints,
+		pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud)
+{
+	if(viewpoints->size() && cloud.size())
+	{
+		pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
+		tree->setInputCloud (viewpoints);
+
+		for(unsigned int i=0; i<cloud.size(); ++i)
+		{
+			std::vector<int> indices;
+			std::vector<float> dist;
+			tree->nearestKSearch(pcl::PointXYZ(cloud.points[i].x, cloud.points[i].y, cloud.points[i].z), 1, indices, dist);
+			UASSERT(indices.size() == 1);
+
+			Eigen::Vector3f v = viewpoints->at(indices[0]).getVector3fMap() - cloud.points[i].getVector3fMap();
+			Eigen::Vector3f n(cloud.points[i].normal_x, cloud.points[i].normal_y, cloud.points[i].normal_z);
+
+			float result = v.dot(n);
+			if(result < 0)
+			{
+				//reverse normal
+				cloud.points[i].normal_x *= -1.0f;
+				cloud.points[i].normal_y *= -1.0f;
+				cloud.points[i].normal_z *= -1.0f;
+			}
+		}
+	}
+}
+
 }
 
 }

corelib/src/util3d_transforms.cpp

@@ -51,6 +51,14 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformPointCloud(
 	pcl::transformPointCloud(*cloud, *output, transform.toEigen4f());
 	return output;
 }
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const Transform & transform)
+{
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	pcl::transformPointCloudWithNormals(*cloud, *output, transform.toEigen4f());
+	return output;
+}
 
 pcl::PointXYZ transformPoint(
 		const pcl::PointXYZ & pt,

guilib/include/rtabmap/gui/CloudViewer.h

@@ -72,6 +72,12 @@ public:
 		const std::string & id,
 		const Transform & pose); //including mesh
 
+	bool updateCloud(
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = QColor());
+
 	bool updateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -84,6 +90,12 @@ public:
 		const Transform & pose = Transform::getIdentity(),
 		const QColor & color = QColor());
 
+	bool addOrUpdateCloud(
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = QColor());
+
 	bool addOrUpdateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -101,30 +113,37 @@ public:
 			const pcl::PCLPointCloud2Ptr & binaryCloud,
 			const Transform & pose,
 			bool rgb,
+			bool haveNormals,
 			const QColor & color = QColor());
 
 	bool addCloud(
-		const std::string & id,
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const Transform & pose = Transform::getIdentity(),
-		const QColor & color = QColor());
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = QColor());
 
 	bool addCloud(
-		const std::string & id,
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const Transform & pose = Transform::getIdentity(),
-		const QColor & color = QColor());
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = QColor());
+
+	bool addCloud(
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = QColor());
 
 	bool addCloudMesh(
-		const std::string & id,
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const std::vector<pcl::Vertices> & polygons,
-		const Transform & pose = Transform::getIdentity());
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+			const std::vector<pcl::Vertices> & polygons,
+			const Transform & pose = Transform::getIdentity());
 
 	bool addCloudMesh(
-		const std::string & id,
-		const pcl::PolygonMesh::Ptr & mesh,
-		const Transform & pose = Transform::getIdentity());
+			const std::string & id,
+			const pcl::PolygonMesh::Ptr & mesh,
+			const Transform & pose = Transform::getIdentity());
 
 	bool addOccupancyGridMap(
 			const cv::Mat & map8U,

guilib/include/rtabmap/gui/MainWindow.h

@@ -226,23 +226,19 @@ private:
 	void exportPoses(int format);
 	QString captureScreen();
 
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr getAssembledCloud(
-			const std::map<int, Transform> & poses,
-			float assembledVoxelSize,
-			bool regenerateClouds,
-			int regenerateDecimation,
-			float regenerateVoxelSize,
-			float regenerateMaxDepth) const;
-	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > getClouds(
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr > getClouds(
 			const std::map<int, Transform> & poses,
 			bool regenerateClouds,
 			int regenerateDecimation,
 			float regenerateVoxelSize,
-			float regenerateMaxDepth) const;
+			float regenerateMaxDepth,
+			int normalKSearch,
+			bool mls,
+			float mlsRadius) const;
 
 	bool getExportedScans(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans);
-	bool getExportedClouds(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds, std::map<int, pcl::PolygonMesh::Ptr> & meshes, bool toSave);
-	void saveClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds, bool binaryMode = true);
+	bool getExportedClouds(std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds, std::map<int, pcl::PolygonMesh::Ptr> & meshes, bool toSave);
+	void saveClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds, bool binaryMode = true);
 	void saveMeshes(const std::map<int, pcl::PolygonMesh::Ptr> & meshes, bool binaryMode = true);
 	void saveScans(const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr> & clouds, bool binaryMode = true);
 

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -156,6 +156,7 @@ public:
 
 	int getMeshNormalKSearch() const;
 	double getMeshGP3Radius() const;
+	double getMeshGP3Mu() const;
 	bool getMeshSmoothing() const;
 	double getMeshSmoothingRadius() const;
 

guilib/src/CloudViewer.cpp

@@ -318,6 +318,26 @@ bool CloudViewer::updateCloudPose(
 	return false;
 }
 
+bool CloudViewer::updateCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const Transform & pose,
+		const QColor & color)
+{
+	if(_addedClouds.contains(id))
+	{
+		UDEBUG("Updating %s with %d points", id.c_str(), (int)cloud->size());
+		int index = _visualizer->getColorHandlerIndex(id);
+		this->removeCloud(id);
+		if(this->addCloud(id, cloud, pose, color))
+		{
+			_visualizer->updateColorHandlerIndex(id, index);
+			return true;
+		}
+	}
+	return false;
+}
+
 bool CloudViewer::updateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -358,6 +378,19 @@ bool CloudViewer::updateCloud(
 	return false;
 }
 
+bool CloudViewer::addOrUpdateCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const Transform & pose,
+		const QColor & color)
+{
+	if(!updateCloud(id, cloud, pose, color))
+	{
+		return addCloud(id, cloud, pose, color);
+	}
+	return true;
+}
+
 bool CloudViewer::addOrUpdateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -389,6 +422,7 @@ bool CloudViewer::addCloud(
 		const pcl::PCLPointCloud2Ptr & binaryCloud,
 		const Transform & pose,
 		bool rgb,
+		bool haveNormals,
 		const QColor & color)
 {
 	if(!_addedClouds.contains(id))
@@ -422,7 +456,21 @@ bool CloudViewer::addCloud(
 				//rgb
 				colorHandler.reset(new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PCLPointCloud2>(binaryCloud));
 				_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+			}
 
+			if(haveNormals)
+			{
+				//normals
+				colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_x"));
+				_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+				colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_y"));
+				_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+				colorHandler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (binaryCloud, "normal_z"));
+				_visualizer->addPointCloud (binaryCloud, colorHandler, origin, orientation, id);
+			}
+
+			if(rgb)
+			{
 				_visualizer->updateColorHandlerIndex(id, 5);
 			}
 			else if(color.isValid())
@@ -437,6 +485,23 @@ bool CloudViewer::addCloud(
 	return false;
 }
 
+bool CloudViewer::addCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const Transform & pose,
+		const QColor & color)
+{
+	if(!_addedClouds.contains(id))
+	{
+		UDEBUG("Adding %s with %d points", id.c_str(), (int)cloud->size());
+
+		pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
+		pcl::toPCLPointCloud2(*cloud, *binaryCloud);
+		return addCloud(id, binaryCloud, pose, true, true, color);
+	}
+	return false;
+}
+
 bool CloudViewer::addCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -449,7 +514,7 @@ bool CloudViewer::addCloud(
 
 		pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
 		pcl::toPCLPointCloud2(*cloud, *binaryCloud);
-		return addCloud(id, binaryCloud, pose, true, color);
+		return addCloud(id, binaryCloud, pose, true, false, color);
 	}
 	return false;
 }
@@ -466,7 +531,7 @@ bool CloudViewer::addCloud(
 
 		pcl::PCLPointCloud2Ptr binaryCloud(new pcl::PCLPointCloud2);
 		pcl::toPCLPointCloud2(*cloud, *binaryCloud);
-		return addCloud(id, binaryCloud, pose, false, color);
+		return addCloud(id, binaryCloud, pose, false, false, color);
 	}
 	return false;
 }

guilib/src/ExportCloudsDialog.cpp

@@ -52,6 +52,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->groupBox_gp3, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
 	connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_gp3Radius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_gp3Mu, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 }
 
 ExportCloudsDialog::~ExportCloudsDialog()
@@ -77,6 +78,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("mesh", this->getMesh());
 	settings.setValue("mesh_k", this->getMeshNormalKSearch());
 	settings.setValue("mesh_radius", this->getMeshGp3Radius());
+	settings.setValue("mesh_mu", this->getMeshGp3Mu());
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -101,6 +103,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	this->setMesh(settings.value("mesh", this->getMesh()).toBool());
 	this->setMeshNormalKSearch(settings.value("mesh_k", this->getMeshNormalKSearch()).toInt());
 	this->setMeshGp3Radius(settings.value("mesh_radius", this->getMeshGp3Radius()).toDouble());
+	this->setMeshGp3Mu(settings.value("mesh_mu", this->getMeshGp3Mu()).toDouble());
 	if(!group.isEmpty())
 	{
 		settings.endGroup();
@@ -124,6 +127,7 @@ void ExportCloudsDialog::restoreDefaults()
 	setMesh(false);
 	setMeshNormalKSearch(20);
 	setMeshGp3Radius(0.04);
+	setMeshGp3Radius(2.5);
 }
 
 void ExportCloudsDialog::setSaveButton()
@@ -209,6 +213,10 @@ double ExportCloudsDialog::getMeshGp3Radius() const
 {
 	return _ui->doubleSpinBox_gp3Radius->value();
 }
+double ExportCloudsDialog::getMeshGp3Mu() const
+{
+	return _ui->doubleSpinBox_gp3Mu->value();
+}
 
 //setters
 void ExportCloudsDialog::setAssemble(bool on)
@@ -259,6 +267,9 @@ void ExportCloudsDialog::setMeshGp3Radius(double radius)
 {
 	_ui->doubleSpinBox_gp3Radius->setValue(radius);
 }
-
+void ExportCloudsDialog::setMeshGp3Mu(double mu)
+{
+	_ui->doubleSpinBox_gp3Mu->setValue(mu);
+}
 
 }

guilib/src/ExportCloudsDialog.h

@@ -65,6 +65,7 @@ public:
 	bool getMesh() const;
 	int getMeshNormalKSearch() const;
 	double getMeshGp3Radius() const;
+	double getMeshGp3Mu() const;
 
 	//setters
 	void setAssemble(bool on);
@@ -79,6 +80,7 @@ public:
 	void setMesh(bool on);
 	void setMeshNormalKSearch(int k);
 	void setMeshGp3Radius(double radius);
+	void setMeshGp3Mu(double mu);
 
 signals:
 	void configChanged();

guilib/src/MainWindow.cpp

@@ -1751,7 +1751,7 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 							_preferencesDialog->getCloudVoxelSize(0),
 							_preferencesDialog->getSubstractFilteringMinPts());
 					UDEBUG("Filtering %d from %d -> %d", (int)previousCloud->size(), (int)cloud->size(), (int)cloudFiltered->size());
-
+					_createdClouds.at(link.from()) = cloudFiltered;
 				}
 			}
 		}
@@ -1764,13 +1764,21 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
 				if(_preferencesDialog->getMeshSmoothing())
 				{
-					cloudWithNormals = util3d::computeNormalsSmoothed(cloudFiltered, (float)_preferencesDialog->getMeshSmoothingRadius());
+					cloudWithNormals = util3d::computeNormalsSmoothed(
+							cloudFiltered,
+							(float)_preferencesDialog->getMeshSmoothingRadius(),
+							false,
+							(float)_preferencesDialog->getCloudVoxelSize(0));
+					//if(_preferencesDialog->getCloudVoxelSize(0))
+					//{
+					//	cloudWithNormals = util3d::voxelize(cloudWithNormals, _preferencesDialog->getCloudVoxelSize(0));
+					//}
 				}
 				else
 				{
 					cloudWithNormals = util3d::computeNormals(cloudFiltered, _preferencesDialog->getMeshNormalKSearch());
 				}
-				mesh = util3d::createMesh(cloudWithNormals,	_preferencesDialog->getMeshGP3Radius());
+				mesh = util3d::createMesh(cloudWithNormals,	_preferencesDialog->getMeshGP3Radius(), _preferencesDialog->getMeshGP3Mu());
 			}
 
 			if(mesh->polygons.size())
@@ -1788,7 +1796,11 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 			if(_preferencesDialog->getMeshSmoothing())
 			{
 				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
-				cloudWithNormals = util3d::computeNormalsSmoothed(cloudFiltered, (float)_preferencesDialog->getMeshSmoothingRadius());
+				cloudWithNormals = util3d::computeNormalsSmoothed(
+						cloudFiltered,
+						(float)_preferencesDialog->getMeshSmoothingRadius(),
+						false,
+						(float)_preferencesDialog->getCloudVoxelSize(0));
 				cloudFiltered.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
 				pcl::copyPointCloud(*cloudWithNormals, *cloudFiltered);
 			}
@@ -4386,7 +4398,7 @@ bool MainWindow::getExportedScans(std::map<int, pcl::PointCloud<pcl::PointXYZ>::
 
 void MainWindow::exportClouds()
 {
-	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
 	std::map<int, pcl::PolygonMesh::Ptr> meshes;
 
 	if(getExportedClouds(clouds, meshes, true))
@@ -4405,7 +4417,7 @@ void MainWindow::exportClouds()
 
 void MainWindow::viewClouds()
 {
-	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
 	std::map<int, pcl::PolygonMesh::Ptr> meshes;
 
 	if(getExportedClouds(clouds, meshes, false))
@@ -4449,7 +4461,7 @@ void MainWindow::viewClouds()
 		}
 		else if(clouds.size())
 		{
-			for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
+			for(std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr>::iterator iter = clouds.begin(); iter!=clouds.end(); ++iter)
 			{
 				_initProgressDialog->appendText(tr("Viewing the cloud %1 (%2 points)...").arg(iter->first).arg(iter->second->size()));
 				_initProgressDialog->incrementStep();
@@ -4470,7 +4482,7 @@ void MainWindow::viewClouds()
 }
 
 bool MainWindow::getExportedClouds(
-		std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds,
+		std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds,
 		std::map<int, pcl::PolygonMesh::Ptr> & meshes,
 		bool toSave)
 {
@@ -4493,68 +4505,140 @@ bool MainWindow::getExportedClouds(
 
 		_initProgressDialog->resetProgress();
 		_initProgressDialog->show();
-		int mul = _exportDialog->getMesh()&&!_exportDialog->getGenerate()?3:_exportDialog->getMLS()&&!_exportDialog->getGenerate()?2:1;
+		int mul = 1;
+		if(_exportDialog->getMesh())
+		{
+			mul+=1;
+		}
+		if(_exportDialog->getAssemble())
+		{
+			mul+=1;
+		}
 		_initProgressDialog->setMaximumSteps(int(poses.size())*mul+1);
 
+		if(_exportDialog->getMLS())
+		{
+			_initProgressDialog->appendText(tr("Smoothing the surface using Moving Least Squares (MLS) algorithm... "
+					"[search radius=%1m voxel=%2m]").arg(_exportDialog->getMLSRadius()).arg(_exportDialog->getGenerateVoxel()));
+		}
+		_initProgressDialog->appendText(tr("Computing surface normals... "
+					"[K neighbors=%1]").arg(_exportDialog->getMeshNormalKSearch()));
+
+		clouds = this->getClouds(
+				poses,
+				_exportDialog->getGenerate(),
+				_exportDialog->getGenerateDecimation(),
+				_exportDialog->getGenerateVoxel(),
+				_exportDialog->getGenerateMaxDepth(),
+				_exportDialog->getMeshNormalKSearch(),
+				_exportDialog->getAssemble()?false:_exportDialog->getMLS(),
+				(float)_exportDialog->getMLSRadius());
+
 		if(_exportDialog->getAssemble())
 		{
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = this->getAssembledCloud(
-					poses,
-					_exportDialog->getAssembleVoxel(),
-					_exportDialog->getGenerate(),
-					_exportDialog->getGenerateDecimation(),
-					_exportDialog->getGenerateVoxel(),
-					_exportDialog->getGenerateMaxDepth());
+			_initProgressDialog->appendText(tr("Assembling %1 clouds...").arg(clouds.size()));
+			QApplication::processEvents();
 
-			clouds.insert(std::make_pair(0, cloud));
-		}
-		else
-		{
-			clouds = this->getClouds(
-					poses,
-					_exportDialog->getGenerate(),
-					_exportDialog->getGenerateDecimation(),
-					_exportDialog->getGenerateVoxel(),
-					_exportDialog->getGenerateMaxDepth());
-		}
-
-		if(_exportDialog->getMLS() || _exportDialog->getMesh())
-		{
-			for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::iterator iter=clouds.begin();
+			int i =0;
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+			for(std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr>::iterator iter=clouds.begin();
 				iter!= clouds.end();
 				++iter)
 			{
-				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
-				if(_exportDialog->getMLS())
+				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformed = util3d::transformPointCloud(iter->second, poses.at(iter->first));
+				*assembledCloud += *transformed;
+
+				_initProgressDialog->appendText(tr("Assembled cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(clouds.size()));
+				_initProgressDialog->incrementStep();
+
+				if(i % 100 == 0)
 				{
-					_initProgressDialog->appendText(tr("Smoothing the surface of cloud %1 using Moving Least Squares (MLS) algorithm... "
-							"[search radius=%2m]").arg(iter->first).arg(_exportDialog->getMLSRadius()));
-					_initProgressDialog->incrementStep();
 					QApplication::processEvents();
-
-					cloudWithNormals = util3d::computeNormalsSmoothed(iter->second, (float)_exportDialog->getMLSRadius());
-
-					iter->second->clear();
-					pcl::copyPointCloud(*cloudWithNormals, *iter->second);
 				}
-				else if(_exportDialog->getMesh())
-				{
-					_initProgressDialog->appendText(tr("Computing surface normals of cloud %1 (without smoothing)... "
-								"[K neighbors=%2]").arg(iter->first).arg(_exportDialog->getMeshNormalKSearch()));
-					_initProgressDialog->incrementStep();
-					QApplication::processEvents();
+			}
 
-					cloudWithNormals = util3d::computeNormals(iter->second, _exportDialog->getMeshNormalKSearch());
+			if(_exportDialog->getMLS())
+			{
+				_initProgressDialog->appendText(tr("Voxelize assembled cloud (%1 points, voxel size = %2 m)...")
+						.arg(assembledCloud->size())
+						.arg(_exportDialog->getGenerateVoxel()));
+				QApplication::processEvents();
+				if(_exportDialog->getGenerateVoxel())
+				{
+					assembledCloud = util3d::voxelize(
+							assembledCloud,
+							_exportDialog->getGenerateVoxel());
 				}
 
-				if(_exportDialog->getMesh())
+				_initProgressDialog->appendText(tr("Smoothing (MLS) of the assembled cloud (%1 points)...").arg(assembledCloud->size()));
+				QApplication::processEvents();
+
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloudXYZRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
+				pcl::copyPointCloud(*assembledCloud, *assembledCloudXYZRGB);
+				assembledCloud = util3d::computeNormalsSmoothed(
+						assembledCloudXYZRGB,
+						(float)_exportDialog->getMLSRadius(),
+						true,
+						_exportDialog->getGenerateVoxel());
+
+				if(_exportDialog->getAssembleVoxel())
 				{
-					_initProgressDialog->appendText(tr("Greedy projection triangulation... [radius=%1m]").arg(_exportDialog->getMeshGp3Radius()));
-					_initProgressDialog->incrementStep();
+					_initProgressDialog->appendText(tr("Voxelize assembled cloud (%1 points, voxel size = %2 m)...")
+							.arg(assembledCloud->size())
+							.arg(_exportDialog->getAssembleVoxel()));
 					QApplication::processEvents();
 
-					pcl::PolygonMesh::Ptr mesh = util3d::createMesh(cloudWithNormals, _exportDialog->getMeshGp3Radius());
-					meshes.insert(std::make_pair(iter->first, mesh));
+					assembledCloud = util3d::voxelize(
+							assembledCloud,
+							_exportDialog->getAssembleVoxel());
+				}
+
+				_initProgressDialog->appendText(tr("Update %1 normals with %2 camera views...").arg(assembledCloud->size()).arg(poses.size()));
+				QApplication::processEvents();
+
+				pcl::PointCloud<pcl::PointXYZ>::Ptr viewpoints(new pcl::PointCloud<pcl::PointXYZ>);
+				viewpoints->resize(poses.size());
+				int oi=0;
+				for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+				{
+					(*viewpoints)[oi].x = iter->second.x();
+					(*viewpoints)[oi].y = iter->second.y();
+					(*viewpoints)[oi++].z = iter->second.z();
+				}
+				util3d::adjustNormalsToViewPoints(viewpoints, *assembledCloud);
+			}
+			else if(_exportDialog->getAssembleVoxel())
+			{
+				_initProgressDialog->appendText(tr("Voxelize assembled cloud (%1 points)...").arg(assembledCloud->size()));
+				QApplication::processEvents();
+				assembledCloud = util3d::voxelize(
+						assembledCloud,
+						_exportDialog->getAssembleVoxel());
+				_initProgressDialog->appendText(tr("Voxelized assembled cloud (%1 points)").arg(assembledCloud->size()));
+			}
+
+			clouds.clear();
+			clouds.insert(std::make_pair(0, assembledCloud));
+		}
+
+		if(_exportDialog->getMesh())
+		{
+			_initProgressDialog->appendText(tr("Greedy projection triangulation... [radius=%1m]").arg(_exportDialog->getMeshGp3Radius()));
+			QApplication::processEvents();
+
+			int i=0;
+			for(std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr>::iterator iter=clouds.begin();
+				iter!= clouds.end();
+				++iter)
+			{
+				pcl::PolygonMesh::Ptr mesh = util3d::createMesh(iter->second, _exportDialog->getMeshGp3Radius(), _exportDialog->getMeshGp3Mu());
+				meshes.insert(std::make_pair(iter->first, mesh));
+
+				_initProgressDialog->appendText(tr("Mesh %1 created with %2 polygons (%3/%4).").arg(iter->first).arg(mesh->polygons.size()).arg(++i).arg(clouds.size()));
+				_initProgressDialog->incrementStep();
+				if(i % 100 == 0)
+				{
+					QApplication::processEvents();
 				}
 			}
 		}
@@ -4746,7 +4830,7 @@ void MainWindow::dataRecorderDestroyed()
 
 //END ACTIONS
 
-void MainWindow::saveClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> & clouds, bool binaryMode)
+void MainWindow::saveClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds, bool binaryMode)
 {
 	if(clouds.size() == 1)
 	{
@@ -4811,11 +4895,11 @@ void MainWindow::saveClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGB
 
 				if(ok)
 				{
-					for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter)
+					for(std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr >::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter)
 					{
 						if(iter->second->size())
 						{
-							pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud;
+							pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud;
 							transformedCloud = util3d::transformPointCloud(iter->second, _currentPosesMap.at(iter->first));
 
 							QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix);
@@ -5082,119 +5166,17 @@ void MainWindow::saveScans(const std::map<int, pcl::PointCloud<pcl::PointXYZ>::P
 	}
 }
 
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
-		const std::map<int, Transform> & poses,
-		float assembledVoxelSize,
-		bool regenerateClouds,
-		int regenerateDecimation,
-		float regenerateVoxelSize,
-		float regenerateMaxDepth) const
-{
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-	int i=0;
-	int count = 0;
-	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
-	{
-		bool inserted = false;
-		if(!iter->second.isNull())
-		{
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-			if(regenerateClouds)
-			{
-				if(_cachedSignatures.contains(iter->first))
-				{
-					const Signature & s = _cachedSignatures.find(iter->first).value();
-					SensorData d = s.sensorData();
-					cv::Mat image, depth;
-					d.uncompressData(&image, &depth, 0);
-
-					if(!image.empty() && !depth.empty())
-					{
-						UASSERT(iter->first == d.id());
-						cloud = util3d::cloudRGBFromSensorData(
-								d,
-								regenerateDecimation,
-								regenerateMaxDepth,
-								regenerateVoxelSize);
-						if(cloud->size())
-						{
-							cloud = util3d::transformPointCloud(cloud, iter->second);
-						}
-					}
-					else if(s.getWords3().size())
-					{
-						cloud->resize(s.getWords3().size());
-						int oi=0;
-						for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=s.getWords3().begin(); jter!=s.getWords3().end(); ++jter)
-						{
-							(*cloud)[oi].x = jter->second.x;
-							(*cloud)[oi].y = jter->second.y;
-							(*cloud)[oi].z = jter->second.z;
-							(*cloud)[oi].r = 255;
-							(*cloud)[oi].g = 255;
-							(*cloud)[oi++].b = 255;
-						}
-					}
-				}
-				else
-				{
-					UWARN("Cloud %d not found in cache!", iter->first);
-				}
-			}
-			else if(uContains(_createdClouds, iter->first))
-			{
-				cloud = util3d::transformPointCloud(_createdClouds.at(iter->first), iter->second);
-			}
-
-			if(cloud->size())
-			{
-				*assembledCloud += *cloud;
-
-				inserted = true;
-				++count;
-			}
-		}
-		else
-		{
-			UERROR("transform is null!?");
-		}
-
-		if(inserted)
-		{
-			_initProgressDialog->appendText(tr("Generated cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size()));
-
-			if(count % 100 == 0)
-			{
-				if(assembledCloud->size() && assembledVoxelSize)
-				{
-					assembledCloud = util3d::voxelize(assembledCloud, assembledVoxelSize);
-				}
-			}
-		}
-		else
-		{
-			_initProgressDialog->appendText(tr("Ignored cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(poses.size()));
-		}
-		_initProgressDialog->incrementStep();
-		QApplication::processEvents();
-	}
-
-	if(assembledCloud->size() && assembledVoxelSize)
-	{
-		assembledCloud = util3d::voxelize(assembledCloud, assembledVoxelSize);
-	}
-
-	return assembledCloud;
-}
-
-std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::getClouds(
+std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr > MainWindow::getClouds(
 		const std::map<int, Transform> & poses,
 		bool regenerateClouds,
 		int regenerateDecimation,
 		float regenerateVoxelSize,
-		float regenerateMaxDepth) const
+		float regenerateMaxDepth,
+		int normalKSearch,
+		bool mls,
+		float mlsRadius) const
 {
-	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
+	std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
 	int i=0;
 	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 	{
@@ -5246,7 +5228,28 @@ std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::getClouds(
 
 			if(cloud->size())
 			{
-				clouds.insert(std::make_pair(iter->first, cloud));
+				if(mls)
+				{
+					pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals = util3d::computeNormalsSmoothed(
+							cloud,
+							mlsRadius,
+							true,
+							regenerateVoxelSize);
+
+					if(regenerateVoxelSize)
+					{
+						cloudWithNormals = util3d::voxelize(
+								cloudWithNormals,
+								regenerateVoxelSize);
+					}
+
+					cloud->clear();
+					pcl::copyPointCloud(*cloudWithNormals, *cloud);
+				}
+
+				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals = util3d::computeNormals(cloud, _exportDialog->getMeshNormalKSearch());
+
+				clouds.insert(std::make_pair(iter->first, cloudWithNormals));
 				inserted = true;
 			}
 		}

guilib/src/PreferencesDialog.cpp

@@ -297,6 +297,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 
 	connect(_ui->checkBox_meshing, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_gp3Radius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_gp3Mu, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_mls, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_mlsRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -1029,6 +1030,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 
 		_ui->checkBox_meshing->setChecked(false);
 		_ui->doubleSpinBox_gp3Radius->setValue(0.04);
+		_ui->doubleSpinBox_gp3Mu->setValue(2.5);
 		_ui->spinBox_normalKSearch->setValue(20);
 		_ui->checkBox_mls->setChecked(false);
 		_ui->doubleSpinBox_mlsRadius->setValue(0.04);
@@ -1319,6 +1321,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 
 	_ui->checkBox_meshing->setChecked(settings.value("meshing", _ui->checkBox_meshing->isChecked()).toBool());
 	_ui->doubleSpinBox_gp3Radius->setValue(settings.value("meshGP3Radius", _ui->doubleSpinBox_gp3Radius->value()).toDouble());
+	_ui->doubleSpinBox_gp3Mu->setValue(settings.value("meshGP3Mu", _ui->doubleSpinBox_gp3Mu->value()).toDouble());
 	_ui->spinBox_normalKSearch->setValue(settings.value("meshNormalKSearch", _ui->spinBox_normalKSearch->value()).toInt());
 	_ui->checkBox_mls->setChecked(settings.value("meshSmoothing", _ui->checkBox_mls->isChecked()).toBool());
 	_ui->doubleSpinBox_mlsRadius->setValue(settings.value("meshSmoothingRadius", _ui->doubleSpinBox_mlsRadius->value()).toDouble());
@@ -1630,6 +1633,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 
 	settings.setValue("meshing",             _ui->checkBox_meshing->isChecked());
 	settings.setValue("meshGP3Radius",       _ui->doubleSpinBox_gp3Radius->value());
+	settings.setValue("meshGP3Mu",       _ui->doubleSpinBox_gp3Mu->value());
 	settings.setValue("meshNormalKSearch",   _ui->spinBox_normalKSearch->value());
 	settings.setValue("meshSmoothing",       _ui->checkBox_mls->isChecked());
 	settings.setValue("meshSmoothingRadius", _ui->doubleSpinBox_mlsRadius->value());
@@ -3237,6 +3241,10 @@ double PreferencesDialog::getMeshGP3Radius() const
 {
 	return _ui->doubleSpinBox_gp3Radius->value();
 }
+double PreferencesDialog::getMeshGP3Mu() const
+{
+	return _ui->doubleSpinBox_gp3Mu->value();
+}
 bool PreferencesDialog::getMeshSmoothing() const
 {
 	return _ui->checkBox_mls->isChecked();

guilib/src/ui/exportCloudsDialog.ui

@@ -242,7 +242,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
       <item row="0" column="1">
        <widget class="QLabel" name="label_71">
         <property name="text">
-         <string>Set the number of k nearest neighbors to use for the normal estimation to create the mesh. Not used when mesh smoothing (MLS) above is used.</string>
+         <string>Set the number of k nearest neighbors to use for the normal estimation to create the mesh.</string>
         </property>
         <property name="wordWrap">
          <bool>true</bool>
@@ -279,6 +279,38 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
         </property>
        </widget>
       </item>
+      <item row="2" column="1">
+       <widget class="QLabel" name="label_169">
+        <property name="text">
+         <string>Set the multiplier of the nearest neighbor distance to obtain the final search radius for each point  (this will make the algorithm adapt to different point densities in the cloud).</string>
+        </property>
+        <property name="wordWrap">
+         <bool>true</bool>
+        </property>
+       </widget>
+      </item>
+      <item row="2" column="0">
+       <widget class="QDoubleSpinBox" name="doubleSpinBox_gp3Mu">
+        <property name="suffix">
+         <string>0</string>
+        </property>
+        <property name="decimals">
+         <number>1</number>
+        </property>
+        <property name="minimum">
+         <double>0.100000000000000</double>
+        </property>
+        <property name="maximum">
+         <double>100.000000000000000</double>
+        </property>
+        <property name="singleStep">
+         <double>1.000000000000000</double>
+        </property>
+        <property name="value">
+         <double>2.500000000000000</double>
+        </property>
+       </widget>
+      </item>
      </layout>
     </widget>
    </item>

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
-            <width>755</width>
-            <height>1591</height>
+            <y>-394</y>
+            <width>759</width>
+            <height>938</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>4</number>
+                <number>1</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -1087,7 +1087,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                        <item row="16" column="2">
                         <widget class="QLabel" name="label_168">
                          <property name="text">
-                          <string>Sphere radius that is to be used for determining the k-nearest neighbors used for triangulating (GP3). Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
+                          <string>(GP3) Sphere radius that is to be used for determining the k-nearest neighbors used for triangulating. Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -1250,6 +1250,41 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
+                       <item row="17" column="2">
+                        <widget class="QLabel" name="label_246">
+                         <property name="text">
+                          <string>(GP3) Set the multiplier of the nearest neighbor distance to obtain the final search radius for each point (this will make the algorithm adapt to different point densities in the cloud).</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="17" column="0">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_gp3Mu">
+                         <property name="suffix">
+                          <string/>
+                         </property>
+                         <property name="decimals">
+                          <number>1</number>
+                         </property>
+                         <property name="minimum">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="maximum">
+                          <double>100.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>1.000000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>2.500000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>