Fixing various issues detected by rtabmap_slam tests (#1773)

* Fixing getNodeData missing compressed grids

* Fixing roundtrip laserScan <-> Pointcloud2 on all formats.

* Keep already compressed user_data and laser_scan if possible

* fixed double compression of user_data

* reorder headers

* removed dead function declaration

* expand test coverage

corelib/include/rtabmap/core/SensorData.h

@@ -731,11 +731,15 @@ public:
 
 	/**
 	 * Set user data. Detect automatically if raw or compressed. If raw, the data is
-	 * compressed too. A matrix of type CV_8UC1 with 1 row is considered as compressed.
+	 * compressed too, unless compressed user data is already set (only possible with
+	 * @p clearPreviousData=false), which is then assumed to be that raw data compressed
+	 * and kept as is. A matrix of type CV_8UC1 with 1 row is considered as compressed.
 	 * If you have one dimension unsigned 8 bits raw data, make sure to transpose it
 	 * (to have multiple rows instead of multiple columns) in order to be detected as
 	 * not compressed.
 	 * @param clearPreviousData, clear previous raw and compressed user data before setting the new one.
+	 *        With false, setting the raw data of compressed user data already set keeps
+	 *        the compressed one, like setLaserScan() and setRGBDImage() do.
 	 */
 	void setUserData(const cv::Mat & userData, bool clearPreviousData = true);
 	const cv::Mat & userDataRaw() const {return _userDataRaw;}

corelib/include/rtabmap/core/util3d_registration.h

@@ -41,10 +41,6 @@ namespace rtabmap
 namespace util3d
 {
 
-int RTABMAP_CORE_EXPORT getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
-							const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
-							float maxDistance);
-
 /**
  * @brief Estimates the rigid 3D transformation between two point clouds using SVD.
  *

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)
@@ -6669,6 +6672,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		bool reuseCompressedDepthConfidence =
 				depthConfidence.data == data.depthConfidenceRaw().data &&
 				!data.depthConfidenceCompressed().empty();
+		bool reuseCompressedUserData = !data.userDataCompressed().empty();
+		bool reuseCompressedScan =
+				laserScan.data().data == data.laserScanRaw().data().data &&
+				!data.laserScanCompressed().isEmpty();
 
 		cv::Mat compressedImage;
 		cv::Mat compressedDepth;
@@ -6694,11 +6701,11 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			{
 				ctDepthConfidence.start();
 			}
-			if(!laserScan.isEmpty())
+			if(!laserScan.isEmpty() && !reuseCompressedScan)
 			{
 				ctLaserScan.start();
 			}
-			if(!data.userDataRaw().empty())
+			if(!data.userDataRaw().empty() && !reuseCompressedUserData)
 			{
 				ctUserData.start();
 			}
@@ -6711,16 +6718,16 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			compressedImage = ctImage.getCompressedData();
 			compressedDepth = ctDepth.getCompressedData();
 			compressedDepthConfidence = ctDepthConfidence.getCompressedData();
-			compressedScan = ctLaserScan.getCompressedData();
-			compressedUserData = ctUserData.getCompressedData();
+			compressedScan = reuseCompressedScan?data.laserScanCompressed().data():ctLaserScan.getCompressedData();
+			compressedUserData = reuseCompressedUserData?data.userDataCompressed():ctUserData.getCompressedData();
 		}
 		else
 		{
 			compressedImage = reuseCompressedImage?cv::Mat():compressImage2(image, _rgbCompressionFormat);
 			compressedDepth = reuseCompressedDepth?cv::Mat():compressImage2(depthOrRightImage, depthOrRightImage.type() == CV_32FC1 || depthOrRightImage.type() == CV_16UC1?_depthCompressionFormat:_rgbCompressionFormat);
 			compressedDepthConfidence = reuseCompressedDepthConfidence?cv::Mat():compressData2(depthConfidence);
-			compressedScan = compressData2(laserScan.data());
-			compressedUserData = compressData2(data.userDataRaw());
+			compressedScan = reuseCompressedScan?data.laserScanCompressed().data():compressData2(laserScan.data());
+			compressedUserData = reuseCompressedUserData?data.userDataCompressed():compressData2(data.userDataRaw());
 		}
 
 		s = new Signature(id,
@@ -6784,28 +6791,32 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		// just compress user data and laser scan (scans can be used for local scan matching)
 		cv::Mat compressedScan;
 		cv::Mat compressedUserData;
+		bool reuseCompressedUserData = !data.userDataCompressed().empty();
+		bool reuseCompressedScan =
+				laserScan.data().data == data.laserScanRaw().data().data &&
+				!data.laserScanCompressed().isEmpty();
 		if(_compressionParallelized)
 		{
 			rtabmap::CompressionThread ctUserData(data.userDataRaw());
 			rtabmap::CompressionThread ctLaserScan(laserScan.data());
-			if(!data.userDataRaw().empty() && !isIntermediateNode)
+			if(!data.userDataRaw().empty() && !isIntermediateNode && !reuseCompressedUserData)
 			{
 				ctUserData.start();
 			}
-			if(!laserScan.isEmpty() && !isIntermediateNode)
+			if(!laserScan.isEmpty() && !isIntermediateNode && !reuseCompressedScan)
 			{
 				ctLaserScan.start();
 			}
 			ctUserData.join();
 			ctLaserScan.join();
 
-			compressedScan = ctLaserScan.getCompressedData();
-			compressedUserData = ctUserData.getCompressedData();
+			compressedScan = reuseCompressedScan?data.laserScanCompressed().data():ctLaserScan.getCompressedData();
+			compressedUserData = reuseCompressedUserData && !isIntermediateNode?data.userDataCompressed():ctUserData.getCompressedData();
 		}
 		else
 		{
-			compressedScan = compressData2(laserScan.data());
-			compressedUserData = compressData2(data.userDataRaw());
+			compressedScan = reuseCompressedScan?data.laserScanCompressed().data():compressData2(laserScan.data());
+			compressedUserData = reuseCompressedUserData?data.userDataCompressed():compressData2(data.userDataRaw());
 		}
 
 		s = new Signature(id,

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/src/SensorData.cpp

@@ -568,7 +568,7 @@ void SensorData::setUserData(const cv::Mat & userData, bool clearPreviousData)
 	else
 	{
 		_userDataRaw = userData;
-		if(!userData.empty())
+		if(!userData.empty() && _userDataCompressed.empty())
 		{
 			_userDataCompressed = compressData2(userData);
 		}

corelib/src/util3d.cpp

@@ -2328,10 +2328,59 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con
 	{
 		pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
 	}
-	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT || laserScan.format() == LaserScan::kXYZIRT)
+	else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI)
 	{
 		pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
 	}
+	else if(laserScan.format() == LaserScan::kXYZIT || laserScan.format() == LaserScan::kXYZIRT)
+	{
+		// PCL has no point type with time (and ring): append them to the XYZI fields, with
+		// the types laserScanFromPointCloud() reads back (time FLOAT32, ring UINT16).
+		pcl::PCLPointCloud2 xyzi;
+		pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), xyzi);
+		const bool hasRing = laserScan.format() == LaserScan::kXYZIRT;
+
+		cloud->header = xyzi.header;
+		cloud->height = xyzi.height;
+		cloud->width = xyzi.width;
+		cloud->is_bigendian = xyzi.is_bigendian;
+		cloud->is_dense = xyzi.is_dense;
+		cloud->fields = xyzi.fields;
+		pcl::PCLPointField time;
+		time.name = "time";
+		time.offset = xyzi.point_step;
+		time.datatype = pcl::PCLPointField::FLOAT32;
+		time.count = 1;
+		cloud->fields.push_back(time);
+		cloud->point_step = xyzi.point_step + 4;
+		pcl::PCLPointField ring;
+		if(hasRing)
+		{
+			ring.name = "ring";
+			ring.offset = cloud->point_step;
+			ring.datatype = pcl::PCLPointField::UINT16;
+			ring.count = 1;
+			cloud->fields.push_back(ring);
+			cloud->point_step += 4; // keep points 4-byte aligned
+		}
+		cloud->row_step = cloud->point_step * cloud->width;
+		cloud->data.resize(size_t(cloud->row_step) * cloud->height, 0);
+
+		const int cols = laserScan.data().cols;
+		const size_t points = size_t(cloud->width) * cloud->height;
+		for(size_t i=0; i<points; ++i)
+		{
+			unsigned char * dst = &cloud->data[i * cloud->point_step];
+			memcpy(dst, &xyzi.data[i * xyzi.point_step], xyzi.point_step);
+			const float * src = laserScan.data().ptr<float>(int(i) / cols, int(i) % cols);
+			memcpy(dst + time.offset, src + laserScan.getTimeOffset(), sizeof(float));
+			if(hasRing)
+			{
+				const std::uint16_t r = (std::uint16_t)src[laserScan.getRingOffset()];
+				memcpy(dst + ring.offset, &r, sizeof(r));
+			}
+		}
+	}
 	else if(laserScan.format() == LaserScan::kXYNormal || laserScan.format() == LaserScan::kXYZNormal)
 	{
 		pcl::toPCLPointCloud2(*laserScanToPointCloudNormal(laserScan, transform), *cloud);

corelib/test/test_dbdriver.cpp

@@ -956,6 +956,53 @@ TEST_F(DbDriverFixture, LabelAndGraphQueries)
 	EXPECT_TRUE(lastNodeIds.count(4));
 }
 
+// getNodeData() answers from the trash -- signatures waiting to be written -- when it can.
+// For a saved signature, that is only when the compressed payload asked for is still in
+// it: saving drops a signature's compressed occupancy grid but keeps the raw cells, and so
+// the cell size, which alone does not mean the compressed grid is there.
+TEST_F(DbDriverFixture, GetNodeDataTakesTheGridFromTheTrashOnlyIfCompressed)
+{
+	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);
+	}
+
+	// Node 1 in the database, with its compressed grid.
+	Signature * written = new Signature(1);
+	attachSensorDataForDatabaseSave(*written);
+	written->sensorData().setOccupancyGrid(cv::Mat(), obstacles, cv::Mat(), 0.05f, cv::Point3f());
+	saveSignature(written);
+
+	// Node 2, saved but only in the trash, with its compressed grid: taken from there.
+	Signature * pending = new Signature(2);
+	pending->sensorData().setOccupancyGrid(cv::Mat(), obstacles, cv::Mat(), 0.05f, cv::Point3f());
+	pending->setSaved(true);
+	driver_->asyncSave(pending);
+	{
+		SensorData data;
+		driver_->getNodeData(2, data, false, false, false, true);
+		EXPECT_FALSE(data.gridObstacleCellsCompressed().empty());
+		EXPECT_FLOAT_EQ(0.05f, data.gridCellSize());
+	}
+
+	// A copy of node 1 in the trash, its compressed grid dropped as saving does, the raw
+	// cells and the cell size kept: the grid comes from the database instead.
+	Signature * stale = new Signature(1);
+	stale->sensorData().setOccupancyGrid(cv::Mat(), obstacles, cv::Mat(), 0.05f, cv::Point3f());
+	stale->sensorData().clearCompressedData(false, false, false, true);
+	stale->setSaved(true);
+	ASSERT_TRUE(stale->sensorData().gridObstacleCellsCompressed().empty());
+	ASSERT_FLOAT_EQ(0.05f, stale->sensorData().gridCellSize());
+	driver_->asyncSave(stale);
+	{
+		SensorData data;
+		driver_->getNodeData(1, data, false, false, false, true);
+		EXPECT_FALSE(data.gridObstacleCellsCompressed().empty());
+		EXPECT_FLOAT_EQ(0.05f, data.gridCellSize());
+	}
+}
+
 TEST_F(DbDriverFixture, GetNodeDataAndLocalFeatures)
 {
 	Signature * sig = new Signature(1);

corelib/test/test_memory.cpp

@@ -3131,6 +3131,142 @@ TEST(MemoryTest, GetNodeDataReturnsInMemoryPayloadsWhenSignatureNotSaved)
 	EXPECT_EQ(r.imageCompressed().cols, s->sensorData().imageCompressed().cols);
 }
 
+TEST(MemoryTest, UpdateKeepsUserDataThatArrivesCompressed)
+{
+	// User data can reach update() already compressed, with no raw copy -- as it does
+	// from a serialized SensorData (e.g., rtabmap_ros's SensorData messages). It must be
+	// stored as it is, like already-compressed images, rather than dropped for lack of
+	// raw data to compress. Checked with and without Mem/BinDataKept, which build the
+	// signature in two different branches, and with and without parallel compression.
+	const cv::Mat userData = (cv::Mat_<float>(1, 4) << 1.0f, 2.0f, 3.0f, 4.0f);
+	for(const char * binDataKept : {"true", "false"})
+	{
+		for(const char * parallel : {"true", "false"})
+		{
+			SCOPED_TRACE(std::string("Mem/BinDataKept=") + binDataKept +
+					" Mem/CompressionParallelized=" + parallel);
+			ParametersMap params = defaultMemoryParams();
+			params[Parameters::kMemBinDataKept()] = binDataKept;
+			params[Parameters::kMemCompressionParallelized()] = parallel;
+			Memory memory(params);
+
+			SensorData data(cv::Mat(8, 8, CV_8UC1, cv::Scalar(128)));
+			data.setUserData(compressData2(userData)); // bytes: taken as already compressed
+			ASSERT_TRUE(data.userDataRaw().empty());
+			ASSERT_FALSE(data.userDataCompressed().empty());
+
+			ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
+			const Signature * s = memory.getSignature(memory.getLastSignatureId());
+			ASSERT_NE(s, nullptr);
+			ASSERT_FALSE(s->sensorData().userDataCompressed().empty());
+			const cv::Mat stored = uncompressData(s->sensorData().userDataCompressed());
+			ASSERT_EQ(stored.size(), userData.size());
+			ASSERT_EQ(stored.type(), userData.type());
+			EXPECT_EQ(0.0, cv::norm(stored, userData, cv::NORM_INF));
+		}
+	}
+}
+
+TEST(MemoryTest, UpdateReusesTheGivenCompressedData)
+{
+	// Data given both raw and compressed is not compressed again: the compressed copy
+	// given is stored as is, sharing its buffer. For the scan, only while Memory has not
+	// filtered it, since a filtered scan no longer matches the compressed one given.
+	cv::Mat points(1, 10, CV_32FC3);
+	for(int i = 0; i < points.cols; ++i)
+	{
+		points.at<cv::Vec3f>(0, i) = cv::Vec3f(1.0f + i, 0.5f * i, 0.0f);
+	}
+	for(const char * binDataKept : {"true", "false"})
+	{
+		for(const char * parallel : {"true", "false"})
+		{
+			for(const char * downsample : {"1", "2"})
+			{
+				SCOPED_TRACE(std::string("Mem/BinDataKept=") + binDataKept +
+						" Mem/CompressionParallelized=" + parallel +
+						" Mem/LaserScanDownsampleStepSize=" + downsample);
+				ParametersMap params = defaultMemoryParams();
+				params[Parameters::kMemBinDataKept()] = binDataKept;
+				params[Parameters::kMemCompressionParallelized()] = parallel;
+				params[Parameters::kMemLaserScanDownsampleStepSize()] = downsample;
+				Memory memory(params);
+
+				SensorData data(cv::Mat(8, 8, CV_8UC1, cv::Scalar(128)));
+				const LaserScan compressedScan(compressData2(points), points.cols, 10.0f, LaserScan::kXYZ);
+				data.setLaserScan(compressedScan);
+				data.setLaserScan(LaserScan(points, points.cols, 10.0f, LaserScan::kXYZ), false);
+				data.setUserData(points.t()); // raw, several rows: compressed by setUserData()
+				ASSERT_FALSE(data.userDataCompressed().empty());
+				ASSERT_FALSE(data.laserScanRaw().isEmpty());
+				ASSERT_FALSE(data.laserScanCompressed().isEmpty());
+
+				ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), cv::Mat::eye(6, 6, CV_64FC1) * 0.01));
+				const Signature * s = memory.getSignature(memory.getLastSignatureId());
+				ASSERT_NE(s, nullptr);
+
+				EXPECT_EQ(s->sensorData().userDataCompressed().data, data.userDataCompressed().data);
+
+				const LaserScan & stored = s->sensorData().laserScanCompressed();
+				ASSERT_FALSE(stored.isEmpty());
+				if(std::string(downsample) == "1")
+				{
+					EXPECT_EQ(stored.data().data, compressedScan.data().data);
+				}
+				else
+				{
+					EXPECT_NE(stored.data().data, compressedScan.data().data);
+					EXPECT_EQ(uncompressData(stored.data()).cols, points.cols / 2);
+				}
+			}
+		}
+	}
+}
+
+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 +3406,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();

corelib/test/test_sensordata.cpp

@@ -1,5 +1,6 @@
 #include <gtest/gtest.h>
 #include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/Compression.h>
 #include <rtabmap/core/CameraModel.h>
 #include <rtabmap/core/StereoCameraModel.h>
 #include <rtabmap/core/LaserScan.h>
@@ -595,6 +596,43 @@ TEST(SensorDataTest, SetUserData)
     EXPECT_EQ(data.userDataRaw().cols, 100);
 }
 
+TEST(SensorDataTest, SetUserDataCompressesRawData)
+{
+    SensorData data;
+    const cv::Mat userData = (cv::Mat_<float>(1, 4) << 1.0f, 2.0f, 3.0f, 4.0f);
+
+    data.setUserData(userData);
+
+    ASSERT_FALSE(data.userDataCompressed().empty());
+    EXPECT_EQ(0.0, cv::norm(uncompressData(data.userDataCompressed()), userData, cv::NORM_INF));
+}
+
+// Without clearing, the raw data of compressed user data already set is added to it:
+// the compressed copy is kept rather than compressed again, as setLaserScan() and
+// setRGBDImage() do. With nothing compressed yet, the raw data is still compressed.
+TEST(SensorDataTest, SetUserDataWithoutClearingKeepsTheCompressedCopy)
+{
+    const cv::Mat userData = (cv::Mat_<float>(1, 4) << 1.0f, 2.0f, 3.0f, 4.0f);
+    const cv::Mat compressed = compressData2(userData);
+
+    SensorData data;
+    data.setUserData(compressed);
+    ASSERT_TRUE(data.userDataRaw().empty());
+    data.setUserData(userData, false);
+    EXPECT_EQ(data.userDataRaw().data, userData.data);
+    EXPECT_EQ(data.userDataCompressed().data, compressed.data);
+
+    SensorData fresh;
+    fresh.setUserData(userData, false);
+    ASSERT_FALSE(fresh.userDataCompressed().empty());
+    EXPECT_EQ(0.0, cv::norm(uncompressData(fresh.userDataCompressed()), userData, cv::NORM_INF));
+
+    // Clearing, the default, compresses the new data again.
+    data.setUserData(userData);
+    EXPECT_NE(data.userDataCompressed().data, compressed.data);
+    EXPECT_EQ(0.0, cv::norm(uncompressData(data.userDataCompressed()), userData, cv::NORM_INF));
+}
+
 // Occupancy Grid Tests
 
 TEST(SensorDataTest, SetOccupancyGrid)

corelib/test/test_util3d.cpp

@@ -1024,6 +1024,47 @@ TEST(Util3dTest, LaserScanFromPointCloudXYZINormal) {
     EXPECT_FLOAT_EQ(pt.normal_z, 1.0f);
 }
 
+// laserScanToPointCloud2() and laserScanFromPointCloud() are each other's inverse, in
+// every format: the per-point time and ring included, which no PCL point type holds, and
+// 2D scans, which come back 2D when is2D is set.
+TEST(Util3dTest, LaserScanPointCloud2RoundTripEveryFormat) {
+    for(int f = LaserScan::kXY; f <= LaserScan::kXYZIRT; ++f)
+    {
+        const LaserScan::Format format = (LaserScan::Format)f;
+        SCOPED_TRACE(LaserScan::formatName(format));
+        const int channels = LaserScan::channels(format);
+
+        // Small integers: exact through float and through the ring's UINT16 field.
+        cv::Mat points(1, 3, CV_32FC(channels));
+        for(int i = 0; i < points.cols; ++i)
+        {
+            float * p = points.ptr<float>(0, i);
+            for(int c = 0; c < channels; ++c)
+            {
+                p[c] = float(1 + i + c);
+            }
+        }
+        const LaserScan scan(points, 360, 10.0f, format);
+        if(scan.hasRGB())
+        {
+            for(int i = 0; i < points.cols; ++i)
+            {
+                const uint32_t rgb = 0x00102030u + i; // packed 0x00RRGGBB, as PCL stores it
+                memcpy(points.ptr<float>(0, i) + scan.getRGBOffset(), &rgb, sizeof(float));
+            }
+        }
+
+        pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(scan);
+        ASSERT_EQ(cloud->width * cloud->height, (unsigned int)points.cols);
+        const LaserScan out = util3d::laserScanFromPointCloud(*cloud, true, scan.is2d());
+
+        EXPECT_EQ(out.format(), format);
+        ASSERT_EQ(out.data().size(), scan.data().size());
+        ASSERT_EQ(out.data().type(), scan.data().type());
+        EXPECT_EQ(0, memcmp(out.data().data, scan.data().data, scan.data().total() * scan.data().elemSize()));
+    }
+}
+
 TEST(Util3dTest, LaserScan2dFromPointCloudXYZ) {
     pcl::PointCloud<pcl::PointXYZ> cloud;
     cloud.push_back(pcl::PointXYZ(1.0f, 2.0f, 3.0f));