From 66c72be7db6863b65fafbc7fa134148503e0b815 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 26 Sep 2026 21:07:59 -0700 Subject: [PATCH] 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 | 6 +- .../rtabmap/core/util3d_registration.h | 4 - corelib/src/DBDriver.cpp | 5 +- corelib/src/Memory.cpp | 37 +++-- corelib/src/Rtabmap.cpp | 5 + corelib/src/SensorData.cpp | 2 +- corelib/src/util3d.cpp | 51 ++++++- corelib/test/test_dbdriver.cpp | 47 ++++++ corelib/test/test_memory.cpp | 137 ++++++++++++++++++ corelib/test/test_rtabmap.cpp | 44 ++++++ corelib/test/test_sensordata.cpp | 38 +++++ corelib/test/test_util3d.cpp | 41 ++++++ 12 files changed, 396 insertions(+), 21 deletions(-) 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/include/rtabmap/core/util3d_registration.h b/corelib/include/rtabmap/core/util3d_registration.h index 0cae8e66..3f85f51e 100644 --- a/corelib/include/rtabmap/core/util3d_registration.h +++ b/corelib/include/rtabmap/core/util3d_registration.h @@ -41,10 +41,6 @@ namespace rtabmap namespace util3d { -int RTABMAP_CORE_EXPORT getCorrespondencesCount(const pcl::PointCloud::ConstPtr & cloud_source, - const pcl::PointCloud::ConstPtr & cloud_target, - float maxDistance); - /** * @brief Estimates the rigid 3D transformation between two point clouds using SVD. * diff --git a/corelib/src/DBDriver.cpp b/corelib/src/DBDriver.cpp index 6692368e..f466d9f0 100644 --- a/corelib/src/DBDriver.cpp +++ b/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) diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 067ae53a..4bef1a52 100644 --- a/corelib/src/Memory.cpp +++ b/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, diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 4096be38..22f2e885 100644 --- a/corelib/src/Rtabmap.cpp +++ b/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]); 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/src/util3d.cpp b/corelib/src/util3d.cpp index 757fb3e8..895e158a 100644 --- a/corelib/src/util3d.cpp +++ b/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; idata[i * cloud->point_step]; + memcpy(dst, &xyzi.data[i * xyzi.point_step], xyzi.point_step); + const float * src = laserScan.data().ptr(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); diff --git a/corelib/test/test_dbdriver.cpp b/corelib/test/test_dbdriver.cpp index adf25e3c..73a5d1af 100644 --- a/corelib/test/test_dbdriver.cpp +++ b/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(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); diff --git a/corelib/test/test_memory.cpp b/corelib/test/test_memory.cpp index 0171232d..a08b1d15 100644 --- a/corelib/test/test_memory.cpp +++ b/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_(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(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(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. diff --git a/corelib/test/test_rtabmap.cpp b/corelib/test/test_rtabmap.cpp index aba74094..5cebc8a8 100644 --- a/corelib/test/test_rtabmap.cpp +++ b/corelib/test/test_rtabmap.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include #include @@ -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(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(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(); diff --git a/corelib/test/test_sensordata.cpp b/corelib/test/test_sensordata.cpp index c4f266e8..2ddfa438 100644 --- a/corelib/test/test_sensordata.cpp +++ b/corelib/test/test_sensordata.cpp @@ -1,5 +1,6 @@ #include #include +#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) diff --git a/corelib/test/test_util3d.cpp b/corelib/test/test_util3d.cpp index 9c3aeb78..de36a4e7 100644 --- a/corelib/test/test_util3d.cpp +++ b/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(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(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 cloud; cloud.push_back(pcl::PointXYZ(1.0f, 2.0f, 3.0f));