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

More octomap checks

Browse Source
This commit is contained in:
matlabbe
2026-05-25 21:10:24 -07:00
parent 0286eb65be
commit 919990ea1f
1 changed files with 147 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/test/test_rtabmap_integration.cpp
+147
View File
@@ -15,7 +15,14 @@
#include <gtest/gtest.h>
#include <rtabmap/core/DBReader.h>
#include <rtabmap/core/LocalGrid.h>
#include <rtabmap/core/OccupancyGrid.h>
#include <rtabmap/core/Odometry.h>
#include <rtabmap/core/Signature.h>
#include <rtabmap/core/Version.h>
#ifdef RTABMAP_OCTOMAP
#include <rtabmap/core/OctoMap.h>
#endif
#include <rtabmap/core/OdometryInfo.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Rtabmap.h>
@@ -70,6 +77,16 @@ struct ReplayResult
int finalGlobalGraphSize = 0; // poses returned by Rtabmap::getGraph(global=true)
std::map<int, Transform> finalLocalPoses; // optimized, global=false
std::map<int, Transform> finalGlobalPoses; // optimized, global=true
// Occupancy-grid cell counts after assembling the global grid from per-
// node local maps (only populated when RGBD/CreateOccupancyGrid=true).
int gridEmptyCells = 0;
int gridObstacleCells = 0;
// OctoMap leaf count (only populated when rtabmap is built with
// RTABMAP_OCTOMAP and RGBD/CreateOccupancyGrid=true). 0 means the
// build didn't have octomap support or the map was empty.
int octomapNodes = 0;
int octomapEmptyCells = 0;
int octomapObstacleCells = 0;
};
// Synchronous replay: DBReader -> Odometry::process -> Rtabmap::process.
@@ -283,6 +300,86 @@ ReplayResult replayDatabase(
result.finalGlobalGraphSize = (int)result.finalGlobalPoses.size();
}
// Assemble the global occupancy grid from the per-node local grids that
// rtabmap saved (only meaningful when RGBD/CreateOccupancyGrid=true).
// Pull poses + signatures with grids attached, prime a LocalGridCache,
// run OccupancyGrid::update, then count cells in the resulting cv::Mat
// (-1 = unknown, 0 = empty, 100 = obstacle).
bool createOccupancyGrid = Parameters::defaultRGBDCreateOccupancyGrid();
Parameters::parse(rtabmapParameters,
Parameters::kRGBDCreateOccupancyGrid(), createOccupancyGrid);
if(createOccupancyGrid)
{
std::map<int, Transform> poses;
std::multimap<int, Link> constraints;
std::map<int, Signature> signatures;
rtabmap.getGraph(poses, constraints,
/*optimized=*/true, /*global=*/true, &signatures,
/*withImages=*/false, /*withScan=*/false, /*withUserData=*/false,
/*withGrid=*/true, /*withWords=*/false,
/*withGlobalDescriptors=*/false);
LocalGridCache mapCache;
for(auto & p : signatures)
{
cv::Mat ground, obstacles, empty;
p.second.sensorData().uncompressDataConst(
0, 0, 0, 0, &ground, &obstacles, &empty);
if(!ground.empty() || !obstacles.empty() || !empty.empty())
{
mapCache.add(p.first, ground, obstacles, empty,
p.second.sensorData().gridCellSize(),
p.second.sensorData().gridViewPoint());
}
}
OccupancyGrid grid(&mapCache, rtabmapParameters);
grid.update(poses);
float xMin = 0.0f, yMin = 0.0f;
const cv::Mat gridMat = grid.getMap(xMin, yMin);
for(int i = 0; i < gridMat.rows; ++i)
{
for(int j = 0; j < gridMat.cols; ++j)
{
const signed char v = gridMat.at<signed char>(i, j);
if(v == 0) ++result.gridEmptyCells;
else if(v == 100) ++result.gridObstacleCells;
}
}
#ifdef RTABMAP_OCTOMAP
// Build a 3D OctoMap from the same cache+poses. Useful when
// Grid/RayTracing=true (which needs OctoMap support per the param
// doc) and a quick sanity check on the resulting tree size.
// Pin the OctoMap's resolution to the local grids' actual cell size
// (set when Memory built them at process() time) so the OcTree leaf
// size matches the data, regardless of any subsequent Grid/CellSize
// edits in `rtabmapParameters`.
ParametersMap octoMapParameters = rtabmapParameters;
for(const auto & p : signatures)
{
const float cs = p.second.sensorData().gridCellSize();
if(cs > 0.0f)
{
octoMapParameters[Parameters::kGridCellSize()] = uNumber2Str(cs);
break;
}
}
OctoMap octomap(&mapCache, octoMapParameters);
octomap.update(poses);
if(octomap.octree() != nullptr)
{
result.octomapNodes = (int)octomap.octree()->size();
std::vector<int> obstacleIdx, emptyIdx, groundIdx;
octomap.createCloud(/*treeDepth=*/0, &obstacleIdx, &emptyIdx, &groundIdx);
result.octomapObstacleCells = (int)obstacleIdx.size();
// Treat ground-classified cells as empty for the purposes of this
// test (they're free space, just labelled separately).
result.octomapEmptyCells = (int)emptyIdx.size() + (int)groundIdx.size();
}
#endif
}
std::cout << "[ ] Replay summary:"
<< " framesRead=" << result.framesRead
<< " odomNonNull=" << result.odomNonNull
@@ -293,6 +390,11 @@ ReplayResult replayDatabase(
<< " proximity=" << result.proximityDetections
<< " localGraph=" << result.finalLocalGraphSize
<< " globalGraph=" << result.finalGlobalGraphSize
<< " gridEmpty=" << result.gridEmptyCells
<< " gridObstacle=" << result.gridObstacleCells
<< " octomap=" << result.octomapNodes
<< " octomapEmpty=" << result.octomapEmptyCells
<< " octomapObstacle=" << result.octomapObstacleCells
<< " rmse=" << result.translationalRmseFinal << "m"
<< std::endl;
@@ -360,7 +462,9 @@ TEST_F(RtabmapIntegrationFixture, NetherdroneLidar3D)
// Grid/GroundIsObstacle=true: UAV use case -- no ground segmentation,
// every point is an obstacle in the occupancy grid.
ParametersMap rtabmapParams = baseRtabmapParams();
rtabmapParams[Parameters::kGridCellSize()] = kVoxelSize;
rtabmapParams[Parameters::kGridGroundIsObstacle()] = "true";
rtabmapParams[Parameters::kGridRayTracing()] = "true";
rtabmapParams[Parameters::kGridSensor()] = "0";
rtabmapParams[Parameters::kIcpEpsilon()] = "0.001";
rtabmapParams[Parameters::kIcpIterations()] = "10";
@@ -435,6 +539,15 @@ TEST_F(RtabmapIntegrationFixture, NetherdroneLidar3D)
EXPECT_EQ(31, result.finalLocalGraphSize);
EXPECT_EQ(165, result.finalGlobalGraphSize);
#ifdef RTABMAP_OCTOMAP
// Grid/RayTracing requires OctoMap support; verify the 3D map was
// actually assembled when the build has it. ICP-only replay is fully
// deterministic so the leaf counts are exact across runs.
EXPECT_EQ(21372, result.octomapNodes);
EXPECT_EQ(16178, result.octomapEmptyCells);
EXPECT_EQ(1845, result.octomapObstacleCells);
#endif
// Replay is deterministic; matching golden GT was captured from a clean
// run of this exact configuration, so RMSE should be ~0.
ASSERT_GE(result.translationalRmseFinal, 0.0f)
@@ -466,6 +579,18 @@ TEST_F(RtabmapIntegrationFixture, PR2_Scan2D_Stereo)
EXPECT_EQ(27, result.finalGlobalGraphSize);
EXPECT_GE(result.proximityDetections, 1)
<< "PR2 2D-scan dataset should produce proximity detections";
// Observed across 5 runs: empty 531-553, obstacle 5074-5102.
EXPECT_GE(result.gridEmptyCells, 450);
EXPECT_LE(result.gridEmptyCells, 650);
EXPECT_GE(result.gridObstacleCells, 4900);
EXPECT_LE(result.gridObstacleCells, 5300);
#ifdef RTABMAP_OCTOMAP
// Observed across 5 runs: empty 1834-1857, obstacle 21502-21671.
EXPECT_GE(result.octomapEmptyCells, 1700);
EXPECT_LE(result.octomapEmptyCells, 2000);
EXPECT_GE(result.octomapObstacleCells, 21000);
EXPECT_LE(result.octomapObstacleCells, 22000);
#endif
// Stereo F2M visual odom + visual loop closure -- observed RMSE ~3 cm,
// 5 cm bound gives ~50% headroom for run-to-run feature variance.
ASSERT_GE(result.translationalRmseFinal, 0.0f)
@@ -497,6 +622,18 @@ TEST_F(RtabmapIntegrationFixture, PR2_Scan2D_RGBD)
EXPECT_EQ(21, result.finalGlobalGraphSize);
EXPECT_GE(result.proximityDetections, 1)
<< "PR2 2D-scan dataset should produce proximity detections";
// Observed across 5 runs: empty 2900-3001, obstacle 4574-4698.
EXPECT_GE(result.gridEmptyCells, 2700);
EXPECT_LE(result.gridEmptyCells, 3200);
EXPECT_GE(result.gridObstacleCells, 4400);
EXPECT_LE(result.gridObstacleCells, 4900);
#ifdef RTABMAP_OCTOMAP
// Observed across 5 runs: empty 6452-7474, obstacle 41125-42883.
EXPECT_GE(result.octomapEmptyCells, 6000);
EXPECT_LE(result.octomapEmptyCells, 8000);
EXPECT_GE(result.octomapObstacleCells, 40000);
EXPECT_LE(result.octomapObstacleCells, 44000);
#endif
// RGB-D F2M visual odom + visual loop closure -- observed RMSE ~13 cm
// (less stable than the stereo/ICP paths), 20 cm bound gives ~50%
// headroom for visual-only loop-closure variability.
@@ -551,6 +688,16 @@ TEST_F(RtabmapIntegrationFixture, PR2_Scan2D_RGBD_IcpReg)
EXPECT_EQ(21, result.finalGlobalGraphSize);
EXPECT_GE(result.proximityDetections, 1)
<< "PR2 2D-scan dataset should produce proximity detections";
// Observed across 5 runs: empty 22785-22850, obstacle 1308-1316.
EXPECT_GE(result.gridEmptyCells, 22500);
EXPECT_LE(result.gridEmptyCells, 23200);
EXPECT_GE(result.gridObstacleCells, 1250);
EXPECT_LE(result.gridObstacleCells, 1400);
#ifdef RTABMAP_OCTOMAP
// 2D-laser-only signatures: nothing to assemble into a 3D OctoMap.
EXPECT_EQ(0, result.octomapEmptyCells);
EXPECT_EQ(0, result.octomapObstacleCells);
#endif
// Scan-based ICP loop closure with the PR2's 2D laser should align the
// final trajectory to within 2 cm of the stored ground truth.
ASSERT_GE(result.translationalRmseFinal, 0.0f)