i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-04 00:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

added doc/gtest for util3d_mapping.h (missing hpp functions)

Browse Source
This commit is contained in:
matlabbe
2025-06-22 18:45:43 -07:00
parent 114f673ec2
commit 442a868929
4 changed files with 643 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/util3d_mapping.h
+187 -14
View File
@@ -62,6 +62,33 @@ RTABMAP_DEPRECATED void RTABMAP_CORE_EXPORT occupancy2DFromLaserScan(
bool unknownSpaceFilled = false,
float scanMaxRange = 0.0f);
/**
* @brief Generates 2D occupancy grid maps (free and occupied cells) from laser scan data.
*
* This function takes in a laser scan composed of hit and no-hit points and computes two
* 2D maps:
* - `empty`: 2D coordinates of free space (where the laser passed without hitting obstacles).
* - `occupied`: precise 2D coordinates of obstacle hits (where the laser reflected).
*
* The internal representation uses a temporary occupancy map generated from `create2DMap()`,
* from which free cells are extracted. Obstacle points are directly passed through, potentially
* clipped by a maximum range filter.
*
* @param[in] scanHitIn CV_32FC2 or CV_32FC(n>=2) matrix representing obstacle hits in 2D or 3D space (relative to base frame, not laser frame).
* @param[in] scanNoHitIn CV_32FC2 or CV_32FC(n>=2) matrix representing laser rays that did not hit an obstacle (relative to base frame, not laser frame).
* @param[in] viewpoint The viewpoint (sensor origin) from which the scan was taken, in 3D space, relative to base frame.
* This used to determinate the origin of ray tracing.
* @param[out] empty Output matrix (CV_32FC2) of free space points derived from ray tracing.
* @param[out] occupied Output matrix (CV_32FC2) of occupied (hit) points, filtered by max range if required.
* @param[in] cellSize The resolution of the occupancy map grid (in meters per cell).
* @param[in] unknownSpaceFilled If true, unknown space between hits is also filled via ray tracing.
* @param[in] scanMaxRange Maximum range of the scan (in meters). Values beyond this are clipped.
*
* @note The function assumes a single scan already converted in base frame for internal processing.
* @note If `scanMaxRange <= cellSize`, no range filtering is applied to the occupied points.
*
* @see create2DMap(), util3d::rangeFiltering()
*/
void RTABMAP_CORE_EXPORT occupancy2DFromLaserScan(
const cv::Mat & scanHit, // in /base_link frame
const cv::Mat & scanNoHit, // in /base_link frame
@@ -72,6 +99,42 @@ void RTABMAP_CORE_EXPORT occupancy2DFromLaserScan(
bool unknownSpaceFilled = false,
float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true
/**
* @brief Creates a 2D occupancy grid map from local occupancy data.
*
* Generates a 2D occupancy grid (`CV_8S`) where:
* - -1 indicates unknown space,
* - 0 indicates free space,
* - 100 indicates an occupied (obstacle) cell.
*
* This function transforms and merges local empty/occupied occupancy maps
* from multiple robot poses into a single global 2D grid map.
*
* @param posesIn Map of robot poses, indexed by node ID.
* @param occupancy Map of local occupancy data, indexed by node ID.
* Each pair contains two cv::Mat elements:
* - First: empty cells (CV_32FC2) relative to base frame,
* - Second: occupied cells (CV_32FC2) relative to base frame.
* @param cellSize The resolution of the map in meters per cell.
* @param[out] xMin Minimum x-coordinate (origin offset) of the resulting map (in meters).
* @param[out] yMin Minimum y-coordinate (origin offset) of the resulting map (in meters).
* @param minMapSize Minimum width/height of the output map in meters.
* If 0, size is computed from poses and occupancy data.
* @param erode Whether to post-process (erode) noisy obstacles.
* This helps remove isolated or thin obstacle artifacts.
* @param footprintRadius Radius of the robot footprint (in meters).
* Free space will be cleared under the robot.
*
* @return cv::Mat The resulting occupancy grid map (CV_8S):
* -1 = unknown,
* 0 = free space,
* 100 = obstacle.
*
* @warning The output map can be very large if poses are far apart or cellSize is small.
* The function will not create a map if the estimated size exceeds reasonable limits (e.g. > 1.5 km).
*
* @note The function will fill small holes surrounded by known cells and optionally erode noisy borders.
*/
cv::Mat RTABMAP_CORE_EXPORT create2DMapFromOccupancyLocalMaps(
const std::map<int, Transform> & poses,
const std::map<int, std::pair<cv::Mat, cv::Mat> > & occupancy,
@@ -104,19 +167,39 @@ RTABMAP_DEPRECATED cv::Mat RTABMAP_CORE_EXPORT create2DMap(const std::map<int, T
float scanMaxRange = 0.0f);
/**
* Create 2d Occupancy grid (CV_8S)
* -1 = unknown
* 0 = empty space
* 100 = obstacle
* @param poses
* @param scans, should be CV_32FC2 type!
* @param viewpoints
* @param cellSize m
* @param unknownSpaceFilled if false no fill, otherwise a virtual laser sweeps the unknown space from each pose (stopping on detected obstacle)
* @param xMin
* @param yMin
* @param minMapSize minimum map size in meters
* @param scanMaxRange laser scan maximum range, would be set if unknownSpaceFilled=true
* @brief Generates a 2D occupancy grid map from a set of poses, laser scans, and viewpoints.
*
* This function aggregates laser scan data from multiple poses and creates a 2D occupancy grid
* (CV_8S: signed 8-bit) where:
* - `-1` represents unknown space,
* - `0` represents free space,
* - `100` represents occupied space (obstacles).
*
* The scans are first transformed into the global map frame using the corresponding pose.
* Obstacles and free space are inserted using ray tracing. Optionally, unknown areas between
* known rays can also be filled using radial sweeping.
*
* @param poses A map of node IDs to 3D poses (used to transform local scans to the global frame).
* @param scans A map of node IDs to pairs of laser scans (`<hit, no-hit>`), each as a `cv::Mat` of type `CV_32FC2`.
* - `first`: endpoints of beams hitting obstacles (relative to base frame, not laser frame).
* - `second`: endpoints of beams not hitting any obstacle (relative to base frame, not laser frame).
* @param viewpoints A map of node IDs to local sensor origin offsets relative to each pose (e.g., lidar offset /base_link -> /base_scan).
* This is used to determinate the starting point for each ray trace.
* @param cellSize The size of each grid cell in meters.
* @param unknownSpaceFilled If true, fills areas between known rays (fan sweeping) up to `scanMaxRange`.
* @param[out] xMin The minimum x value (in meters) of the grid origin relative to map coordinates.
* @param[out] yMin The minimum y value (in meters) of the grid origin relative to map coordinates.
* @param minMapSize The minimum width and height (in meters) of the map. Ensures the output map has a minimum footprint.
* @param scanMaxRange The maximum range (in meters) of the sensor. Used to limit ray tracing and padding.
*
* @return A 2D occupancy grid map (`cv::Mat` of type `CV_8S`) where:
* - `-1` = unknown
* - `0` = free space
* - `100` = obstacle
*
* @note If `scanMaxRange <= 0`, map size is determined based on scan data bounds.
* @note Grid coordinates are calculated with padding to ensure all points fall within the map.
* @note This function uses ray tracing internally via the `rayTrace()` function.
*/
cv::Mat RTABMAP_CORE_EXPORT create2DMap(const std::map<int, Transform> & poses,
const std::map<int, std::pair<cv::Mat, cv::Mat> > & scans, // <id, <hit, no hit> >, in /base_link frame
@@ -126,16 +209,106 @@ cv::Mat RTABMAP_CORE_EXPORT create2DMap(const std::map<int, Transform> & poses,
float & xMin,
float & yMin,
float minMapSize = 0.0f,
float scanMaxRange = 0.0f); // would be set if unknownSpaceFilled=true
float scanMaxRange = 0.0f);
/**
* @brief Performs a 2D ray tracing operation between two points on a grid map.
*
* This function draws a line from the `start` point to the `end` point on a grid (e.g., occupancy grid),
* marking all traversed cells as free (value = 0) unless an obstacle (value = 100) is encountered.
* The line follows an integer rasterization algorithm (like Bresenham’s line), accounting for steep slopes
* by transposing axes when needed.
*
* @param start The starting point of the ray (2D grid coordinates).
* @param end The ending point of the ray (2D grid coordinates). This point is clipped to the grid bounds.
* @param grid A mutable 2D grid represented as a `cv::Mat` of signed char values.
* Assumes 100 denotes obstacles; 0 denotes free space.
* @param stopOnObstacle If true, the ray trace stops upon hitting a cell marked with 100 (an obstacle).
*
* @note
* - If the slope of the line is steep (outside the range [-1, 1]), the algorithm swaps x and y axes for correctness.
* - The function ensures both the start and end points are within the bounds of the grid.
* - All visited cells along the path (except obstacles when `stopOnObstacle` is true) will be updated to 0 (free).
* - The grid must have type `CV_8SC1` (signed 8-bit single-channel matrix).
*
*/
void RTABMAP_CORE_EXPORT rayTrace(const cv::Point2i & start,
const cv::Point2i & end,
cv::Mat & grid,
bool stopOnObstacle);
/**
* @brief Converts an occupancy grid map (CV_8S) to a grayscale image (CV_8U).
*
* This function takes a signed 8-bit occupancy grid map and produces a corresponding
* 8-bit unsigned grayscale image. The pixel values are mapped based on the occupancy values:
*
* - `0` (free space) → 178 (normal) or 254 (PGM format)
* - `100` (obstacle) → 0 (black)
* - `-2` (robot footprint) → 200 (normal) or 254 (PGM format)
* - `-1` (unknown) → 89 (normal) or 205 (PGM format)
* - `v > 50` (partial obstacle): scaled to range [0, 89]
* - `v < 50` (partial free): scaled to range [89, 178]
*
* If `pgmFormat` is true, the vertical axis is flipped (for PGM format compatibility).
*
* @param map8S The input occupancy grid map as a CV_8S single-channel matrix.
* Must contain values such as -1 (unknown), 0 (free), 100 (occupied).
* @param pgmFormat If true, output will be formatted for PGM file format (inverted Y-axis and different gray scale mapping).
*
* @return A CV_8U grayscale image with pixel values representing occupancy status.
*
* @throws UASSERT if the input map is not a single-channel CV_8S matrix.
*/
cv::Mat RTABMAP_CORE_EXPORT convertMap2Image8U(const cv::Mat & map8S, bool pgmFormat = false);
/**
* @brief Converts a grayscale occupancy image (CV_8U) to an occupancy grid map (CV_8S).
*
* This function interprets grayscale pixel values from an input image and converts
* them into occupancy values used in a typical occupancy grid map:
* - 100: Occupied
* - 0: Free
* - -1: Unknown
* - -2: Free space under robot footprint (non-PGM only)
*
* The interpretation differs slightly depending on whether the input image is in
* PGM format (common in ROS map_server) or in standard grayscale.
*
* @param map8U Input grayscale image (type CV_8U, single-channel).
* @param pgmFormat If true, assumes PGM format:
* - 0 = occupied
* - 254 = free
* - 205 = unknown
* If false (normal format):
* - 0 = occupied
* - 178 = free
* - 200 = footprint (free space under robot)
* - 89 = unknown
*
* @return A CV_8S occupancy grid map with encoded occupancy values.
*
* @throws Assertion failure if input image is not of type CV_8U or not single-channel.
*/
cv::Mat RTABMAP_CORE_EXPORT convertImage8U2Map(const cv::Mat & map8U, bool pgmFormat = false);
/**
* @brief Performs erosion on an occupancy grid map to reduce small noisy obstacles.
*
* This function scans a given occupancy grid (`CV_8SC1` format) and removes
* obstacle cells (value `100`) that are surrounded by at least 3 empty cells (value `0`)
* and no adjacent unknown cells (value `-1`). These obstacles are likely noise
* and are converted into empty space (value `0`) in the resulting map.
*
* @param map Input occupancy grid map of type `CV_8SC1` where:
* - `100` represents obstacles,
* - `0` represents free space,
* - `-1` represents unknown space.
*
* @return A new `cv::Mat` of the same size and type as the input map,
* with eroded obstacles.
*
*/
cv::Mat RTABMAP_CORE_EXPORT erodeMap(const cv::Mat & map);
template<typename PointT>
corelib/src/util3d_mapping.cpp
+29 -11
View File
@@ -818,18 +818,29 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid
ptA = start;
ptB = end;
float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
// clip end point
ptB.x = std::min(std::max(ptB.x, 0), grid.cols-1);
ptB.y = std::min(std::max(ptB.y, 0), grid.rows-1);
if(ptA == ptB) {
signed char * v = &grid.at<signed char>(ptA.y, ptA.x);
if(*v == 100 && stopOnObstacle)
{
return;
}
else
{
*v = 0; // free space
}
return;
}
float slope = ptB.x - ptA.x != 0 ? float(ptB.y - ptA.y)/float(ptB.x - ptA.x) : std::numeric_limits<float>::quiet_NaN();
bool swapped = false;
if(slope<-1.0f || slope>1.0f)
if(!uIsFinite(slope) || slope<-1.0f || slope>1.0f)
{
// swap x and y
slope = 1.0f/slope;
int tmp = ptA.x;
ptA.x = ptA.y;
ptA.y = tmp;
@@ -838,6 +849,13 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid
ptB.x = ptB.y;
ptB.y = tmp;
if(!uIsFinite(slope)) {
slope = 0.0f;
}
else {
slope = 1.0f/slope;
}
swapped = true;
}
@@ -860,13 +878,13 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid
if(!swapped)
{
UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.rows, uFormat("lowerbound=%f grid.rows=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.rows, x, slope, b, x).c_str());
UASSERT_MSG(upperbound >= 0 && upperbound < grid.rows, uFormat("upperbound=%f grid.rows=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.rows, x+1, slope, b, x).c_str());
UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.rows, uFormat("lowerbound=%d upperbound=%d grid.rows=%d x=%d slope=%f b=%f", lowerbound, upperbound, grid.rows, x, slope, b).c_str());
UASSERT_MSG(upperbound >= 0 && upperbound < grid.rows, uFormat("lowerbound=%d upperbound=%d grid.rows=%d x+1=%d slope=%f b=%f", lowerbound, upperbound, grid.rows, x+1, slope, b).c_str());
}
else
{
UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.cols, uFormat("lowerbound=%f grid.cols=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.cols, x, slope, b, x).c_str());
UASSERT_MSG(upperbound >= 0 && upperbound < grid.cols, uFormat("upperbound=%f grid.cols=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.cols, x+1, slope, b, x).c_str());
UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.cols, uFormat("lowerbound=%d upperbound=%d grid.cols=%d x=%d slope=%f b=%f", lowerbound, upperbound, grid.cols, x, slope, b).c_str());
UASSERT_MSG(upperbound >= 0 && upperbound < grid.cols, uFormat("lowerbound=%d upperbound=%d grid.cols=%d x+1=%d slope=%f b=%f", lowerbound, upperbound, grid.cols, x+1, slope, b).c_str());
}
for(int y = lowerbound; y<=(int)upperbound; ++y)
@@ -989,9 +1007,9 @@ cv::Mat erodeMap(const cv::Mat & map)
{
UASSERT(map.type() == CV_8SC1);
cv::Mat erodedMap = map.clone();
for(int i=0; i<map.rows; ++i)
for(int i=1; i<map.rows-1; ++i)
{
for(int j=0; j<map.cols; ++j)
for(int j=1; j<map.cols-1; ++j)
{
if(map.at<signed char>(i, j) == 100)
{
corelib/test/CMakeLists.txt
+6 -1
View File
@@ -34,4 +34,9 @@ gtest_discover_tests(test_util3d_features)
#util3d_correspondences.h
add_executable(test_util3d_correspondences test_util3d_correspondences.cpp)
target_link_libraries(test_util3d_correspondences gtest_main rtabmap_core)
gtest_discover_tests(test_util3d_correspondences)
gtest_discover_tests(test_util3d_correspondences)
#util3d_mapping.h
add_executable(test_util3d_mapping test_util3d_mapping.cpp)
target_link_libraries(test_util3d_mapping gtest_main rtabmap_core)
gtest_discover_tests(test_util3d_mapping)
corelib/test/test_util3d_mapping.cpp
+421
View File
@@ -0,0 +1,421 @@
#include "gtest/gtest.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util3d_mapping.h"
#include "rtabmap/core/CameraModel.h"
#include "rtabmap/utilite/UException.h"
#include "rtabmap/utilite/UConversion.h"
#include <pcl/io/pcd_io.h>
using namespace rtabmap;
TEST(Util3dMapping, rayTraceClearsFreePathWithoutObstacle) {
cv::Mat grid = cv::Mat::ones(10, 10, CV_8SC1) * 50; // Initial grid (non-zero for testing)
cv::Point2i start(2, 2);
cv::Point2i end(7, 7);
util3d::rayTrace(start, end, grid, false);
// Check that the path has been cleared (set to 0)
for (int i = 2; i < 7; ++i) {
ASSERT_EQ(grid.at<signed char>(i, i), 0);
}
}
TEST(Util3dMapping, rayTraceStopsOnObstacleWhenFlagTrue) {
cv::Mat grid = cv::Mat::ones(10, 10, CV_8SC1) * 50;
grid.at<signed char>(5, 5) = 100; // Add obstacle
cv::Point2i start(2, 2);
cv::Point2i end(7, 7);
util3d::rayTrace(start, end, grid, true);
// Ensure points before the obstacle are cleared, and after are not
for (int i = 2; i < 5; ++i) {
EXPECT_EQ(grid.at<signed char>(i, i), 0);
}
EXPECT_EQ(grid.at<signed char>(5, 5), 100); // Obstacle should remain
EXPECT_NE(grid.at<signed char>(6, 6), 0); // Not cleared after obstacle
}
TEST(Util3dMapping, rayTraceIgnoresObstacleWhenFlagFalse) {
cv::Mat grid = cv::Mat::ones(10, 10, CV_8SC1) * 50;
grid.at<signed char>(5, 5) = 100; // Add obstacle
cv::Point2i start(2, 2);
cv::Point2i end(7, 7);
util3d::rayTrace(start, end, grid, false);
// All cells in the line should be cleared regardless of obstacle
for (int i = 2; i < 7; ++i) {
EXPECT_EQ(grid.at<signed char>(i, i), 0);
}
}
TEST(Util3dMapping, rayTraceHandlesSteepSlopeCorrectly) {
// Create a 10x10 grid filled with 50
cv::Mat grid = cv::Mat::ones(10, 10, CV_8SC1) * 50;
// Set a steep slope: vertical-like line from bottom to top
cv::Point2i start(5, 1); // near the bottom
cv::Point2i end(6, 8); // almost vertical but not perfectly, so slope > 1
util3d::rayTrace(start, end, grid, false);
// Check that some pixels along that steep line are cleared
// We expect the values along the approximate path to be set to 0
int clearedCount = 0;
for (int y = 1; y <= 8; ++y) {
for (int x = 5; x <= 6; ++x) {
if (grid.at<signed char>(y, x) == 0) {
clearedCount++;
}
}
}
// At least 5 pixels should have been cleared along the steep slope
EXPECT_GE(clearedCount, 5) << "Steep slope did not clear expected cells.";
grid = cv::Mat::ones(10, 10, CV_8SC1) * 50;
// Same thing, but inverted
util3d::rayTrace(end, start, grid, false);
// Check that some pixels along that steep line are cleared
// We expect the values along the approximate path to be set to 0
clearedCount = 0;
for (int y = 1; y <= 8; ++y) {
for (int x = 5; x <= 6; ++x) {
if (grid.at<signed char>(y, x) == 0) {
clearedCount++;
}
}
}
// At least 5 pixels should have been cleared along the steep slope
EXPECT_GE(clearedCount, 5) << "Steep slope did not clear expected cells.";
}
TEST(Util3dMapping, rayTraceHandlesHorizontalVerticalSlopesCorrectly) {
// Create a 10x10 grid filled with 50
cv::Mat grid = cv::Mat::ones(10, 10, CV_8SC1) * 50;
// Set horizontal slope
cv::Point2i start(1, 1);
cv::Point2i end(8, 1);
util3d::rayTrace(start, end, grid, false);
for (int x = 1; x < 8; ++x) {
EXPECT_EQ(grid.at<signed char>(1, x), 0);
}
start = cv::Point2i(4, 2);
end = cv::Point2i(4, 8);
util3d::rayTrace(start, end, grid, false);
for (int y = 2; y < 8; ++y) {
EXPECT_EQ(grid.at<signed char>(y, 4), 0);
}
}
TEST(Util3dMapping, rayTraceClipsEndPointToBoundary) {
cv::Mat grid = cv::Mat::ones(10, 10, CV_8SC1) * 50;
cv::Point2i start(3, 3);
cv::Point2i end(20, 20); // Way outside bounds
util3d::rayTrace(start, end, grid, false);
// All cells along the diagonal from (3,3) to (8,8) should be cleared
for (int i = 3; i < grid.rows-1 && i < grid.cols-1; ++i) {
EXPECT_EQ(grid.at<signed char>(i, i), 0);
}
}
TEST(Util3dMapping, create2DMapBasicMapGeneration)
{
std::map<int, Transform> poses;
std::map<int, std::pair<cv::Mat, cv::Mat>> scans;
std::map<int, cv::Point3f> viewpoints;
int id = 1;
float cellSize = 0.05f;
float xMin, yMin;
float minMapSize = 1.0f;
float scanMaxRange = 0.5f;
bool unknownSpaceFilled = false;
// Fake pose (origin)
poses[id] = Transform::getIdentity();
// Fake viewpoint
viewpoints[id] = cv::Point3f(-0.1f, -0.0f, 0);
cv::Mat hit(1, 2, CV_32FC2);
// Create a hit point under max scan range
hit.at<cv::Vec2f>(0, 0) = cv::Vec2f(0.2f, 0.2f);
// Create a hit point over max scan range
hit.at<cv::Vec2f>(0, 1) = cv::Vec2f(3.0f, 0.0f);
// No miss points
cv::Mat noHit(1, 1, CV_32FC2);
noHit.at<cv::Vec2f>(0, 0) = cv::Vec2f(0.0f, 0.2f);
scans[id] = std::make_pair(hit, noHit);
// Call the function
cv::Mat map = util3d::create2DMap(poses, scans, viewpoints, cellSize, unknownSpaceFilled, xMin, yMin, minMapSize, scanMaxRange);
// Verify map size
ASSERT_FALSE(map.empty());
EXPECT_EQ(map.type(), CV_8S);
EXPECT_EQ(map.cols, minMapSize/cellSize+20);
EXPECT_EQ(map.rows, minMapSize/cellSize+20);
// Verify that the viewpoint is cleared
EXPECT_EQ(map.at<signed char>(
(viewpoints[id].y - yMin) / cellSize,
(viewpoints[id].x - xMin) / cellSize),
0);
// Verify that the hit cell is marked as obstacle (100)
EXPECT_EQ(map.at<signed char>(
(hit.at<cv::Vec2f>(0, 0)[1] - yMin) / cellSize,
(hit.at<cv::Vec2f>(0, 0)[0] - xMin) / cellSize),
100);
// Verify that ray tracing worked only up to max scan range (from offset viewpoint)
EXPECT_EQ(map.at<signed char>(
(hit.at<cv::Vec2f>(0, 1)[1] + viewpoints[id].y - yMin) / cellSize,
(scanMaxRange + viewpoints[id].x - xMin) / cellSize -1),
0);
EXPECT_EQ(map.at<signed char>(
(hit.at<cv::Vec2f>(0, 1)[1] + viewpoints[id].y - yMin) / cellSize,
(scanMaxRange + viewpoints[id].x - xMin) / cellSize),
-1);
// Verify that the nohit cell is marked as empty (0)
EXPECT_EQ(map.at<signed char>(
(noHit.at<cv::Vec2f>(0, 0)[1] - yMin) / cellSize -1, // ray tracing only up to the nohit cell
(noHit.at<cv::Vec2f>(0, 0)[0] - xMin) / cellSize),
0);
EXPECT_EQ(map.at<signed char>(
(noHit.at<cv::Vec2f>(0, 0)[1] - yMin) / cellSize,
(noHit.at<cv::Vec2f>(0, 0)[0] - xMin) / cellSize),
-1);
}
TEST(Util3dMapping, occupancy2DFromLaserScanBasicTest)
{
// Synthetic scan with 3 hits and 2 no-hits (in 2D)
cv::Mat scanHit(1, 3, CV_32FC2);
scanHit.at<cv::Vec2f>(0, 0) = cv::Vec2f(1.0f, 0.0f);
scanHit.at<cv::Vec2f>(0, 1) = cv::Vec2f(0.0f, 1.0f);
scanHit.at<cv::Vec2f>(0, 2) = cv::Vec2f(1.0f, 1.0f);
cv::Mat scanNoHit(1, 2, CV_32FC2);
scanNoHit.at<cv::Vec2f>(0, 0) = cv::Vec2f(0.5f, 0.5f);
scanNoHit.at<cv::Vec2f>(0, 1) = cv::Vec2f(-1.0f, -1.0f);
cv::Point3f viewpoint(0.0f, 0.0f, 0.0f);
cv::Mat empty, occupied;
float cellSize = 0.1f;
float scanMaxRange = 2.0f;
util3d::occupancy2DFromLaserScan(scanHit, scanNoHit, viewpoint, empty, occupied, cellSize, true, scanMaxRange);
// Validate sizes
ASSERT_FALSE(occupied.empty()) << "Occupied matrix should not be empty.";
ASSERT_EQ(occupied.cols, 3) << "Expected 3 occupied points.";
ASSERT_EQ(occupied.type(), CV_32FC2) << "Occupied should be CV_32FC2.";
ASSERT_FALSE(empty.empty()) << "Empty matrix should not be empty.";
ASSERT_EQ(empty.type(), CV_32FC2) << "Empty should be CV_32FC2.";
ASSERT_GT(empty.cols, 20) << "There should be some empty cells.";
}
TEST(Util3dMapping, create2DMapFromOccupancyLocalMapsBasic)
{
// Setup poses
std::map<int, Transform> poses;
poses[1] = Transform::getIdentity(); // Assume (0, 0)
// Setup occupancy data
std::map<int, std::pair<cv::Mat, cv::Mat>> occupancy;
// Create dummy empty cells
cv::Mat empty(1, 2, CV_32FC2);
empty.at<cv::Vec2f>(0, 0) = cv::Vec2f(1.0f, 1.0f);
empty.at<cv::Vec2f>(0, 1) = cv::Vec2f(2.0f, 1.0f);
// Create dummy occupied cells
cv::Mat occupied(1, 1, CV_32FC2);
occupied.at<cv::Vec2f>(0, 0) = cv::Vec2f(1.5f, 2.0f);
occupancy[1] = std::make_pair(empty, occupied);
// Output parameters
float xMin = 0.0f;
float yMin = 0.0f;
float cellSize = 0.1f;
// Run function
cv::Mat map = util3d::create2DMapFromOccupancyLocalMaps(
poses,
occupancy,
cellSize,
xMin,
yMin,
0.0f, // minMapSize
false, // erode
0.0f // footprintRadius
);
// Verify output
ASSERT_FALSE(map.empty());
ASSERT_EQ(map.type(), CV_8S);
// Check expected occupancy value at occupied cell
int col = static_cast<int>((occupied.at<cv::Vec2f>(0, 0)[0] - xMin) / cellSize);
int row = static_cast<int>((occupied.at<cv::Vec2f>(0, 0)[1] - yMin) / cellSize);
ASSERT_GE(row, 0);
ASSERT_GE(col, 0);
ASSERT_LT(row, map.rows);
ASSERT_LT(col, map.cols);
EXPECT_EQ(map.at<signed char>(row, col), 100); // Obstacle
// Check empty cells
int rowEmpty = static_cast<int>((empty.at<cv::Vec2f>(0, 0)[1] - yMin) / cellSize);
int colEmpty1 = static_cast<int>((empty.at<cv::Vec2f>(0, 0)[0] - xMin) / cellSize);
int colEmpty2 = static_cast<int>((empty.at<cv::Vec2f>(0, 1)[0] - xMin) / cellSize);
EXPECT_EQ(map.at<signed char>(rowEmpty, colEmpty1), 0); // Free
EXPECT_EQ(map.at<signed char>(rowEmpty, colEmpty2), 0); // Free
}
TEST(Util3dMapping, convertMap2Image8UBasicConversionNormalFormat)
{
// Create a simple 3x3 CV_8S occupancy grid
cv::Mat map8S = (cv::Mat_<signed char>(3, 3) <<
-1, 0, 100,
-2, 50, 75,
25, -1, 0);
// Call the function in normal format
cv::Mat result = util3d::convertMap2Image8U(map8S, false);
ASSERT_EQ(result.type(), CV_8U);
ASSERT_EQ(result.rows, 3);
ASSERT_EQ(result.cols, 3);
// Expected grayscale values for normal format
EXPECT_EQ(result.at<uchar>(0, 0), 89); // -1 (unknown)
EXPECT_EQ(result.at<uchar>(0, 1), 178); // 0 (free)
EXPECT_EQ(result.at<uchar>(0, 2), 0); // 100 (obstacle)
EXPECT_EQ(result.at<uchar>(1, 0), 200); // -2 (footprint)
EXPECT_EQ(result.at<uchar>(1, 1), 89); // 50
EXPECT_LT(result.at<uchar>(1, 2), 89); // 75 → scaled toward obstacle (0–89)
EXPECT_GT(result.at<uchar>(2, 0), 89); // 25 → scaled toward free (89–178)
EXPECT_EQ(result.at<uchar>(2, 1), 89); // -1
EXPECT_EQ(result.at<uchar>(2, 2), 178); // 0
}
TEST(Util3dMapping, convertMap2Image8UBasicConversionPGMFormat)
{
cv::Mat map8S = (cv::Mat_<signed char>(2, 2) <<
-1, 0,
-2, 100);
// Call with pgmFormat = true
cv::Mat result = util3d::convertMap2Image8U(map8S, true);
ASSERT_EQ(result.type(), CV_8U);
ASSERT_EQ(result.rows, 2);
ASSERT_EQ(result.cols, 2);
// Because pgmFormat flips vertically, test accordingly
EXPECT_EQ(result.at<uchar>(0, 0), 254); // 0 (was at [1,0])
EXPECT_EQ(result.at<uchar>(0, 1), 0); // 100
EXPECT_EQ(result.at<uchar>(1, 0), 205); // -1
EXPECT_EQ(result.at<uchar>(1, 1), 254); // -2
}
TEST(Util3dMapping, convertImage8U2MapNonPGMFormat)
{
// Create a 2x2 grayscale image using non-PGM expected values
cv::Mat input = (cv::Mat_<uchar>(2, 2) << 178, 0, 200, 89);
// Call conversion
cv::Mat map = util3d::convertImage8U2Map(input, false);
ASSERT_EQ(map.type(), CV_8S);
ASSERT_EQ(map.rows, 2);
ASSERT_EQ(map.cols, 2);
// Expected values: 0 (free), 100 (occupied), -2 (footprint), -1 (unknown)
EXPECT_EQ(map.at<signed char>(0, 0), 0);
EXPECT_EQ(map.at<signed char>(0, 1), 100);
EXPECT_EQ(map.at<signed char>(1, 0), -2);
EXPECT_EQ(map.at<signed char>(1, 1), -1);
}
TEST(Util3dMapping, convertImage8U2MapPGMFormat)
{
// Create a 2x2 grayscale image using PGM expected values
cv::Mat input = (cv::Mat_<uchar>(2, 2) << 254, 0, 205, 205);
// Call conversion
cv::Mat map = util3d::convertImage8U2Map(input, true);
ASSERT_EQ(map.type(), CV_8S);
ASSERT_EQ(map.rows, 2);
ASSERT_EQ(map.cols, 2);
// Because PGM inverts rows vertically, we need to check accordingly
// map.at<signed char>(i, j) corresponds to input.at<uchar>((rows-1)-i, j)
EXPECT_EQ(map.at<signed char>(0, 0), -1); // input(1, 0) = 205
EXPECT_EQ(map.at<signed char>(0, 1), -1); // input(1, 1) = 205
EXPECT_EQ(map.at<signed char>(1, 0), 0); // input(0, 0) = 254
EXPECT_EQ(map.at<signed char>(1, 1), 100); // input(0, 1) = 0
}
TEST(Util3dMapping, erodeMapBasicErosion)
{
// Create a 5x5 map with CV_8SC1 type
// -1 = unknown, 0 = free, 100 = obstacle
cv::Mat map = (cv::Mat_<signed char>(5,5) <<
0, 0, 0, 0, 0,
0, 100, 100, 100, 0,
0, 0, 100, 100, 100,
0, 100, 100, 100, 100,
0, 0, 100, 100, 100);
cv::Mat erodedMap = util3d::erodeMap(map);
cv::Mat expected = (cv::Mat_<signed char>(5,5) <<
0, 0, 0, 0, 0,
0, 0, 100, 100, 0,
0, 0, 100, 100, 100,
0, 0, 100, 100, 100,
0, 0, 100, 100, 100);
cv::Mat diff;
cv::compare(erodedMap, expected, diff, cv::CMP_NE);
// Count non-zero elements in diff (means different)
EXPECT_EQ(cv::countNonZero(diff), 0);
}
TEST(Util3dMapping, erodeMapNoErosionWithUnknown)
{
// Create a 3x3 map with obstacle touching unknown cell
cv::Mat map = (cv::Mat_<signed char>(3,3) <<
0, 0, 0,
0, 100, -1,
0, 0, 0);
cv::Mat erodedMap = util3d::erodeMap(map);
// Obstacle should NOT be eroded because adjacent unknown cell (-1)
EXPECT_EQ(erodedMap.at<signed char>(1,1), 100);
}