Added LocalGrid tests

corelib/include/rtabmap/core/LocalGrid.h

@@ -35,30 +35,72 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class LocalGrid
+ * @brief Local occupancy grid cells for one map node (ground, obstacles, empty).
+ *
+ * Each layer is stored as a 1×N `cv::Mat` of cell coordinates in the robot/base
+ * frame. Layouts match @ref LaserScan grid storage (@ref LaserScan::Format):
+ * - `CV_32FC2` (2 ch): `kXY` — (x, y)
+ * - `CV_32FC3` (3 ch): `kXYZ` or `kXYI` — (x, y, z) or (x, y, intensity)
+ * - `CV_32FC4` (4 ch): `kXYZI` or `kXYZRGB` — (x, y, z, intensity) or (x, y, z, RGB)
+ * - `CV_32FC5` (5 ch): `kXYNormal` or `kXYZIT` — 2D (x, y, nx, ny, nz) or 3D (x, y, z, intensity, time)
+ * - `CV_32FC6` (6 ch): `kXYINormal`, `kXYZNormal` or `kXYZIRT`
+ * - `CV_32FC7` (7 ch): `kXYZINormal` or `kXYZRGBNormal`
+ *
+ * @ref is3D() is true when every non-empty layer is `CV_32FC3`, `CV_32FC4` or `CV_32FC6`
+ * (5-channel layers are treated as 2D by @ref OccupancyGrid). Extra fields are not used for pose/projection.
+ * @ref cellSize is the grid resolution in meters (must be > 0).
+ * @ref viewPoint is the sensor/view origin used when the grid was built.
+ *
+ * @see LaserScan
+ * @see LocalGridMaker
+ * @see OccupancyGrid
+ * @see LocalGridCache
+ */
 class RTABMAP_CORE_EXPORT LocalGrid
 {
 public:
+	/**
+	 * @brief Builds a local grid from cell matrices.
+	 * @param ground Ground cell coordinates (may be empty).
+	 * @param obstacles Obstacle cell coordinates (may be empty).
+	 * @param empty Empty/free cell coordinates (may be empty).
+	 * @param cellSize Grid cell size in meters (must be > 0).
+	 * @param viewPoint View/sensor origin in the grid frame.
+	 */
 	LocalGrid(const cv::Mat & ground,
 		 const cv::Mat & obstacles,
 		 const cv::Mat & empty,
 		 float cellSize,
 		 const cv::Point3f & viewPoint = cv::Point3f(0,0,0));
 	virtual ~LocalGrid() {}
+
+	/** @return True if every non-empty layer is `CV_32FC3`, `CV_32FC4` or `CV_32FC6` (see class doc). */
 	bool is3D() const;
-public:
-	cv::Mat groundCells;
-	cv::Mat obstacleCells;
-	cv::Mat emptyCells;
-	float cellSize;
-	cv::Point3f viewPoint;
+
+	cv::Mat groundCells;   ///< Ground cells (1×N, `CV_32FC2`–`CV_32FC7`, see class doc).
+	cv::Mat obstacleCells; ///< Obstacle cells (same layout as @ref groundCells).
+	cv::Mat emptyCells;    ///< Empty/free cells (same layout as @ref groundCells).
+	float cellSize;        ///< Cell size in meters.
+	cv::Point3f viewPoint; ///< View point used to build the grid.
 };
 
+/**
+ * @class LocalGridCache
+ * @brief Cache of @ref LocalGrid entries keyed by map node id.
+ *
+ * Used by global mapping (e.g. @ref OccupancyGrid) to hold per-node local grids
+ * before assembly. Node id `0` is stored as `-1` (temporary grid). Negative ids
+ * passed to @ref add() are rejected.
+ */
 class RTABMAP_CORE_EXPORT LocalGridCache
 {
 public:
 	LocalGridCache() {}
 	virtual ~LocalGridCache() {}
 
+	/** @brief Inserts or replaces the grid for @p nodeId (from separate cell mats). */
 	void add(int nodeId,
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
@@ -66,11 +108,22 @@ public:
 			float cellSize,
 			const cv::Point3f & viewPoint = cv::Point3f(0,0,0));
 
+	/** @brief Inserts or replaces the grid for @p nodeId. */
 	void add(int nodeId, const LocalGrid & localGrid);
 
+	/**
+	 * @brief Copies a grid to @p anotherCache if present here and absent there.
+	 * @return True if a grid was shared.
+	 */
 	bool shareTo(int nodeId, LocalGridCache & anotherCache) const;
 
+	/** @brief Approximate memory used by cached grids (bytes). */
 	unsigned long getMemoryUsed() const;
+
+	/**
+	 * @brief Removes cached grids.
+	 * @param temporaryOnly If true, removes only entries with negative ids (e.g. `-1`).
+	 */
 	void clear(bool temporaryOnly = false);
 
 	size_t size() const {return localGrids_.size();}

corelib/test/CMakeLists.txt

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

corelib/test/test_localgrid.cpp

@@ -0,0 +1,152 @@
+#include <gtest/gtest.h>
+#include <rtabmap/core/LocalGrid.h>
+#include <rtabmap/utilite/UException.h>
+#include <opencv2/core.hpp>
+
+using namespace rtabmap;
+
+namespace {
+
+static cv::Mat grid2D(int cols)
+{
+	return cv::Mat(1, cols, CV_32FC2, cv::Scalar(1.f, 2.f));
+}
+
+static cv::Mat grid3D(int cols)
+{
+	return cv::Mat(1, cols, CV_32FC3, cv::Scalar(1.f, 2.f, 3.f));
+}
+
+} // namespace
+
+TEST(LocalGridTest, ConstructorStoresFields)
+{
+	const cv::Mat ground = grid2D(2);
+	const cv::Mat obstacles = grid2D(1);
+	const cv::Mat empty = grid2D(3);
+	const cv::Point3f viewPoint(1.f, 2.f, 3.f);
+
+	const LocalGrid grid(ground, obstacles, empty, 0.05f, viewPoint);
+
+	EXPECT_EQ(grid.groundCells.rows, 1);
+	EXPECT_EQ(grid.groundCells.cols, 2);
+	EXPECT_EQ(grid.obstacleCells.cols, 1);
+	EXPECT_EQ(grid.emptyCells.cols, 3);
+	EXPECT_FLOAT_EQ(grid.cellSize, 0.05f);
+	EXPECT_FLOAT_EQ(grid.viewPoint.x, 1.f);
+	EXPECT_FLOAT_EQ(grid.viewPoint.y, 2.f);
+	EXPECT_FLOAT_EQ(grid.viewPoint.z, 3.f);
+	EXPECT_FALSE(grid.is3D());
+}
+
+TEST(LocalGridTest, InvalidCellSizeThrows)
+{
+	const cv::Mat ground = grid2D(1);
+	ASSERT_THROW(LocalGrid(ground, cv::Mat(), cv::Mat(), 0.f), UException);
+}
+
+TEST(LocalGridTest, Is3DEmptyCells)
+{
+	const LocalGrid grid(cv::Mat(), cv::Mat(), cv::Mat(), 0.1f);
+	EXPECT_TRUE(grid.is3D());
+}
+
+TEST(LocalGridTest, Is3DDetects2DAnd3DCellTypes)
+{
+	LocalGrid grid2d(grid2D(1), cv::Mat(), cv::Mat(), 0.1f);
+	EXPECT_FALSE(grid2d.is3D());
+
+	LocalGrid grid3d(grid3D(1), cv::Mat(), cv::Mat(), 0.1f);
+	EXPECT_TRUE(grid3d.is3D());
+
+	LocalGrid mixed(grid2D(1), grid3D(1), cv::Mat(), 0.1f);
+	EXPECT_FALSE(mixed.is3D());
+}
+
+TEST(LocalGridTest, CacheAddFindAndIterate)
+{
+	LocalGridCache cache;
+	const LocalGrid grid(grid2D(2), grid2D(1), cv::Mat(), 0.1f);
+
+	cache.add(5, grid);
+	EXPECT_EQ(cache.size(), 1u);
+	EXPECT_FALSE(cache.empty());
+
+	const auto it = cache.find(5);
+	ASSERT_NE(it, cache.end());
+	EXPECT_EQ(it->first, 5);
+	EXPECT_EQ(it->second.groundCells.cols, 2);
+
+	EXPECT_EQ(cache.begin(), cache.find(5));
+	EXPECT_EQ(cache.find(99), cache.end());
+	EXPECT_EQ(cache.localGrids().size(), 1u);
+}
+
+TEST(LocalGridTest, CacheNodeIdZeroStoredAsTemporary)
+{
+	LocalGridCache cache;
+	cache.add(0, grid2D(1), cv::Mat(), cv::Mat(), 0.1f);
+	EXPECT_EQ(cache.size(), 1u);
+	EXPECT_NE(cache.find(-1), cache.end());
+	EXPECT_EQ(cache.find(0), cache.end());
+}
+
+TEST(LocalGridTest, CacheNegativeNodeIdIgnored)
+{
+	LocalGridCache cache;
+	cache.add(-2, grid2D(1), cv::Mat(), cv::Mat(), 0.1f);
+	EXPECT_TRUE(cache.empty());
+}
+
+TEST(LocalGridTest, ShareToCopiesWhenAbsentInTarget)
+{
+	LocalGridCache source;
+	LocalGridCache target;
+	source.add(3, grid2D(2), grid2D(1), cv::Mat(), 0.2f, cv::Point3f(4.f, 5.f, 6.f));
+
+	EXPECT_TRUE(source.shareTo(3, target));
+	EXPECT_EQ(target.size(), 1u);
+	const auto it = target.find(3);
+	ASSERT_NE(it, target.end());
+	EXPECT_EQ(it->second.groundCells.cols, 2);
+	EXPECT_FLOAT_EQ(it->second.cellSize, 0.2f);
+	EXPECT_FLOAT_EQ(it->second.viewPoint.z, 6.f);
+
+	EXPECT_FALSE(source.shareTo(3, target));
+	EXPECT_FALSE(source.shareTo(99, target));
+}
+
+TEST(LocalGridTest, GetMemoryUsed)
+{
+	LocalGridCache cache;
+	const unsigned long memEmpty = cache.getMemoryUsed();
+
+	cache.add(1, grid2D(10), grid2D(5), grid2D(3), 0.1f);
+	EXPECT_GT(cache.getMemoryUsed(), memEmpty);
+}
+
+TEST(LocalGridTest, ClearRemovesAllEntries)
+{
+	LocalGridCache cache;
+	const unsigned long memEmpty = cache.getMemoryUsed();
+
+	cache.add(1, grid2D(2), cv::Mat(), cv::Mat(), 0.1f);
+	cache.clear();
+
+	EXPECT_TRUE(cache.empty());
+	EXPECT_EQ(cache.getMemoryUsed(), memEmpty);
+}
+
+TEST(LocalGridTest, ClearTemporaryRemovesNodeIdZero)
+{
+	LocalGridCache cache;
+	cache.add(0, grid2D(2), cv::Mat(), cv::Mat(), 0.1f); // node id 0 -> key -1
+	cache.add(2, grid2D(3), cv::Mat(), cv::Mat(), 0.1f);
+	EXPECT_EQ(cache.size(), 2u);
+
+	cache.clear(true);
+
+	EXPECT_EQ(cache.size(), 1u);
+	EXPECT_NE(cache.find(2), cache.end());
+	EXPECT_EQ(cache.find(-1), cache.end());
+}