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

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