fixed double compression of user_data

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/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/test/test_memory.cpp

@@ -3167,11 +3167,11 @@ TEST(MemoryTest, UpdateKeepsUserDataThatArrivesCompressed)
 	}
 }
 
-TEST(MemoryTest, UpdateReusesTheGivenCompressedScanUnlessItFiltersIt)
+TEST(MemoryTest, UpdateReusesTheGivenCompressedData)
 {
-	// A scan given both raw and compressed is not compressed again: the compressed copy
-	// given is stored as is, sharing its buffer -- but only while Memory has not filtered
-	// the scan, since a filtered scan no longer matches the compressed one given.
+	// 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)
 	{
@@ -3196,6 +3196,8 @@ TEST(MemoryTest, UpdateReusesTheGivenCompressedScanUnlessItFiltersIt)
 				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());
 
@@ -3203,6 +3205,8 @@ TEST(MemoryTest, UpdateReusesTheGivenCompressedScanUnlessItFiltersIt)
 				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")

corelib/test/test_sensordata.cpp

@@ -1,3 +1,4 @@
+#include <rtabmap/core/Compression.h>
 #include <gtest/gtest.h>
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/core/CameraModel.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)