Export: added random sample filter, added proportional radius filter, refactored when normals are computed (now after the clouds are assembled and voxelized), moved ceiling and floor filtering inside assembling loop.

corelib/include/rtabmap/core/util3d_filtering.h

@@ -160,9 +160,21 @@ inline pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr uniformSampling(
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP randomSampling(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		int samples);
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP randomSampling(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		int samples);
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP randomSampling(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		int samples);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP randomSampling(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		int samples);
+pcl::PointCloud<pcl::PointXYZI>::Ptr RTABMAP_EXP randomSampling(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		int samples);
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP randomSampling(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		int samples);
 
 
 pcl::IndicesPtr RTABMAP_EXP passThrough(
@@ -449,6 +461,99 @@ pcl::IndicesPtr RTABMAP_EXP radiusFiltering(
 		float radiusSearch,
 		int minNeighborsInRadius);
 
+/* for convenience */
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale=1.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale=1.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale=1.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale=1.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale=1.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale=1.0f);
+
+/**
+ * @brief Filter points based on distance from their viewpoint.
+ *
+ * @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 viewpointIndices should be same size than the input cloud, it tells the viewpoint index in viewpoints for each point.
+ * @param viewpoints the viewpoints.
+ * @param factor will determine the search radius based on the distance from a point and its viewpoint. Setting it higher will filter points farther from accurate points (but processing time will be also higher).
+ * @param neighborScale will scale the search radius of neighbors found around a point. Setting it higher will accept more noisy points close to accurate points (but processing time will be also higher).
+ * @return the indices of the points satisfying the parameters.
+ */
+
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale=1.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale=1.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale=1.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale=1.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale=1.0f);
+pcl::IndicesPtr RTABMAP_EXP proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale=1.0f);
+
 /**
  * For convenience.
  */

corelib/include/rtabmap/core/util3d_surface.h

@@ -487,6 +487,11 @@ void RTABMAP_EXP adjustNormalsToViewPoints(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
 		const std::vector<int> & rawCameraIndices,
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud);
+void RTABMAP_EXP adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
+		const std::vector<int> & rawCameraIndices,
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud);
 
 void RTABMAP_EXP adjustNormalsToViewPoints(
 		const std::map<int, Transform> & viewpoints,

corelib/src/RegistrationIcp.cpp

@@ -277,8 +277,12 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 #ifndef RTABMAP_CCCORELIB
 	if(_strategy==2)
 	{
-		UWARN("Parameter %s is set to true but RTAB-Map has not been built with CCCoreLib support. Setting to 0.", Parameters::kIcpStrategy().c_str());
+#ifdef RTABMAP_POINTMATCHER
+		_strategy = 1;
+#else
 		_strategy = 0;
+#endif
+		UWARN("Parameter %s is set to 2 but RTAB-Map has not been built with CCCoreLib support. Setting to %d.", Parameters::kIcpStrategy().c_str(), _strategy);
 	}
 #else
 	if(_strategy==2 && _pointToPlane)

corelib/src/util3d_filtering.cpp

@@ -664,6 +664,7 @@ typename pcl::PointCloud<PointT>::Ptr randomSamplingImpl(
 	typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
 	pcl::RandomSample<PointT> filter;
 	filter.setSample(samples);
+	filter.setSeed (std::rand ());
 	filter.setInputCloud(cloud);
 	filter.filter(*output);
 	return output;
@@ -672,10 +673,26 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr randomSampling(const pcl::PointCloud<pcl::Po
 {
 	return randomSamplingImpl<pcl::PointXYZ>(cloud, samples);
 }
+pcl::PointCloud<pcl::PointNormal>::Ptr randomSampling(const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud, int samples)
+{
+	return randomSamplingImpl<pcl::PointNormal>(cloud, samples);
+}
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int samples)
 {
 	return randomSamplingImpl<pcl::PointXYZRGB>(cloud, samples);
 }
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, int samples)
+{
+	return randomSamplingImpl<pcl::PointXYZRGBNormal>(cloud, samples);
+}
+pcl::PointCloud<pcl::PointXYZI>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud, int samples)
+{
+	return randomSamplingImpl<pcl::PointXYZI>(cloud, samples);
+}
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr randomSampling(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, int samples)
+{
+	return randomSamplingImpl<pcl::PointXYZINormal>(cloud, samples);
+}
 
 template<typename PointT>
 pcl::IndicesPtr passThroughImpl(
@@ -1160,6 +1177,239 @@ pcl::IndicesPtr radiusFiltering(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr
 	return radiusFilteringImpl<pcl::PointXYZINormal>(cloud, indices, radiusSearch, minNeighborsInRadius);
 }
 
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return proportionalRadiusFiltering(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+
+template<typename PointT>
+pcl::IndicesPtr proportionalRadiusFilteringImpl(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>(false));
+
+	UASSERT(cloud->size() == viewpointIndices.size());
+	UASSERT(factor>0.0f);
+	UASSERT(neighborScale>=1.0f);
+
+	if(!indices->empty())
+	{
+		std::vector<bool> kept(indices->size());
+		tree->setInputCloud(cloud, indices);
+		for(size_t i=0; i<indices->size(); ++i)
+		{
+			int index = indices->at(i);
+			std::vector<int> kIndices;
+			std::vector<float> kDistances;
+			std::map<int, Transform>::const_iterator viewpointIter = viewpoints.find(viewpointIndices[index]);
+			UASSERT(viewpointIter != viewpoints.end());
+			cv::Point3f viewpoint(viewpointIter->second.x(), viewpointIter->second.y(), viewpointIter->second.z());
+			cv::Point3f point = cv::Point3f(cloud->at(index).x,cloud->at(index).y, cloud->at(index).z);
+			float radiusSearch = factor * cv::norm(viewpoint-point);
+			int k = tree->radiusSearch(cloud->at(index), radiusSearch, kIndices, kDistances);
+			bool keep = k>0;
+			for(int j=0; j<k && keep; ++j)
+			{
+				if(kIndices[j] != index)
+				{
+					cv::Point3f pointTmp(cloud->at(kIndices[j]).x,cloud->at(kIndices[j]).y, cloud->at(kIndices[j]).z);
+					cv::Point3f tmp = pointTmp - point;
+					float distPtSqr = tmp.dot(tmp); // L2sqr
+					viewpointIter = viewpoints.find(viewpointIndices[kIndices[j]]);
+					UASSERT(viewpointIter != viewpoints.end());
+					viewpoint = cv::Point3f(viewpointIter->second.x(), viewpointIter->second.y(), viewpointIter->second.z());
+					float radiusSearchTmp = factor * cv::norm(viewpoint-pointTmp) * neighborScale;
+					if(distPtSqr > radiusSearchTmp*radiusSearchTmp)
+					{
+						keep = false;
+					}
+				}
+			}
+			kept[i] = keep;
+		}
+		pcl::IndicesPtr output(new std::vector<int>(indices->size()));
+		int oi = 0;
+		for(size_t i=0; i<indices->size(); ++i)
+		{
+			if(kept[i])
+			{
+				output->at(oi++) = indices->at(i);
+			}
+		}
+		output->resize(oi);
+		return output;
+	}
+	else
+	{
+		std::vector<bool> kept(cloud->size());
+		tree->setInputCloud(cloud);
+		#pragma omp parallel for
+		for(size_t i=0; i<cloud->size(); ++i)
+		{
+			std::vector<int> kIndices;
+			std::vector<float> kDistances;
+			std::map<int, Transform>::const_iterator viewpointIter = viewpoints.find(viewpointIndices[i]);
+			UASSERT(viewpointIter != viewpoints.end());
+			cv::Point3f viewpoint(viewpointIter->second.x(), viewpointIter->second.y(), viewpointIter->second.z());
+			cv::Point3f point = cv::Point3f(cloud->at(i).x,cloud->at(i).y, cloud->at(i).z);
+			float radiusSearch = factor * cv::norm(viewpoint-point);
+			int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
+			bool keep = k>0;
+			for(int j=0; j<k && keep; ++j)
+			{
+				if(kIndices[j] != (int)i)
+				{
+					cv::Point3f pointTmp(cloud->at(kIndices[j]).x,cloud->at(kIndices[j]).y, cloud->at(kIndices[j]).z);
+					cv::Point3f tmp = pointTmp - point;
+					float distPtSqr = tmp.dot(tmp); // L2sqr
+					viewpointIter = viewpoints.find(viewpointIndices[kIndices[j]]);
+					UASSERT(viewpointIter != viewpoints.end());
+					viewpoint = cv::Point3f(viewpointIter->second.x(), viewpointIter->second.y(), viewpointIter->second.z());
+					float radiusSearchTmp = factor * cv::norm(viewpoint-pointTmp) * neighborScale;
+					if(distPtSqr > radiusSearchTmp*radiusSearchTmp)
+					{
+						keep = false;
+					}
+				}
+			}
+			kept[i] = keep;
+		}
+		pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
+		int oi = 0;
+		for(size_t i=0; i<cloud->size(); ++i)
+		{
+			if(kept[i])
+			{
+				output->at(oi++) = i;
+			}
+		}
+		output->resize(oi);
+		return output;
+	}
+}
+
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	return proportionalRadiusFilteringImpl<pcl::PointXYZ>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	return proportionalRadiusFilteringImpl<pcl::PointNormal>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	return proportionalRadiusFilteringImpl<pcl::PointXYZRGB>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	return proportionalRadiusFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZI>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	return proportionalRadiusFilteringImpl<pcl::PointXYZI>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+pcl::IndicesPtr proportionalRadiusFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const std::vector<int> & viewpointIndices,
+		const std::map<int, Transform> & viewpoints,
+		float factor,
+		float neighborScale)
+{
+	return proportionalRadiusFilteringImpl<pcl::PointXYZINormal>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
+}
+
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,

corelib/src/util3d_surface.cpp

@@ -3493,6 +3493,7 @@ void adjustNormalsToViewPointImpl(
 		const Eigen::Vector3f & viewpoint,
 		float groundNormalsUp)
 {
+	#pragma omp parallel for
 	for(unsigned int i=0; i<cloud->size(); ++i)
 	{
 		pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
@@ -3559,17 +3560,20 @@ void adjustNormalsToViewPoint(
 	adjustNormalsToViewPointImpl<pcl::PointXYZINormal>(cloud, viewpoint, groundNormalsUp);
 }
 
-void adjustNormalsToViewPoints(
+
+template<typename PointT>
+void adjustNormalsToViewPointsImpl(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
 		const std::vector<int> & rawCameraIndices,
-		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud)
+		typename pcl::PointCloud<PointT>::Ptr & cloud)
 {
 	if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size())
 	{
 		pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
 		rawTree->setInputCloud (rawCloud);
 
+		#pragma omp parallel for
 		for(unsigned int i=0; i<cloud->size(); ++i)
 		{
 			pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
@@ -3581,7 +3585,7 @@ void adjustNormalsToViewPoints(
 				UASSERT(indices.size() == 1);
 				if(indices.size() && indices[0]>=0)
 				{
-					Transform p = poses.at(rawCameraIndices[indices[0]]);
+					const Transform & p = poses.at(rawCameraIndices[indices[0]]);
 					pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
 					Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
 
@@ -3605,52 +3609,31 @@ void adjustNormalsToViewPoints(
 	}
 }
 
+void adjustNormalsToViewPoints(
+		const std::map<int, Transform> & poses,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
+		const std::vector<int> & rawCameraIndices,
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud)
+{
+	adjustNormalsToViewPointsImpl<pcl::PointNormal>(poses, rawCloud, rawCameraIndices, cloud);
+}
+
 void adjustNormalsToViewPoints(
 		const std::map<int, Transform> & poses,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
 		const std::vector<int> & rawCameraIndices,
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud)
 {
-	UASSERT(rawCloud.get() && cloud.get());
+	adjustNormalsToViewPointsImpl<pcl::PointXYZRGBNormal>(poses, rawCloud, rawCameraIndices, cloud);
-	UDEBUG("poses=%d, rawCloud=%d, rawCameraIndices=%d, cloud=%d", (int)poses.size(), (int)rawCloud->size(), (int)rawCameraIndices.size(), (int)cloud->size());
+}
-	if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size())
-	{
-		pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
-		rawTree->setInputCloud (rawCloud);
-		for(unsigned int i=0; i<cloud->size(); ++i)
-		{
-			pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
-			if(pcl::isFinite(normal))
-			{
-				std::vector<int> indices;
-				std::vector<float> dist;
-				rawTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist);
-				if(indices.size() && indices[0]>=0)
-				{
-					UASSERT_MSG(indices[0]<(int)rawCameraIndices.size(), uFormat("indices[0]=%d rawCameraIndices.size()=%d", indices[0], (int)rawCameraIndices.size()).c_str());
-					UASSERT(uContains(poses, rawCameraIndices[indices[0]]));
-					Transform p = poses.at(rawCameraIndices[indices[0]]);
-					pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
-					Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
 
-					Eigen::Vector3f n(normal.x, normal.y, normal.z);
+void adjustNormalsToViewPoints(
-
+		const std::map<int, Transform> & poses,
-					float result = v.dot(n);
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
-					if(result < 0)
+		const std::vector<int> & rawCameraIndices,
-					{
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud)
-						//reverse normal
+{
-						cloud->points[i].normal_x *= -1.0f;
+	adjustNormalsToViewPointsImpl<pcl::PointXYZINormal>(poses, rawCloud, rawCameraIndices, cloud);
-						cloud->points[i].normal_y *= -1.0f;
-						cloud->points[i].normal_z *= -1.0f;
-					}
-				}
-				else
-				{
-					UWARN("Not found camera viewpoint for point %d!?", i);
-				}
-			}
-		}
-	}
 }
 
 void adjustNormalsToViewPoints(
@@ -3665,6 +3648,7 @@ void adjustNormalsToViewPoints(
 		pcl::PointCloud<pcl::PointXYZ>::Ptr rawCloud = util3d::laserScanToPointCloud(rawScan);
 		pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
 		rawTree->setInputCloud (rawCloud);
+		#pragma omp parallel for
 		for(int i=0; i<scan.size(); ++i)
 		{
 			pcl::PointNormal point = util3d::laserScanToPointNormal(scan, i);

guilib/src/ExportCloudsDialog.cpp

@@ -151,6 +151,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->checkBox_assemble, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_assemble, SIGNAL(clicked(bool)), this, SLOT(updateReconstructionFlavor()));
 	connect(_ui->doubleSpinBox_voxelSize_assembled, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->spinBox_randomSamples_assembled, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->comboBox_frame, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->comboBox_frame, SIGNAL(currentIndexChanged(int)), this, SLOT(updateReconstructionFlavor()));
 
@@ -383,6 +384,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 
 	settings.setValue("assemble", _ui->checkBox_assemble->isChecked());
 	settings.setValue("assemble_voxel",_ui->doubleSpinBox_voxelSize_assembled->value());
+	settings.setValue("assemble_samples",_ui->spinBox_randomSamples_assembled->value());
 	settings.setValue("frame",_ui->comboBox_frame->currentIndex());
 
 	settings.setValue("subtract",_ui->checkBox_subtraction->isChecked());
@@ -555,6 +557,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 		_ui->checkBox_assemble->setChecked(settings.value("assemble", _ui->checkBox_assemble->isChecked()).toBool());
 	}
 	_ui->doubleSpinBox_voxelSize_assembled->setValue(settings.value("assemble_voxel", _ui->doubleSpinBox_voxelSize_assembled->value()).toDouble());
+	_ui->spinBox_randomSamples_assembled->setValue(settings.value("assemble_samples", _ui->spinBox_randomSamples_assembled->value()).toInt());
 	_ui->comboBox_frame->setCurrentIndex(settings.value("frame", _ui->comboBox_frame->currentIndex()).toInt());
 
 	_ui->checkBox_subtraction->setChecked(settings.value("subtract",_ui->checkBox_subtraction->isChecked()).toBool());
@@ -724,6 +727,7 @@ void ExportCloudsDialog::restoreDefaults()
 
 	_ui->checkBox_assemble->setChecked(true);
 	_ui->doubleSpinBox_voxelSize_assembled->setValue(0.01);
+	_ui->spinBox_randomSamples_assembled->setValue(0);
 	_ui->comboBox_frame->setCurrentIndex(0);
 
 	_ui->checkBox_subtraction->setChecked(false);
@@ -1900,6 +1904,7 @@ bool ExportCloudsDialog::getExportedClouds(
 					assembledCloud->points[i].normal_y = normals->points[i].normal_y;
 					assembledCloud->points[i].normal_z = normals->points[i].normal_z;
 				}
+				_progressDialog->appendText(tr("Adjusting normals to viewpoints (%1 points)...").arg(assembledCloud->size()));
 
 				// adjust with point of views
 				util3d::adjustNormalsToViewPoints(
@@ -1909,6 +1914,18 @@ bool ExportCloudsDialog::getExportedClouds(
 											assembledCloud);
 			}
 
+			if(_ui->spinBox_randomSamples_assembled->value()>0 &&
+			   (int)assembledCloud->size() > _ui->spinBox_randomSamples_assembled->value())
+			{
+				_progressDialog->appendText(tr("Random samples filtering (in=%1 points, samples=%2)...")
+														.arg(assembledCloud->size())
+														.arg(_ui->spinBox_randomSamples_assembled->value()));
+				assembledCloud = util3d::randomSampling(assembledCloud, _ui->spinBox_randomSamples_assembled->value());
+				_progressDialog->appendText(tr("Random samples filtering (out=%1 points, samples=%2)... done!")
+																		.arg(assembledCloud->size())
+																		.arg(_ui->spinBox_randomSamples_assembled->value()));
+			}
+
 			clouds.insert(std::make_pair(0, std::make_pair(assembledCloud, indices)));
 		}
 

guilib/src/ui/exportCloudsDialog.ui

@@ -23,83 +23,25 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>-3537</y>
+        <y>0</y>
         <width>998</width>
-        <height>5673</height>
+        <height>5713</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_13">
        <item>
         <layout class="QGridLayout" name="gridLayout_8" columnstretch="0,1">
-         <item row="1" column="0">
+         <item row="17" column="0">
-          <widget class="QComboBox" name="comboBox_pipeline">
+          <widget class="QCheckBox" name="checkBox_meshing">
-           <item>
-            <property name="text">
-             <string>Organized Point Cloud</string>
-            </property>
-           </item>
-           <item>
-            <property name="text">
-             <string>Dense Point Cloud</string>
-            </property>
-           </item>
-          </widget>
-         </item>
-         <item row="1" column="1">
-          <widget class="QLabel" name="label_12">
-           <property name="text">
-            <string>Reconstruction flavor.</string>
-           </property>
-          </widget>
-         </item>
-         <item row="11" column="0">
-          <widget class="QCheckBox" name="checkBox_regenerate">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="12" column="0">
+         <item row="17" column="1">
-          <widget class="QCheckBox" name="checkBox_filtering">
+          <widget class="QLabel" name="label_binaryFile_12">
            <property name="text">
-            <string/>
+            <string>Meshing.</string>
-           </property>
-          </widget>
-         </item>
-         <item row="11" column="1">
-          <widget class="QLabel" name="label_regenerate">
-           <property name="text">
-            <string>Regenerate clouds. This can be used to regenerate the point clouds at higher density than those used for online visualization.</string>
-           </property>
-           <property name="wordWrap">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="8" column="1">
-          <widget class="QLabel" name="label_voxel">
-           <property name="text">
-            <string>Voxel size. Set 0 to disable. When organized meshes are assembled, this is the radius in which the vertices of the polygons are merged.</string>
-           </property>
-           <property name="wordWrap">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="4" column="1">
-          <widget class="QLabel" name="label_binaryFile_2">
-           <property name="text">
-            <string>Assemble clouds/meshes to a single output cloud/mesh.</string>
-           </property>
-           <property name="wordWrap">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="5" column="1">
-          <widget class="QLabel" name="label_frame">
-           <property name="text">
-            <string>Output frame.</string>
            </property>
            <property name="wordWrap">
             <bool>true</bool>
@@ -116,17 +58,37 @@
            </property>
           </widget>
          </item>
-         <item row="13" column="0">
+         <item row="5" column="1">
+          <widget class="QLabel" name="label_frame">
+           <property name="text">
+            <string>Output frame.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="11" column="1">
+          <widget class="QLabel" name="label_binaryFile_10">
+           <property name="text">
+            <string>Nodes filtering. Filter nodes to be exported in a specified region .</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="14" column="0">
           <widget class="QCheckBox" name="checkBox_smoothing">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="12" column="1">
+         <item row="14" column="1">
-          <widget class="QLabel" name="label_binaryFile_9">
+          <widget class="QLabel" name="label_smoothing">
            <property name="text">
-            <string>Cloud filtering.</string>
+            <string>Cloud smoothing using Moving Least Squares algorithm (MLS).</string>
            </property>
            <property name="wordWrap">
             <bool>true</bool>
@@ -143,37 +105,25 @@
            </property>
           </widget>
          </item>
-         <item row="13" column="1">
+         <item row="16" column="0">
-          <widget class="QLabel" name="label_smoothing">
+          <widget class="QCheckBox" name="checkBox_cameraProjection">
            <property name="text">
-            <string>Cloud smoothing using Moving Least Squares algorithm (MLS).</string>
+            <string/>
+           </property>
+          </widget>
+         </item>
+         <item row="0" column="1">
+          <widget class="QLabel" name="label_binaryFile_11">
+           <property name="text">
+            <string>From RGB-D images. If not checked, clouds will be generated from laser scans.</string>
            </property>
            <property name="wordWrap">
             <bool>true</bool>
            </property>
           </widget>
          </item>
-         <item row="8" column="0">
+         <item row="11" column="0">
-          <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize_assembled">
+          <widget class="QCheckBox" name="checkBox_nodes_filtering">
-           <property name="suffix">
-            <string> m</string>
-           </property>
-           <property name="decimals">
-            <number>3</number>
-           </property>
-           <property name="maximum">
-            <double>1.000000000000000</double>
-           </property>
-           <property name="singleStep">
-            <double>0.010000000000000</double>
-           </property>
-           <property name="value">
-            <double>0.005000000000000</double>
-           </property>
-          </widget>
-         </item>
-         <item row="4" column="0">
-          <widget class="QCheckBox" name="checkBox_assemble">
            <property name="text">
             <string/>
            </property>
@@ -189,6 +139,56 @@
            </property>
           </widget>
          </item>
+         <item row="12" column="0">
+          <widget class="QCheckBox" name="checkBox_regenerate">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
+         <item row="15" column="1">
+          <widget class="QLabel" name="label_gainCompensation">
+           <property name="text">
+            <string>Gain compensation. Normalize brightness of images.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="1" column="0">
+          <widget class="QComboBox" name="comboBox_pipeline">
+           <item>
+            <property name="text">
+             <string>Organized Point Cloud</string>
+            </property>
+           </item>
+           <item>
+            <property name="text">
+             <string>Dense Point Cloud</string>
+            </property>
+           </item>
+          </widget>
+         </item>
+         <item row="10" column="0">
+          <widget class="QComboBox" name="comboBox_intensityColormap">
+           <item>
+            <property name="text">
+             <string>GrayScale</string>
+            </property>
+           </item>
+           <item>
+            <property name="text">
+             <string>RedYellow</string>
+            </property>
+           </item>
+           <item>
+            <property name="text">
+             <string>Rainbow</string>
+            </property>
+           </item>
+          </widget>
+         </item>
          <item row="5" column="0">
           <widget class="QComboBox" name="comboBox_frame">
            <item>
@@ -213,6 +213,100 @@
            </item>
           </widget>
          </item>
+         <item row="4" column="0">
+          <widget class="QCheckBox" name="checkBox_assemble">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
+         <item row="10" column="1">
+          <widget class="QLabel" name="label_intensityColormap">
+           <property name="text">
+            <string>Intensity colormap.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="8" column="0">
+          <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize_assembled">
+           <property name="suffix">
+            <string> m</string>
+           </property>
+           <property name="decimals">
+            <number>3</number>
+           </property>
+           <property name="maximum">
+            <double>1.000000000000000</double>
+           </property>
+           <property name="singleStep">
+            <double>0.010000000000000</double>
+           </property>
+           <property name="value">
+            <double>0.005000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="0" column="0">
+          <widget class="QCheckBox" name="checkBox_fromDepth">
+           <property name="text">
+            <string/>
+           </property>
+           <property name="checked">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="7" column="0">
+          <widget class="QDoubleSpinBox" name="doubleSpinBox_normalRadiusSearch">
+           <property name="singleStep">
+            <double>0.010000000000000</double>
+           </property>
+          </widget>
+         </item>
+         <item row="15" column="0">
+          <widget class="QCheckBox" name="checkBox_gainCompensation">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
+         <item row="12" column="1">
+          <widget class="QLabel" name="label_regenerate">
+           <property name="text">
+            <string>Regenerate clouds. This can be used to regenerate the point clouds at higher density than those used for online visualization.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="1" column="1">
+          <widget class="QLabel" name="label_12">
+           <property name="text">
+            <string>Reconstruction flavor.</string>
+           </property>
+          </widget>
+         </item>
+         <item row="13" column="0">
+          <widget class="QCheckBox" name="checkBox_filtering">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
+         <item row="4" column="1">
+          <widget class="QLabel" name="label_binaryFile_2">
+           <property name="text">
+            <string>Assemble clouds/meshes to a single output cloud/mesh.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
          <item row="3" column="0">
           <widget class="QCheckBox" name="checkBox_binary">
            <property name="text">
@@ -223,22 +317,22 @@
            </property>
           </widget>
          </item>
-         <item row="0" column="1">
+         <item row="8" column="1">
-          <widget class="QLabel" name="label_binaryFile_11">
+          <widget class="QLabel" name="label_voxel">
            <property name="text">
-            <string>From RGB-D images. If not checked, clouds will be generated from laser scans.</string>
+            <string>Voxel size. Set 0 to disable. When organized meshes are assembled, this is the radius in which the vertices of the polygons are merged.</string>
            </property>
            <property name="wordWrap">
             <bool>true</bool>
            </property>
           </widget>
          </item>
-         <item row="0" column="0">
+         <item row="13" column="1">
-          <widget class="QCheckBox" name="checkBox_fromDepth">
+          <widget class="QLabel" name="label_binaryFile_9">
            <property name="text">
-            <string/>
+            <string>Cloud filtering.</string>
            </property>
-           <property name="checked">
+           <property name="wordWrap">
             <bool>true</bool>
            </property>
           </widget>
@@ -253,53 +347,7 @@
            </property>
           </widget>
          </item>
-         <item row="7" column="0">
+         <item row="16" column="1">
-          <widget class="QDoubleSpinBox" name="doubleSpinBox_normalRadiusSearch">
-           <property name="singleStep">
-            <double>0.010000000000000</double>
-           </property>
-          </widget>
-         </item>
-         <item row="9" column="1">
-          <widget class="QLabel" name="label_intensityColormap">
-           <property name="text">
-            <string>Intensity colormap.</string>
-           </property>
-           <property name="wordWrap">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="9" column="0">
-          <widget class="QComboBox" name="comboBox_intensityColormap">
-           <item>
-            <property name="text">
-             <string>GrayScale</string>
-            </property>
-           </item>
-           <item>
-            <property name="text">
-             <string>RedYellow</string>
-            </property>
-           </item>
-           <item>
-            <property name="text">
-             <string>Rainbow</string>
-            </property>
-           </item>
-          </widget>
-         </item>
-         <item row="14" column="1">
-          <widget class="QLabel" name="label_gainCompensation">
-           <property name="text">
-            <string>Gain compensation. Normalize brightness of images.</string>
-           </property>
-           <property name="wordWrap">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="15" column="1">
           <widget class="QLabel" name="label_cameraProjection">
            <property name="text">
             <string>Camera projection. This can be used to colorize point cloud created from scans and/or export camera IDs for each point of the cloud.</string>
@@ -309,51 +357,29 @@
            </property>
           </widget>
          </item>
-         <item row="14" column="0">
+         <item row="9" column="1">
-          <widget class="QCheckBox" name="checkBox_gainCompensation">
+          <widget class="QLabel" name="label_voxel_2">
            <property name="text">
-            <string/>
+            <string>Number of samples to keep, done with a random sample filter. Only used when clouds are assembled. Set 0 to disable.</string>
-           </property>
-          </widget>
-         </item>
-         <item row="15" column="0">
-          <widget class="QCheckBox" name="checkBox_cameraProjection">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
-         <item row="16" column="1">
-          <widget class="QLabel" name="label_binaryFile_12">
-           <property name="text">
-            <string>Meshing.</string>
            </property>
            <property name="wordWrap">
             <bool>true</bool>
            </property>
           </widget>
          </item>
-         <item row="16" column="0">
+         <item row="9" column="0">
-          <widget class="QCheckBox" name="checkBox_meshing">
+          <widget class="QSpinBox" name="spinBox_randomSamples_assembled">
-           <property name="text">
+           <property name="minimum">
-            <string/>
+            <number>0</number>
            </property>
-          </widget>
+           <property name="maximum">
-         </item>
+            <number>99999999</number>
-         <item row="10" column="1">
-          <widget class="QLabel" name="label_binaryFile_10">
-           <property name="text">
-            <string>Nodes filtering. Filter nodes to be exported in a specified region .</string>
            </property>
-           <property name="wordWrap">
+           <property name="singleStep">
-            <bool>true</bool>
+            <number>10000</number>
            </property>
-          </widget>
+           <property name="value">
-         </item>
+            <number>0</number>
-         <item row="10" column="0">
-          <widget class="QCheckBox" name="checkBox_nodes_filtering">
-           <property name="text">
-            <string/>
            </property>
           </widget>
          </item>

tools/Export/main.cpp

@@ -109,6 +109,9 @@ void showUsage()
 			"    --voxel         #     Voxel size of the created clouds (default 0.01 m, 0 m with --scan).\n"
 			"    --noise_radius  #     Noise filtering search radius (default 0, 0=disabled).\n"
 			"    --noise_k       #     Noise filtering minimum neighbors in search radius (default 5, 0=disabled).\n"
+			"    --prop_radius_factor #  Proportional radius filter factor (default 0, 0=disabled). Start tuning from 0.01.\n"
+			"    --prop_radius_scale  #  Proportional radius filter neighbor scale (default 1).\n"
+			"    --random_samples #    Number of output samples using a random filter (default 0, 0=disabled).\n"
 			"    --color_radius  #     Radius used to colorize polygons (default 0.05 m, 0 m with --scan). Set 0 for nearest color.\n"
 			"    --scan                Use laser scan for the point cloud.\n"
 			"    --save_in_db          Save resulting assembled point cloud or mesh in the database.\n"
@@ -164,6 +167,9 @@ int main(int argc, char * argv[])
 	float voxelSize = -1.0f;
 	float noiseRadius = 0.0f;
 	int noiseMinNeighbors = 5;
+	float proportionalRadiusFactor = 0.0f;
+	float proportionalRadiusScale = 1.0f;
+	int randomSamples = 0;
 	int textureSize = 8192;
 	int textureCount = 1;
 	float textureRange = 0;
@@ -602,6 +608,42 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
+		else if(std::strcmp(argv[i], "--prop_radius_factor") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				proportionalRadiusFactor = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--prop_radius_scale") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				proportionalRadiusScale = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--random_samples") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				randomSamples = uStr2Int(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
 		else if(std::strcmp(argv[i], "--color_radius") == 0)
 		{
 			++i;
@@ -893,8 +935,8 @@ int main(int argc, char * argv[])
 
 	// Construct the cloud
 	printf("Create and assemble the clouds...\n");
-	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-	pcl::PointCloud<pcl::PointXYZINormal>::Ptr mergedCloudsI(new pcl::PointCloud<pcl::PointXYZINormal>);
+	pcl::PointCloud<pcl::PointXYZI>::Ptr assembledCloudI(new pcl::PointCloud<pcl::PointXYZI>);
 	std::map<int, rtabmap::Transform> robotPoses;
 	std::vector<std::map<int, rtabmap::Transform> > cameraPoses;
 	std::map<int, rtabmap::Transform> scanPoses;
@@ -902,6 +944,8 @@ int main(int argc, char * argv[])
 	std::map<int, std::vector<rtabmap::CameraModel> > cameraModels;
 	std::map<int, cv::Mat> cameraDepths;
 	int imagesExported = 0;
+	std::vector<int> rawViewpointIndices;
+	std::map<int, Transform> rawViewpoints;
 	for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(1); iter!=optimizedPoses.end(); ++iter)
 	{
 		Signature node = nodes.find(iter->first)->second;
@@ -1043,40 +1087,61 @@ int main(int argc, char * argv[])
 		else if(cloudI.get() && !cloudI->empty())
 			cloudI = rtabmap::util3d::transformPointCloud(cloudI, iter->second);
 
+		if(filter_ceiling != 0.0 || filter_floor != 0.0f)
+		{
+			if(cloud.get() && !cloud->empty())
+			{
+				cloud = util3d::passThrough(cloud, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits<int>::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits<int>::max());
+			}
+			if(cloudI.get() && !cloudI->empty())
+			{
+				cloudI = util3d::passThrough(cloudI, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits<int>::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits<int>::max());
+			}
+		}
 
-		Eigen::Vector3f viewpoint(iter->second.x(), iter->second.y(), iter->second.z());
 		if(cloudFromScan)
 		{
 			Transform lidarViewpoint = iter->second * node.sensorData().laserScanRaw().localTransform();
-			viewpoint = Eigen::Vector3f(iter->second.x(),  iter->second.y(),  iter->second.z());
+			rawViewpoints.insert(std::make_pair(iter->first, lidarViewpoint));
 		}
+		else if(!node.sensorData().cameraModels().empty() && !node.sensorData().cameraModels()[0].localTransform().isNull())
+		{
+			Transform cameraViewpoint = iter->second * node.sensorData().cameraModels()[0].localTransform(); // take the first camera
+			rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint));
+		}
+		else if(!node.sensorData().stereoCameraModel().localTransform().isNull())
+		{
+			Transform cameraViewpoint = iter->second * node.sensorData().stereoCameraModel().localTransform();
+			rawViewpoints.insert(std::make_pair(iter->first, cameraViewpoint));
+		}
+		else
+		{
+			rawViewpoints.insert(*iter);
+		}
+
 		if(cloud.get() && !cloud->empty())
 		{
-			pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(cloud, 20, 0.0f, viewpoint);
+			if(assembledCloud->empty())
-			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-			pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
-			if(mergedClouds->size() == 0)
 			{
-				*mergedClouds = *cloudWithNormals;
+				*assembledCloud = *cloud;
 			}
 			else
 			{
-				*mergedClouds += *cloudWithNormals;
+				*assembledCloud += *cloud;
 			}
+			rawViewpointIndices.resize(assembledCloud->size(), iter->first);
 		}
 		else if(cloudI.get() && !cloudI->empty())
 		{
-			pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(cloudI, 20, 0.0f, viewpoint);
+			if(assembledCloudI->empty())
-			pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals(new pcl::PointCloud<pcl::PointXYZINormal>);
-			pcl::concatenateFields(*cloudI, *normals, *cloudIWithNormals);
-			if(mergedCloudsI->size() == 0)
 			{
-				*mergedCloudsI = *cloudIWithNormals;
+				*assembledCloudI = *cloudI;
 			}
 			else
 			{
-				*mergedCloudsI += *cloudIWithNormals;
+				*assembledCloudI += *cloudI;
 			}
+			rawViewpointIndices.resize(assembledCloudI->size(), iter->first);
 		}
 
 		if(models.empty() && node.sensorData().stereoCameraModel().isValidForProjection())
@@ -1111,14 +1176,14 @@ int main(int argc, char * argv[])
 			scanPoses.insert(std::make_pair(iter->first, iter->second*node.sensorData().laserScanCompressed().localTransform()));
 		}
 	}
-	printf("Create and assemble the clouds... done (%fs, %d points).\n", timer.ticks(), !mergedClouds->empty()?(int)mergedClouds->size():(int)mergedCloudsI->size());
+	printf("Create and assemble the clouds... done (%fs, %d points).\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
 
 	if(imagesExported>0)
 		printf("%d images exported!\n", imagesExported);
 
 	ConsoleProgessState progressState;
 
-	if(!mergedClouds->empty() || !mergedCloudsI->empty())
+	if(!assembledCloud->empty() || !assembledCloudI->empty())
 	{
 		if(saveInDb)
 		{
@@ -1160,35 +1225,112 @@ int main(int argc, char * argv[])
 			}
 		}
 
-		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudToExport = mergedClouds;
+		if(proportionalRadiusFactor>0.0f)
-		pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIToExport = mergedCloudsI;
-
-		if(filter_ceiling != 0.0 || filter_floor != 0.0f)
 		{
-			printf("Passthrough filtering of the assembled cloud along z axis... (min=%f, max=%f, %d points)\n", filter_floor, filter_ceiling, !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
+			printf("Proportional radius filtering of the assembled cloud... (factor=%f scale=%f, %d points)\n", proportionalRadiusFactor, proportionalRadiusScale, !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
-			if(!cloudToExport->empty())
+			pcl::IndicesPtr indices;
+			if(!assembledCloud->empty())
 			{
-				cloudToExport = util3d::passThrough(cloudToExport, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits<int>::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits<int>::max());
+				indices = util3d::proportionalRadiusFiltering(assembledCloud, rawViewpointIndices, rawViewpoints, proportionalRadiusFactor, proportionalRadiusScale);
+				pcl::PointCloud<pcl::PointXYZRGB> tmp;
+				pcl::copyPointCloud(*assembledCloud, *indices, tmp);
+				*assembledCloud = tmp;
 			}
-			if(!cloudIToExport->empty())
+			else if(!assembledCloudI->empty())
 			{
-				cloudIToExport = util3d::passThrough(cloudIToExport, "z", filter_floor!=0.0f?filter_floor:(float)std::numeric_limits<int>::min(), filter_ceiling!=0.0f?filter_ceiling:(float)std::numeric_limits<int>::max());
+				indices = util3d::proportionalRadiusFiltering(assembledCloudI, rawViewpointIndices, rawViewpoints, proportionalRadiusFactor, proportionalRadiusScale);
+				pcl::PointCloud<pcl::PointXYZI> tmp;
+				pcl::copyPointCloud(*assembledCloudI, *indices, tmp);
+				*assembledCloudI = tmp;
 			}
-			printf("Passthrough filtering of the assembled cloud alog z axis.... done! (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
+			if(indices.get())
+			{
+				std::vector<int> rawCameraIndicesTmp(indices->size());
+				for (std::size_t i = 0; i < indices->size(); ++i)
+					rawCameraIndicesTmp[i] = rawViewpointIndices[indices->at(i)];
+				rawViewpointIndices = rawCameraIndicesTmp;
+			}
+			printf("Proportional radius filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
 		}
 
+		pcl::PointCloud<pcl::PointXYZ>::Ptr rawAssembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
+		if(!assembledCloud->empty())
+			pcl::copyPointCloud(*assembledCloud, *rawAssembledCloud); // used to adjust normal orientation
+		else if(!assembledCloudI->empty())
+			pcl::copyPointCloud(*assembledCloudI, *rawAssembledCloud); // used to adjust normal orientation
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithoutNormals = rawAssembledCloud;
+
 		if(voxelSize>0.0f)
 		{
-			printf("Voxel grid filtering of the assembled cloud... (voxel=%f, %d points)\n", voxelSize, !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
+			printf("Voxel grid filtering of the assembled cloud... (voxel=%f, %d points)\n", voxelSize, !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
+			if(!assembledCloud->empty())
+			{
+				assembledCloud = util3d::voxelize(assembledCloud, voxelSize);
+				cloudWithoutNormals.reset(new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::copyPointCloud(*assembledCloud, *cloudWithoutNormals);
+			}
+			else if(!assembledCloudI->empty())
+			{
+				assembledCloudI = util3d::voxelize(assembledCloudI, voxelSize);
+				cloudWithoutNormals.reset(new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::copyPointCloud(*assembledCloudI, *cloudWithoutNormals);
+			}
+			printf("Voxel grid filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
+		}
+
+		printf("Computing normals of the assembled cloud... (k=20, %d points)\n", !assembledCloud->empty()?(int)assembledCloud->size():(int)assembledCloudI->size());
+		pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, 20, 0);
+
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudToExport(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIToExport(new pcl::PointCloud<pcl::PointXYZINormal>);
+		if(!assembledCloud->empty())
+		{
+			UASSERT(assembledCloud->size() == normals->size());
+			pcl::concatenateFields(*assembledCloud, *normals, *cloudToExport);
+			printf("Computing normals of the assembled cloud... done! (%fs, %d points)\n", timer.ticks(), (int)assembledCloud->size());
+			assembledCloud->clear();
+
+			// adjust with point of views
+			printf("Adjust normals to viewpoints of the assembled cloud... (%d points)\n", (int)cloudToExport->size());
+			util3d::adjustNormalsToViewPoints(
+										rawViewpoints,
+										rawAssembledCloud,
+										rawViewpointIndices,
+										cloudToExport);
+			printf("Adjust normals to viewpoints of the assembled cloud... (%fs, %d points)\n", timer.ticks(), (int)cloudToExport->size());
+		}
+		else if(!assembledCloudI->empty())
+		{
+			UASSERT(assembledCloudI->size() == normals->size());
+			pcl::concatenateFields(*assembledCloudI, *normals, *cloudIToExport);
+			printf("Computing normals of the assembled cloud... done! (%fs, %d points)\n", timer.ticks(), (int)assembledCloudI->size());
+			assembledCloudI->clear();
+
+			// adjust with point of views
+			printf("Adjust normals to viewpoints of the assembled cloud... (%d points)\n", (int)cloudIToExport->size());
+			util3d::adjustNormalsToViewPoints(
+										rawViewpoints,
+										rawAssembledCloud,
+										rawViewpointIndices,
+										cloudIToExport);
+			printf("Adjust normals to viewpoints of the assembled cloud... (%fs, %d points)\n", timer.ticks(), (int)cloudIToExport->size());
+		}
+		cloudWithoutNormals->clear();
+		rawAssembledCloud->clear();
+
+		if(randomSamples>0)
+		{
+			printf("Random samples filtering of the assembled cloud... (samples=%d, %d points)\n", randomSamples, !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
 			if(!cloudToExport->empty())
 			{
-				cloudToExport = util3d::voxelize(cloudToExport, voxelSize);
+				cloudToExport = util3d::randomSampling(cloudToExport, randomSamples);
 			}
 			else if(!cloudIToExport->empty())
 			{
-				cloudIToExport = util3d::voxelize(cloudIToExport, voxelSize);
+				cloudIToExport = util3d::randomSampling(cloudIToExport, randomSamples);
 			}
-			printf("Voxel grid filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
+			printf("Random samples filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
 		}
 
 		std::vector<int> pointToCamId;
@@ -1449,14 +1591,14 @@ int main(int argc, char * argv[])
 		// Meshing...
 		if(mesh || texture)
 		{
-			if(!mergedCloudsI->empty())
+			if(!cloudIToExport->empty())
 			{
-				pcl::copyPointCloud(*mergedCloudsI, *mergedClouds);
+				pcl::copyPointCloud(*cloudIToExport, *cloudToExport);
-				mergedCloudsI->clear();
+				cloudIToExport->clear();
 			}
 
 			Eigen::Vector4f min,max;
-			pcl::getMinMax3D(*mergedClouds, min, max);
+			pcl::getMinMax3D(*cloudToExport, min, max);
 			float mapLength = uMax3(max[0]-min[0], max[1]-min[1], max[2]-min[2]);
 			int optimizedDepth = 12;
 			for(int i=6; i<12; ++i)
@@ -1477,7 +1619,7 @@ int main(int argc, char * argv[])
 			pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
 			pcl::Poisson<pcl::PointXYZRGBNormal> poisson;
 			poisson.setDepth(optimizedDepth);
-			poisson.setInputCloud(mergedClouds);
+			poisson.setInputCloud(cloudToExport);
 			poisson.reconstruct(*mesh);
 			printf("Mesh reconstruction... done (%fs, %d polygons).\n", timer.ticks(), (int)mesh->polygons.size());
 
@@ -1491,7 +1633,7 @@ int main(int argc, char * argv[])
 						mesh,
 						0.0f,
 						maxPolygons,
-						mergedClouds,
+						cloudToExport,
 						colorRadius,
 						!texture,
 						doClean,