Added new 2d feature ORB OcTree (approach used in ORB_SLAM2). report tool: fixed assert caused by bidirectional links. detectModeLoopClosures tool: check if input path exists.

2026-09-02 17:40:23 +08:00 · 2019-04-18 19:50:58 -04:00
parent 8a6c0dad00
commit a0f74eaee2
15 changed files with 1585 additions and 111 deletions
--- a/corelib/src/CMakeLists.txt
+++ b/corelib/src/CMakeLists.txt
@@ -110,6 +110,8 @@ SET(SRC_FILES
 	clams/discrete_depth_distortion_model.cpp
 	clams/frame_projector.cpp
 	clams/slam_calibrator.cpp
+	
+	opencv/ORBextractor.cc
 )

 IF(OpenCV_VERSION_MAJOR EQUAL 2)
--- a/corelib/src/Features2d.cpp
+++ b/corelib/src/Features2d.cpp
@@ -40,6 +40,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/core/version.hpp>
 #include <opencv2/opencv_modules.hpp>

+#ifdef RTABMAP_ORB_OCTREE
+#include "opencv/ORBextractor.h"
+#endif
+
 #if CV_MAJOR_VERSION < 3
 #include "opencv/Orb.h"
 #ifdef HAVE_OPENCV_GPU
@@ -445,6 +449,14 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	}
 #endif

+#ifndef RTABMAP_ORB_OCTREE
+	if(type == Feature2D::kFeatureOrbOctree)
+	{
+		UWARN("ORB OcTree feature cannot be used as RTAB-Map is not built with the option enabled. ORB is used instead.");
+		type = Feature2D::kFeatureOrb;
+	}
+#endif
+
 	Feature2D * feature2D = 0;
 	switch(type)
 	{
@@ -478,6 +490,9 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	case Feature2D::kFeatureKaze:
 		feature2D = new KAZE(parameters);
 		break;
+	case Feature2D::kFeatureOrbOctree:
+		feature2D = new ORBOctree(parameters);
+		break;
 #ifdef RTABMAP_NONFREE
 	default:
 		feature2D = new SURF(parameters);
@@ -494,7 +509,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	return feature2D;
 }

-std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, const cv::Mat & maskIn) const
+std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, const cv::Mat & maskIn)
 {
 	UASSERT(!image.empty());
 	UASSERT(image.type() == CV_8UC1);
@@ -725,7 +740,7 @@ void SURF::parseParameters(const ParametersMap & parameters)
 #endif
 }

-std::vector<cv::KeyPoint> SURF::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
+std::vector<cv::KeyPoint> SURF::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
@@ -833,7 +848,7 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 #endif
 }

-std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
+std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
@@ -956,7 +971,7 @@ void ORB::parseParameters(const ParametersMap & parameters)
 	}
 }

-std::vector<cv::KeyPoint> ORB::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
+std::vector<cv::KeyPoint> ORB::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
@@ -1164,7 +1179,7 @@ void FAST::parseParameters(const ParametersMap & parameters)
 	}
 }

-std::vector<cv::KeyPoint> FAST::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
+std::vector<cv::KeyPoint> FAST::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
@@ -1330,7 +1345,7 @@ void GFTT::parseParameters(const ParametersMap & parameters)
 #endif
 }

-std::vector<cv::KeyPoint> GFTT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
+std::vector<cv::KeyPoint> GFTT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
@@ -1499,7 +1514,7 @@ void BRISK::parseParameters(const ParametersMap & parameters)
 #endif
 }

-std::vector<cv::KeyPoint> BRISK::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
+std::vector<cv::KeyPoint> BRISK::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
@@ -1559,7 +1574,7 @@ void KAZE::parseParameters(const ParametersMap & parameters)
 #endif
 }

-std::vector<cv::KeyPoint> KAZE::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask) const
+std::vector<cv::KeyPoint> KAZE::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
 {
 	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
 	std::vector<cv::KeyPoint> keypoints;
@@ -1589,4 +1604,71 @@ cv::Mat KAZE::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 	return descriptors;
 }

+//////////////////////////
+//ORBOctree
+//////////////////////////
+ORBOctree::ORBOctree(const ParametersMap & parameters) :
+		scaleFactor_(Parameters::defaultORBScaleFactor()),
+		nLevels_(Parameters::defaultORBNLevels()),
+		fastThreshold_(Parameters::defaultFASTThreshold())
+{
+	parseParameters(parameters);
+}
+
+ORBOctree::~ORBOctree()
+{
+}
+
+void ORBOctree::parseParameters(const ParametersMap & parameters)
+{
+	Feature2D::parseParameters(parameters);
+
+	Parameters::parse(parameters, Parameters::kORBScaleFactor(), scaleFactor_);
+	Parameters::parse(parameters, Parameters::kORBNLevels(), nLevels_);
+
+	Parameters::parse(parameters, Parameters::kFASTThreshold(), fastThreshold_);
+	Parameters::parse(parameters, Parameters::kFASTMinThreshold(), fastMinThreshold_);
+
+#ifdef RTABMAP_ORB_OCTREE
+	_orb.reset(new ORBextractor(this->getMaxFeatures(), scaleFactor_, nLevels_, fastThreshold_, fastMinThreshold_));
+#else
+	UWARN("RTAB-Map is not built with ORB OcTree option enabled so ORB OcTree feature cannot be used!");
+#endif
+}
+
+std::vector<cv::KeyPoint> ORBOctree::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
+{
+	std::vector<cv::KeyPoint> keypoints;
+	descriptors_ = cv::Mat();
+#ifdef RTABMAP_ORB_OCTREE
+	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
+	cv::Mat imgRoi(image, roi);
+	cv::Mat maskRoi;
+	if(!mask.empty())
+	{
+		maskRoi = cv::Mat(mask, roi);
+	}
+
+	(*_orb)(imgRoi, maskRoi, keypoints, descriptors_);
+
+	if((int)keypoints.size() > this->getMaxFeatures())
+	{
+		limitKeypoints(keypoints, descriptors_, this->getMaxFeatures());
+	}
+#else
+	UWARN("RTAB-Map is not built with ORB OcTree option enabled so ORB OcTree feature cannot be used!");
+#endif
+	return keypoints;
+}
+
+cv::Mat ORBOctree::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
+{
+#ifdef RTABMAP_ORB_OCTREE
+	UASSERT_MSG((int)keypoints.size() == descriptors_.rows, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors_.rows).c_str());
+#else
+	UWARN("RTAB-Map is not built with ORB OcTree option enabled so ORB OcTree feature cannot be used!");
+#endif
+	return descriptors_;
+}
+
 }
--- a/corelib/src/Parameters.cpp
+++ b/corelib/src/Parameters.cpp
@@ -585,6 +585,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With ORB OcTree:";
+#ifdef RTABMAP_ORB_OCTREE
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With TORO:";
 #ifdef RTABMAP_TORO
--- a/corelib/src/opencv/ORBextractor.cc
+++ b/corelib/src/opencv/ORBextractor.cc
--- a/corelib/src/opencv/ORBextractor.h
+++ b/corelib/src/opencv/ORBextractor.h
@@ -0,0 +1,124 @@
+/**
+ * This code is a version of ORB from OpenCV but
+ * modified by ORB_SLAM2 project to use an OcTree on
+ * keypoint detection. This code is licensed under GPLv3 and BSD licenses.
+ * If you cannot comply to those licenses, please set WITH_ORB_OCTREE=OFF
+ * when building RTAB-Map so that it is not included in the compiled binary.
+ */
+
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 is free software: you can redistribute it and/or modify
+* it under the terms of the GNU General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* (at your option) any later version.
+*
+* ORB-SLAM2 is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef RTABMAP_ORBEXTRACTOR_H
+#define RTABMAP_ORBEXTRACTOR_H
+
+#include <vector>
+#include <list>
+#include <opencv/cv.h>
+
+
+namespace rtabmap
+{
+
+class ExtractorNode
+{
+public:
+    ExtractorNode():bNoMore(false){}
+
+    void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4);
+
+    std::vector<cv::KeyPoint> vKeys;
+    cv::Point2i UL, UR, BL, BR;
+    std::list<ExtractorNode>::iterator lit;
+    bool bNoMore;
+};
+
+class ORBextractor
+{
+public:
+    
+    enum {HARRIS_SCORE=0, FAST_SCORE=1 };
+
+    ORBextractor(int nfeatures, float scaleFactor, int nlevels,
+                 int iniThFAST, int minThFAST);
+
+    ~ORBextractor(){}
+
+    // Compute the ORB features and descriptors on an image.
+    // ORB are dispersed on the image using an octree.
+    // Mask is ignored in the current implementation.
+    void operator()( cv::InputArray image, cv::InputArray mask,
+      std::vector<cv::KeyPoint>& keypoints,
+      cv::OutputArray descriptors);
+
+    int inline GetLevels(){
+        return nlevels;}
+
+    float inline GetScaleFactor(){
+        return scaleFactor;}
+
+    std::vector<float> inline GetScaleFactors(){
+        return mvScaleFactor;
+    }
+
+    std::vector<float> inline GetInverseScaleFactors(){
+        return mvInvScaleFactor;
+    }
+
+    std::vector<float> inline GetScaleSigmaSquares(){
+        return mvLevelSigma2;
+    }
+
+    std::vector<float> inline GetInverseScaleSigmaSquares(){
+        return mvInvLevelSigma2;
+    }
+
+    std::vector<cv::Mat> mvImagePyramid;
+
+protected:
+
+    void ComputePyramid(cv::Mat image);
+    void ComputeKeyPointsOctTree(std::vector<std::vector<cv::KeyPoint> >& allKeypoints);    
+    std::vector<cv::KeyPoint> DistributeOctTree(const std::vector<cv::KeyPoint>& vToDistributeKeys, const int &minX,
+                                           const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level);
+
+    void ComputeKeyPointsOld(std::vector<std::vector<cv::KeyPoint> >& allKeypoints);
+    std::vector<cv::Point> pattern;
+
+    int nfeatures;
+    double scaleFactor;
+    int nlevels;
+    int iniThFAST;
+    int minThFAST;
+
+    std::vector<int> mnFeaturesPerLevel;
+
+    std::vector<int> umax;
+
+    std::vector<float> mvScaleFactor;
+    std::vector<float> mvInvScaleFactor;    
+    std::vector<float> mvLevelSigma2;
+    std::vector<float> mvInvLevelSigma2;
+};
+
+} //namespace rtabmap
+
+#endif
+