Added GlobalMap tests

corelib/include/rtabmap/core/GlobalMap.h

@@ -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

@@ -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
+ * Used by @ref GlobalMap and its subclasses (e.g. @ref OccupancyGrid) to hold per-node
- * before assembly. Node id `0` is stored as `-1` (temporary grid). Negative ids
+ * 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

@@ -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

@@ -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);
+}