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

Added parameter KpMaxDepth to limit the features extracted under a specified maximum depth.

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1288 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-04-03 19:59:46 +00:00
parent 1b6a8b93cb
commit 491553a80f
11 changed files with 798 additions and 657 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
corelib/src/Features2d.cpp
View File

@@ -18,6 +18,7 @@
*/
#include "rtabmap/core/Features2d.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/ULogger.h"
@@ -36,6 +37,63 @@
namespace rtabmap {
void filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
float depthConstant,
float maxDepth)
{
cv::Mat descriptors;
filterKeypointsByDepth(keypoints, descriptors, depth, depthConstant, maxDepth);
}
void filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors,
const cv::Mat & depth,
float depthConstant,
float maxDepth)
{
if(!depth.empty() && depthConstant > 0.0f && maxDepth > 0.0f && (descriptors.empty() || descriptors.rows == keypoints.size()))
{
std::vector<cv::KeyPoint> output(keypoints.size());
std::vector<int> indexes(keypoints.size(), 0);
int oi=0;
for(unsigned int i=0; i<keypoints.size(); ++i)
{
pcl::PointXYZ pt = util3d::getDepth(depth, keypoints[i].pt.x, keypoints[i].pt.y, depthConstant);
if(uIsFinite(pt.z) && pt.z < maxDepth)
{
output[oi++] = keypoints[i];
indexes[i] = 1;
}
}
output.resize(oi);
keypoints = output;
if(!descriptors.empty() && keypoints.size() != descriptors.rows)
{
if(keypoints.size() == 0)
{
descriptors = cv::Mat();
}
else
{
cv::Mat newDescriptors(keypoints.size(), descriptors.cols, descriptors.type());
int di = 0;
for(unsigned int i=0; i<indexes.size(); ++i)
{
if(indexes[i] == 1)
{
memcpy(newDescriptors.ptr<float>(di++), descriptors.ptr<float>(i), descriptors.cols*sizeof(float));
}
}
descriptors = newDescriptors;
}
}
}
}
void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
{
cv::Mat descriptors;

12
corelib/src/Memory.cpp
View File

@@ -68,6 +68,7 @@ Memory::Memory(const ParametersMap & parameters) :
_badSignRatio(Parameters::defaultKpBadSignRatio()),
_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
_parallelized(Parameters::defaultKpParallelized()),
_wordsMaxDepth(Parameters::defaultKpMaxDepth()),
_wordsPerImageTarget(Parameters::defaultKpWordsPerImage()),
_roiRatios(std::vector<float>(4, 0.0f)),
@@ -358,6 +359,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kKpTfIdfLikelihoodUsed(), _tfIdfLikelihoodUsed);
Parameters::parse(parameters, Parameters::kKpParallelized(), _parallelized);
Parameters::parse(parameters, Parameters::kKpBadSignRatio(), _badSignRatio);
Parameters::parse(parameters, Parameters::kKpMaxDepth(), _wordsMaxDepth);
Parameters::parse(parameters, Parameters::kKpWordsPerImage(), _wordsPerImageTarget);
if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
@@ -2771,6 +2773,8 @@ void Memory::copyData(const Signature * from, Signature * to)
void Memory::extractKeypointsAndDescriptors(
const cv::Mat & image,
const cv::Mat & depth,
float depthConstant,
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors)
{
@@ -2780,8 +2784,11 @@ void Memory::extractKeypointsAndDescriptors(
if(_keypointDetector)
{
cv::Rect roi = KeypointDetector::computeRoi(image, _roiRatios);
keypoints = _keypointDetector->generateKeypoints(image, _wordsPerImageTarget, roi);
keypoints = _keypointDetector->generateKeypoints(image, 0, roi);
UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());
filterKeypointsByDepth(keypoints, depth, depthConstant, _wordsMaxDepth);
limitKeypoints(keypoints, _wordsPerImageTarget);
}
if(keypoints.size())
@@ -2876,12 +2883,13 @@ Signature * Memory::createSignature(const Image & image, bool keepRawData)
descriptors = image.descriptors();
keypoints = image.keypoints();
}
filterKeypointsByDepth(keypoints, descriptors, image.depth(), image.depthConstant(), _wordsMaxDepth);
limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
}
else
{
// IMAGE RAW
this->extractKeypointsAndDescriptors(image.image(), keypoints, descriptors);
this->extractKeypointsAndDescriptors(image.image(), image.depth(), image.depthConstant(), keypoints, descriptors);
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))

6
corelib/src/Odometry.cpp
View File

@@ -187,7 +187,7 @@ Transform OdometryBinary::computeTransform(Image & image)
if(_lastKeypoints.size())
{
if(newDescriptors.rows && newDescriptors.rows > _lastKeypoints.size()/2) // at least 50% keypoints
if(newDescriptors.rows && newDescriptors.rows > (int)_lastKeypoints.size()/2) // at least 50% keypoints
{
cv::Mat results;
cv::Mat dists;
@@ -379,6 +379,7 @@ OdometryBOW::OdometryBOW(
{
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxWords)));
customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(maxDepth)));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(detectorType)));
customParameters.insert(ParametersPair(Parameters::kSURFHessianThreshold(), uNumber2Str(surfHessianThreshold)));
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
@@ -396,6 +397,7 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
{
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(this->getMaxFeatures()))); // hack
customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
if(!_memory->init("", false, customParameters, false))
@@ -427,7 +429,7 @@ Transform OdometryBOW::computeTransform(Image & image)
std::vector<cv::KeyPoint> keypoints;
cv::Mat descriptors;
_memory->extractKeypointsAndDescriptors(image.image(), keypoints, descriptors);
_memory->extractKeypointsAndDescriptors(image.image(), image.depth(), image.depthConstant(), keypoints, descriptors);
image.setDescriptors(descriptors);
image.setKeypoints(keypoints);

12
corelib/src/util3d.cpp
View File

@@ -405,6 +405,18 @@ void findCorrespondences(
inliers2.resize(oi);
}
pcl::PointXYZ getDepth(
const cv::Mat & depthImage,
int x, int y,
float depthConstant)
{
return getDepth(depthImage, x, y,
(float)depthImage.cols/2,
(float)depthImage.rows/2,
1.0f/depthConstant,
1.0f/depthConstant);
}
pcl::PointXYZ getDepth(const cv::Mat & depthImage,
int x, int y,
float cx, float cy,