i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-05 01:27:46 +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
corelib/src/Features2d.cpp
+58
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;