Fixing getNodeData missing compressed grids

corelib/src/DBDriver.cpp

@@ -707,7 +707,10 @@ void DBDriver::getNodeData(
 			((!images || !s->sensorData().imageCompressed().empty()) &&
 			 (!scan || !s->sensorData().laserScanCompressed().isEmpty()) &&
 			 (!userData || !s->sensorData().userDataCompressed().empty()) &&
-			 (!occupancyGrid || s->sensorData().gridCellSize() != 0.0f))))
+			 (!occupancyGrid ||
+				!s->sensorData().gridGroundCellsCompressed().empty() ||
+				!s->sensorData().gridObstacleCellsCompressed().empty() ||
+				!s->sensorData().gridEmptyCellsCompressed().empty()))))
 		{
 			data = (SensorData)s->sensorData();
 			if(!images)

corelib/src/Memory.cpp

@@ -4795,7 +4795,10 @@ SensorData Memory::getNodeData(int locationId, bool images, bool scan, bool user
 			((!images || !s->sensorData().imageCompressed().empty()) &&
 			 (!scan || !s->sensorData().laserScanCompressed().isEmpty()) &&
 			 (!userData || !s->sensorData().userDataCompressed().empty()) &&
-			 (!occupancyGrid || s->sensorData().gridCellSize() != 0.0f))))
+			 (!occupancyGrid ||
+				!s->sensorData().gridGroundCellsCompressed().empty() ||
+				!s->sensorData().gridObstacleCellsCompressed().empty() ||
+				!s->sensorData().gridEmptyCellsCompressed().empty()))))
 	{
 		r = s->sensorData();
 		if(!images)

corelib/src/Rtabmap.cpp

@@ -5920,6 +5920,11 @@ Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userDat
 				s.sensorData().setGlobalDescriptors(globalDescriptors);
 			}
 		}
+		if(!withGlobalDescriptors)
+		{
+			// Node data taken from memory comes with its global descriptors.
+			s.sensorData().clearGlobalDescriptors();
+		}
 		if(velocity.size()==6)
 		{
 			s.setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]);

corelib/test/test_memory.cpp

@@ -3131,6 +3131,50 @@ TEST(MemoryTest, GetNodeDataReturnsInMemoryPayloadsWhenSignatureNotSaved)
 	EXPECT_EQ(r.imageCompressed().cols, s->sensorData().imageCompressed().cols);
 }
 
+TEST(MemoryTest, GetNodeDataLoadsTheGridOfASavedSignatureFromDatabase)
+{
+	// Once a signature still in WM is saved (Rtabmap::process() does it right after
+	// adding it, when the database is not in memory), saveLocationData() drops its
+	// compressed data but keeps the raw grid cells, so gridCellSize() stays set. A
+	// request for the grid alone must not be answered from memory on the strength of
+	// that cell size: it would return the raw cells only, and callers that only read
+	// the compressed ones (e.g., rtabmap_ros's conversion to messages) would get an
+	// empty grid. It has to be loaded from the database, like the other payloads.
+	const std::string dbPath = uniqueDbPath();
+	ParametersMap params = defaultMemoryParams();
+	params[Parameters::kMemBinDataKept()] = "true";
+	params[Parameters::kRGBDCreateOccupancyGrid()] = "true";
+	Memory memory(params);
+	ASSERT_TRUE(memory.init(dbPath));
+
+	SensorData data(cv::Mat(8, 8, CV_8UC1, cv::Scalar(128)));
+	cv::Mat obstacles(1, 3, CV_32FC3);
+	for(int i = 0; i < 3; ++i)
+	{
+		obstacles.at<cv::Vec3f>(0, i) = cv::Vec3f(float(i) * 0.1f, 0.0f, 0.0f);
+	}
+	const float kCellSize = 0.05f;
+	data.setOccupancyGrid(cv::Mat(), obstacles, cv::Mat(), kCellSize, cv::Point3f(0, 0, 0));
+	ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
+	const int id = memory.getLastSignatureId();
+
+	memory.saveLocationData(id);
+	const Signature * s = memory.getSignature(id);
+	ASSERT_NE(s, nullptr);
+	ASSERT_TRUE(s->isSaved());
+	ASSERT_TRUE(s->sensorData().gridObstacleCellsCompressed().empty()); // dropped by the save
+	ASSERT_FLOAT_EQ(s->sensorData().gridCellSize(), kCellSize);         // but still set
+	memory.emptyTrash(); // flush the async writer so the row can be read back
+
+	SensorData r = memory.getNodeData(id, /*images=*/false, /*scan=*/false, /*userData=*/false, /*occupancyGrid=*/true);
+	EXPECT_FALSE(r.gridObstacleCellsCompressed().empty());
+	EXPECT_FLOAT_EQ(r.gridCellSize(), kCellSize);
+	EXPECT_EQ(r.imageCompressed().rows, 0);
+
+	memory.close(false);
+	UFile::erase(dbPath);
+}
+
 TEST(MemoryTest, GetNodeDataMasksFieldsThatWereNotRequested)
 {
 	// Even when a signature has all payloads populated, getNodeData must clear the
@@ -3270,6 +3314,7 @@ TEST(MemoryTest, GetNodeDataLoadsEachPayloadTypeFromDatabase)
 		expectScanEmpty(r.laserScanCompressed());
 		EXPECT_EQ(r.userDataCompressed().rows, 0);
 		EXPECT_FLOAT_EQ(r.gridCellSize(), kCellSize);
+		EXPECT_FALSE(r.gridObstacleCellsCompressed().empty()); // the cells, not just the cell size
 	}
 
 	// All four together.

corelib/test/test_rtabmap.cpp

@@ -1,5 +1,6 @@
 #include <gtest/gtest.h>
 #include <rtabmap/core/Rtabmap.h>
+#include <rtabmap/core/GlobalDescriptor.h>
 #include <rtabmap/core/GPS.h>
 #include <rtabmap/core/Landmark.h>
 #include <rtabmap/core/Memory.h>
@@ -1337,6 +1338,49 @@ TEST(RtabmapTest, GetSignatureCopyReturnsRequestedPayloads)
 	rtabmap.close(false);
 }
 
+TEST(RtabmapTest, GetSignatureCopyReturnsOnlyTheSavedGridWhenAskedAlone)
+{
+	// With a database on disk, process() saves each new node right away, which drops its
+	// compressed data from memory but keeps its global descriptors and its raw grid. A
+	// copy asking for the grid alone must still return the compressed grid, and must not
+	// return global descriptors that were not asked for.
+	const std::string dbPath = uniqueDbPath();
+	ParametersMap params = defaultRtabmapParams();
+	params[Parameters::kMemBinDataKept()] = "true";
+	params[Parameters::kRGBDCreateOccupancyGrid()] = "true";
+	Rtabmap rtabmap;
+	rtabmap.init(params, dbPath);
+
+	SensorData data(cv::Mat(8, 8, CV_8UC1, cv::Scalar(128)));
+	data.setId(1);
+	cv::Mat obstacles(1, 3, CV_32FC3);
+	for(int i = 0; i < 3; ++i)
+	{
+		obstacles.at<cv::Vec3f>(0, i) = cv::Vec3f(float(i) * 0.1f, 0.0f, 0.0f);
+	}
+	data.setOccupancyGrid(cv::Mat(), obstacles, cv::Mat(), 0.05f, cv::Point3f(0, 0, 0));
+	data.setGlobalDescriptors(std::vector<GlobalDescriptor>(1, GlobalDescriptor(1, cv::Mat::ones(1, 8, CV_32FC1))));
+	ASSERT_TRUE(rtabmap.process(data, Transform(0, 0, 0, 0, 0, 0), cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
+	const int id = rtabmap.getLastLocationId();
+	ASSERT_NE(rtabmap.getMemory()->getSignature(id), nullptr);
+	ASSERT_TRUE(rtabmap.getMemory()->getSignature(id)->isSaved());
+
+	const Signature grid = rtabmap.getSignatureCopy(id, /*images=*/false,
+			/*scan=*/false, /*userData=*/false, /*occupancyGrid=*/true,
+			/*withWords=*/false, /*withGlobalDescriptors=*/false);
+	EXPECT_FALSE(grid.sensorData().gridObstacleCellsCompressed().empty());
+	EXPECT_FLOAT_EQ(grid.sensorData().gridCellSize(), 0.05f);
+	EXPECT_TRUE(grid.sensorData().globalDescriptors().empty());
+
+	const Signature descriptors = rtabmap.getSignatureCopy(id, /*images=*/false,
+			/*scan=*/false, /*userData=*/false, /*occupancyGrid=*/true,
+			/*withWords=*/false, /*withGlobalDescriptors=*/true);
+	EXPECT_EQ(descriptors.sensorData().globalDescriptors().size(), 1u);
+
+	rtabmap.close(false);
+	UFile::erase(dbPath);
+}
+
 TEST(RtabmapTest, GetSignatureCopyOmitsImageWhenNotRequested)
 {
 	ParametersMap params = defaultRtabmapParams();