From 8da6ea1707a5ee3ddd62eaa9c4c8a46cce442780 Mon Sep 17 00:00:00 2001
From: Borong Yuan <yuanborong@hotmail.com>
Date: Fri, 27 Oct 2023 04:15:34 +0800
Subject: [PATCH] Add missing filterKeypointsByDepth step (#1150)

* add missing filterKeypointsByDepth step

* Fix ORBOctree not using the depth mask

---------

Co-authored-by: matlabbe <matlabbe@gmail.com>
---
 corelib/src/Features2d.cpp | 28 +++++++++++++++++++++++-----
 corelib/src/Memory.cpp     |  4 ++++
 2 files changed, 27 insertions(+), 5 deletions(-)

diff --git a/corelib/src/Features2d.cpp b/corelib/src/Features2d.cpp
index 5df9f05bc8..d87a04e2fe 100644
--- a/corelib/src/Features2d.cpp
+++ b/corelib/src/Features2d.cpp
@@ -732,19 +732,19 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 		for (int j = 0; j<gridCols_; ++j)
 		{
 			cv::Rect roi(globalRoi.x + j*colSize, globalRoi.y + i*rowSize, colSize, rowSize);
-			std::vector<cv::KeyPoint> sub_keypoints;
-			sub_keypoints = this->generateKeypointsImpl(image, roi, mask);
-			limitKeypoints(sub_keypoints, maxFeatures);
+			std::vector<cv::KeyPoint> subKeypoints;
+			subKeypoints = this->generateKeypointsImpl(image, roi, mask);
+			limitKeypoints(subKeypoints, maxFeatures);
 			if(roi.x || roi.y)
 			{
 				// Adjust keypoint position to raw image
-				for(std::vector<cv::KeyPoint>::iterator iter=sub_keypoints.begin(); iter!=sub_keypoints.end(); ++iter)
+				for(std::vector<cv::KeyPoint>::iterator iter=subKeypoints.begin(); iter!=subKeypoints.end(); ++iter)
 				{
 					iter->pt.x += roi.x;
 					iter->pt.y += roi.y;
 				}
 			}
-			keypoints.insert( keypoints.end(), sub_keypoints.begin(), sub_keypoints.end() );
+			keypoints.insert( keypoints.end(), subKeypoints.begin(), subKeypoints.end() );
 		}
 	}
 	UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (grid=%dx%d, mask empty=%d)",
@@ -2119,6 +2119,24 @@ std::vector<cv::KeyPoint> ORBOctree::generateKeypointsImpl(const cv::Mat & image
 
 	(*_orb)(imgRoi, maskRoi, keypoints, descriptors_);
 
+	// OrbOctree ignores the mask, so we have to apply it manually here
+	if(!keypoints.empty() && !maskRoi.empty())
+	{
+		std::vector<cv::KeyPoint> validKeypoints;
+		validKeypoints.reserve(keypoints.size());
+		cv::Mat validDescriptors;
+		for(size_t i=0; i<keypoints.size(); ++i)
+		{
+			if(maskRoi.at<unsigned char>(keypoints[i].pt.y+roi.y, keypoints[i].pt.x+roi.x) != 0)
+			{
+				validKeypoints.push_back(keypoints[i]);
+				validDescriptors.push_back(descriptors_.row(i));
+			}
+		}
+		keypoints = validKeypoints;
+		descriptors_ = validDescriptors;
+	}
+
 	if((int)keypoints.size() > this->getMaxFeatures())
 	{
 		limitKeypoints(keypoints, descriptors_, this->getMaxFeatures());
diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp
index f694849de0..9de4886d06 100644
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -4970,6 +4970,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
 					UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
 				}
+				if(depthMask.empty() && (_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f))
+				{
+					_feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth());
+				}
 			}
 		}
 		else if(data.imageRaw().empty())