OccupancyGrid: added Grid/Footprint*** parameters, templated segmentCloud() function, added Grid/ClusterRadius parameter. Parameters: fixed getDefaultParameters(group) function to correctly compare groups.

2026-09-02 17:40:23 +08:00 · 2016-08-30 18:22:29 -04:00
parent 6fe7d5a856
commit c404234635
10 changed files with 522 additions and 114 deletions
--- a/corelib/include/rtabmap/core/impl/OccupancyGrid.hpp
+++ b/corelib/include/rtabmap/core/impl/OccupancyGrid.hpp
@@ -0,0 +1,162 @@
+/*
+Copyright (c) 2010-2016, 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 CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_
+#define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_
+
+#include <rtabmap/core/util3d_mapping.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
+		const typename pcl::PointCloud<PointT>::Ptr & cloudIn,
+		const pcl::IndicesPtr & indicesIn,
+		const Transform & pose,
+		const cv::Point3f & viewPoint,
+		pcl::IndicesPtr & groundIndices,
+		pcl::IndicesPtr & obstaclesIndices,
+		pcl::IndicesPtr * flatObstacles) const
+{
+	typename pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
+
+	// voxelize to grid cell size
+	cloud = util3d::voxelize(cloudIn, indicesIn, cellSize_);
+	pcl::IndicesPtr indices(new std::vector<int>);
+	indices->resize(cloud->size());
+	for(unsigned int i=0; i<indices->size(); ++i)
+	{
+		indices->at(i) = i;
+	}
+
+	// add pose rotation without yaw
+	float roll, pitch, yaw;
+	pose.getEulerAngles(roll, pitch, yaw);
+	UDEBUG("node.getPose()=%s projMapFrame_=%d", pose.prettyPrint().c_str(), projMapFrame_?1:0);
+	cloud = util3d::transformPointCloud(cloud, Transform(0,0, projMapFrame_?pose.z():0, roll, pitch, 0));
+
+	// filter footprint
+	if(footprintLength_ > 0.0f || footprintWidth_ > 0.0f || footprintHeight_ > 0.0f)
+	{
+		indices = util3d::cropBox(
+				cloud,
+				indices,
+				Eigen::Vector4f(
+						footprintLength_>0.0f?-footprintLength_/2.0f:std::numeric_limits<int>::min(),
+						footprintWidth_>0.0f&&footprintLength_>0.0f?-footprintWidth_/2.0f:std::numeric_limits<int>::min(),
+						0,
+						1),
+				Eigen::Vector4f(
+						footprintLength_>0.0f?footprintLength_/2.0f:std::numeric_limits<int>::max(),
+						footprintWidth_>0.0f&&footprintLength_>0.0f?footprintWidth_/2.0f:std::numeric_limits<int>::max(),
+						footprintHeight_>0.0f&&footprintLength_>0.0f&&footprintWidth_>0.0f?footprintHeight_:std::numeric_limits<int>::max(),
+						1),
+				Transform::getIdentity(),
+				true);
+	}
+
+	// filter ground/obstacles zone
+	if(minGroundHeight_ != 0.0f || maxObstacleHeight_ > 0.0f)
+	{
+		indices = util3d::passThrough(cloud, indices, "z",
+				minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(),
+				maxObstacleHeight_>0.0f?maxObstacleHeight_:std::numeric_limits<int>::max());
+	}
+
+	if(indices->size())
+	{
+		if(normalsSegmentation_)
+		{
+			UDEBUG("normalKSearch=%d", normalKSearch_);
+			UDEBUG("maxGroundAngle=%f", maxGroundAngle_);
+			UDEBUG("Cluster radius=%f", clusterRadius_);
+			UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
+			UDEBUG("maxGroundHeight=%f", maxGroundHeight_?1:0);
+			util3d::segmentObstaclesFromGround<PointT>(
+					cloud,
+					indices,
+					groundIndices,
+					obstaclesIndices,
+					normalKSearch_,
+					maxGroundAngle_,
+					clusterRadius_,
+					minClusterSize_,
+					flatObstaclesDetected_,
+					maxGroundHeight_,
+					flatObstacles,
+					Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1));
+			UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
+			//UWARN("Saving ground.pcd and obstacles.pcd");
+			//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
+			//pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices);
+		}
+		else
+		{
+			UDEBUG("");
+			// passthrough filter
+			groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z", minGroundHeight_<0.0f?minGroundHeight_:std::numeric_limits<int>::min(), maxGroundHeight_);
+			obstaclesIndices = rtabmap::util3d::extractIndices(cloud, groundIndices, true);
+		}
+
+		UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
+
+		// Do radius filtering after voxel filtering ( a lot faster)
+		if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
+		{
+			UDEBUG("");
+			if(groundIndices->size())
+			{
+				groundIndices = rtabmap::util3d::radiusFiltering(cloud, groundIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
+			}
+			if(obstaclesIndices->size())
+			{
+				obstaclesIndices = rtabmap::util3d::radiusFiltering(cloud, obstaclesIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
+			}
+			if(flatObstacles && (*flatObstacles)->size())
+			{
+				*flatObstacles = rtabmap::util3d::radiusFiltering(cloud, *flatObstacles, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
+			}
+
+			if(groundIndices->empty() && obstaclesIndices->empty())
+			{
+				UWARN("Cloud (with %d points) is empty after noise "
+						"filtering. Occupancy grid cannot be "
+						"created.",
+						(int)cloud->size());
+
+			}
+		}
+	}
+	return cloud;
+}
+
+}
+
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMPL_OCCUPANCYGRID_HPP_ */