Added GridAdaptor for FAST feature (issue #43). Fixed RegistrationVis feature parameters not set. Added FAST_ORB feature type.

2026-09-01 17:10:26 +08:00 · 2015-11-25 19:56:14 -05:00
parent da1129cf79
commit 63c96f39a3
8 changed files with 331 additions and 71 deletions
--- a/corelib/include/rtabmap/core/Features2d.h
+++ b/corelib/include/rtabmap/core/Features2d.h
@@ -99,7 +99,8 @@ public:
 		kFeatureGfttFreak=5,
 		kFeatureGfttBrief=6,
 		kFeatureBrisk=7,
-		kFeatureGfttOrb=8}; //new 0.10.11
+		kFeatureGfttOrb=8,   //new 0.10.11
+		kFeatureFastOrb=9};  //new 0.11.0

 	static Feature2D * create(Feature2D::Type & type, const ParametersMap & parameters);

@@ -253,8 +254,13 @@ private:
 	bool nonmaxSuppression_;
 	bool gpu_;
 	double gpuKeypointsRatio_;
+	int minThreshold_;
+	int maxThreshold_;
+	int maxTotalKeypoints_;
+	int gridRows_;
+	int gridCols_;

-	cv::Ptr<CV_FAST> _fast;
+	cv::Ptr<cv::FeatureDetector> _fast;
 	cv::Ptr<CV_FAST_GPU> _gpuFast;
 };

@@ -299,6 +305,23 @@ private:
 	cv::Ptr<CV_FREAK> _freak;
 };

+//FAST_ORB
+class RTABMAP_EXP FAST_ORB : public FAST
+{
+public:
+	FAST_ORB(const ParametersMap & parameters = ParametersMap());
+	virtual ~FAST_ORB();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureFastOrb;}
+
+private:
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	ORB _orb;
+};
+
 //GFTT
 class RTABMAP_EXP GFTT : public Feature2D
 {
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/corelib/include/rtabmap/core/Parameters.h
@@ -244,12 +244,16 @@ class RTABMAP_EXP Parameters

 	RTABMAP_PARAM(BRIEF, Bytes,         	   int, 32, 	"Bytes is a length of descriptor in bytes. It can be equal 16, 32 or 64 bytes.");

-	RTABMAP_PARAM(FAST, Threshold,          int, 30, 	   "Threshold on difference between intensity of the central pixel and pixels of a circle around this pixel.");
+	RTABMAP_PARAM(FAST, Threshold,          int, 30, 	    "Threshold on difference between intensity of the central pixel and pixels of a circle around this pixel.");
 	RTABMAP_PARAM(FAST, NonmaxSuppression,  bool, true, 	"If true, non-maximum suppression is applied to detected corners (keypoints).");
 	RTABMAP_PARAM(FAST, Gpu,                bool, false, 	"GPU-FAST: Use GPU version of FAST. This option is enabled only if OpenCV is built with CUDA and GPUs are detected.");
 	RTABMAP_PARAM(FAST, GpuKeypointsRatio,  double, 0.05, 	"Used with FAST GPU.");
+	RTABMAP_PARAM(FAST, MinThreshold,       int, 1, 	    "Minimum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
+	RTABMAP_PARAM(FAST, MaxThreshold,       int, 200, 	    "Maximum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
+	RTABMAP_PARAM(FAST, GridRows,           int, 4,         "Grid rows (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
+	RTABMAP_PARAM(FAST, GridCols,           int, 4,         "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");

-	RTABMAP_PARAM(GFTT, QualityLevel, double, 0.0001, "");
+	RTABMAP_PARAM(GFTT, QualityLevel, double, 0.01, "");
 	RTABMAP_PARAM(GFTT, MinDistance, double, 10, "");
 	RTABMAP_PARAM(GFTT, BlockSize, int, 3, "");
 	RTABMAP_PARAM(GFTT, UseHarrisDetector, bool, false, "");
@@ -312,7 +316,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, ProximityPathRawPosesUsed,    bool, true,   "When comparing to a local path, merge the scan using the odometry poses (with neighbor link optimizations) instead of the ones in the optimized local graph.");

 	// Graph optimization
-	RTABMAP_PARAM(RGBD, OptimizeStrategy,          int, 0,       "Graph optimization strategy: 0=TORO and 1=g2o.");
+	RTABMAP_PARAM(RGBD, OptimizeStrategy,          int, 0,       "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
 	RTABMAP_PARAM(RGBD, OptimizeIterations,        int, 100,     "Optimization iterations.");
 	RTABMAP_PARAM(RGBD, OptimizeSlam2D,            bool, false,  "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
 	RTABMAP_PARAM(RGBD, OptimizeVarianceIgnored,   bool, false,  "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
--- a/corelib/include/rtabmap/core/RegistrationVis.h
+++ b/corelib/include/rtabmap/core/RegistrationVis.h
@@ -38,7 +38,7 @@ class RegistrationVis : public Registration
 {
 public:
 	RegistrationVis(const ParametersMap & parameters = ParametersMap());
-	virtual ~RegistrationVis() {}
+	virtual ~RegistrationVis();

 	virtual void parseParameters(const ParametersMap & parameters);

@@ -67,18 +67,7 @@ private:
 	double _PnPReprojError;
 	int _PnPFlags;

-	int _NNType;
-	float _NNDR;
-	int _featureType;
-	int _maxWords;
-	float _maxDepth;
-	float _minDepth;
-	std::string _roiRatios;
-
-	int _subPixWinSize;
-	int _subPixIterations;
-	double _subPixEps;
-
+	ParametersMap _featureParameters;
 };

 }
--- a/corelib/include/rtabmap/core/Signature.h
+++ b/corelib/include/rtabmap/core/Signature.h
@@ -45,8 +45,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {

-class Memory;
-
 class RTABMAP_EXP Signature
 {

--- a/corelib/src/Features2d.cpp
+++ b/corelib/src/Features2d.cpp
@@ -386,12 +386,18 @@ Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & para
 	case Feature2D::kFeatureFastFreak:
 		feature2D = new FAST_FREAK(parameters);
 		break;
+	case Feature2D::kFeatureFastOrb:
+		feature2D = new FAST_ORB(parameters);
+		break;
 	case Feature2D::kFeatureGfttFreak:
 		feature2D = new GFTT_FREAK(parameters);
 		break;
 	case Feature2D::kFeatureGfttBrief:
 		feature2D = new GFTT_BRIEF(parameters);
 		break;
+	case Feature2D::kFeatureGfttOrb:
+		feature2D = new GFTT_ORB(parameters);
+		break;
 	case Feature2D::kFeatureBrisk:
 		feature2D = new BRISK(parameters);
 		break;
@@ -408,6 +414,7 @@ Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & para
 #endif

 	}
+
 	return feature2D;
 }
 std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, const cv::Rect & roi) const
@@ -798,7 +805,12 @@ FAST::FAST(const ParametersMap & parameters) :
 		threshold_(Parameters::defaultFASTThreshold()),
 		nonmaxSuppression_(Parameters::defaultFASTNonmaxSuppression()),
 		gpu_(Parameters::defaultFASTGpu()),
-		gpuKeypointsRatio_(Parameters::defaultFASTGpuKeypointsRatio())
+		gpuKeypointsRatio_(Parameters::defaultFASTGpuKeypointsRatio()),
+		minThreshold_(Parameters::defaultFASTMinThreshold()),
+		maxThreshold_(Parameters::defaultFASTMaxThreshold()),
+		maxTotalKeypoints_(Parameters::defaultKpWordsPerImage()),
+		gridRows_(Parameters::defaultFASTGridRows()),
+		gridCols_(Parameters::defaultFASTGridCols())
 {
 	parseParameters(parameters);
 }
@@ -816,6 +828,17 @@ void FAST::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kFASTGpu(), gpu_);
 	Parameters::parse(parameters, Parameters::kFASTGpuKeypointsRatio(), gpuKeypointsRatio_);

+	Parameters::parse(parameters, Parameters::kFASTMinThreshold(), minThreshold_);
+	Parameters::parse(parameters, Parameters::kFASTMaxThreshold(), maxThreshold_);
+	Parameters::parse(parameters, Parameters::kKpWordsPerImage(), maxTotalKeypoints_);
+	Parameters::parse(parameters, Parameters::kFASTGridRows(), gridRows_);
+	Parameters::parse(parameters, Parameters::kFASTGridCols(), gridCols_);
+
+	UWARN("minThreshold_=%d", minThreshold_);
+
+	UASSERT_MSG(threshold_ >= minThreshold_, uFormat("%d vs %d", threshold_, minThreshold_).c_str());
+	UASSERT_MSG(threshold_ <= maxThreshold_, uFormat("%d vs %d", threshold_, maxThreshold_).c_str());
+
 #if CV_MAJOR_VERSION < 3
 	if(gpu_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
 	{
@@ -854,7 +877,25 @@ void FAST::parseParameters(const ParametersMap & parameters)
 	else
 	{
 #if CV_MAJOR_VERSION < 3
-		_fast = cv::Ptr<CV_FAST>(new CV_FAST(threshold_, nonmaxSuppression_));
+		if(gridRows_ > 0 && gridCols_ > 0)
+		{
+			cv::Ptr<cv::FeatureDetector> fastAdjuster = cv::Ptr<cv::FastAdjuster>(new cv::FastAdjuster(threshold_, nonmaxSuppression_, minThreshold_, maxThreshold_));
+			_fast = cv::Ptr<cv::FeatureDetector>(new cv::GridAdaptedFeatureDetector(fastAdjuster, maxTotalKeypoints_, gridRows_, gridCols_));
+		}
+		else
+		{
+			if(gridRows_ > 0)
+			{
+				UWARN("Parameter \"%s\" is set (value=%d) but not \"%s\"! Grid adaptor will not be added.",
+						Parameters::kFASTGridRows().c_str(), gridRows_, Parameters::kFASTGridCols().c_str());
+			}
+			else if(gridCols_ > 0)
+			{
+				UWARN("Parameter \"%s\" is set (value=%d) but not \"%s\"! Grid adaptor will not be added.",
+						Parameters::kFASTGridCols().c_str(), gridCols_, Parameters::kFASTGridRows().c_str());
+			}
+			_fast = cv::Ptr<cv::FeatureDetector>(new CV_FAST(threshold_, nonmaxSuppression_));
+		}
 #else
 		_fast = CV_FAST::create(threshold_, nonmaxSuppression_);
 #endif
@@ -983,6 +1024,32 @@ cv::Mat FAST_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<c
 	return descriptors;
 }

+//////////////////////////
+//FAST-ORB
+//////////////////////////
+FAST_ORB::FAST_ORB(const ParametersMap & parameters) :
+	FAST(parameters),
+	_orb(parameters)
+{
+	parseParameters(parameters);
+}
+
+FAST_ORB::~FAST_ORB()
+{
+}
+
+void FAST_ORB::parseParameters(const ParametersMap & parameters)
+{
+	FAST::parseParameters(parameters);
+	_orb.parseParameters(parameters);
+}
+
+cv::Mat FAST_ORB::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	return _orb.generateDescriptors(image, keypoints);
+}
+
 //////////////////////////
 //GFTT
 //////////////////////////
--- a/corelib/src/RegistrationVis.cpp
+++ b/corelib/src/RegistrationVis.cpp
@@ -47,18 +47,29 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters) :
 		_epipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
 		_estimationType(Parameters::defaultVisEstimationType()),
 		_PnPReprojError(Parameters::defaultVisPnPReprojError()),
-		_PnPFlags(Parameters::defaultVisPnPFlags()),
-		_NNType(Parameters::defaultVisNNType()),
-		_NNDR(Parameters::defaultVisNNDR()),
-		_featureType(Parameters::defaultVisFeatureType()),
-		_maxWords(Parameters::defaultVisMaxFeatures()),
-		_maxDepth(Parameters::defaultVisMaxDepth()),
-		_minDepth(Parameters::defaultVisMinDepth()),
-		_roiRatios(Parameters::defaultVisRoiRatios()),
-		_subPixWinSize(Parameters::defaultKpSubPixWinSize()),
-		_subPixIterations(Parameters::defaultKpSubPixIterations()),
-		_subPixEps(Parameters::defaultKpSubPixEps())
+		_PnPFlags(Parameters::defaultVisPnPFlags())
 {
+	_featureParameters = Parameters::getDefaultParameters();
+	uInsert(_featureParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
+	uInsert(_featureParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
+	uInsert(_featureParameters, ParametersPair(Parameters::kMemBinDataKept(), "false"));
+	uInsert(_featureParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true")); // make sure it is incremental
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
+	uInsert(_featureParameters, ParametersPair(Parameters::kMemGenerateIds(), "true"));
+
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisNNType())));
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), _featureParameters.at(Parameters::kVisNNDR())));
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), _featureParameters.at(Parameters::kVisFeatureType())));
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpWordsPerImage(), _featureParameters.at(Parameters::kVisMaxFeatures())));
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), _featureParameters.at(Parameters::kVisMaxDepth())));
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpMinDepth(), _featureParameters.at(Parameters::kVisMinDepth())));
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpRoiRatios(), _featureParameters.at(Parameters::kVisRoiRatios())));
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixEps(), _featureParameters.at(Parameters::kVisSubPixWinSize())));
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixIterations(), _featureParameters.at(Parameters::kVisSubPixIterations())));
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), _featureParameters.at(Parameters::kVisSubPixEps())));
+
 	this->parseParameters(parameters);
 }

@@ -76,19 +87,71 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
 	Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);

-	Parameters::parse(parameters, Parameters::kVisNNType(), _NNType);
-	Parameters::parse(parameters, Parameters::kVisNNDR(), _NNDR);
-	Parameters::parse(parameters, Parameters::kVisFeatureType(), _featureType);
-	Parameters::parse(parameters, Parameters::kVisMaxFeatures(), _maxWords);
-	Parameters::parse(parameters, Parameters::kVisMaxDepth(), _maxDepth);
-
-	Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
-	Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
-	Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps);
-
 	UASSERT_MSG(_minInliers >= 1, uFormat("value=%d", _minInliers).c_str());
 	UASSERT_MSG(_inlierDistance > 0.0f, uFormat("value=%f", _inlierDistance).c_str());
 	UASSERT_MSG(_iterations > 0, uFormat("value=%d", _iterations).c_str());
+
+	// override feature parameters
+	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
+	{
+		std::string group = uSplit(iter->first, '/').front();
+		if(group.compare("SURF") == 0 ||
+			group.compare("SIFT") == 0 ||
+			group.compare("ORB") == 0 ||
+			group.compare("FAST") == 0 ||
+			group.compare("FREAK") == 0 ||
+			group.compare("BRIEF") == 0 ||
+			group.compare("GFTT") == 0 ||
+			group.compare("BRISK") == 0)
+		{
+			uInsert(_featureParameters, ParametersPair(iter->first, iter->second));
+		}
+	}
+
+	if(uContains(parameters, Parameters::kVisNNType()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), parameters.at(Parameters::kVisNNType())));
+	}
+	if(uContains(parameters, Parameters::kVisNNDR()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), parameters.at(Parameters::kVisNNDR())));
+	}
+	if(uContains(parameters, Parameters::kVisFeatureType()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
+	}
+	if(uContains(parameters, Parameters::kVisMaxFeatures()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kKpWordsPerImage(), parameters.at(Parameters::kVisMaxFeatures())));
+	}
+	if(uContains(parameters, Parameters::kVisMaxDepth()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
+	}
+	if(uContains(parameters, Parameters::kVisMinDepth()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kKpMinDepth(), parameters.at(Parameters::kVisMinDepth())));
+	}
+	if(uContains(parameters, Parameters::kVisRoiRatios()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kKpRoiRatios(), parameters.at(Parameters::kVisRoiRatios())));
+	}
+	if(uContains(parameters, Parameters::kVisSubPixEps()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixEps(), parameters.at(Parameters::kVisSubPixEps())));
+	}
+	if(uContains(parameters, Parameters::kVisSubPixIterations()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixIterations(), parameters.at(Parameters::kVisSubPixIterations())));
+	}
+	if(uContains(parameters, Parameters::kVisSubPixWinSize()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), parameters.at(Parameters::kVisSubPixWinSize())));
+	}
+}
+
+RegistrationVis::~RegistrationVis()
+{
 }

 Transform RegistrationVis::computeTransformation(
@@ -117,28 +180,7 @@ Transform RegistrationVis::computeTransformation(
 	if(fromSignature.getWords().size() == 0 && toSignature.getWords().size() == 0)
 	{
 		// Use the Memory class to extract features
-		ParametersMap customParameters;
-		// override some parameters
-		uInsert(customParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
-		uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
-		uInsert(customParameters, ParametersPair(Parameters::kMemBinDataKept(), "false"));
-		uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
-		uInsert(customParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true")); // make sure it is incremental
-		uInsert(customParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
-		uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_NNType))); // bruteforce
-		uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_NNDR)));
-		uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_featureType))); // FAST/BRIEF
-		uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_maxWords)));
-		uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_maxDepth)));
-		uInsert(customParameters, ParametersPair(Parameters::kKpMinDepth(), uNumber2Str(_minDepth)));
-		uInsert(customParameters, ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(_subPixEps)));
-		uInsert(customParameters, ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(_subPixIterations)));
-		uInsert(customParameters, ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(_subPixWinSize)));
-		uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
-		uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), _roiRatios));
-		uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "true"));
-
-		Memory memory(customParameters);
+		Memory memory(_featureParameters);

 		// Add signatures
 		SensorData dataFrom = fromSignature.sensorData();