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