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

Added GlobalMap tests

Browse Source
This commit is contained in:
matlabbe
2026-05-17 17:10:08 -07:00
parent 3cc21c636f
commit d5deecaf1a
4 changed files with 280 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/GlobalMap.h
+53 -1
View File
@@ -37,14 +37,31 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
/**
* @class GlobalMap
* @brief Abstract base for assembling per-node @ref LocalGrid data into a global map.
*
* Subclasses (@ref OccupancyGrid, @ref OctoMap, @ref GridMap, @ref CloudMap) implement
* @ref assemble() to merge new node poses from @ref LocalGridCache into their representation.
*
* @ref update() decides which poses need assembly (not yet in @ref addedNodes(), present in
* cache, id > 0) and may call @ref clear() when @ref fullUpdateNeeded() detects graph
* optimization or a disjoint pose set (see @ref kGridGlobalUpdateError).
*
* Log-odds helpers @ref logodds() and @ref probability() convert between occupancy
* probability and the internal log-odds representation used for global hit/miss/clamping
* parameters (GridGlobal/Prob*).
*/
class RTABMAP_CORE_EXPORT GlobalMap
{
public:
/** @brief Converts probability in (0, 1) to log-odds. */
inline static float logodds(double probability)
{
return (float) log(probability/(1-probability));
}
/** @brief Converts log-odds back to probability in (0, 1). */
inline static double probability(double logodds)
{
return 1. - ( 1. / (1. + exp(logodds)));
@@ -53,31 +70,66 @@ public:
public:
virtual ~GlobalMap();
/**
* @brief True if the map should be rebuilt from cache (loop closure or disjoint graph).
*
* Compares @p poses to @ref addedNodes() using @ref getUpdateError(), which is set from
* **Grid/GlobalUpdateError** (`Parameters::kGridGlobalUpdateError()`): if any assembled
* node's pose moved farther than that threshold (meters), or if none of the assembled
* nodes appear in @p poses, returns true and @ref update() will call @ref clear() first.
*
* @param poses Current graph poses (node id → transform).
*/
bool fullUpdateNeeded(const std::map<int, Transform> & poses) const;
bool update(const std::map<int, Transform> & poses); // return true if map has changed
/**
* @brief Incrementally assemble new nodes from @p poses.
* @param poses Graph poses; ids &gt; 0 with a cached @ref LocalGrid are candidates.
* @return True if @ref assemble() was called (at least one new pose processed).
*/
bool update(const std::map<int, Transform> & poses);
/** @brief Clears assembled nodes and grid bounds; does not clear @ref LocalGridCache. */
virtual void clear();
/** @return Grid cell size in meters (Grid/CellSize). */
float getCellSize() const {return cellSize_;}
/** @return Pose change threshold for full rebuild (Grid/GlobalUpdateError). */
float getUpdateError() const {return updateError_;}
/** @return Poses of nodes already assembled into the global map. */
const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
/** @brief 2D grid minimum (x, y) in meters. */
void getGridMin(double & x, double & y) const {x=minValues_[0];y=minValues_[1];}
/** @brief 2D grid maximum (x, y) in meters. */
void getGridMax(double & x, double & y) const {x=maxValues_[0];y=maxValues_[1];}
/** @brief 3D grid minimum (x, y, z) in meters. */
void getGridMin(double & x, double & y, double & z) const {x=minValues_[0];y=minValues_[1];z=minValues_[2];}
/** @brief 3D grid maximum (x, y, z) in meters. */
void getGridMax(double & x, double & y, double & z) const {x=maxValues_[0];y=maxValues_[1];z=maxValues_[2];}
/** @brief Approximate memory used by assembled-node bookkeeping (bytes). */
virtual unsigned long getMemoryUsed() const;
protected:
/**
* @brief Constructs the base map; subclasses call from their constructor.
* @param cache Non-null cache of per-node local grids.
* @param parameters Optional Grid/ and GridGlobal/ parameter overrides.
*/
GlobalMap(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap());
/** @brief Subclass hook: merge @p newPoses into the global map; call @ref addAssembledNode(). */
virtual void assemble(const std::list<std::pair<int, Transform> > & newPoses) = 0;
/** @return Cached local grids keyed by node id. */
const std::map<int, LocalGrid> & cache() const {return cache_->localGrids();}
/** @return Poses of nodes already assembled (same as @ref addedNodes()). */
const std::map<int, Transform> & assembledNodes() const {return addedNodes_;}
/** @return True if @p id is already in the assembled set. */
bool isNodeAssembled(int id) {return addedNodes_.find(id) != addedNodes_.end();}
/** @brief Records a successfully assembled node (ids &lt;= 0 are ignored). */
void addAssembledNode(int id, const Transform & pose);
protected:
corelib/include/rtabmap/core/LocalGrid.h
+2 -2
View File
@@ -90,8 +90,8 @@ public:
* @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
* Used by @ref GlobalMap and its subclasses (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
corelib/test/CMakeLists.txt
+5
View File
@@ -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)
#GlobalMap.h
add_executable(test_globalmap test_globalmap.cpp)
target_link_libraries(test_globalmap gtest_main rtabmap_core)
add_test(NAME test_globalmap COMMAND test_globalmap)
#LocalGridMaker.h
add_executable(test_localgridmaker test_localgridmaker.cpp)
target_link_libraries(test_localgridmaker gtest_main rtabmap_core)
corelib/test/test_globalmap.cpp
+220
View File
@@ -0,0 +1,220 @@
#include <gtest/gtest.h>
#include <rtabmap/core/GlobalMap.h>
#include <rtabmap/core/LocalGrid.h>
#include <rtabmap/core/Parameters.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));
}
class MockGlobalMap : public GlobalMap
{
public:
MockGlobalMap(const LocalGridCache * cache, const ParametersMap & parameters = ParametersMap()) :
GlobalMap(cache, parameters)
{
}
std::list<std::pair<int, Transform> > lastAssembled;
int assembleCount = 0;
protected:
void assemble(const std::list<std::pair<int, Transform> > & newPoses) override
{
lastAssembled = newPoses;
++assembleCount;
for(std::list<std::pair<int, Transform> >::const_iterator it = newPoses.begin();
it != newPoses.end();
++it)
{
if(it->first > 0)
{
addAssembledNode(it->first, it->second);
}
}
}
};
static ParametersMap globalMapTestParams()
{
ParametersMap params;
params.insert(ParametersPair(Parameters::kGridCellSize(), "0.2"));
params.insert(ParametersPair(Parameters::kGridGlobalUpdateError(), "0.1"));
return params;
}
static void addGridToCache(LocalGridCache & cache, int nodeId)
{
cache.add(nodeId, grid2D(1), grid2D(1), cv::Mat(), 0.2f);
}
} // namespace
TEST(GlobalMapTest, LogOddsProbabilityRoundTrip)
{
const double p = 0.7;
const float lo = GlobalMap::logodds(p);
EXPECT_NEAR(GlobalMap::probability(lo), p, 1e-6);
EXPECT_GT(lo, 0.f);
}
TEST(GlobalMapTest, ConstructorParsesParameters)
{
LocalGridCache cache;
addGridToCache(cache, 1);
MockGlobalMap map(&cache, globalMapTestParams());
EXPECT_FLOAT_EQ(map.getCellSize(), 0.2f);
EXPECT_FLOAT_EQ(map.getUpdateError(), 0.1f);
EXPECT_TRUE(map.addedNodes().empty());
}
TEST(GlobalMapTest, ClearResetsAssembledNodes)
{
LocalGridCache cache;
addGridToCache(cache, 1);
MockGlobalMap map(&cache, globalMapTestParams());
std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform::getIdentity()));
ASSERT_TRUE(map.update(poses));
ASSERT_EQ(map.addedNodes().size(), 1u);
map.clear();
EXPECT_TRUE(map.addedNodes().empty());
}
TEST(GlobalMapTest, UpdateAssemblesCachedNodes)
{
LocalGridCache cache;
addGridToCache(cache, 1);
addGridToCache(cache, 2);
MockGlobalMap map(&cache, globalMapTestParams());
std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform::getIdentity()));
poses.insert(std::make_pair(2, Transform(1, 0, 0, 0, 0, 0, 1)));
EXPECT_TRUE(map.update(poses));
EXPECT_EQ(map.assembleCount, 1);
EXPECT_EQ(map.lastAssembled.size(), 2u);
EXPECT_EQ(map.addedNodes().size(), 2u);
EXPECT_EQ(map.addedNodes().count(1), 1u);
EXPECT_EQ(map.addedNodes().count(2), 1u);
}
TEST(GlobalMapTest, UpdateSkipsNodesNotInCache)
{
LocalGridCache cache;
addGridToCache(cache, 1);
MockGlobalMap map(&cache, globalMapTestParams());
std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform::getIdentity()));
poses.insert(std::make_pair(2, Transform(1, 0, 0, 0, 0, 0, 1)));
EXPECT_TRUE(map.update(poses));
EXPECT_EQ(map.lastAssembled.size(), 1u);
EXPECT_EQ(map.addedNodes().size(), 1u);
}
TEST(GlobalMapTest, UpdateReturnsFalseWhenNothingToAssemble)
{
LocalGridCache cache;
addGridToCache(cache, 1);
MockGlobalMap map(&cache, globalMapTestParams());
std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform::getIdentity()));
ASSERT_TRUE(map.update(poses));
EXPECT_FALSE(map.update(poses));
EXPECT_EQ(map.assembleCount, 1);
}
TEST(GlobalMapTest, UpdateAppendsPoseZeroAsTemporaryNode)
{
LocalGridCache cache;
addGridToCache(cache, 1);
MockGlobalMap map(&cache, globalMapTestParams());
std::map<int, Transform> poses;
poses.insert(std::make_pair(0, Transform(0, 0, 1, 0, 0, 0, 1)));
poses.insert(std::make_pair(1, Transform::getIdentity()));
EXPECT_TRUE(map.update(poses));
ASSERT_EQ(map.lastAssembled.size(), 2u);
EXPECT_EQ(map.lastAssembled.back().first, -1);
EXPECT_EQ(map.addedNodes().size(), 1u);
EXPECT_EQ(map.addedNodes().count(-1), 0u);
}
TEST(GlobalMapTest, FullUpdateNeededWhenPoseMovesBeyondThreshold)
{
LocalGridCache cache;
addGridToCache(cache, 1);
MockGlobalMap map(&cache, globalMapTestParams());
Transform t0 = Transform::getIdentity();
std::map<int, Transform> poses;
poses.insert(std::make_pair(1, t0));
ASSERT_TRUE(map.update(poses));
Transform t1(0.5f, 0, 0, 0, 0, 0, 1);
poses[1] = t1;
EXPECT_TRUE(map.fullUpdateNeeded(poses));
}
TEST(GlobalMapTest, FullUpdateNeededWhenAssembledGraphDisjoint)
{
LocalGridCache cache;
addGridToCache(cache, 1);
MockGlobalMap map(&cache, globalMapTestParams());
std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform::getIdentity()));
ASSERT_TRUE(map.update(poses));
std::map<int, Transform> newPoses;
newPoses.insert(std::make_pair(2, Transform::getIdentity()));
EXPECT_TRUE(map.fullUpdateNeeded(newPoses));
}
TEST(GlobalMapTest, FullUpdateClearsMapBeforeReassemble)
{
LocalGridCache cache;
addGridToCache(cache, 1);
addGridToCache(cache, 2);
MockGlobalMap map(&cache, globalMapTestParams());
std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform::getIdentity()));
ASSERT_TRUE(map.update(poses));
std::map<int, Transform> newPoses;
newPoses.insert(std::make_pair(2, Transform::getIdentity()));
EXPECT_TRUE(map.fullUpdateNeeded(newPoses));
EXPECT_TRUE(map.update(newPoses));
EXPECT_EQ(map.addedNodes().size(), 1u);
EXPECT_EQ(map.addedNodes().count(2), 1u);
EXPECT_EQ(map.addedNodes().count(1), 0u);
}
TEST(GlobalMapTest, GetMemoryUsed)
{
LocalGridCache cache;
addGridToCache(cache, 1);
MockGlobalMap map(&cache, globalMapTestParams());
EXPECT_GT(map.getMemoryUsed(), 0u);
std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform::getIdentity()));
ASSERT_TRUE(map.update(poses));
EXPECT_GT(map.getMemoryUsed(), 0u);
}