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..932231d9 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) 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/test/test_memory.cpp b/corelib/test/test_memory.cpp index 0171232d..3da426d0 100644 --- a/corelib/test/test_memory.cpp +++ b/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(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. 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();