From 9ed83a71db77aec3629757b1dd75f1c8cd3f7907 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 19 Sep 2026 23:48:45 -0700 Subject: [PATCH] Odom: support features-only frames (#1767) * Odom: support features-only frames * odom: fixed input keypoint scaling when Odom/Decimation is used * fixing octave scaling when decimating image in Memory * Gating negative octave scaling on decimation * backward compatibility with octave issue * narrowing the change, cleanup comments * fixing corrupted file copy on windows --- CMakeLists.txt | 2 +- corelib/include/rtabmap/core/Memory.h | 1 + corelib/src/Memory.cpp | 35 ++++++- corelib/src/Odometry.cpp | 29 +++++- corelib/src/SensorCaptureThread.cpp | 2 +- corelib/test/test_memory.cpp | 138 ++++++++++++++++++++++++++ package.xml | 2 +- utilite/src/UFile.cpp | 6 +- utilite/test/test_ufile.cpp | 29 ++++++ 9 files changed, 234 insertions(+), 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0f622cbb..5783aba7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,7 +22,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") ####################### SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MINOR_VERSION 23) -SET(RTABMAP_PATCH_VERSION 11) +SET(RTABMAP_PATCH_VERSION 12) SET(RTABMAP_VERSION ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index 43b80e1a..b7beb1b8 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -847,6 +847,7 @@ private: bool _stereoFromMotion; unsigned int _imagePreDecimation; unsigned int _imagePostDecimation; + bool _legacyDecimatedOctave; bool _compressionParallelized; float _laserScanDownsampleStepSize; float _laserScanVoxelSize; diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 3d443034..067ae53a 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -101,6 +101,7 @@ Memory::Memory(const ParametersMap & parameters) : _stereoFromMotion(Parameters::defaultMemStereoFromMotion()), _imagePreDecimation(Parameters::defaultMemImagePreDecimation()), _imagePostDecimation(Parameters::defaultMemImagePostDecimation()), + _legacyDecimatedOctave(false), _compressionParallelized(Parameters::defaultMemCompressionParallelized()), _laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()), _laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()), @@ -220,6 +221,23 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter if(_dbDriver->openConnection(dbUrl, dbOverwritten, isReadOnly())) { success = true; + + // Before 0.23.12 the octave of a keypoint scaled into a decimated image was + // moved the wrong way, which changes the pyramid level its descriptor is + // taken from. A map filled that way stays self-consistent only if we keep + // filling it that way; a new one gets the corrected scaling. + _legacyDecimatedOctave = + uStrNumCmp(_dbDriver->getDatabaseVersion(), "0.23.12") < 0; + // Only where the descriptors stored in the map end up different: keypoints + // from odometry, scaled into the pre-decimated image before being described. + if(_legacyDecimatedOctave && _useOdometryFeatures && _imagePreDecimation > 1) + { + UWARN("Database \"%s\" was created by version %s, before the octave of " + "decimated keypoints was corrected (0.23.12). Its features keep " + "being described the old way so that they stay comparable with " + "those already in it.", + dbUrl.c_str(), _dbDriver->getDatabaseVersion().c_str()); + } if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database \"") + dbUrl + "\", done!")); } else @@ -5660,7 +5678,13 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor if(_imagePreDecimation > 1 || useProvided3dPoints) { float decimationRatio = 1.0f / float(_imagePreDecimation); - double log2value = log(double(_imagePreDecimation))/log(2.0); + // The octave a feature was found at moves with the image it is + // expressed in, by the same ratio as its position: a decimated + // image is already that many pyramid levels down, so scaling the + // keypoints into it lowers their octave. Databases older than + // 0.23.12 were filled with it raised instead; see _legacyDecimatedOctave. + double log2value = log(double(_legacyDecimatedOctave? + double(_imagePreDecimation):double(decimationRatio)))/log(2.0); for(unsigned int i=0; i < keypoints.size(); ++i) { cv::KeyPoint & kpt = keypoints[i]; @@ -5669,7 +5693,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor kpt.pt.x *= decimationRatio; kpt.pt.y *= decimationRatio; kpt.size *= decimationRatio; - kpt.octave += log2value; + // Never below the finest level of the image it is now + // expressed in: the detail it was found at is not in there + // any more, and ORB refuses a negative octave outright. + kpt.octave = std::max(0, int(kpt.octave + log2value)); } if(useProvided3dPoints) { @@ -6246,7 +6273,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor UASSERT(keypoints3D.size() == 0 || keypoints3D.size() == wordIds.size()); unsigned int i=0; float decimationRatio = float(preDecimation) / float(_imagePostDecimation); - double log2value = log(double(preDecimation))/log(2.0); + double log2value = log(double(decimationRatio))/log(2.0); for(std::list::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i) { cv::KeyPoint kpt = keypoints[i]; @@ -6256,7 +6283,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor kpt.pt.x *= decimationRatio; kpt.pt.y *= decimationRatio; kpt.size *= decimationRatio; - kpt.octave += log2value; + kpt.octave = std::max(0, int(kpt.octave + log2value)); } words.insert(std::make_pair(*iter, words.size())); wordsKpts.push_back(kpt); diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index ebcd1db1..4b71a7f4 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -779,6 +779,26 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet } + // Features that came with the frame are placed in the full size image, while what + // is about to be registered is the decimated one and the calibration that goes + // with it, so bring them along. They are scaled back below with whatever the + // registration returns, leaving the caller its own frame of reference. + if(!decimatedData.keypoints().empty()) + { + std::vector decimatedKpts = decimatedData.keypoints(); + double log2value = log(double(_imageDecimation))/log(2.0); + for(unsigned int i=0; icomputeTransform(decimatedData, guess, info); @@ -817,7 +837,14 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet } } } - else if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty() || (this->canProcessAsyncIMU() && !data.imu().empty())) + // A frame that brings its own features carries no image, and a frame whose scene was + // empty carries no feature either, so neither says whether there is a frame at all. + // The calibration does: it is there when a camera produced this data. + else if(!data.imageRaw().empty() || + !data.cameraModels().empty() || + !data.stereoCameraModels().empty() || + !data.laserScanRaw().isEmpty() || + (this->canProcessAsyncIMU() && !data.imu().empty())) { t = this->computeTransform(data, guess, info); } diff --git a/corelib/src/SensorCaptureThread.cpp b/corelib/src/SensorCaptureThread.cpp index 9246755a..ab284816 100644 --- a/corelib/src/SensorCaptureThread.cpp +++ b/corelib/src/SensorCaptureThread.cpp @@ -700,7 +700,7 @@ void SensorCaptureThread::postUpdate(SensorData * dataPtr, SensorCaptureInfo * i kpts[i].pt.x /= _imageDecimation; kpts[i].pt.y /= _imageDecimation; kpts[i].size /= _imageDecimation; - kpts[i].octave -= log2value; + kpts[i].octave = std::max(0, int(kpts[i].octave - log2value)); } data.setFeatures(kpts, data.keypoints3D(), data.descriptors()); } diff --git a/corelib/test/test_memory.cpp b/corelib/test/test_memory.cpp index 94848a7d..0171232d 100644 --- a/corelib/test/test_memory.cpp +++ b/corelib/test/test_memory.cpp @@ -2844,6 +2844,144 @@ TEST_F(MemoryFixture, CreateSignatureAutoIncrementsIdWhenGenerateIdsOn) EXPECT_EQ(memory_->getLastSignatureId(), id1 + 1); } +namespace { + +// Mem/ImagePreDecimation with keypoints provided by odometry: createSignature scales them +// into the decimated image it describes them in, and back to the final image size after. +// These parameters and this frame are what the four tests below vary the surroundings of. +ParametersMap decimatedOctaveParams() +{ + ParametersMap params = defaultMemoryParams(); + params[Parameters::kKpMaxFeatures()] = "100"; // let descriptors be extracted + params[Parameters::kMemUseOdomFeatures()] = "true"; + params[Parameters::kMemImagePreDecimation()] = "2"; + params[Parameters::kMemImagePostDecimation()] = "1"; + params[Parameters::kRtabmapImagesAlreadyRectified()] = "true"; // skip rectification + return params; +} + +// One keypoint at @p octave, with its 3D point but no descriptor -- the missing descriptor +// is what sends createSignature down the branch that describes provided keypoints from the +// image. The image is big enough that the keypoint stays far from the border of the +// decimated one, where a descriptor cannot be computed and the keypoint would be dropped. +SensorData decimatedOctaveFrame(int octave) +{ + cv::Mat image(256, 256, CV_8UC1); + cv::RNG rng(7); + rng.fill(image, cv::RNG::UNIFORM, 0, 255); + const CameraModel model(100.0, 100.0, 128.0, 128.0, + CameraModel::opticalRotation(), 0.0, cv::Size(256, 256)); + + SensorData data; + data.setRGBDImage(image, cv::Mat(), std::vector{model}); + data.setId(0); + + cv::KeyPoint kpt(128.0f, 120.0f, 8.0f); + kpt.octave = octave; + data.setFeatures(std::vector(1, kpt), + std::vector(1, cv::Point3f(0.0f, 0.0f, 1.0f)), + cv::Mat()); + return data; +} + +const cv::KeyPoint & theOnlyWord(const Memory & memory) +{ + const Signature * s = memory.getSignature(memory.getLastSignatureId()); + UASSERT(s != 0 && s->getWordsKpts().size() == 1); + return s->getWordsKpts()[0]; +} + +} // namespace + +TEST(MemoryTest, PreDecimationGivesBackProvidedKeypointsAsTheyCameIn) +{ + // With no post-decimation the two conversions undo each other, which is the whole of + // what this test knows: what comes out is what went in, the octave included -- it + // moves down with the image and back up again, a decimated image being that many + // pyramid levels down already. + Memory memory(decimatedOctaveParams()); + const SensorData sent = decimatedOctaveFrame(2); + SensorData data = sent; + + ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), + cv::Mat::eye(6, 6, CV_64FC1) * 0.01)); + const cv::KeyPoint & word = theOnlyWord(memory); + EXPECT_FLOAT_EQ(word.pt.x, sent.keypoints()[0].pt.x); + EXPECT_FLOAT_EQ(word.pt.y, sent.keypoints()[0].pt.y); + EXPECT_FLOAT_EQ(word.size, sent.keypoints()[0].size); + EXPECT_EQ(word.octave, sent.keypoints()[0].octave); +} + +TEST(MemoryTest, PreDecimationKeepsProvidedKeypointsAtTheFinestLevelAvailable) +{ + // The same for a keypoint found at the finest level there is. Scaling it into a + // decimated image would put it below level 0, which does not exist -- the detail it + // was found at was decimated away -- and which ORB rejects outright rather than + // describing. It stays at 0 instead, and so cannot come back at 0: the level it would + // need to return to is the one that was lost. + Memory memory(decimatedOctaveParams()); + const SensorData sent = decimatedOctaveFrame(0); + SensorData data = sent; + + ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), + cv::Mat::eye(6, 6, CV_64FC1) * 0.01)); + const cv::KeyPoint & word = theOnlyWord(memory); + // Where it is and how big it is are unaffected, those having room to scale. + EXPECT_FLOAT_EQ(word.pt.x, sent.keypoints()[0].pt.x); + EXPECT_FLOAT_EQ(word.pt.y, sent.keypoints()[0].pt.y); + EXPECT_FLOAT_EQ(word.size, sent.keypoints()[0].size); + EXPECT_GE(word.octave, 0); +} + +TEST(MemoryTest, PreDecimationOnANewDatabaseUsesTheCorrectedOctaveScaling) +{ + // A database this version created is filled the corrected way. Worth its own test + // because the choice is made from the database's version string: were that to come + // back empty or unreadable, every map would silently be treated as an old one. + const std::string dbPath = uniqueDbPath(); + Memory memory(decimatedOctaveParams()); + ASSERT_TRUE(memory.init(dbPath, true)); + + SensorData data = decimatedOctaveFrame(2); + ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), + cv::Mat::eye(6, 6, CV_64FC1) * 0.01)); + EXPECT_EQ(theOnlyWord(memory).octave, 2); + + memory.close(false); + UFile::erase(dbPath); +} + +TEST(MemoryTest, PreDecimationOnAnOlderDatabaseKeepsTheScalingItWasFilledWith) +{ + // A map made before 0.23.12 holds features described one pyramid level too coarse. + // Adding to it keeps doing that, so that what goes in now can still be matched + // against what is already there; the corrected scaling starts with a new map. Here + // the octave comes back at 2+1+1 rather than 2-1+1. + const std::string source = + std::string(RTABMAP_TEST_DATA_ROOT) + "/tests/pr2_scan2d_corridor_50s.db"; + if(!UFile::exists(source)) + { + GTEST_SKIP() << "Test data not found: " << source + << " (run scripts/fetch_test_data.sh to populate)"; + } + const std::string dbPath = uniqueDbPath(); + UFile::copy(source, dbPath); + + Memory memory(decimatedOctaveParams()); + ASSERT_TRUE(memory.init(dbPath)); + ASSERT_LT(uStrNumCmp(memory.getDatabaseVersion(), "0.23.12"), 0) + << "this fixture is supposed to predate the correction"; + + SensorData data = decimatedOctaveFrame(2); + ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), + cv::Mat::eye(6, 6, CV_64FC1) * 0.01)); + EXPECT_EQ(theOnlyWord(memory).octave, 4) + << "an older map has to keep being filled the way it was"; + + memory.close(false); + UFile::erase(dbPath); +} + TEST(MemoryTest, CreateSignaturePostDecimatesImageWhenPostDecimationGreaterThanOne) { // kMemImagePostDecimation > 1 causes createSignature to downsample the RGB image diff --git a/package.xml b/package.xml index e77d1284..fb89be56 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rtabmap - 0.23.11 + 0.23.12 RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints. Mathieu Labbe Mathieu Labbe diff --git a/utilite/src/UFile.cpp b/utilite/src/UFile.cpp index b057e7c3..da942fe1 100644 --- a/utilite/src/UFile.cpp +++ b/utilite/src/UFile.cpp @@ -96,8 +96,10 @@ std::string UFile::getExtension(const std::string &filePath) void UFile::copy(const std::string & from, const std::string & to) { - std::ifstream src(from.c_str()); - std::ofstream dst(to.c_str()); + // Binary, or Windows translates line endings and stops at the first 0x1A, which + // silently truncates or corrupts anything that is not text -- a database, an image. + std::ifstream src(from.c_str(), std::ios::binary); + std::ofstream dst(to.c_str(), std::ios::binary); dst << src.rdbuf(); } diff --git a/utilite/test/test_ufile.cpp b/utilite/test/test_ufile.cpp index 6472fb75..058781a3 100644 --- a/utilite/test/test_ufile.cpp +++ b/utilite/test/test_ufile.cpp @@ -3,6 +3,8 @@ #include "rtabmap/utilite/UDirectory.h" #include #include +#include +#include TEST(UFileTest, Exists) { @@ -108,6 +110,33 @@ TEST(UFileTest, Copy) std::remove(destFile.c_str()); } +TEST(UFileTest, CopyKeepsBinaryContentByteForByte) +{ + // The bytes a text-mode copy does not survive on Windows: a lone \n, which it turns + // into \r\n, and 0x1A, which it reads as end of file and truncates at. A database or + // an image copied that way comes out corrupted. + const std::string sourceFile = "test_file_binary_source.bin"; + const std::string destFile = "test_file_binary_dest.bin"; + const std::string content("a\nb\r\nc\x1a" "d", 8); + + std::ofstream file(sourceFile.c_str(), std::ios::binary); + file.write(content.data(), content.size()); + file.close(); + + UFile::copy(sourceFile, destFile); + + std::ifstream copied(destFile.c_str(), std::ios::binary); + const std::string copiedContent( + (std::istreambuf_iterator(copied)), std::istreambuf_iterator()); + copied.close(); + + EXPECT_EQ(copiedContent, content); + + // Cleanup + std::remove(sourceFile.c_str()); + std::remove(destFile.c_str()); +} + TEST(UFileTest, InstanceMethods) { std::string testFile = "test_file_instance.txt";