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

Added FastCV support (see FAST/CV parameter). VWDictionary: loading fixed dictionary only on updates (to avoid loading it multiple times when memory is reset).

Browse Source
This commit is contained in:
matlabbe
2019-08-14 12:09:56 -04:00
parent d5b4b25287
commit 4b118ae660
15 changed files with 612 additions and 329 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
corelib/include/rtabmap/core/Features2d.h
View File

@@ -289,6 +289,14 @@ private:
int maxThreshold_;
int gridRows_;
int gridCols_;
int fastCV_;
bool fastCVinit_;
int fastCVMaxFeatures_;
int fastCVLastImageHeight_;
uint32_t* fastCVCorners_= NULL;
uint32_t* fastCVCornerScores_ = NULL;
void* fastCVTempBuf_ = NULL;
cv::Ptr<cv::FeatureDetector> _fast;
cv::Ptr<CV_FAST_GPU> _gpuFast;

1
corelib/include/rtabmap/core/Parameters.h
View File

@@ -290,6 +290,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(FAST, MaxThreshold, int, 200, "Maximum threshold. Used only when FAST/GridRows and FAST/GridCols are set.");
RTABMAP_PARAM(FAST, GridRows, int, 0, "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, 0, "Grid cols (0 to disable). Adapts the detector to partition the source image into a grid and detect points in each cell.");
RTABMAP_PARAM(FAST, CV, int, 0, "Enable FastCV implementation if non-zero (and RTAB-Map is built with FastCV support). Values should be 9 and 10.");
RTABMAP_PARAM(GFTT, QualityLevel, double, 0.001, "");
RTABMAP_PARAM(GFTT, MinDistance, double, 3, "");

3
corelib/include/rtabmap/core/VWDictionary.h
View File

@@ -115,7 +115,8 @@ private:
bool _incrementalFlann;
float _rebalancingFactor;
float _nndrRatio;
std::string _dictionaryPath; // a pre-computed dictionary (.txt)
std::string _dictionaryPath; // a pre-computed dictionary (.txt or .db)
std::string _newDictionaryPath; // a pre-computed dictionary (.txt or .db)
bool _newWordsComparedTogether;
int _lastWordId;
bool useDistanceL1_;

193
corelib/src/CMakeLists.txt
View File

@@ -1,18 +1,18 @@
SET(SRC_FILES
Rtabmap.cpp
RtabmapThread.cpp
Statistics.cpp
Memory.cpp
DBDriver.cpp
DBDriverSqlite3.cpp
DBReader.cpp
Recovery.cpp
Rtabmap.cpp
RtabmapThread.cpp
Statistics.cpp
Memory.cpp
DBDriver.cpp
DBDriverSqlite3.cpp
DBReader.cpp
Recovery.cpp
Camera.cpp
CameraThread.cpp
CameraModel.cpp
@@ -33,88 +33,88 @@ SET(SRC_FILES
camera/CameraStereoImages.cpp
camera/CameraStereoVideo.cpp
camera/CameraStereoZed.cpp
camera/CameraStereoTara.cpp
camera/CameraStereoTara.cpp
camera/CameraVideo.cpp
EpipolarGeometry.cpp
VisualWord.cpp
VWDictionary.cpp
BayesFilter.cpp
Parameters.cpp
VisualWord.cpp
VWDictionary.cpp
BayesFilter.cpp
Parameters.cpp
Signature.cpp
Features2d.cpp
Transform.cpp
GeodeticCoords.cpp
util2d.cpp
util3d.cpp
util3d_filtering.cpp
util3d_mapping.cpp
util3d_transforms.cpp
util3d_registration.cpp
util3d_surface.cpp
util3d_features.cpp
util3d_correspondences.cpp
util3d_motion_estimation.cpp
SensorData.cpp
Graph.cpp
Compression.cpp
Link.cpp
LaserScan.cpp
Optimizer.cpp
optimizer/OptimizerTORO.cpp
optimizer/OptimizerG2O.cpp
optimizer/OptimizerGTSAM.cpp
optimizer/OptimizerCVSBA.cpp
Registration.cpp
RegistrationIcp.cpp
RegistrationVis.cpp
Odometry.cpp
OdometryThread.cpp
odometry/OdometryF2M.cpp
odometry/OdometryMono.cpp
odometry/OdometryF2F.cpp
odometry/OdometryFovis.cpp
odometry/OdometryViso2.cpp
odometry/OdometryDVO.cpp
odometry/OdometryOkvis.cpp
odometry/OdometryORBSLAM2.cpp
odometry/OdometryLOAM.cpp
odometry/OdometryMSCKF.cpp
odometry/OdometryVINS.cpp
IMUThread.cpp
IMUFilter.cpp
imufilter/ComplementaryFilter.cpp
Stereo.cpp
StereoDense.cpp
StereoCameraModel.cpp
stereo/StereoBM.cpp
stereo/StereoSGBM.cpp
OccupancyGrid.cpp
MarkerDetector.cpp
GainCompensator.cpp
rtflann/ext/lz4.c
rtflann/ext/lz4hc.c
FlannIndex.cpp
#clams stuff
clams/discrete_depth_distortion_model_helpers.cpp
clams/discrete_depth_distortion_model.cpp
clams/frame_projector.cpp
clams/slam_calibrator.cpp
opencv/ORBextractor.cc
Features2d.cpp
Transform.cpp
GeodeticCoords.cpp
util2d.cpp
util3d.cpp
util3d_filtering.cpp
util3d_mapping.cpp
util3d_transforms.cpp
util3d_registration.cpp
util3d_surface.cpp
util3d_features.cpp
util3d_correspondences.cpp
util3d_motion_estimation.cpp
SensorData.cpp
Graph.cpp
Compression.cpp
Link.cpp
LaserScan.cpp
Optimizer.cpp
optimizer/OptimizerTORO.cpp
optimizer/OptimizerG2O.cpp
optimizer/OptimizerGTSAM.cpp
optimizer/OptimizerCVSBA.cpp
Registration.cpp
RegistrationIcp.cpp
RegistrationVis.cpp
Odometry.cpp
OdometryThread.cpp
odometry/OdometryF2M.cpp
odometry/OdometryMono.cpp
odometry/OdometryF2F.cpp
odometry/OdometryFovis.cpp
odometry/OdometryViso2.cpp
odometry/OdometryDVO.cpp
odometry/OdometryOkvis.cpp
odometry/OdometryORBSLAM2.cpp
odometry/OdometryLOAM.cpp
odometry/OdometryMSCKF.cpp
odometry/OdometryVINS.cpp
IMUThread.cpp
IMUFilter.cpp
imufilter/ComplementaryFilter.cpp
Stereo.cpp
StereoDense.cpp
StereoCameraModel.cpp
stereo/StereoBM.cpp
stereo/StereoSGBM.cpp
OccupancyGrid.cpp
MarkerDetector.cpp
GainCompensator.cpp
rtflann/ext/lz4.c
rtflann/ext/lz4hc.c
FlannIndex.cpp
#clams stuff
clams/discrete_depth_distortion_model_helpers.cpp
clams/discrete_depth_distortion_model.cpp
clams/frame_projector.cpp
clams/slam_calibrator.cpp
opencv/ORBextractor.cc
)
IF(OpenCV_VERSION_MAJOR EQUAL 2)
@@ -348,6 +348,17 @@ IF(libpointmatcher_FOUND)
)
ENDIF(libpointmatcher_FOUND)
IF(FastCV_FOUND)
SET(INCLUDE_DIRS
${INCLUDE_DIRS}
${FastCV_INCLUDE_DIRS}
)
SET(LIBRARIES
${LIBRARIES}
${FastCV_LIBRARIES}
)
ENDIF(FastCV_FOUND)
IF(loam_velodyne_FOUND)
SET(INCLUDE_DIRS
${INCLUDE_DIRS}

125
corelib/src/Features2d.cpp
View File

@@ -65,6 +65,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/xfeatures2d/cuda.hpp>
#endif
#ifdef RTABMAP_FASTCV
#include <fastcv.h>
#endif
namespace rtabmap {
void Feature2D::filterKeypointsByDepth(
@@ -1081,13 +1085,66 @@ FAST::FAST(const ParametersMap & parameters) :
minThreshold_(Parameters::defaultFASTMinThreshold()),
maxThreshold_(Parameters::defaultFASTMaxThreshold()),
gridRows_(Parameters::defaultFASTGridRows()),
gridCols_(Parameters::defaultFASTGridCols())
gridCols_(Parameters::defaultFASTGridCols()),
fastCV_(Parameters::defaultFASTCV()),
fastCVinit_(false),
fastCVMaxFeatures_(10000),
fastCVLastImageHeight_(0)
{
#ifdef RTABMAP_FASTCV
char sVersion[128] = { 0 };
fcvGetVersion(sVersion, 128);
UINFO("fastcv version = %s", sVersion);
int ix;
if ((ix = fcvSetOperationMode(FASTCV_OP_PERFORMANCE)))
{
UERROR("fcvSetOperationMode return=%d, OpenCV FAST will be used instead!", ix);
fastCV_ = 0;
}
else
{
fcvMemInit();
if (!(fastCVCorners_ = (uint32_t*)fcvMemAlloc(fastCVMaxFeatures_ * sizeof(uint32_t) * 2, 16)) ||
!(fastCVCornerScores_ = (uint32_t*)fcvMemAlloc( fastCVMaxFeatures_ * sizeof(uint32_t), 16 )))
{
UERROR("could not alloc fastcv mem, using opencv fast instead!");
if (fastCVCorners_)
{
fcvMemFree(fastCVCorners_);
fastCVCorners_ = NULL;
}
if (fastCVCornerScores_)
{
fcvMemFree(fastCVCornerScores_);
fastCVCornerScores_ = NULL;
}
}
else
{
fastCVinit_ = true;
}
}
#endif
parseParameters(parameters);
}
FAST::~FAST()
{
#ifdef RTABMAP_FASTCV
if(fastCVinit_)
{
fcvMemDeInit();
if (fastCVCorners_)
fcvMemFree(fastCVCorners_);
if (fastCVCornerScores_)
fcvMemFree(fastCVCornerScores_);
if (fastCVTempBuf_)
fcvMemFree(fastCVTempBuf_);
}
#endif
}
void FAST::parseParameters(const ParametersMap & parameters)
@@ -1104,6 +1161,9 @@ void FAST::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kFASTGridRows(), gridRows_);
Parameters::parse(parameters, Parameters::kFASTGridCols(), gridCols_);
Parameters::parse(parameters, Parameters::kFASTCV(), fastCV_);
UASSERT(fastCV_ == 0 || fastCV_ == 9 || fastCV_ == 10);
UASSERT_MSG(threshold_ >= minThreshold_, uFormat("%d vs %d", threshold_, minThreshold_).c_str());
UASSERT_MSG(threshold_ <= maxThreshold_, uFormat("%d vs %d", threshold_, maxThreshold_).c_str());
@@ -1183,6 +1243,69 @@ std::vector<cv::KeyPoint> FAST::generateKeypointsImpl(const cv::Mat & image, con
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
std::vector<cv::KeyPoint> keypoints;
#ifdef RTABMAP_FASTCV
if(fastCV_>0)
{
// Note: mask not supported, it should be the inverse of the current mask used (0=where to extract)
uint32_t nCorners = 0;
UASSERT(fastCVCorners_ != NULL && fastCVCornerScores_ != NULL);
if (nonmaxSuppression_)
{
if(fastCVTempBuf_==NULL || (fastCVTempBuf_!= NULL && fastCVLastImageHeight_!= image.rows))
{
if (fastCVTempBuf_)
{
fcvMemFree(fastCVTempBuf_);
fastCVTempBuf_ = NULL;
}
if(!(fastCVTempBuf_ = (uint32_t*)fcvMemAlloc( (3*fastCVMaxFeatures_+image.rows+1)*4, 16 )))
{
UERROR("could not alloc fastcv mem for temp buf (%s=true)", Parameters::kFASTNonmaxSuppression().c_str());
fastCVLastImageHeight_ = 0;
return keypoints;
}
fastCVLastImageHeight_ = image.rows;
}
}
// image.data should be 128 bits aligned
UDEBUG("%dx%d (step=%d) thr=%d maxFeatures=%d", image.cols, image.rows, image.step1(), threshold_, fastCVMaxFeatures_);
if(fastCV_ == 10)
{
fcvCornerFast10Scoreu8(image.data, image.cols, image.rows, 0, threshold_, 0, fastCVCorners_, fastCVCornerScores_, fastCVMaxFeatures_, &nCorners, nonmaxSuppression_?1:0, fastCVTempBuf_);
}
else
{
fcvCornerFast9Scoreu8_v2(image.data, image.cols, image.rows, image.step1(), threshold_, 0, fastCVCorners_, fastCVCornerScores_, fastCVMaxFeatures_, &nCorners, nonmaxSuppression_?1:0, fastCVTempBuf_);
}
UDEBUG("number of corners found = %d:", nCorners);
keypoints.resize(nCorners);
for (uint32_t i = 0; i < nCorners; i++)
{
keypoints[i].pt.x = fastCVCorners_[i * 2];
keypoints[i].pt.y = fastCVCorners_[(i * 2) + 1];
keypoints[i].size = 3;
keypoints[i].response = fastCVCornerScores_[i];
}
if(this->getMaxFeatures() > 0)
{
this->limitKeypoints(keypoints, this->getMaxFeatures());
}
return keypoints;
}
#endif
if(fastCV_>0)
{
UWARN( "RTAB-Map is not built with FastCV support. OpenCV's FAST is used instead. "
"Please set %s to 0. This message will only appear once.",
Parameters::kFASTCV().c_str());
fastCV_ = 0;
}
cv::Mat imgRoi(image, roi);
cv::Mat maskRoi;
if(!mask.empty())

10
corelib/src/FlannIndex.cpp
View File

@@ -49,6 +49,7 @@ FlannIndex::~FlannIndex()
void FlannIndex::release()
{
UDEBUG("");
if(index_)
{
if(featuresType_ == CV_8UC1)
@@ -76,6 +77,7 @@ void FlannIndex::release()
isLSH_ = false;
addedDescriptors_.clear();
removedIndexes_.clear();
UDEBUG("");
}
unsigned int FlannIndex::indexedFeatures() const
@@ -138,6 +140,7 @@ void FlannIndex::buildLinearIndex(
bool useDistanceL1,
float rebalancingFactor)
{
UDEBUG("");
this->release();
UASSERT(index_ == 0);
UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
@@ -178,6 +181,7 @@ void FlannIndex::buildLinearIndex(
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ = features.rows;
UDEBUG("");
}
void FlannIndex::buildKDTreeIndex(
@@ -186,6 +190,7 @@ void FlannIndex::buildKDTreeIndex(
bool useDistanceL1,
float rebalancingFactor)
{
UDEBUG("");
this->release();
UASSERT(index_ == 0);
UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
@@ -226,6 +231,7 @@ void FlannIndex::buildKDTreeIndex(
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ = features.rows;
UDEBUG("");
}
void FlannIndex::buildKDTreeSingleIndex(
@@ -235,6 +241,7 @@ void FlannIndex::buildKDTreeSingleIndex(
bool useDistanceL1,
float rebalancingFactor)
{
UDEBUG("");
this->release();
UASSERT(index_ == 0);
UASSERT(features.type() == CV_32FC1 || features.type() == CV_8UC1);
@@ -275,6 +282,7 @@ void FlannIndex::buildKDTreeSingleIndex(
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ = features.rows;
UDEBUG("");
}
void FlannIndex::buildLSHIndex(
@@ -284,6 +292,7 @@ void FlannIndex::buildLSHIndex(
unsigned int multi_probe_level,
float rebalancingFactor)
{
UDEBUG("");
this->release();
UASSERT(index_ == 0);
UASSERT(features.type() == CV_8UC1);
@@ -300,6 +309,7 @@ void FlannIndex::buildLSHIndex(
addedDescriptors_.insert(std::make_pair(nextIndex_, features));
nextIndex_ = features.rows;
UDEBUG("");
}
bool FlannIndex::isBuilt()

6
corelib/src/Parameters.cpp
View File

@@ -605,6 +605,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 FastCV:";
#ifdef RTABMAP_FASTCV
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 Madgwick:";
#ifdef RTABMAP_MADGWICK

48
corelib/src/VWDictionary.cpp
View File

@@ -66,7 +66,7 @@ VWDictionary::VWDictionary(const ParametersMap & parameters) :
_incrementalFlann(Parameters::defaultKpIncrementalFlann()),
_rebalancingFactor(Parameters::defaultKpFlannRebalancingFactor()),
_nndrRatio(Parameters::defaultKpNndrRatio()),
_dictionaryPath(Parameters::defaultKpDictionaryPath()),
_newDictionaryPath(Parameters::defaultKpDictionaryPath()),
_newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()),
_lastWordId(0),
useDistanceL1_(false),
@@ -93,11 +93,10 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
UASSERT_MSG(_nndrRatio > 0.0f, uFormat("String=%s value=%f", uContains(parameters, Parameters::kKpNndrRatio())?parameters.at(Parameters::kKpNndrRatio()).c_str():"", _nndrRatio).c_str());
std::string dictionaryPath = _dictionaryPath;
bool incrementalDictionary = _incrementalDictionary;
if((iter=parameters.find(Parameters::kKpDictionaryPath())) != parameters.end())
{
dictionaryPath = (*iter).second.c_str();
_newDictionaryPath = (*iter).second.c_str();
}
if((iter=parameters.find(Parameters::kKpIncrementalDictionary())) != parameters.end())
{
@@ -115,11 +114,7 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
{
this->setIncrementalDictionary();
}
else
{
this->setFixedDictionary(dictionaryPath);
}
_incrementalDictionary = incrementalDictionary;
}
void VWDictionary::setIncrementalDictionary()
@@ -133,21 +128,23 @@ void VWDictionary::setIncrementalDictionary()
}
}
_dictionaryPath = "";
_newDictionaryPath = "";
}
void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
{
UDEBUG("");
if(!dictionaryPath.empty())
{
if((!_incrementalDictionary && _dictionaryPath.compare(dictionaryPath) != 0) ||
_visualWords.size() == 0)
{
UDEBUG("incremental=%d, oldPath=%s newPath=%s, visual words=%d",
UINFO("incremental=%d, oldPath=%s newPath=%s, visual words=%d",
_incrementalDictionary?1:0, _dictionaryPath.c_str(), dictionaryPath.c_str(), (int)_visualWords.size());
if(UFile::getExtension(dictionaryPath).compare("db") == 0)
{
UDEBUG("Loading fixed vocabulary \"%s\", this may take a while...", dictionaryPath.c_str());
UWARN("Loading fixed vocabulary \"%s\", this may take a while...", dictionaryPath.c_str());
DBDriver * driver = DBDriver::create();
if(driver->openConnection(dictionaryPath, false))
{
@@ -252,9 +249,11 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
}
else
{
this->update();
_dictionaryPath = dictionaryPath;
_newDictionaryPath = dictionaryPath;
_incrementalDictionary = false;
UDEBUG("Loaded %d words!", (int)_visualWords.size());
this->update();
UWARN("Loaded %d words!", (int)_visualWords.size());
}
}
else if(!_incrementalDictionary)
@@ -275,6 +274,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
_incrementalDictionary = false;
}
_dictionaryPath = dictionaryPath;
_newDictionaryPath = dictionaryPath;
}
void VWDictionary::setNNStrategy(NNStrategy strategy)
@@ -402,13 +402,19 @@ cv::Mat VWDictionary::convert32FToBin(const cv::Mat & descriptorsIn)
void VWDictionary::update()
{
ULOGGER_DEBUG("");
if(!_incrementalDictionary && !_notIndexedWords.size())
ULOGGER_DEBUG("incremental=%d", _incrementalDictionary?1:0);
if(!_incrementalDictionary)
{
// No need to update the search index if we
// use a fixed dictionary and the index is
// already built
return;
// reload the fixed dictionary if it has been cleared or not yet initialized
this->setFixedDictionary(_newDictionaryPath);
if(!_incrementalDictionary && !_notIndexedWords.size())
{
// No need to update the search index if we
// use a fixed dictionary and the index is
// already built
return;
}
}
if(_notIndexedWords.size() || _visualWords.size() == 0 || _removedIndexedWords.size())
@@ -643,12 +649,6 @@ void VWDictionary::clear(bool printWarningsIfNotEmpty)
_unusedWords.clear();
_flannIndex->release();
useDistanceL1_ = false;
if(!_incrementalDictionary)
{
// reload the fixed dictionary
this->setFixedDictionary(_dictionaryPath);
}
}
int VWDictionary::getNextId()