Added LocalGridMaker tests

corelib/include/rtabmap/core/LocalGridMaker.h

@@ -39,17 +39,61 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class LocalGridMaker
+ * @brief Builds per-node local occupancy grids from laser scans or depth clouds.
+ *
+ * Configured via `Grid/` parameters (@ref parseParameters()). Used by @ref Memory
+ * to populate @ref LocalGrid cells (ground / obstacles / empty) stored on
+ * @ref SensorData and cached in @ref LocalGridCache for global maps
+ * (@ref OccupancyGrid, @ref OctoMap, @ref GridMap).
+ *
+ * **Sensor source** (`Grid/Sensor`, @ref isGridFromDepth()):
+ * - `0` — laser scan only
+ * - `1` — depth image(s) only (default)
+ * - `2` — laser scan and depth
+ *
+ * **2D laser path:** projects a 2D @ref LaserScan with @ref util3d::occupancy2DFromLaserScan().
+ *
+ * **3D path:** segments the scan/cloud into ground and obstacles (@ref segmentCloud()),
+ * then outputs @ref LocalGrid cell matrices (2D projection or 3D cells per `Grid/3D`).
+ *
+ * @see LocalGrid
+ * @see Memory
+ */
 class RTABMAP_CORE_EXPORT LocalGridMaker
 {
 public:
+	/** @brief Constructs with @ref parseParameters() on @p parameters (or defaults). */
 	LocalGridMaker(const ParametersMap & parameters = ParametersMap());
 	virtual ~LocalGridMaker();
+
+	/** @brief Updates grid settings from `Grid/` entries in @p parameters. */
 	virtual void parseParameters(const ParametersMap & parameters);
 
+	/** @return Current `Grid/CellSize` (m). */
 	float getCellSize() const {return cellSize_;}
+	/** @return True if occupancy is built from depth (`Grid/Sensor` is 1 or 2). */
 	bool isGridFromDepth() const {return occupancySensor_;}
+	/** @return True if `Grid/MapFrameProjection` is enabled. */
 	bool isMapFrameProjection() const {return projMapFrame_;}
 
+	/**
+	 * @brief Segments a point cloud into ground and obstacle indices.
+	 *
+	 * Applies optional voxel filtering (`Grid/PreVoxelFiltering`, leaf size `Grid/CellSize`),
+	 * footprint crop, height filtering, then either normal-based ground segmentation
+	 * (`Grid/NormalsSegmentation`) or Z passthrough.
+	 *
+	 * @param cloud Input cloud (sensor frame, transformed internally using @p pose). Must be non-null; an empty cloud is allowed and returns empty outputs.
+	 * @param indices Subset of @p cloud to process (must be non-null; empty = all points when the cloud is dense).
+	 * @param pose Node pose (used for map-frame projection and footprint).
+	 * @param viewPoint Sensor origin for segmentation / ray tracing.
+	 * @param groundIndices Output indices of ground points.
+	 * @param obstaclesIndices Output indices of obstacle points.
+	 * @param flatObstacles Optional output for flat obstacle clusters.
+	 * @return Segmented cloud (voxel-downsampled when `Grid/PreVoxelFiltering` is true, using `Grid/CellSize`).
+	 */
 	template<typename PointT>
 	typename pcl::PointCloud<PointT>::Ptr segmentCloud(
 			const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -60,6 +104,22 @@ public:
 			pcl::IndicesPtr & obstaclesIndices,     // output cloud indices
 			pcl::IndicesPtr * flatObstacles = 0) const; // output cloud indices
 
+	/**
+	 * @brief Creates a local grid from a @ref Signature's laser scan or depth data.
+	 * @param node Signature with sensor data (pose used for 3D scans).
+	 * @param groundCells Output ground cells (@ref LocalGrid format).
+	 * @param obstacleCells Output obstacle cells.
+	 * @param emptyCells Output empty/free cells. Behavior depends on the sensor path:
+	 *                   - **2D laser, laser-only mode** (`Grid/Sensor`=0 and a 2D @ref LaserScan):
+	 *                     uses @ref util3d::occupancy2DFromLaserScan() / @ref util3d::create2DMap().
+	 *                     Free space along each hit beam is always ray-traced (sensor → obstacle).
+	 *                     `Grid/Scan2dUnknownSpaceFilled` is separate: when true, it additionally
+	 *                     sweeps unknown angular gaps between the first and last hit (sparse FOV /
+	 *                     “holes” in coverage) out to `Grid/RangeMax` or scan max range.
+	 *                   - **Other cases** (3D scan, depth, `Grid/Sensor`=2, etc.): see
+	 *                     @ref createLocalMap(const LaserScan&,...).
+	 * @param viewPoint Output view point used for the grid.
+	 */
 	void createLocalMap(
 			const Signature & node,
 			cv::Mat & groundCells,
@@ -67,6 +127,19 @@ public:
 			cv::Mat & emptyCells,
 			cv::Point3f & viewPoint);
 
+	/**
+	 * @brief Creates a local grid from a 3D (or organized) @ref LaserScan.
+	 * @param cloud Input scan in sensor frame.
+	 * @param pose Node pose in map/odom frame.
+	 * @param groundCells Output ground cells.
+	 * @param obstacleCells Output obstacle cells.
+	 * @param emptyCells Output empty/free cells. Filled when `Grid/RayTracing` is true:
+	 *                   3D via OctoMap if `Grid/3D` and OctoMap support are enabled
+	 *                   (`Grid/RangeMax`, `Grid/CellSize`); otherwise 2D ray fill via
+	 *                   `occupancy2DFromLaserScan` when `Grid/3D` is false (`Grid/RangeMax`,
+	 *                   `Grid/CellSize`).
+	 * @param viewPointInOut View point (may be rotated if @ref isMapFrameProjection()).
+	 */
 	void createLocalMap(
 			const LaserScan & cloud,
 			const Transform & pose,
@@ -112,4 +185,4 @@ protected:
 
 #include <rtabmap/core/impl/LocalMapMaker.hpp>
 
-#endif /* SRC_MAP_H_ */
+#endif /* SRC_LOCAL_MAP_H_ */

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

@@ -51,6 +51,13 @@ typename pcl::PointCloud<PointT>::Ptr LocalGridMaker::segmentCloud(
 		flatObstacles->reset(new std::vector<int>);
 	}
 
+	UASSERT(cloudIn.get());
+	UASSERT(indicesIn.get());
+	if(cloudIn->empty())
+	{
+		return cloudIn;
+	}
+
 	typename pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
 	pcl::IndicesPtr indices(new std::vector<int>);
 
@@ -82,6 +89,11 @@ typename pcl::PointCloud<PointT>::Ptr LocalGridMaker::segmentCloud(
 		}
 	}
 
+	if(cloud->empty())
+	{
+		return cloud;
+	}
+
 	// add pose rotation without yaw
 	float roll, pitch, yaw;
 	pose.getEulerAngles(roll, pitch, yaw);

corelib/test/CMakeLists.txt

@@ -131,6 +131,11 @@ add_executable(test_localgrid test_localgrid.cpp)
 target_link_libraries(test_localgrid gtest_main rtabmap_core)
 add_test(NAME test_localgrid COMMAND test_localgrid)
 
+#LocalGridMaker.h
+add_executable(test_localgridmaker test_localgridmaker.cpp)
+target_link_libraries(test_localgridmaker gtest_main rtabmap_core)
+add_test(NAME test_localgridmaker COMMAND test_localgridmaker)
+
 #LASWriter.h (optional libLAS)
 IF(libLAS_FOUND)
 	add_executable(test_laswriter test_laswriter.cpp)