i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-09 04:50:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

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.

Browse Source
This commit is contained in:
matlabbe
2019-04-18 19:50:58 -04:00
parent 8a6c0dad00
commit a0f74eaee2
15 changed files with 1585 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Features2d.h
+37 -11
View File
@@ -84,9 +84,10 @@ typedef cv::cuda::ORB CV_ORB_GPU;
typedef cv::cuda::FastFeatureDetector CV_FAST_GPU;
#endif
namespace rtabmap {
class ORBextractor;
class Stereo;
#if CV_MAJOR_VERSION < 3
class CV_ORB;
@@ -105,7 +106,8 @@ public:
kFeatureGfttBrief=6,
kFeatureBrisk=7,
kFeatureGfttOrb=8, //new 0.10.11
kFeatureKaze=9}; //new 0.13.2
kFeatureKaze=9, //new 0.13.2
kFeatureOrbOctree=10}; //new 0.19.2
static Feature2D * create(const ParametersMap & parameters = ParametersMap());
static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
@@ -149,7 +151,7 @@ public:
std::vector<cv::KeyPoint> generateKeypoints(
const cv::Mat & image,
const cv::Mat & mask = cv::Mat()) const;
const cv::Mat & mask = cv::Mat());
cv::Mat generateDescriptors(
const cv::Mat & image,
std::vector<cv::KeyPoint> & keypoints) const;
@@ -165,7 +167,7 @@ protected:
Feature2D(const ParametersMap & parameters = ParametersMap());
private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const = 0;
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) = 0;
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;
private:
@@ -194,7 +196,7 @@ public:
virtual Feature2D::Type getType() const {return kFeatureSurf;}
private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
@@ -221,7 +223,7 @@ public:
virtual Feature2D::Type getType() const {return kFeatureSift;}
private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
@@ -244,7 +246,7 @@ public:
virtual Feature2D::Type getType() const {return kFeatureOrb;}
private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
@@ -275,7 +277,7 @@ public:
virtual Feature2D::Type getType() const {return kFeatureUndef;}
private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const {return cv::Mat();}
private:
@@ -343,7 +345,7 @@ public:
virtual void parseParameters(const ParametersMap & parameters);
private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
private:
double _qualityLevel;
@@ -424,7 +426,7 @@ public:
virtual Feature2D::Type getType() const {return kFeatureBrisk;}
private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
@@ -446,7 +448,7 @@ public:
virtual Feature2D::Type getType() const { return kFeatureKaze; }
private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
@@ -462,6 +464,30 @@ private:
#endif
};
//ORB OCTREE
class RTABMAP_EXP ORBOctree : public Feature2D
{
public:
ORBOctree(const ParametersMap & parameters = ParametersMap());
virtual ~ORBOctree();
virtual void parseParameters(const ParametersMap & parameters);
virtual Feature2D::Type getType() const {return kFeatureOrbOctree;}
private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
private:
float scaleFactor_;
int nLevels_;
int fastThreshold_;
int fastMinThreshold_;
cv::Ptr<ORBextractor> _orb;
cv::Mat descriptors_;
};
}
corelib/src/CMakeLists.txt
+2
View File
@@ -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)
corelib/src/Features2d.cpp
+90 -8
View File
@@ -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_;
}
}
corelib/src/Parameters.cpp
+6
View File
@@ -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
corelib/src/opencv/ORBextractor.cc
+1142
View File
File diff suppressed because it is too large Load Diff
corelib/src/opencv/ORBextractor.h
+124
View File
@@ -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