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.

2026-09-02 01:20:25 +08:00 · 2022-01-14 13:36:01 -05:00
parent 12c2dd707c
commit 9e0173f4cb
8 changed files with 796 additions and 263 deletions
--- a/corelib/include/rtabmap/core/util3d_filtering.h
+++ b/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.
 */
--- a/corelib/include/rtabmap/core/util3d_surface.h
+++ b/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,
--- a/corelib/src/RegistrationIcp.cpp
+++ b/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)
--- a/corelib/src/util3d_filtering.cpp
+++ b/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,
--- a/corelib/src/util3d_surface.cpp
+++ b/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());
-	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();
+	adjustNormalsToViewPointsImpl<pcl::PointXYZRGBNormal>(poses, rawCloud, rawCameraIndices, cloud);
+}

-					Eigen::Vector3f n(normal.x, normal.y, 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;
-					}
-				}
-				else
-				{
-					UWARN("Not found camera viewpoint for point %d!?", i);
-				}
-			}
-		}
-	}
+void 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)
+{
+	adjustNormalsToViewPointsImpl<pcl::PointXYZINormal>(poses, rawCloud, rawCameraIndices, cloud);
 }

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