Added util3d::segmentObstaclesFromGround() method, templated some PCL methods (new rtabmap/core/impl/util3d.hpp)

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1921 f169173b-cf89-36c8-b27e-44dbe73f0c83

Version.h.in

@@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define RTABMAP_VERSION_MINOR @PROJECT_VERSION_MINOR@
 #define RTABMAP_VERSION_PATCH @PROJECT_VERSION_PATCH@
 
-#define RTABMAP_VERSION_COMPARE(major, minor, patch)  (major>=@PROJECT_VERSION_MAJOR@ && minor>=@PROJECT_VERSION_MINOR@ && patch >=@PROJECT_VERSION_PATCH@)
+#define RTABMAP_VERSION_COMPARE(major, minor, patch)  (major>=@PROJECT_VERSION_MAJOR@ || (major==@PROJECT_VERSION_MAJOR@ && minor>=@PROJECT_VERSION_MINOR@) || (major==@PROJECT_VERSION_MAJOR@ && minor==@PROJECT_VERSION_MINOR@ && patch >=@PROJECT_VERSION_PATCH@))
 
 #endif /* VERSION_H_ */
 

corelib/include/rtabmap/core/impl/util3d.hpp

@@ -0,0 +1,428 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef UTIL3D_HPP_
+#define UTIL3D_HPP_
+
+#include <rtabmap/utilite/ULogger.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/random_sample.h>
+#include <pcl/filters/passthrough.h>
+#include <pcl/filters/filter.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/common/transforms.h>
+#include <pcl/common/common.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/segmentation/extract_clusters.h>
+
+namespace rtabmap{
+namespace util3d{
+
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr voxelize(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		float voxelSize)
+{
+	typedef typename pcl::PointCloud<PointT> PointCloud;
+	typedef typename PointCloud::Ptr PointCloudPtr;
+	UASSERT(voxelSize > 0.0f);
+	PointCloudPtr output(new PointCloud);
+	pcl::VoxelGrid<PointT> filter;
+	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
+	filter.setInputCloud(cloud);
+	filter.filter(*output);
+	return output;
+}
+
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr sampling(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud, int samples)
+{
+	typedef typename pcl::PointCloud<PointT> PointCloud;
+	typedef typename PointCloud::Ptr PointCloudPtr;
+	UASSERT(samples > 0);
+	PointCloudPtr output(new PointCloud);
+	pcl::RandomSample<PointT> filter;
+	filter.setSample(samples);
+	filter.setInputCloud(cloud);
+	filter.filter(*output);
+	return output;
+}
+
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr passThrough(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const std::string & axis,
+		float min,
+		float max)
+{
+	typedef typename pcl::PointCloud<PointT> PointCloud;
+	typedef typename PointCloud::Ptr PointCloudPtr;
+	UASSERT(max > min);
+	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
+
+	PointCloudPtr output(new PointCloud);
+	pcl::PassThrough<PointT> filter;
+	filter.setFilterFieldName(axis);
+	filter.setFilterLimits(min, max);
+	filter.setInputCloud(cloud);
+	filter.filter(*output);
+	return output;
+}
+
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud)
+{
+	typedef typename pcl::PointCloud<PointT> PointCloud;
+	typedef typename PointCloud::Ptr PointCloudPtr;
+	PointCloudPtr output(new PointCloud);
+	std::vector<int> indices;
+	pcl::removeNaNFromPointCloud<PointT>(*cloud, *output, indices);
+	return output;
+}
+
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud)
+{
+	typedef typename pcl::PointCloud<PointT> PointCloud;
+	typedef typename PointCloud::Ptr PointCloudPtr;
+	PointCloudPtr output(new PointCloud);
+	std::vector<int> indices;
+	pcl::removeNaNNormalsFromPointCloud<PointT>(*cloud, *output, indices);
+	return output;
+}
+
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr RTABMAP_EXP transformPointCloud(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const Transform & transform)
+{
+	typedef typename pcl::PointCloud<PointT> PointCloud;
+	typedef typename PointCloud::Ptr PointCloudPtr;
+	PointCloudPtr output(new PointCloud);
+	pcl::transformPointCloud<PointT>(*cloud, *output, transformToEigen4f(transform));
+	return output;
+}
+
+template<typename PointT>
+PointT transformPoint(
+		const PointT & pt,
+		const Transform & transform)
+{
+	return pcl::transformPoint(pt, transformToEigen3f(transform));
+}
+
+template<typename PointT>
+void segmentObstaclesFromGround(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		pcl::IndicesPtr & ground,
+		pcl::IndicesPtr & obstacles,
+		float normalRadiusSearch,
+		float groundNormalAngle,
+		int minClusterSize)
+{
+	ground.reset(new std::vector<int>);
+	obstacles.reset(new std::vector<int>);
+
+	// Find the ground
+	pcl::IndicesPtr flatSurfaces = util3d::normalFiltering<PointT>(
+			cloud,
+			groundNormalAngle,
+			Eigen::Vector4f(0,0,1,0),
+			normalRadiusSearch*2.0f,
+			Eigen::Vector4f(0,0,100,0));
+
+	int biggestFlatSurfaceIndex;
+	std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = util3d::extractClusters<PointT>(
+			cloud,
+			flatSurfaces,
+			normalRadiusSearch*2.0f,
+			minClusterSize,
+			std::numeric_limits<int>::max(),
+			&biggestFlatSurfaceIndex);
+
+
+	// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
+	ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
+	Eigen::Vector4f min,max;
+	pcl::getMinMax3D<PointT>(*cloud, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
+
+	for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
+	{
+		if((int)i!=biggestFlatSurfaceIndex)
+		{
+			Eigen::Vector4f centroid;
+			pcl::compute3DCentroid<PointT>(*cloud, *clusteredFlatSurfaces.at(i), centroid);
+			if(centroid[2] >= min[2] && centroid[2] <= max[2])
+			{
+				ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
+			}
+		}
+	}
+
+	if(ground->size() != cloud->size())
+	{
+		// Remove ground
+		pcl::IndicesPtr otherStuffIndices = util3d::extractNegativeIndices<PointT>(cloud, ground);
+
+		//Cluster remaining stuff (obstacles)
+		std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters<PointT>(
+				cloud,
+				otherStuffIndices,
+				normalRadiusSearch*2.0f,
+				minClusterSize);
+
+		// merge indices
+		obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
+	}
+}
+
+template<typename PointT>
+void projectCloudOnXYPlane(
+		typename pcl::PointCloud<PointT>::Ptr & cloud)
+{
+	for(unsigned int i=0; i<cloud->size(); ++i)
+	{
+		cloud->at(i).z = 0;
+	}
+}
+
+template<typename PointT>
+pcl::IndicesPtr radiusFiltering(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		float radiusSearch,
+		int minNeighborsInRadius)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return radiusFiltering<PointT>(cloud, indices, radiusSearch, minNeighborsInRadius);
+}
+
+template<typename PointT>
+pcl::IndicesPtr radiusFiltering(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float radiusSearch,
+		int minNeighborsInRadius)
+{
+	typedef typename pcl::search::KdTree<PointT> KdTree;
+	typedef typename KdTree::Ptr KdTreePtr;
+	KdTreePtr tree (new KdTree(false));
+
+	if(indices->size())
+	{
+		pcl::IndicesPtr output(new std::vector<int>(indices->size()));
+		int oi = 0; // output iterator
+		tree->setInputCloud(cloud, indices);
+		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
+		tree->setInputCloud(cloud);
+		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;
+	}
+}
+
+template<typename PointT>
+pcl::IndicesPtr normalFiltering(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		float angleMax,
+		const Eigen::Vector4f & normal,
+		float radiusSearch,
+		const Eigen::Vector4f & viewpoint)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return normalFiltering<PointT>(cloud, indices, angleMax, normal, radiusSearch, viewpoint);
+}
+
+template<typename PointT>
+pcl::IndicesPtr normalFiltering(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float angleMax,
+		const Eigen::Vector4f & normal,
+		float radiusSearch,
+		const Eigen::Vector4f & viewpoint)
+{
+	typedef typename pcl::search::KdTree<PointT> KdTree;
+	typedef typename KdTree::Ptr KdTreePtr;
+
+	pcl::NormalEstimation<PointT, pcl::Normal> ne;
+	ne.setInputCloud (cloud);
+	if(indices->size())
+	{
+		ne.setIndices(indices);
+	}
+
+	KdTreePtr tree (new KdTree(false));
+
+	if(indices->size())
+	{
+		tree->setInputCloud(cloud, indices);
+	}
+	else
+	{
+		tree->setInputCloud(cloud);
+	}
+	ne.setSearchMethod (tree);
+
+	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
+
+	ne.setRadiusSearch (radiusSearch);
+	if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
+	{
+		ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
+	}
+
+	ne.compute (*cloud_normals);
+
+	pcl::IndicesPtr output(new std::vector<int>(cloud_normals->size()));
+	int oi = 0; // output iterator
+	Eigen::Vector3f n(normal[0], normal[1], normal[2]);
+	for(unsigned int i=0; i<cloud_normals->size(); ++i)
+	{
+		Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
+		float angle = pcl::getAngle3D(normal, v);
+		if(angle < angleMax)
+		{
+			output->at(oi++) = indices->size()!=0?indices->at(i):i;
+		}
+	}
+	output->resize(oi);
+
+	return output;
+}
+
+template<typename PointT>
+std::vector<pcl::IndicesPtr> extractClusters(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		float clusterTolerance,
+		int minClusterSize,
+		int maxClusterSize,
+		int * biggestClusterIndex)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return extractClusters<PointT>(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
+}
+
+template<typename PointT>
+std::vector<pcl::IndicesPtr> extractClusters(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float clusterTolerance,
+		int minClusterSize,
+		int maxClusterSize,
+		int * biggestClusterIndex)
+{
+	typedef typename pcl::search::KdTree<PointT> KdTree;
+	typedef typename KdTree::Ptr KdTreePtr;
+
+	KdTreePtr tree(new KdTree);
+	pcl::EuclideanClusterExtraction<PointT> ec;
+	ec.setClusterTolerance (clusterTolerance);
+	ec.setMinClusterSize (minClusterSize);
+	ec.setMaxClusterSize (maxClusterSize);
+	ec.setInputCloud (cloud);
+
+	if(indices->size())
+	{
+		ec.setIndices(indices);
+		tree->setInputCloud(cloud, indices);
+	}
+	else
+	{
+		tree->setInputCloud(cloud);
+	}
+	ec.setSearchMethod (tree);
+
+	std::vector<pcl::PointIndices> cluster_indices;
+	ec.extract (cluster_indices);
+
+	int maxIndex=-1;
+	unsigned int maxSize = 0;
+	std::vector<pcl::IndicesPtr> output(cluster_indices.size());
+	for(unsigned int i=0; i<cluster_indices.size(); ++i)
+	{
+		output[i] = pcl::IndicesPtr(new std::vector<int>(cluster_indices[i].indices));
+
+		if(maxSize < cluster_indices[i].indices.size())
+		{
+			maxSize = cluster_indices[i].indices.size();
+			maxIndex = i;
+		}
+	}
+	if(biggestClusterIndex)
+	{
+		*biggestClusterIndex = maxIndex;
+	}
+
+	return output;
+}
+
+template<typename PointT>
+pcl::IndicesPtr extractNegativeIndices(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & indices)
+{
+	pcl::IndicesPtr output(new std::vector<int>);
+	pcl::ExtractIndices<PointT> extract;
+	extract.setInputCloud (cloud);
+	extract.setIndices(indices);
+	extract.setNegative(true);
+	extract.filter(*output);
+	return output;
+}
+
+} // util3d
+} // rtabmap
+#endif //UTIL3D_HPP_

corelib/include/rtabmap/core/util3d.h

@@ -148,55 +148,6 @@ pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(
 		bool smoothing,
 		float maxZError = 0.03f);
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		float voxelSize);
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP voxelize(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		float voxelSize);
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP sampling(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		int samples);
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP sampling(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		int samples);
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP passThrough(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const std::string & axis,
-		float min,
-		float max);
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP passThrough(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const std::string & axis,
-		float min,
-		float max);
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud);
-
-pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
-		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud);
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
-		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud);
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const Transform & transform);
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const Transform & transform);
-
-pcl::PointXYZ RTABMAP_EXP transformPoint(
-		const pcl::PointXYZ & pt,
-		const Transform & transform);
-pcl::PointXYZRGB RTABMAP_EXP transformPoint(
-		const pcl::PointXYZRGB & pt,
-		const Transform & transform);
-
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDepth(
 		const cv::Mat & imageDepth,
 		float cx, float cy,
@@ -546,98 +497,6 @@ void RTABMAP_EXP rayTrace(const cv::Point2i & start,
 
 cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);
 
-void RTABMAP_EXP projectCloudOnXYPlane(
-		pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
-
-/**
- * For convenience.
- */
-pcl::IndicesPtr radiusFiltering(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		float radiusSearch,
-		int minNeighborsInRadius);
-
-/**
- * @brief Wrapper of the pcl::RadiusOutlierRemoval class.
- *
- * Points in the cloud which have less than a minimum of neighbors in the
- * specified radius are filtered.
- * @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 radiusSearch the radius in meter.
- * @param minNeighborsInRadius the minimum of neighbors to keep the point.
- * @return the indices of the points satisfying the parameters.
- */
-pcl::IndicesPtr radiusFiltering(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const pcl::IndicesPtr & indices,
-		float radiusSearch,
-		int minNeighborsInRadius);
-
-/**
- * For convenience.
- */
-pcl::IndicesPtr RTABMAP_EXP normalFiltering(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		float angleMax,
-		const Eigen::Vector4f & normal,
-		float radiusSearch,
-		const Eigen::Vector4f & viewpoint);
-
-/**
- * @brief Given a normal and a maximum angle error, keep all points of the cloud
- * respecting this normal.
- *
- * The normals are computed using the radius search parameter (pcl::NormalEstimation class is used for this), then
- * for each normal, the corresponding point is filtered if the
- * angle (using pcl::getAngle3D()) with the normal specified by the user is larger than the maximum
- * angle specified by the user.
- * @param cloud the input cloud.
- * @param indices the input indices of the cloud to process, if empty, all points in the cloud are processed.
- * @param angleMax the maximum angle.
- * @param normal the normal to which each point's normal is compared.
- * @param radiusSearch radius parameter used for normal estimation (see pcl::NormalEstimation).
- * @param viewpoint from which viewpoint the normals should be estimated (see pcl::NormalEstimation).
- * @return the indices of the points which respect the normal constraint.
- */
-pcl::IndicesPtr RTABMAP_EXP normalFiltering(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const pcl::IndicesPtr & indices,
-		float angleMax,
-		const Eigen::Vector4f & normal,
-		float radiusSearch,
-		const Eigen::Vector4f & viewpoint);
-
-/**
- * For convenience.
- */
-std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		float clusterTolerance,
-		int minClusterSize,
-		int maxClusterSize = std::numeric_limits<int>::max(),
-		int * biggestClusterIndex = 0);
-
-/**
- * @brief Wrapper of the pcl::EuclideanClusterExtraction class.
- *
- * Extract all clusters from a point cloud given a maximum cluster distance tolerance.
- * @param cloud the input cloud.
- * @param indices the input indices of the cloud to process, if empty, all points in the cloud are processed.
- * @param clusterTolerance the cluster distance tolerance (see pcl::EuclideanClusterExtraction).
- * @param minClusterSize minimum size of the clusters to return (see pcl::EuclideanClusterExtraction).
- * @param maxClusterSize maximum size of the clusters to return (see pcl::EuclideanClusterExtraction).
- * @param biggestClusterIndex the index of the biggest cluster, if the clusters are empty, a negative index is set.
- * @return the indices of each cluster found.
- */
-std::vector<pcl::IndicesPtr> RTABMAP_EXP extractClusters(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const pcl::IndicesPtr & indices,
-		float clusterTolerance,
-		int minClusterSize,
-		int maxClusterSize = std::numeric_limits<int>::max(),
-		int * biggestClusterIndex = 0);
-
 /**
  * @brief Concatenate a vector of indices to a single vector.
  *
@@ -664,11 +523,161 @@ pcl::IndicesPtr RTABMAP_EXP concatenate(
 		const pcl::IndicesPtr & indicesA,
 		const pcl::IndicesPtr & indicesB);
 
-pcl::IndicesPtr RTABMAP_EXP extractNegativeIndices(
+///////////////////
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+// Templated PCL methods
+///////////////////
+
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr voxelize(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		float voxelSize);
+
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr sampling(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		int samples);
+
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr passThrough(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const std::string & axis,
+		float min,
+		float max);
+
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr removeNaNFromPointCloud(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud);
+
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr removeNaNNormalsFromPointCloud(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud);
+
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr transformPointCloud(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const Transform & transform);
+
+template<typename PointT>
+PointT transformPoint(
+		const PointT & pt,
+		const Transform & transform);
+
+template<typename PointT>
+void segmentObstaclesFromGround(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		pcl::IndicesPtr & ground,
+		pcl::IndicesPtr & obstacles,
+		float normalRadiusSearch,
+		float groundNormalAngle,
+		int minClusterSize);
+
+template<typename PointT>
+void projectCloudOnXYPlane(
+		typename pcl::PointCloud<PointT>::Ptr & cloud);
+
+/**
+ * For convenience.
+ */
+template<typename PointT>
+pcl::IndicesPtr radiusFiltering(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		float radiusSearch,
+		int minNeighborsInRadius);
+
+/**
+ * @brief Wrapper of the pcl::RadiusOutlierRemoval class.
+ *
+ * Points in the cloud which have less than a minimum of neighbors in the
+ * specified radius are filtered.
+ * @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 radiusSearch the radius in meter.
+ * @param minNeighborsInRadius the minimum of neighbors to keep the point.
+ * @return the indices of the points satisfying the parameters.
+ */
+template<typename PointT>
+pcl::IndicesPtr radiusFiltering(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float radiusSearch,
+		int minNeighborsInRadius);
+
+/**
+ * For convenience.
+ */
+template<typename PointT>
+pcl::IndicesPtr normalFiltering(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		float angleMax,
+		const Eigen::Vector4f & normal,
+		float radiusSearch,
+		const Eigen::Vector4f & viewpoint);
+
+/**
+ * @brief Given a normal and a maximum angle error, keep all points of the cloud
+ * respecting this normal.
+ *
+ * The normals are computed using the radius search parameter (pcl::NormalEstimation class is used for this), then
+ * for each normal, the corresponding point is filtered if the
+ * angle (using pcl::getAngle3D()) with the normal specified by the user is larger than the maximum
+ * angle specified by the user.
+ * @param cloud the input cloud.
+ * @param indices the input indices of the cloud to process, if empty, all points in the cloud are processed.
+ * @param angleMax the maximum angle.
+ * @param normal the normal to which each point's normal is compared.
+ * @param radiusSearch radius parameter used for normal estimation (see pcl::NormalEstimation).
+ * @param viewpoint from which viewpoint the normals should be estimated (see pcl::NormalEstimation).
+ * @return the indices of the points which respect the normal constraint.
+ */
+template<typename PointT>
+pcl::IndicesPtr normalFiltering(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float angleMax,
+		const Eigen::Vector4f & normal,
+		float radiusSearch,
+		const Eigen::Vector4f & viewpoint);
+
+/**
+ * For convenience.
+ */
+template<typename PointT>
+std::vector<pcl::IndicesPtr> extractClusters(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		float clusterTolerance,
+		int minClusterSize,
+		int maxClusterSize = std::numeric_limits<int>::max(),
+		int * biggestClusterIndex = 0);
+
+/**
+ * @brief Wrapper of the pcl::EuclideanClusterExtraction class.
+ *
+ * Extract all clusters from a point cloud given a maximum cluster distance tolerance.
+ * @param cloud the input cloud.
+ * @param indices the input indices of the cloud to process, if empty, all points in the cloud are processed.
+ * @param clusterTolerance the cluster distance tolerance (see pcl::EuclideanClusterExtraction).
+ * @param minClusterSize minimum size of the clusters to return (see pcl::EuclideanClusterExtraction).
+ * @param maxClusterSize maximum size of the clusters to return (see pcl::EuclideanClusterExtraction).
+ * @param biggestClusterIndex the index of the biggest cluster, if the clusters are empty, a negative index is set.
+ * @return the indices of each cluster found.
+ */
+template<typename PointT>
+std::vector<pcl::IndicesPtr> extractClusters(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float clusterTolerance,
+		int minClusterSize,
+		int maxClusterSize = std::numeric_limits<int>::max(),
+		int * biggestClusterIndex = 0);
+
+template<typename PointT>
+pcl::IndicesPtr extractNegativeIndices(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
 		const pcl::IndicesPtr & indices);
 
 } // namespace util3d
 } // namespace rtabmap
 
+#include "rtabmap/core/impl/util3d.hpp"
+
 #endif /* UTIL3D_H_ */

corelib/src/CMakeLists.txt

@@ -126,6 +126,6 @@ INSTALL(TARGETS rtabmap_core
 install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ 
 		DESTINATION "${INSTALL_INCLUDE_DIR}"
 		COMPONENT devel 
-		FILES_MATCHING PATTERN "*.h" 
+		FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 
 		PATTERN ".svn" EXCLUDE)
 

corelib/src/Memory.cpp

@@ -1903,8 +1903,8 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 				pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
 
 				std::vector<int> indices;
-				newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
+				newCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(newCloud);
-				oldCloud = util3d::removeNaNNormalsFromPointCloud(oldCloud);
+				oldCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(oldCloud);
 
 				// 3D
 				double fitness = 0;
@@ -1981,8 +1981,8 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 			//voxelize
 			if(_icp2VoxelSize > 0.0f)
 			{
-				oldCloud = util3d::voxelize(oldCloud, _icp2VoxelSize);
+				oldCloud = util3d::voxelize<pcl::PointXYZ>(oldCloud, _icp2VoxelSize);
-				newCloud = util3d::voxelize(newCloud, _icp2VoxelSize);
+				newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, _icp2VoxelSize);
 			}
 
 			double fitness = 0.0f;
@@ -2000,7 +2000,7 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 
 				//pcl::io::savePCDFile("lccold.pcd", *oldCloud);
 				//pcl::io::savePCDFile("lccnewguess.pcd", *newCloud);
-				newCloud = util3d::transformPointCloud(newCloud, icpT);
+				newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
 				//pcl::io::savePCDFile("lccnewicp.pcd", *newCloud);
 
 				// verify if there are enough correspondences
@@ -2094,7 +2094,7 @@ Transform Memory::computeScanMatchingTransform(
 	//voxelize
 	if(assembledOldClouds->size() && _icp2VoxelSize > 0.0f)
 	{
-		assembledOldClouds = util3d::voxelize(assembledOldClouds, _icp2VoxelSize);
+		assembledOldClouds = util3d::voxelize<pcl::PointXYZ>(assembledOldClouds, _icp2VoxelSize);
 	}
 
 	// get the new cloud
@@ -2106,7 +2106,7 @@ Transform Memory::computeScanMatchingTransform(
 	//voxelize
 	if(newCloud->size() && _icp2VoxelSize > 0.0f)
 	{
-		newCloud = util3d::voxelize(newCloud, _icp2VoxelSize);
+		newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, _icp2VoxelSize);
 	}
 
 	//pcl::io::savePCDFile("old.pcd", *assembledOldClouds);
@@ -2126,7 +2126,7 @@ Transform Memory::computeScanMatchingTransform(
 
 		UDEBUG("icpT=%s", icpT.prettyPrint().c_str());
 
-		newCloud = util3d::transformPointCloud(newCloud, icpT);
+		newCloud = util3d::transformPointCloud<pcl::PointXYZ>(newCloud, icpT);
 		//pcl::io::savePCDFile("newCorrected.pcd", *newCloud);
 
 		// verify if there enough correspondences

corelib/src/Odometry.cpp

@@ -1104,7 +1104,7 @@ Transform OdometryICP::computeTransform(const SensorData & data, int * quality,
 			pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
 
 			std::vector<int> indices;
-			newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
+			newCloud = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(newCloud);
 			if(newCloudXYZ->size() != newCloud->size())
 			{
 				UWARN("removed nan normals...");

corelib/src/util3d.cpp

@@ -33,9 +33,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/registration/transformation_estimation_2D.h>
 #include <pcl/registration/correspondence_rejection_sample_consensus.h>
 #include <pcl/registration/icp_nl.h>
-#include <pcl/search/kdtree.h>
-#include <pcl/filters/passthrough.h>
-#include <pcl/filters/voxel_grid.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/distances.h>
 #include <pcl/surface/gp3.h>
@@ -43,9 +40,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/surface/mls.h>
 #include <pcl/ModelCoefficients.h>
 #include <pcl/segmentation/sac_segmentation.h>
-#include <pcl/filters/crop_box.h>
-#include <pcl/segmentation/extract_clusters.h>
-#include <pcl/filters/extract_indices.h>
 
 #include <opencv2/nonfree/features2d.hpp>
 #include <opencv2/calib3d/calib3d.hpp>
@@ -640,158 +634,6 @@ pcl::PointXYZ projectDepthTo3D(
 	return pt;
 }
 
-pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, float voxelSize)
-{
-	UASSERT(voxelSize > 0.0f);
-	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
-	pcl::VoxelGrid<pcl::PointXYZ> filter;
-	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
-	filter.setInputCloud(cloud);
-	filter.filter(*output);
-	return output;
-}
-
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, float voxelSize)
-{
-	UASSERT(voxelSize > 0.0f);
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
-	pcl::VoxelGrid<pcl::PointXYZRGB> filter;
-	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
-	filter.setInputCloud(cloud);
-	filter.filter(*output);
-	return output;
-}
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr sampling(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, int samples)
-{
-	UASSERT(samples > 0);
-	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
-	pcl::RandomSample<pcl::PointXYZ> filter;
-	filter.setSample(samples);
-	filter.setInputCloud(cloud);
-	filter.filter(*output);
-	return output;
-}
-
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr sampling(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, int samples)
-{
-	UASSERT(samples > 0);
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
-	pcl::RandomSample<pcl::PointXYZRGB> filter;
-	filter.setSample(samples);
-	filter.setInputCloud(cloud);
-	filter.filter(*output);
-	return output;
-}
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr passThrough(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const std::string & axis,
-		float min,
-		float max)
-{
-	UASSERT(max > min);
-	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
-
-	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
-	pcl::PassThrough<pcl::PointXYZ> filter;
-	filter.setFilterFieldName(axis);
-	filter.setFilterLimits(min, max);
-	filter.setInputCloud(cloud);
-	filter.filter(*output);
-	return output;
-}
-
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr passThrough(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const std::string & axis,
-		float min,
-		float max)
-{
-	UASSERT(max > min);
-	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
-
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
-	pcl::PassThrough<pcl::PointXYZRGB> filter;
-	filter.setFilterFieldName(axis);
-	filter.setFilterLimits(min, max);
-	filter.setInputCloud(cloud);
-	filter.filter(*output);
-	return output;
-}
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud)
-{
-	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
-	std::vector<int> indices;
-	pcl::removeNaNFromPointCloud (*cloud, *output, indices);
-	return output;
-}
-
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP removeNaNFromPointCloud(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud)
-{
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
-	std::vector<int> indices;
-	pcl::removeNaNFromPointCloud (*cloud, *output, indices);
-	return output;
-}
-
-pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
-		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud)
-{
-	pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
-	std::vector<int> indices;
-	pcl::removeNaNNormalsFromPointCloud (*cloud, *output, indices);
-	return output;
-}
-
-pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(
-		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud)
-{
-	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-	std::vector<int> indices;
-	pcl::removeNaNNormalsFromPointCloud(*cloud, *output, indices);
-	return output;
-}
-
-pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const Transform & transform)
-{
-	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
-	pcl::transformPointCloud(*cloud, *output, transformToEigen4f(transform));
-	return output;
-}
-
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const Transform & transform)
-{
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
-	pcl::transformPointCloud(*cloud, *output, transformToEigen4f(transform));
-	return output;
-}
-
-pcl::PointXYZ RTABMAP_EXP transformPoint(
-		const pcl::PointXYZ & pt,
-		const Transform & transform)
-{
-	return pcl::transformPoint(pt, transformToEigen3f(transform));
-}
-
-pcl::PointXYZRGB RTABMAP_EXP transformPoint(
-		const pcl::PointXYZRGB & pt,
-		const Transform & transform)
-{
-	return pcl::transformPoint(pt, transformToEigen3f(transform));
-}
-
 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		const cv::Mat & imageDepth,
 		float cx, float cy,
@@ -2029,25 +1871,25 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
 	{
 		if(maxDepth>0.0)
 		{
-			cloud = passThrough(cloud, "z", 0, maxDepth);
+			cloud = passThrough<pcl::PointXYZ>(cloud, "z", 0, maxDepth);
 		}
 
 		if(cloud->size())
 		{
 			if(voxel>0)
 			{
-				cloud = voxelize(cloud, voxel);
+				cloud = voxelize<pcl::PointXYZ>(cloud, voxel);
 			}
 			else if(samples>0 && (int)cloud->size() > samples)
 			{
-				cloud = sampling(cloud, samples);
+				cloud = sampling<pcl::PointXYZ>(cloud, samples);
 			}
 
 			if(cloud->size())
 			{
 				if(!transform.isNull() && !transform.isIdentity())
 				{
-					cloud = transformPointCloud(cloud, transform);
+					cloud = transformPointCloud<pcl::PointXYZ>(cloud, transform);
 				}
 			}
 		}
@@ -2112,7 +1954,7 @@ pcl::PolygonMesh::Ptr createMesh(
 		float gp3MaximumAngle,
 		bool gp3NormalConsistency)
 {
-	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormalsNoNaN = removeNaNNormalsFromPointCloud(cloudWithNormals);
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormalsNoNaN = removeNaNNormalsFromPointCloud<pcl::PointXYZRGBNormal>(cloudWithNormals);
 
 	// Create search tree*
 	pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
@@ -2538,80 +2380,35 @@ bool occupancy2DFromCloud3D(
 	pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
 
 	//voxelize
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelizedCloud = util3d::voxelize(cloud, cellSize);
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelizedCloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, cellSize);
-
-	//convert to XYZ
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ(new pcl::PointCloud<pcl::PointXYZ>);
-	pcl::copyPointCloud(*voxelizedCloud, *cloudXYZ);
 
 	pcl::IndicesPtr groundIndices, obstaclesIndices;
 
-	// Find the ground
+	segmentObstaclesFromGround<pcl::PointXYZRGB>(cloud,
-	pcl::IndicesPtr flatSurfaces = util3d::normalFiltering(
+			groundIndices,
-			cloudXYZ,
+			obstaclesIndices,
+			cellSize,
 			groundNormalAngle,
-			Eigen::Vector4f(0,0,1,0),
+			minClusterSize);
-			cellSize*2.0f,
-			Eigen::Vector4f(0,0,100,0));
 
-	int biggestFlatSurfaceIndex;
+	if(groundIndices->size())
-	std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = util3d::extractClusters(
-			cloudXYZ,
-			flatSurfaces,
-			cellSize*2.0f,
-			minClusterSize,
-			std::numeric_limits<int>::max(),
-			&biggestFlatSurfaceIndex);
-
-
-	// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
-	groundIndices = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
-	Eigen::Vector4f min,max;
-	pcl::getMinMax3D(*cloudXYZ, *clusteredFlatSurfaces.at(biggestFlatSurfaceIndex), min, max);
-
-	for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
 	{
-		if((int)i!=biggestFlatSurfaceIndex)
+		pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
-		{
+		//project on XY plane
-			Eigen::Vector4f centroid;
+		util3d::projectCloudOnXYPlane<pcl::PointXYZ>(groundCloud);
-			pcl::compute3DCentroid(*cloudXYZ, *clusteredFlatSurfaces.at(i), centroid);
+		//voxelize to grid cell size
-			if(centroid[2] >= min[2] && centroid[2] <= max[2])
+		groundCloud = util3d::voxelize<pcl::PointXYZ>(groundCloud, cellSize);
-			{
-				groundIndices = util3d::concatenate(groundIndices, clusteredFlatSurfaces.at(i));
-			}
-		}
 	}
 
-	pcl::copyPointCloud(*cloudXYZ, *groundIndices, *groundCloud);
+	if(obstaclesIndices->size())
-
-	if(groundIndices->size() != cloudXYZ->size())
 	{
-		// Remove ground
+		pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
-		pcl::IndicesPtr otherStuffIndices = util3d::extractNegativeIndices(cloudXYZ, groundIndices);
+		//project on XY plane
-
+		util3d::projectCloudOnXYPlane<pcl::PointXYZ>(obstaclesCloud);
-		//Cluster remaining stuff (obstacles)
+		//voxelize to grid cell size
-		std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
+		obstaclesCloud = util3d::voxelize<pcl::PointXYZ>(obstaclesCloud, cellSize);
-				cloudXYZ,
-				otherStuffIndices,
-				cellSize*2.0f,
-				minClusterSize);
-
-		// merge indices
-		obstaclesIndices = util3d::concatenate(clusteredObstaclesSurfaces);
-		if(obstaclesIndices->size())
-		{
-			pcl::copyPointCloud(*cloudXYZ, *obstaclesIndices, *obstaclesCloud);
-		}
 	}
 
-	//project on XY plane
-	util3d::projectCloudOnXYPlane(groundCloud);
-	util3d::projectCloudOnXYPlane(obstaclesCloud);
-
-	//voxelize to grid cell size
-	groundCloud = util3d::voxelize(groundCloud, cellSize);
-	obstaclesCloud = util3d::voxelize(obstaclesCloud, cellSize);
-
 	ground = cv::Mat();
 	if(groundCloud->size())
 	{
@@ -2846,7 +2643,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
 	{
 		if(uContains(scans, iter->first))
 		{
-			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::transformPointCloud(scans.at(iter->first), iter->second);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = transformPointCloud<pcl::PointXYZ>(scans.at(iter->first), iter->second);
 			pcl::PointXYZ min, max;
 			pcl::getMinMax3D(*cloud, min, max);
 			minMax.push_back(min);
@@ -3052,195 +2849,6 @@ cv::Mat convertMap2Image8U(const cv::Mat & map8S)
 	return map8U;
 }
 
-void projectCloudOnXYPlane(
-		pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud)
-{
-	for(unsigned int i=0; i<cloud->size(); ++i)
-	{
-		cloud->at(i).z = 0;
-	}
-}
-
-pcl::IndicesPtr radiusFiltering(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		float radiusSearch,
-		int minNeighborsInRadius)
-{
-	pcl::IndicesPtr indices(new std::vector<int>);
-	return radiusFiltering(cloud, indices, radiusSearch, minNeighborsInRadius);
-}
-
-pcl::IndicesPtr radiusFiltering(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const pcl::IndicesPtr & indices,
-		float radiusSearch,
-		int minNeighborsInRadius)
-{
-	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> (false));
-
-	if(indices->size())
-	{
-		pcl::IndicesPtr output(new std::vector<int>(indices->size()));
-		int oi = 0; // output iterator
-		tree->setInputCloud(cloud, indices);
-		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
-		tree->setInputCloud(cloud);
-		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,
-		float angleMax,
-		const Eigen::Vector4f & normal,
-		float radiusSearch,
-		const Eigen::Vector4f & viewpoint)
-{
-	pcl::IndicesPtr indices(new std::vector<int>);
-	return normalFiltering(cloud, indices, angleMax, normal, radiusSearch, viewpoint);
-}
-
-pcl::IndicesPtr normalFiltering(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const pcl::IndicesPtr & indices,
-		float angleMax,
-		const Eigen::Vector4f & normal,
-		float radiusSearch,
-		const Eigen::Vector4f & viewpoint)
-{
-	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
-	ne.setInputCloud (cloud);
-	if(indices->size())
-	{
-		ne.setIndices(indices);
-	}
-
-	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
-	if(indices->size())
-	{
-		tree->setInputCloud(cloud, indices);
-	}
-	else
-	{
-		tree->setInputCloud(cloud);
-	}
-	ne.setSearchMethod (tree);
-
-	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
-
-	ne.setRadiusSearch (radiusSearch);
-	if(viewpoint[0] != 0 || viewpoint[1] != 0 || viewpoint[2] != 0)
-	{
-		ne.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
-	}
-
-	ne.compute (*cloud_normals);
-
-	pcl::IndicesPtr output(new std::vector<int>(cloud_normals->size()));
-	int oi = 0; // output iterator
-	Eigen::Vector3f n(normal[0], normal[1], normal[2]);
-	for(unsigned int i=0; i<cloud_normals->size(); ++i)
-	{
-		Eigen::Vector4f v(cloud_normals->at(i).normal_x, cloud_normals->at(i).normal_y, cloud_normals->at(i).normal_z, 0.0f);
-		float angle = pcl::getAngle3D(normal, v);
-		if(angle < angleMax)
-		{
-			output->at(oi++) = indices->size()!=0?indices->at(i):i;
-		}
-	}
-	output->resize(oi);
-
-	return output;
-}
-
-std::vector<pcl::IndicesPtr> extractClusters(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		float clusterTolerance,
-		int minClusterSize,
-		int maxClusterSize,
-		int * biggestClusterIndex)
-{
-	pcl::IndicesPtr indices(new std::vector<int>);
-	return extractClusters(cloud, indices, clusterTolerance, minClusterSize, maxClusterSize, biggestClusterIndex);
-}
-
-std::vector<pcl::IndicesPtr> extractClusters(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const pcl::IndicesPtr & indices,
-		float clusterTolerance,
-		int minClusterSize,
-		int maxClusterSize,
-		int * biggestClusterIndex)
-{
-	pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
-	pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
-	ec.setClusterTolerance (clusterTolerance);
-	ec.setMinClusterSize (minClusterSize);
-	ec.setMaxClusterSize (maxClusterSize);
-	ec.setInputCloud (cloud);
-
-	if(indices->size())
-	{
-		ec.setIndices(indices);
-		kdTree->setInputCloud(cloud, indices);
-	}
-	else
-	{
-		kdTree->setInputCloud(cloud);
-	}
-	ec.setSearchMethod (kdTree);
-
-	std::vector<pcl::PointIndices> cluster_indices;
-	ec.extract (cluster_indices);
-
-	int maxIndex=-1;
-	unsigned int maxSize = 0;
-	std::vector<pcl::IndicesPtr> output(cluster_indices.size());
-	for(unsigned int i=0; i<cluster_indices.size(); ++i)
-	{
-		output[i] = pcl::IndicesPtr(new std::vector<int>(cluster_indices[i].indices));
-
-		if(maxSize < cluster_indices[i].indices.size())
-		{
-			maxSize = cluster_indices[i].indices.size();
-			maxIndex = i;
-		}
-	}
-	if(biggestClusterIndex)
-	{
-		*biggestClusterIndex = maxIndex;
-	}
-
-	return output;
-}
-
 pcl::IndicesPtr concatenate(const std::vector<pcl::IndicesPtr> & indices)
 {
 	//compute total size
@@ -3273,19 +2881,6 @@ pcl::IndicesPtr concatenate(const pcl::IndicesPtr & indicesA, const pcl::Indices
 	return ind;
 }
 
-pcl::IndicesPtr extractNegativeIndices(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const pcl::IndicesPtr & indices)
-{
-	pcl::IndicesPtr output(new std::vector<int>);
-	pcl::ExtractIndices<pcl::PointXYZ> extract;
-	extract.setInputCloud (cloud);
-	extract.setIndices(indices);
-	extract.setNegative(true);
-	extract.filter(*output);
-	return output;
-}
-
 }
 
 }

examples/RGBDMapping/MapBuilder.h

@@ -111,10 +111,10 @@ private slots:
 					2); // decimation // high definition
 				if(cloud->size())
 				{
-					cloud = util3d::passThrough(cloud, "z", 0, 4.0f);
+					cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, 4.0f);
 					if(cloud->size())
 					{
-						cloud = util3d::transformPointCloud(cloud, data.localTransform());
+						cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, data.localTransform());
 					}
 				}
 				if(!cloudViewer_->addOrUpdateCloud("cloudOdom", cloud, pose))
@@ -184,10 +184,10 @@ private slots:
 
 					if(cloud->size())
 					{
-						cloud = util3d::passThrough(cloud, "z", 0, 4.0f);
+						cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, 4.0f);
 						if(cloud->size())
 						{
-							cloud = util3d::transformPointCloud(cloud, localTransform);
+							cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform);
 						}
 					}
 					if(!cloudViewer_->addOrUpdateCloud(cloudName, cloud, iter->second))

guilib/src/DatabaseViewer.cpp

@@ -803,10 +803,10 @@ void DatabaseViewer::view3DMap()
 
 							if(maxDepth)
 							{
-								cloud = rtabmap::util3d::passThrough(cloud, "z", 0, maxDepth);
+								cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
 							}
 
-							cloud = rtabmap::util3d::transformPointCloud(cloud, localTransform);
+							cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, localTransform);
 
 							QColor color = Qt::red;
 							int mapId = memory_->getMapId(iter->first);
@@ -929,10 +929,10 @@ void DatabaseViewer::generate3DMap()
 
 									if(maxDepth)
 									{
-										cloud = rtabmap::util3d::passThrough(cloud, "z", 0, maxDepth);
+										cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
 									}
 
-									cloud = rtabmap::util3d::transformPointCloud(cloud, pose*localTransform);
+									cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, pose*localTransform);
 									std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
 									pcl::io::savePCDFile(name, *cloud);
 									UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
@@ -1427,8 +1427,8 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
 					1);
 		}
 
-		cloudA = rtabmap::util3d::removeNaNFromPointCloud(cloudA);
+		cloudA = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudA);
-		cloudA = rtabmap::util3d::transformPointCloud(cloudA, localTransformA);
+		cloudA = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudA, localTransformA);
 
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
 		if(depthB.type() == CV_8UC1)
@@ -1450,14 +1450,14 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
 					1);
 		}
 
-		cloudB = rtabmap::util3d::removeNaNFromPointCloud(cloudB);
+		cloudB = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudB);
-		cloudB = rtabmap::util3d::transformPointCloud(cloudB, t*localTransformB);
+		cloudB = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudB, t*localTransformB);
 
 		//cloud 2d
 		pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
 		scanA = rtabmap::util3d::depth2DToPointCloud(depth2dA);
 		scanB = rtabmap::util3d::depth2DToPointCloud(depth2dB);
-		scanB = rtabmap::util3d::transformPointCloud(scanB, t);
+		scanB = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
 
 		if(cloudA->size())
 		{
@@ -1746,8 +1746,8 @@ void DatabaseViewer::refineConstraint(int from, int to)
 			//voxelize
 			if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f)
 			{
-				oldCloud = util3d::voxelize(oldCloud, ui_->doubleSpinBox_icp_voxel->value());
+				oldCloud = util3d::voxelize<pcl::PointXYZ>(oldCloud, ui_->doubleSpinBox_icp_voxel->value());
-				newCloud = util3d::voxelize(newCloud, ui_->doubleSpinBox_icp_voxel->value());
+				newCloud = util3d::voxelize<pcl::PointXYZ>(newCloud, ui_->doubleSpinBox_icp_voxel->value());
 			}
 
 			if(newCloud->size() && oldCloud->size())
@@ -1794,13 +1794,13 @@ void DatabaseViewer::refineConstraint(int from, int to)
 			pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, ui_->spinBox_icp_normalKSearch->value());
 			pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, ui_->spinBox_icp_normalKSearch->value());
 
-			cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
+			cloudANormals = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(cloudANormals);
 			if(cloudA->size() != cloudANormals->size())
 			{
 				UWARN("removed nan normals...");
 			}
 
-			cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
+			cloudBNormals = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(cloudBNormals);
 			if(cloudB->size() != cloudBNormals->size())
 			{
 				UWARN("removed nan normals...");
@@ -1848,7 +1848,7 @@ void DatabaseViewer::refineConstraint(int from, int to)
 		}
 		if(ui_->dockWidget_constraints->isVisible())
 		{
-			cloudB = util3d::transformPointCloud(cloudB, transform);
+			cloudB = util3d::transformPointCloud<pcl::PointXYZ>(cloudB, transform);
 			this->updateConstraintView(newLink, cloudA, cloudB);
 		}
 	}

guilib/src/LoopClosureViewer.cpp

@@ -166,17 +166,17 @@ void LoopClosureViewer::updateView(const Transform & transform)
 						sA_->getDepthFx(), sA_->getDepthFy(),
 						decimation);
 			}
-
+
 			cloudA = util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudA);
 
 			if(maxDepth>0.0)
-			{
+			{
 				cloudA = util3d::passThrough<pcl::PointXYZRGB>(cloudA, "z", 0, maxDepth);
 			}
 			if(samples>0 && (int)cloudA->size() > samples)
-			{
+			{
 				cloudA = util3d::sampling<pcl::PointXYZRGB>(cloudA, samples);
-			}
+			}
 			cloudA = util3d::transformPointCloud<pcl::PointXYZRGB>(cloudA, sA_->getLocalTransform());
 
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
@@ -198,23 +198,23 @@ void LoopClosureViewer::updateView(const Transform & transform)
 						sB_->getDepthFx(), sB_->getDepthFy(),
 						decimation);
 			}
-
+
 			cloudB = util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudB);
 
 			if(maxDepth>0.0)
-			{
+			{
 				cloudB = util3d::passThrough<pcl::PointXYZRGB>(cloudB, "z", 0, maxDepth);
 			}
 			if(samples>0 && (int)cloudB->size() > samples)
-			{
+			{
 				cloudB = util3d::sampling<pcl::PointXYZRGB>(cloudB, samples);
-			}
+			}
 			cloudB = util3d::transformPointCloud<pcl::PointXYZRGB>(cloudB, t*sB_->getLocalTransform());
 
 			//cloud 2d
 			pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
 			scanA = util3d::depth2DToPointCloud(depth2dA);
-			scanB = util3d::depth2DToPointCloud(depth2dB);
+			scanB = util3d::depth2DToPointCloud(depth2dB);
 			scanB = util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
 
 			ui_->label_idA->setText(QString("[%1 (%2) -> %3 (%4)]").arg(sB_->id()).arg(cloudB->size()).arg(sA_->id()).arg(cloudA->size()));

guilib/src/MainWindow.cpp

@@ -636,7 +636,7 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality,
 		{
 			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
 			cloud = util3d::depth2DToPointCloud(data.depth2d());
-			cloud = util3d::transformPointCloud(cloud, pose);
+			cloud = util3d::transformPointCloud<pcl::PointXYZ>(cloud, pose);
 			if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
 			{
 				UERROR("Adding scanOdom to viewer failed!");
@@ -3178,13 +3178,13 @@ bool MainWindow::getExportedScans(std::map<int, pcl::PointCloud<pcl::PointXYZ>::
 			{
 				if(assemble)
 				{
-					*assembledScans += *util3d::transformPointCloud(scan, iter->second);;
+					*assembledScans += *util3d::transformPointCloud<pcl::PointXYZ>(scan, iter->second);;
 
 					if(count++ % 100 == 0)
 					{
 						if(assembledScans->size() && voxel)
 						{
-							assembledScans = util3d::voxelize(assembledScans, voxel);
+							assembledScans = util3d::voxelize<pcl::PointXYZ>(assembledScans, voxel);
 						}
 					}
 				}
@@ -3211,7 +3211,7 @@ bool MainWindow::getExportedScans(std::map<int, pcl::PointCloud<pcl::PointXYZ>::
 	{
 		if(voxel && assembledScans->size())
 		{
-			assembledScans = util3d::voxelize(assembledScans, voxel);
+			assembledScans = util3d::voxelize<pcl::PointXYZ>(assembledScans, voxel);
 		}
 		if(assembledScans->size())
 		{
@@ -3529,7 +3529,7 @@ void MainWindow::saveClouds(const std::map<int, pcl::PointCloud<pcl::PointXYZRGB
 						if(iter->second->size())
 						{
 							pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud;
-							transformedCloud = util3d::transformPointCloud(iter->second, _currentPosesMap.at(iter->first));
+							transformedCloud = util3d::transformPointCloud<pcl::PointXYZRGB>(iter->second, _currentPosesMap.at(iter->first));
 
 							QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix);
 							bool success =false;
@@ -3630,7 +3630,7 @@ void MainWindow::saveMeshes(const std::map<int, pcl::PolygonMesh::Ptr> & meshes)
 						mesh.polygons = iter->second->polygons;
 						pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
 						pcl::fromPCLPointCloud2(iter->second->cloud, *tmp);
-						tmp = util3d::transformPointCloud(tmp, _currentPosesMap.at(iter->first));
+						tmp = util3d::transformPointCloud<pcl::PointXYZRGB>(tmp, _currentPosesMap.at(iter->first));
 						pcl::toPCLPointCloud2(*tmp, mesh.cloud);
 
 						QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix);
@@ -3736,7 +3736,7 @@ void MainWindow::saveScans(const std::map<int, pcl::PointCloud<pcl::PointXYZ>::P
 						if(iter->second->size())
 						{
 							pcl::PointCloud<pcl::PointXYZ>::Ptr transformedCloud;
-							transformedCloud = util3d::transformPointCloud(iter->second, _currentPosesMap.at(iter->first));
+							transformedCloud = util3d::transformPointCloud<pcl::PointXYZ>(iter->second, _currentPosesMap.at(iter->first));
 
 							QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix);
 							bool success =false;
@@ -3814,24 +3814,24 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createCloud(
 		bool filtered = false;
 		if(cloud->size() && maxDepth)
 		{
-			cloud = util3d::passThrough(cloud, "z", 0, maxDepth);
+			cloud = util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
 			filtered = true;
 		}
 
 		if(cloud->size() && voxelSize)
 		{
-			cloud = util3d::voxelize(cloud, voxelSize);
+			cloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, voxelSize);
 			filtered = true;
 		}
 
 		if(cloud->size() && !filtered)
 		{
-			cloud = util3d::removeNaNFromPointCloud (cloud);
+			cloud = util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloud);
 		}
 
 		if(cloud->size())
 		{
-			cloud = util3d::transformPointCloud(cloud, pose * localTransform);
+			cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, pose * localTransform);
 		}
 	}
 	UDEBUG("Generated cloud %d (pts=%d) time=%fs", id, (int)cloud->size(), timer.ticks());
@@ -3874,7 +3874,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
 				}
 				else if(uContains(_createdClouds, iter->first))
 				{
-					cloud = util3d::transformPointCloud(_createdClouds.at(iter->first), iter->second);
+					cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(_createdClouds.at(iter->first), iter->second);
 				}
 
 				if(cloud->size())
@@ -3903,7 +3903,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
 			{
 				if(assembledCloud->size() && assembledVoxelSize)
 				{
-					assembledCloud = util3d::voxelize(assembledCloud, assembledVoxelSize);
+					assembledCloud = util3d::voxelize<pcl::PointXYZRGB>(assembledCloud, assembledVoxelSize);
 				}
 			}
 		}
@@ -3917,7 +3917,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::getAssembledCloud(
 
 	if(assembledCloud->size() && assembledVoxelSize)
 	{
-		assembledCloud = util3d::voxelize(assembledCloud, assembledVoxelSize);
+		assembledCloud = util3d::voxelize<pcl::PointXYZRGB>(assembledCloud, assembledVoxelSize);
 	}
 
 	return assembledCloud;

guilib/src/OdometryViewer.cpp

@@ -119,10 +119,10 @@ void OdometryViewer::processData()
 
 		if(voxelSize_ > 0.0f)
 		{
-			cloud = util3d::voxelize(cloud, voxelSize_);
+			cloud = util3d::voxelize<pcl::PointXYZRGB>(cloud, voxelSize_);
 		}
 
-		cloud = util3d::transformPointCloud(cloud, data.localTransform());
+		cloud = util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, data.localTransform());
 
 		if(!data.pose().isNull())
 		{