Added GlobalMap tests

2026-10-04 17:17:47 +08:00 · 2026-05-17 17:10:08 -07:00
parent 3cc21c636f
commit d5deecaf1a
4 changed files with 280 additions and 3 deletions
@@ -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)
@@ -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);
+}