diff --git a/corelib/include/rtabmap/core/SensorData.h b/corelib/include/rtabmap/core/SensorData.h index e55ebbeb..a121ea23 100644 --- a/corelib/include/rtabmap/core/SensorData.h +++ b/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;} diff --git a/corelib/src/SensorData.cpp b/corelib/src/SensorData.cpp index 8604bc35..d9b30ead 100644 --- a/corelib/src/SensorData.cpp +++ b/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); } diff --git a/corelib/test/test_memory.cpp b/corelib/test/test_memory.cpp index 49cb78d9..a08b1d15 100644 --- a/corelib/test/test_memory.cpp +++ b/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") diff --git a/corelib/test/test_sensordata.cpp b/corelib/test/test_sensordata.cpp index c4f266e8..d38b8c2b 100644 --- a/corelib/test/test_sensordata.cpp +++ b/corelib/test/test_sensordata.cpp @@ -1,3 +1,4 @@ +#include #include #include #include @@ -595,6 +596,43 @@ TEST(SensorDataTest, SetUserData) EXPECT_EQ(data.userDataRaw().cols, 100); } +TEST(SensorDataTest, SetUserDataCompressesRawData) +{ + SensorData data; + const cv::Mat userData = (cv::Mat_(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_(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)