latest update

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -132,6 +132,7 @@ private:
 	bool normalsSegmentation_;
 	bool grid3D_;
 	bool groundIsObstacle_;
+	bool labelUndergroundObstaclesAsGround_;
 	float noiseFilteringRadius_;
 	int noiseFilteringMinNeighbors_;
 	bool scan2dUnknownSpaceFilled_;

corelib/include/rtabmap/core/Parameters.h

@@ -760,6 +760,7 @@ class RTABMAP_CORE_EXPORT Parameters
     RTABMAP_PARAM(Grid, 3D,                      bool,   false,   uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is 0.", kGridSensor().c_str()));
 #endif
     RTABMAP_PARAM(Grid, GroundIsObstacle,           bool,   false,   uFormat("[%s=true] Ground segmentation (%s) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str(), kGridNormalsSegmentation().c_str()));
+    RTABMAP_PARAM(Grid, UndergroundIsGround,        bool,   false,   uFormat("[%s=true] Label all underground points under largest flat surface detected as ground.", kGridNormalsSegmentation().c_str()));
     RTABMAP_PARAM(Grid, NoiseFilteringRadius,       float,   0.0,    "Noise filtering radius (0=disabled). Done after segmentation.");
     RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int,     5,      "Noise filtering minimum neighbors.");
     RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,   bool,    false,  uFormat("Unknown space filled. Only used with 2D laser scans. Use %s to set maximum range if laser scan max range is to set.", kGridRangeMax().c_str()));

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

@@ -44,6 +44,8 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 		pcl::IndicesPtr & obstaclesIndices,
 		pcl::IndicesPtr * flatObstacles) const
 {
+	UDEBUG("cloudIn=%dx%d indicesIn=%ld", cloudIn->width, cloudIn->height, indicesIn->size());
+
 	groundIndices.reset(new std::vector<int>);
 	obstaclesIndices.reset(new std::vector<int>);
 	if(flatObstacles)
@@ -54,6 +56,7 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 	typename pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
 	pcl::IndicesPtr indices(new std::vector<int>);
 
+	UDEBUG("preVoxelFiltering=%d", preVoxelFiltering_?1:0);
 	if(preVoxelFiltering_)
 	{
 		// voxelize to grid cell size
@@ -127,6 +130,9 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 			UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
 			UDEBUG("maxGroundHeight=%f", maxGroundHeight_);
 			UDEBUG("groundNormalsUp=%f", groundNormalsUp_);
+			UDEBUG("labelUndergroundObstaclesAsGround=%d", labelUndergroundObstaclesAsGround_?1:0);
+			UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
+			UDEBUG("cloud=%dx%d indices=%ld", cloud->width, cloud->height, indices->size());
 			util3d::segmentObstaclesFromGround<PointT>(
 					cloud,
 					indices,
@@ -140,8 +146,8 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 					maxGroundHeight_,
 					flatObstacles,
 					Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1),
-					groundNormalsUp_);
+					groundNormalsUp_,
-			UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
+					labelUndergroundObstaclesAsGround_);
 			//UWARN("Saving ground.pcd and obstacles.pcd");
 			//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
 			//pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices);
@@ -165,6 +171,42 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
 
 		UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
 
+		if(!preVoxelFiltering_ && (!groundIndices->empty() || !obstaclesIndices->empty()))
+		{
+			// voxelize to grid cell size
+			typename pcl::PointCloud<PointT>::Ptr cloudWithTransform = cloud;
+			cloud.reset(new pcl::PointCloud<PointT>);
+			if(!groundIndices->empty())
+			{
+				*cloud += *util3d::voxelize(cloudWithTransform, groundIndices, cellSize_);
+				groundIndices->resize(cloud->size());
+				for(size_t i=0; i<groundIndices->size(); ++i)
+				{
+					groundIndices->at(i) = i;
+				}
+			}
+			if(!obstaclesIndices->empty())
+			{
+				int previousSize = cloud->size();
+				*cloud += *util3d::voxelize(cloudWithTransform, obstaclesIndices, cellSize_);
+				obstaclesIndices->resize(cloud->size()-previousSize);
+				for(size_t i=0; i<obstaclesIndices->size(); ++i)
+				{
+					obstaclesIndices->at(i) = previousSize+i;
+				}
+			}
+			if(flatObstacles && !(*flatObstacles)->empty())
+			{
+				int previousSize = cloud->size();
+				*cloud += *util3d::voxelize(cloudWithTransform, *flatObstacles, cellSize_);
+				(*flatObstacles)->resize(cloud->size()-previousSize);
+				for(size_t i=0; i<(*flatObstacles)->size(); ++i)
+				{
+					(*flatObstacles)->at(i) = previousSize+i;
+				}
+			}
+		}
+
 		// Do radius filtering after voxel filtering ( a lot faster)
 		if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
 		{

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

@@ -50,6 +50,79 @@ typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
 	return output;
 }
 
+void clusterIndicesFloodfill(std::vector<int> & cluster,
+	float * visitedIndices,
+	int width,
+	int height,
+	float clusterRadius,
+	int currentIndex,
+	float previousHeight);
+
+/**
+ * @brief Cluster indices of an organized cloud
+ * 
+ * @tparam PointT 
+ * @param cloud 
+ * @param indices 
+ * @param minClusterSize 
+ * @param maxClusterSize 
+ * @param biggestClusterIndex 
+ * @return std::vector<pcl::IndicesPtr> 
+ */
+template<typename PointT>
+std::vector<pcl::IndicesPtr> clusterIndices(
+	const typename pcl::PointCloud<PointT>::Ptr & cloud,
+	const typename pcl::IndicesPtr & indices,
+	float clusterRadius,
+	int minClusterSize,
+	int maxClusterSize,
+	int * biggestClusterIndex)
+{
+	std::vector<pcl::IndicesPtr> clusters;
+	if(cloud->empty())
+	{
+		return clusters;
+	}
+
+	UASSERT(cloud->isOrganized());
+
+	cv::Mat visitedIndices = cv::Mat::zeros(cloud->height, cloud->width, CV_32FC1);
+	float * ptr = visitedIndices.ptr<float>();
+	// init search image
+	for(size_t i = 0; i<indices->size(); ++i)
+	{
+		ptr[indices->at(i)] = cloud->at(indices->at(i)).z;
+	}
+
+	int largestCluster = -1;
+	int largestClusterSize = 0;
+	int sum = 0;
+	for(size_t i = 0; i<indices->size(); ++i)
+	{
+		if(ptr[indices->at(i)] != 0.0f)
+		{
+			pcl::IndicesPtr cluster(new pcl::Indices());
+			clusterIndicesFloodfill(*cluster, ptr, visitedIndices.cols, visitedIndices.rows, clusterRadius, indices->at(i), ptr[indices->at(i)]);
+			if(cluster->size()>0 && (int)cluster->size()>=minClusterSize && (int)cluster->size()<=maxClusterSize)
+			{
+				clusters.push_back(cluster);
+				if((int)cluster->size() > largestClusterSize)
+				{
+					sum+=cluster->size();
+					largestCluster = clusters.size()-1;
+					largestClusterSize = cluster->size();
+				}
+			}
+		}
+	}
+	if(biggestClusterIndex)
+	{
+		*biggestClusterIndex = largestCluster;
+	}
+
+	return clusters;
+}
+
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -64,7 +137,8 @@ void segmentObstaclesFromGround(
 		float maxGroundHeight,
 		pcl::IndicesPtr * flatObstacles,
 		const Eigen::Vector4f & viewPoint,
-		float groundNormalsUp)
+		float groundNormalsUp,
+		bool labelUndergroundObstaclesAsGround)
 {
 	ground.reset(new std::vector<int>);
 	obstacles.reset(new std::vector<int>);
@@ -75,6 +149,8 @@ void segmentObstaclesFromGround(
 
 	if(cloud->size())
 	{
+		UDEBUG("Normal filtering.... cloud=%ld indices=%ld organized=%d",
+			cloud->size(), indices->size(), cloud->isOrganized()?1:0);
 		// Find the ground
 		pcl::IndicesPtr flatSurfaces = normalFiltering(
 				cloud,
@@ -84,22 +160,40 @@ void segmentObstaclesFromGround(
 				normalKSearch,
 				viewPoint,
 				groundNormalsUp);
+		UDEBUG("%ld points on flat surfaces (input indices = %ld, total cloud=%ld)",
+			flatSurfaces->size(), indices->size(), cloud->size());
 
+		Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax(0,0,0,0);
 		if(segmentFlatObstacles && flatSurfaces->size())
 		{
 			int biggestFlatSurfaceIndex;
-			std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(
+
+			std::vector<pcl::IndicesPtr> clusteredFlatSurfaces;
+			if(cloud->isOrganized())
+			{
+				clusteredFlatSurfaces = clusterIndices<PointT>(
 					cloud,
 					flatSurfaces,
 					clusterRadius,
 					minClusterSize,
 					std::numeric_limits<int>::max(),
 					&biggestFlatSurfaceIndex);
+					UDEBUG("clusteredFlatSurfaces=%ld", clusteredFlatSurfaces.size());
+			}
+			else
+			{
+				clusteredFlatSurfaces = extractClusters(
+					cloud,
+					flatSurfaces,
+					clusterRadius,
+					minClusterSize,
+					std::numeric_limits<int>::max(),
+					&biggestFlatSurfaceIndex);
+			}
 
 			// cluster all surfaces for which the centroid is in the Z-range of the bigger surface
 			if(clusteredFlatSurfaces.size())
 			{
-				Eigen::Vector4f biggestSurfaceMin,biggestSurfaceMax;
 				if(maxGroundHeight != 0.0f)
 				{
 					// Search for biggest surface under max ground height
@@ -125,9 +219,12 @@ void segmentObstaclesFromGround(
 				if(biggestFlatSurfaceIndex>=0)
 				{
 					ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
+					UDEBUG("Biggest flat surface size = %ld (%d%%) (z min=%f max=%f)",
+						ground->size(), 100*ground->size()/cloud->size(), biggestSurfaceMin[2], biggestSurfaceMax[2]);
 				}
 
-				if(!ground->empty() && (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight))
+				if(!ground->empty() && 
+				  (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight))
 				{
 					for(unsigned int i=0; i<clusteredFlatSurfaces.size(); ++i)
 					{
@@ -135,7 +232,7 @@ void segmentObstaclesFromGround(
 						{
 							Eigen::Vector4f centroid(0,0,0,1);
 							pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
-							if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight || centroid[2] <= biggestSurfaceMax[2]) // epsilon
+							if(centroid[2] <= biggestSurfaceMax[2]) // relative to ground detected
 							{
 								ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
 							}
@@ -145,9 +242,46 @@ void segmentObstaclesFromGround(
 							}
 						}
 					}
+
+					int groundRatio = 100*ground->size()/cloud->size();
+					int minGroundRatio = 10;
+					if(minGroundRatio != 0 && groundRatio<minGroundRatio)
+					{
+						if(labelUndergroundObstaclesAsGround && maxGroundHeight!=0.0f)
+						{
+							// just do passthrough (e.g. reflective floor)
+							UWARN("Failed normal segmentation (ground ratio=%d%%, ground height=%f), fallback to passThrough (label underground as ground is true).",
+								groundRatio, !ground->empty()?biggestSurfaceMin[2]:0.0f);
+							// passthrough filter
+							ground = rtabmap::util3d::passThrough(cloud, indices, "z",
+									std::numeric_limits<int>::min(),
+									maxGroundHeight!=0.0f?maxGroundHeight:std::numeric_limits<int>::max());
+
+							pcl::IndicesPtr notObstacles = ground;
+							if(indices->size())
+							{
+								notObstacles = util3d::extractIndices(cloud, indices, true);
+								notObstacles = util3d::concatenate(notObstacles, ground);
+							}
+							obstacles = rtabmap::util3d::extractIndices(cloud, notObstacles, true);
+							return;
 						}
 						else
 						{
+							UWARN("Failed normal segmentation, ground surface is too small (ground ratio=%d%%, ground height=%f)!",
+								groundRatio, !ground->empty()?biggestSurfaceMin[2]:0.0f);
+							// reject ground!
+							ground.reset(new std::vector<int>);
+							if(flatObstacles)
+							{
+								*flatObstacles = flatSurfaces;
+							}
+						}
+					}
+				}
+				else
+				{
+					UWARN("Failed normal segmentation, could not detect the ground!");
 					// reject ground!
 					ground.reset(new std::vector<int>);
 					if(flatObstacles)
@@ -168,19 +302,35 @@ void segmentObstaclesFromGround(
 			pcl::IndicesPtr notObstacles = ground;
 			if(indices->size())
 			{
+				// This will ignore all points not in input indices for obstacles.
 				notObstacles = util3d::extractIndices(cloud, indices, true);
 				notObstacles = util3d::concatenate(notObstacles, ground);
 			}
-			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
 
-			// If ground height is set, remove obstacles under it
+			// If ground height is set and if we label obstacles under it as ground
-			if(maxGroundHeight != 0.0f)
+			if(labelUndergroundObstaclesAsGround)
 			{
-				otherStuffIndices = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", maxGroundHeight, std::numeric_limits<float>::max());
+				float max = biggestSurfaceMax[2];
+				if(maxGroundHeight > 0)
+				{
+					max += maxGroundHeight;
 				}
 
+				pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
+				pcl::IndicesPtr underground = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", (float)std::numeric_limits<int>::min(), max);
+				if(!underground->empty())
+				{
+					ground = util3d::concatenate(ground, underground);
+					notObstacles = util3d::concatenate(underground, notObstacles);
+				}
+			}
+
+			pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
+
 			//Cluster remaining stuff (obstacles)
 			if(otherStuffIndices->size())
+			{
+				if(minClusterSize>1)
 				{
 					std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
 							cloud,
@@ -191,6 +341,11 @@ void segmentObstaclesFromGround(
 					// merge indices
 					obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
 				}
+				else
+				{
+					obstacles = otherStuffIndices;
+				}
+			}
 		}
 	}
 }
@@ -208,7 +363,8 @@ void segmentObstaclesFromGround(
 		float maxGroundHeight,
 		pcl::IndicesPtr * flatObstacles,
 		const Eigen::Vector4f & viewPoint,
-		float groundNormalsUp)
+		float groundNormalsUp,
+		bool labelUndergroundObstaclesAsGround)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
 	segmentObstaclesFromGround<PointT>(
@@ -224,7 +380,8 @@ void segmentObstaclesFromGround(
 			maxGroundHeight,
 			flatObstacles,
 			viewPoint,
-			groundNormalsUp);
+			groundNormalsUp,
+			labelUndergroundObstaclesAsGround);
 }
 
 template<typename PointT>

corelib/include/rtabmap/core/util3d_mapping.h

@@ -157,7 +157,8 @@ void segmentObstaclesFromGround(
 		float maxGroundHeight = 0.0f,
 		pcl::IndicesPtr * flatObstacles = 0,
 		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
-		float groundNormalsUp = 0);
+		float groundNormalsUp = 0,
+		bool labelUndergroundObstaclesAsGround = false);
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -171,7 +172,8 @@ void segmentObstaclesFromGround(
 		float maxGroundHeight = 0.0f,
 		pcl::IndicesPtr * flatObstacles = 0,
 		const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
-		float groundNormalsUp = 0);
+		float groundNormalsUp = 0,
+		bool labelUndergroundObstaclesAsGround = false);
 
 template<typename PointT>
 void occupancy2DFromGroundObstacles(

corelib/include/rtabmap/core/util3d_surface.h

@@ -381,6 +381,17 @@ pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormal
 		float searchRadius = 0.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float maxDepthChangeFactor = 0.02f,
+		float normalSmoothingSize = 10.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float maxDepthChangeFactor = 0.02f,
+		float normalSmoothingSize = 10.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float maxDepthChangeFactor = 0.02f,

corelib/src/OccupancyGrid.cpp

@@ -66,6 +66,7 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	normalsSegmentation_(Parameters::defaultGridNormalsSegmentation()),
 	grid3D_(Parameters::defaultGrid3D()),
 	groundIsObstacle_(Parameters::defaultGridGroundIsObstacle()),
+	labelUndergroundObstaclesAsGround_(Parameters::defaultGridUndergroundIsGround()),
 	noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
 	noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
 	scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
@@ -128,6 +129,7 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridNormalsSegmentation(), normalsSegmentation_);
 	Parameters::parse(parameters, Parameters::kGrid3D(), grid3D_);
 	Parameters::parse(parameters, Parameters::kGridGroundIsObstacle(), groundIsObstacle_);
+	Parameters::parse(parameters, Parameters::kGridUndergroundIsGround(), labelUndergroundObstaclesAsGround_);
 	Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_);
 	Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
 	Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);

corelib/src/util3d.cpp

@@ -372,34 +372,127 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 	float depthCx = model.cx() * rgbToDepthFactorX;
 	float depthCy = model.cy() * rgbToDepthFactorY;
 
-	UDEBUG("depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
+	bool isMM = imageDepth.type() == CV_16UC1;
+
+	UDEBUG("depth=%dx%d (isMM=%d) fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
 			imageDepth.cols, imageDepth.rows,
+			isMM?1:0,
 			model.fx(), model.fy(), model.cx(), model.cy(),
 			rgbToDepthFactorX,
 			rgbToDepthFactorY,
 			decimation);
 
+	int decimationMode = 1;
 	int oi = 0;
+	if(isMM)
+	{
 		for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
 		{
+			const unsigned short * rowPtr = imageDepth.ptr<unsigned short>(h);
 			for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
 			{
 				pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
 
-			pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
+				pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
-			if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
+
+				if(decimationMode == 1 && decimation>1)
 				{
-				pt.x = ptXYZ.x;
+					// project closest point
-				pt.y = ptXYZ.y;
+					cv::Point2i closestPixel(w,h);
-				pt.z = ptXYZ.z;
+					unsigned short closestDepthMM = 0;
-				if(validIndices)
+					for(int v = h; v < h + decimation; ++v)
+					{
+						const unsigned short * roiRowPtr = imageDepth.ptr<unsigned short>(v);
+						for(int u = w; u < w + decimation; ++u)
+						{
+							const unsigned short & depthMM = roiRowPtr[u];
+							if(depthMM > 0 && (depthMM < closestDepthMM || closestDepthMM == 0))
+							{
+								closestDepthMM = depthMM;
+								closestPixel.x = u;
+								closestPixel.y = v;
+							}
+						}
+					}
+					if(closestDepthMM > 0)
+					{
+						float depth = ((float)closestDepthMM)/1000.0f;
+						if(depth>=minDepth && (maxDepth<=0.0f || depth <= maxDepth))
+						{
+							// Fill in XYZ
+							pt.z = depth;
+							pt.x = ((float)closestPixel.x - depthCx) * pt.z / depthFx;
+							pt.y = ((float)closestPixel.y - depthCy) * pt.z / depthFy;
+						}
+					}
+				}
+				else if(rowPtr[w]>0)
+				{
+					float depth = ((float)rowPtr[w])/1000.0f;
+					if(depth>=minDepth && (maxDepth<=0.0f || depth <= maxDepth))
+					{
+						// Fill in XYZ
+						pt.z = depth;
+						pt.x = ((float)w - depthCx) * pt.z / depthFx;
+						pt.y = ((float)h - depthCy) * pt.z / depthFy;
+					}
+				}
+				if(pcl::isFinite(pt) && validIndices)
 				{
 					validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
 				}
 			}
+		}
+	}
 	else
 	{
+		for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
+		{
+			const float * rowPtr = imageDepth.ptr<float>(h);
+			for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
+			{
+				pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
+
 				pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
+
+				if(decimationMode == 1 && decimation>1)
+				{
+					// project closest point
+					cv::Point2i closestPixel(w,h);
+					float closestDepth = 0.0f;
+					for(int v = h; v < h + decimation; ++v)
+					{
+						const float * roiRowPtr =  imageDepth.ptr<float>(v);
+						for(int u = w; u < w + decimation; ++u)
+						{
+							const float & depth = roiRowPtr[u];
+							if(depth > 0.0f && (depth < closestDepth || closestDepth == 0.0f))
+							{
+								closestDepth = depth;
+								closestPixel.x = u;
+								closestPixel.y = v;
+							}
+						}
+					}
+					if(closestDepth > 0.0f && closestDepth>=minDepth && (maxDepth<=0.0f || closestDepth <= maxDepth))
+					{
+						// Fill in XYZ
+						pt.z = closestDepth;
+						pt.x = ((float)closestPixel.x - depthCx) * pt.z / depthFx;
+						pt.y = ((float)closestPixel.y - depthCy) * pt.z / depthFy;
+					}
+				}
+				else if(rowPtr[w] > 0 && rowPtr[w]>=minDepth && (maxDepth<=0.0f || rowPtr[w] <= maxDepth))
+				{
+					// Fill in XYZ
+					pt.z = rowPtr[w];
+					pt.x = ((float)w - depthCx) * pt.z / depthFx;
+					pt.y = ((float)h - depthCy) * pt.z / depthFy;
+				}
+				if(pcl::isFinite(pt) && validIndices)
+				{
+					validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
+				}
 			}
 		}
 	}

corelib/src/util3d_mapping.cpp

@@ -1026,6 +1026,36 @@ cv::Mat erodeMap(const cv::Mat & map)
 	return erodedMap;
 }
 
+void clusterIndicesFloodfill(std::vector<int> & cluster,
+	float * visitedIndices,
+	int width,
+	int height,
+	float clusterRadius,
+	int currentIndex,
+	float previousHeight)
+{
+	if(visitedIndices[currentIndex] == 0.0f || 
+		(clusterRadius>0.0f && fabs(visitedIndices[currentIndex]-previousHeight)>clusterRadius))
+	{
+		return;
+	}
+
+	int y = currentIndex / width;
+	int x = currentIndex - y*width;
+
+	if(x>=0 && x<width && y>=0 && y<height)
+	{
+		cluster.push_back(currentIndex);
+		float currentHeight = visitedIndices[currentIndex];
+		visitedIndices[currentIndex] = 0;
+
+		clusterIndicesFloodfill(cluster, visitedIndices, width, height, clusterRadius, (y+1)*width + x, currentHeight);
+		clusterIndicesFloodfill(cluster, visitedIndices, width, height, clusterRadius, (y-1)*width + x, currentHeight);
+		clusterIndicesFloodfill(cluster, visitedIndices, width, height, clusterRadius, y*width + x+1, currentHeight);
+		clusterIndicesFloodfill(cluster, visitedIndices, width, height, clusterRadius, y*width + x-1, currentHeight);
+	}
+}
+
 }
 
 }

corelib/src/util3d_surface.cpp

@@ -3033,6 +3033,65 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
 	return computeFastOrganizedNormals2DImpl<pcl::PointXYZI>(cloud, searchK, searchRadius, viewPoint);
 }
 
+template<typename PointT>
+pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormalsImpl(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float maxDepthChangeFactor,
+		float normalSmoothingSize,
+		const Eigen::Vector3f & viewPoint)
+{
+	UASSERT(cloud->isOrganized());
+
+	// Normal estimation
+	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+	pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
+#if PCL_VERSION_COMPARE(<, 1, 7, 0)
+	ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
+	ne.setBorderPolicy(ne.BORDER_POLICY_MIRROR);
+#else
+	ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT);
+	ne.setBorderPolicy(ne.BORDER_POLICY_IGNORE);
+#endif
+	ne.setMaxDepthChangeFactor(maxDepthChangeFactor);
+	ne.setNormalSmoothingSize(normalSmoothingSize);
+	ne.setInputCloud(cloud);
+	// Commented: Keep the output normals size the same as the input cloud
+	//if(indices->size())
+	//{
+	//	ne.setIndices(indices);
+	//}
+
+	// create kdtree search tree (not used by IntegralImageNormalEstimation) to avoid 
+	// "[pcl::OrganizedNeighbor::radiusSearch] Input dataset is not from a projective device!"
+	// on clouds smaller than regular organized clouds from camera (640x480)
+	typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
+	ne.setSearchMethod(tree);
+
+	ne.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
+	ne.compute(*normals);
+
+	return normals;
+}
+pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		float maxDepthChangeFactor,
+		float normalSmoothingSize,
+		const Eigen::Vector3f & viewPoint)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	return computeFastOrganizedNormals(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint);
+}
+pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		float maxDepthChangeFactor,
+		float normalSmoothingSize,
+		const Eigen::Vector3f & viewPoint)
+{
+	return computeFastOrganizedNormalsImpl<pcl::PointXYZ>(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint);
+}
+
 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float maxDepthChangeFactor,
@@ -3049,36 +3108,7 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 		float normalSmoothingSize,
 		const Eigen::Vector3f & viewPoint)
 {
-	UASSERT(cloud->isOrganized());
+	return computeFastOrganizedNormalsImpl<pcl::PointXYZRGB>(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint);
-
-	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
-	if(indices->size())
-	{
-		tree->setInputCloud(cloud, indices);
-	}
-	else
-	{
-		tree->setInputCloud (cloud);
-	}
-
-	// Normal estimation
-	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
-	pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
-	ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
-	ne.setMaxDepthChangeFactor(maxDepthChangeFactor);
-	ne.setNormalSmoothingSize(normalSmoothingSize);
-	ne.setBorderPolicy(ne.BORDER_POLICY_MIRROR);
-	ne.setInputCloud(cloud);
-	// Commented: Keep the output normals size the same as the input cloud
-	//if(indices->size())
-	//{
-	//	ne.setIndices(indices);
-	//}
-	ne.setSearchMethod(tree);
-	ne.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
-	ne.compute(*normals);
-
-	return normals;
 }
 
 float computeNormalsComplexity(