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 ef4bc453..ca5d6059 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,28 @@ 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 worth saying where the two scalings actually differ: on keypoints + // provided by odometry and scaled into a pre-decimated image, and on the + // remap to a post-decimated one, which used to move the octave by the wrong + // amount rather than the wrong way. Pre-decimation on its own changes + // nothing, features found in the decimated image being at its own levels. + if(_legacyDecimatedOctave && + ((_useOdometryFeatures && _imagePreDecimation > 1) || + (_imagePostDecimation > 1 && _imagePreDecimation != _imagePostDecimation))) + { + 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. Start a new map to get the corrected one.", + dbUrl.c_str(), _dbDriver->getDatabaseVersion().c_str()); + } if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database \"") + dbUrl + "\", done!")); } else @@ -5663,8 +5686,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor // 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. - double log2value = log(double(decimationRatio))/log(2.0); + // 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]; @@ -6255,8 +6280,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor float decimationRatio = float(preDecimation) / float(_imagePostDecimation); // Same ratio the positions are remapped by, which is what keeps a keypoint at // the scale it was found at: log2(pre/post), and not log2(pre), those two - // agreeing only when the final image is not decimated at all. - double log2value = log(double(decimationRatio))/log(2.0); + // agreeing only when the final image is not decimated at all. Databases older + // than 0.23.12 were filled with log2(pre); see _legacyDecimatedOctave. + double log2value = log(double(_legacyDecimatedOctave? + double(preDecimation):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]; diff --git a/corelib/test/test_memory.cpp b/corelib/test/test_memory.cpp index 98fe10dc..0171232d 100644 --- a/corelib/test/test_memory.cpp +++ b/corelib/test/test_memory.cpp @@ -2844,28 +2844,31 @@ TEST_F(MemoryFixture, CreateSignatureAutoIncrementsIdWhenGenerateIdsOn) EXPECT_EQ(memory_->getLastSignatureId(), id1 + 1); } -TEST(MemoryTest, PreDecimationGivesBackProvidedKeypointsAsTheyCameIn) +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() { - // Keypoints provided with the frame are found in the full size image, so - // createSignature scales them into the pre-decimated one it describes them in, and - // scales them back to the final image size afterwards. With no post-decimation the - // two 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. 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 - Memory memory(params); + return params; +} - // Big enough that the keypoint stays far from the border of the decimated image, - // where a descriptor cannot be computed and the keypoint would be dropped. +// 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 cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.01; const CameraModel model(100.0, 100.0, 128.0, 128.0, CameraModel::opticalRotation(), 0.0, cv::Size(256, 256)); @@ -2873,65 +2876,110 @@ TEST(MemoryTest, PreDecimationGivesBackProvidedKeypointsAsTheyCameIn) data.setRGBDImage(image, cv::Mat(), std::vector{model}); data.setId(0); - // No descriptors: that is what sends createSignature down the branch where the - // provided keypoints are described from the image rather than taken wholesale. cv::KeyPoint kpt(128.0f, 120.0f, 8.0f); - kpt.octave = 2; + kpt.octave = octave; data.setFeatures(std::vector(1, kpt), std::vector(1, cv::Point3f(0.0f, 0.0f, 1.0f)), cv::Mat()); + return data; +} - ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), covariance)); +const cv::KeyPoint & theOnlyWord(const Memory & memory) +{ const Signature * s = memory.getSignature(memory.getLastSignatureId()); - ASSERT_NE(s, nullptr); - ASSERT_EQ(s->getWordsKpts().size(), 1u); - EXPECT_FLOAT_EQ(s->getWordsKpts()[0].pt.x, kpt.pt.x); - EXPECT_FLOAT_EQ(s->getWordsKpts()[0].pt.y, kpt.pt.y); - EXPECT_FLOAT_EQ(s->getWordsKpts()[0].size, kpt.size); - EXPECT_EQ(s->getWordsKpts()[0].octave, kpt.octave); + 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 companion of the test above, 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. - ParametersMap params = defaultMemoryParams(); - params[Parameters::kKpMaxFeatures()] = "100"; - params[Parameters::kMemUseOdomFeatures()] = "true"; - params[Parameters::kMemImagePreDecimation()] = "2"; - params[Parameters::kMemImagePostDecimation()] = "1"; - params[Parameters::kRtabmapImagesAlreadyRectified()] = "true"; - Memory memory(params); + // 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; - cv::Mat image(256, 256, CV_8UC1); - cv::RNG rng(7); - rng.fill(image, cv::RNG::UNIFORM, 0, 255); - const cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.01; - 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 = 0; - data.setFeatures(std::vector(1, kpt), - std::vector(1, cv::Point3f(0.0f, 0.0f, 1.0f)), - cv::Mat()); - - ASSERT_TRUE(memory.update(data, Transform(0, 0, 0, 0, 0, 0), covariance)); - const Signature * s = memory.getSignature(memory.getLastSignatureId()); - ASSERT_NE(s, nullptr); - ASSERT_EQ(s->getWordsKpts().size(), 1u); + 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(s->getWordsKpts()[0].pt.x, kpt.pt.x); - EXPECT_FLOAT_EQ(s->getWordsKpts()[0].pt.y, kpt.pt.y); - EXPECT_FLOAT_EQ(s->getWordsKpts()[0].size, kpt.size); - EXPECT_GE(s->getWordsKpts()[0].octave, 0); + 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) 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