Merge branch 'master' of github.com:introlab/rtabmap into gtest

corelib/src/Features2d.cpp

@@ -2774,7 +2774,31 @@ cv::Mat SuperPointTorch::generateDescriptorsImpl(const cv::Mat & image, std::vec
 {
 #ifdef RTABMAP_TORCH
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
-	return superPoint_->compute(keypoints);
+	cv::Mat descriptors;
+	if(!keypoints.empty())
+	{
+		descriptors = superPoint_->compute(keypoints);
+		if(descriptors.empty())
+		{
+			// superpoint may have been reset between keypoint detection and now,
+			// re-detect features to re-inialize the descriptors matrix, then
+			// re-extract descriptors with original keypoints.
+			UWARN("Re-initializing superpoint on that image to extract descriptors");
+			if(!superPoint_->detect(image).empty())
+			{
+				descriptors = superPoint_->compute(keypoints);
+				if(descriptors.rows == (int)keypoints.size())
+				{
+					UWARN("Sucessfully re-initialized superpoint, returning %d descriptors.", descriptors.rows);
+				}
+			}
+			else
+			{
+				UWARN("Failed to re-initialize superpoint on that image, returning empty descriptors.");
+			}
+		}
+	}
+	return descriptors;
 #else
 	UWARN("RTAB-Map is not built with Torch support so SuperPoint Torch feature cannot be used!");
 	return cv::Mat();
@@ -2885,7 +2909,31 @@ cv::Mat SuperPointRpautrat::generateDescriptorsImpl(const cv::Mat & image, std::
 {
 #if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
-	return superPoint_->compute(keypoints);
+	cv::Mat descriptors;
+	if(!keypoints.empty())
+	{
+		descriptors = superPoint_->compute(keypoints);
+		if(descriptors.empty())
+		{
+			// superpoint may have been reset between keypoint detection and now,
+			// re-detect features to re-inialize the descriptors matrix, then
+			// re-extract descriptors with original keypoints.
+			UWARN("Re-initializing superpoint on that image to extract descriptors");
+			if(!superPoint_->detect(image).empty())
+			{
+				descriptors = superPoint_->compute(keypoints);
+				if(descriptors.rows == (int)keypoints.size())
+				{
+					UWARN("Sucessfully re-initialized superpoint, returning %d descriptors.", descriptors.rows);
+				}
+			}
+			else
+			{
+				UWARN("Failed to re-initialize superpoint on that image, returning empty descriptors.");
+			}
+		}
+	}
+	return descriptors;
 #else
 	UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!");
 	return cv::Mat();

corelib/src/Memory.cpp

@@ -5213,6 +5213,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		// is using less features than feature2D->getMaxFeatures()
 		meanWordsPerLocation = 0;
 	}
+	UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
 
 	if(_parallelized && !isIntermediateNode)
 	{
@@ -5482,13 +5483,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
 			UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
 
-			UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
-			if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
-			{
-				descriptors = cv::Mat();
-			}
-			else
-			{
 			if(!imagesRectified && decimatedData.cameraModels().size())
 			{
 				UASSERT_MSG((int)keypoints.size() == descriptors.rows, uFormat("%d vs %d", (int)keypoints.size(), descriptors.rows).c_str());
@@ -5636,7 +5630,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				_feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth());
 			}
 		}
-		}
 		else if(data.imageRaw().empty())
 		{
 			UDEBUG("Empty image, cannot extract features...");
@@ -5861,12 +5854,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			t = timer.ticks();
 			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
 			UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
-
-			UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
-			if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
-			{
-				descriptors = cv::Mat();
-			}
 		}
 	}
 
@@ -5892,7 +5879,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	bool addedToDictionary = false;
 	if(!keypoints.empty())
 	{
-		if(descriptors.rows && !isIntermediateNode)
+		if(descriptors.rows && 
+			!isIntermediateNode && // don't add intermediate nodes to dictionary
+			descriptors.rows >= int(_badSignRatio * float(meanWordsPerLocation))) // don't add bad signatures to dictionary
 		{
 			// In case the number of features we want to do quantization is lower
 			// than extracted ones (that would be used for transform estimation)
@@ -5999,7 +5988,11 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		else
 		{
 			// Set all words as not used in dictionary
-			wordIds.resize(keypoints.size(),-1);
+			int negIndex = -1;
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				wordIds.push_back(negIndex--);
+			}
 		}
 
 		t = timer.ticks();

corelib/src/python/PyDetector.cpp

@@ -259,7 +259,15 @@ std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & imag
 
 cv::Mat PyDetector::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
-	UASSERT((int)keypoints.size() == descriptors_.rows);
+	if(!keypoints.empty() && (int)keypoints.size() != descriptors_.rows)
+	{
+		UERROR("The number of keypoints (%ld) doesn't match the number of buffered "
+			"descriptors (%d). PyDetector's descriptors extraction should "
+			"be called right after keypoints detection, with same keypoints "
+			"returned by the detection. Returning empty descriptors.", 
+			keypoints.size(), descriptors_.rows);
+		return cv::Mat();
+	}
 	return descriptors_;
 }
 

corelib/src/superpoint_rpautrat/SuperpointRpautrat.cpp

@@ -130,7 +130,7 @@ cv::Mat SPDetectorRpautrat::compute(const std::vector<cv::KeyPoint> &keypoints)
 {
 	if(!detected_)
 	{
-		UERROR("SPDetector has been reset before extracting the descriptors! detect() should be called before compute().");
+		UERROR("SPDetectorRpautrat has been reset before extracting the descriptors! detect() should be called before compute().");
 		return cv::Mat();
 	}
     if(keypoints.empty())