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

@@ -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})
 

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;

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<int>::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);

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<cv::KeyPoint> decimatedKpts = decimatedData.keypoints();
+			double log2value = log(double(_imageDecimation))/log(2.0);
+			for(unsigned int i=0; i<decimatedKpts.size(); ++i)
+			{
+				decimatedKpts[i].pt.x /= _imageDecimation;
+				decimatedKpts[i].pt.y /= _imageDecimation;
+				decimatedKpts[i].size /= _imageDecimation;
+				// Never below the finest level of the decimated image, which is as fine
+				// as its detail goes; ORB refuses a negative octave outright.
+				decimatedKpts[i].octave = std::max(0, int(decimatedKpts[i].octave - log2value));
+			}
+			decimatedData.setFeatures(decimatedKpts, decimatedData.keypoints3D(), decimatedData.descriptors());
+		}
+
 		// compute transform
 		t = this->computeTransform(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);
 	}

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());
 		}

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<CameraModel>{model});
+	data.setId(0);
+
+	cv::KeyPoint kpt(128.0f, 120.0f, 8.0f);
+	kpt.octave = octave;
+	data.setFeatures(std::vector<cv::KeyPoint>(1, kpt),
+			std::vector<cv::Point3f>(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

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>rtabmap</name>
-  <version>0.23.11</version>
+  <version>0.23.12</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

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();
 }

utilite/test/test_ufile.cpp

@@ -3,6 +3,8 @@
 #include "rtabmap/utilite/UDirectory.h"
 #include <fstream>
 #include <cstdio>
+#include <iterator>
+#include <string>
 
 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<char>(copied)), std::istreambuf_iterator<char>());
+    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";