i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 01:57:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Stereo: Fixed min/max depth filtering

Browse Source
This commit is contained in:
matlabbe
2016-03-13 16:58:36 -04:00
parent be559c181a
commit 04df6ba41c
8 changed files with 202 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Memory.cpp
+65 -8
View File
@@ -3209,6 +3209,34 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
(!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection()))
{
keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
{
UDEBUG("");
//remove all keypoints/descriptors with no valid 3D points
UASSERT((int)keypoints.size() == descriptors.rows &&
keypoints3D.size() == keypoints.size());
std::vector<cv::KeyPoint> validKeypoints(keypoints.size());
std::vector<cv::Point3f> validKeypoints3D(keypoints.size());
cv::Mat validDescriptors(descriptors.size(), descriptors.type());
int oi=0;
for(unsigned int i=0; i<keypoints3D.size(); ++i)
{
if(util3d::isFinite(keypoints3D[i]))
{
validKeypoints[oi] = keypoints[i];
validKeypoints3D[oi] = keypoints3D[i];
descriptors.row(i).copyTo(validDescriptors.row(oi));
++oi;
}
}
UDEBUG("Removed %d invalid 3D points", (int)keypoints3D.size()-oi);
validKeypoints.resize(oi);
validKeypoints3D.resize(oi);
keypoints = validKeypoints;
keypoints3D = validKeypoints3D;
descriptors = validDescriptors.rowRange(0, oi).clone();
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
@@ -3258,19 +3286,48 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
}
if((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
(!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection()))
{
keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
{
UDEBUG("");
//remove all keypoints/descriptors with no valid 3D points
UASSERT((int)keypoints.size() == descriptors.rows &&
keypoints3D.size() == keypoints.size());
std::vector<cv::KeyPoint> validKeypoints(keypoints.size());
std::vector<cv::Point3f> validKeypoints3D(keypoints.size());
cv::Mat validDescriptors(descriptors.size(), descriptors.type());
int oi=0;
for(unsigned int i=0; i<keypoints3D.size(); ++i)
{
if(util3d::isFinite(keypoints3D[i]))
{
validKeypoints[oi] = keypoints[i];
validKeypoints3D[oi] = keypoints3D[i];
descriptors.row(i).copyTo(validDescriptors.row(oi));
++oi;
}
}
UDEBUG("Removed %d invalid 3D points", (int)keypoints3D.size()-oi);
validKeypoints.resize(oi);
validKeypoints3D.resize(oi);
keypoints = validKeypoints;
keypoints3D = validKeypoints3D;
descriptors = validDescriptors.rowRange(0, oi).clone();
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
}
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
{
descriptors = cv::Mat();
}
else if((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
(!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection()))
{
keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
}
}
if(_parallelized)