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

RegVis: fixed Vis/MaxDepth and Vis/MinDepth ignored when features are already extracted.

Browse Source
This commit is contained in:
matlabbe
2020-04-30 15:07:32 -04:00
parent 5d16ae8cfe
commit 803c99cee2
4 changed files with 69 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -125,6 +125,12 @@ public:
const cv::Mat & depth,
float minDepth,
float maxDepth);
static void filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors,
std::vector<cv::Point3f> & keypoints3D,
float minDepth,
float maxDepth);
static void filterKeypointsByDisparity(
std::vector<cv::KeyPoint> & keypoints,

47
corelib/src/Features2d.cpp
View File

@@ -147,6 +147,53 @@ void Feature2D::filterKeypointsByDepth(
}
}
void Feature2D::filterKeypointsByDepth(
std::vector<cv::KeyPoint> & keypoints,
cv::Mat & descriptors,
std::vector<cv::Point3f> & keypoints3D,
float minDepth,
float maxDepth)
{
UDEBUG("");
//remove all keypoints/descriptors with no valid 3D points
UASSERT(((int)keypoints.size() == descriptors.rows || descriptors.empty()) &&
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;
float minDepthSqr = minDepth * minDepth;
float maxDepthSqr = maxDepth * maxDepth;
for(unsigned int i=0; i<keypoints3D.size(); ++i)
{
cv::Point3f & pt = keypoints3D[i];
if(util3d::isFinite(pt))
{
float distSqr = pt.x*pt.x+pt.y*pt.y+pt.z*pt.z;
if(distSqr >= minDepthSqr && (maxDepthSqr==0.0f || distSqr <= maxDepthSqr))
{
validKeypoints[oi] = keypoints[i];
validKeypoints3D[oi] = pt;
if(!descriptors.empty())
{
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;
if(!descriptors.empty())
{
descriptors = validDescriptors.rowRange(0, oi).clone();
}
}
void Feature2D::filterKeypointsByDisparity(
std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & disparity,

32
corelib/src/Memory.cpp
View File

@@ -4469,34 +4469,10 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
(!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();
}
}
if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
{
_feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth());
}
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);

82
corelib/src/RegistrationVis.cpp
View File

@@ -637,50 +637,13 @@ Transform RegistrationVis::computeTransformationImpl(
}
kptsFrom3D = _detectorFrom->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
UDEBUG("generated kptsFrom3D=%d", (int)kptsFrom3D.size());
if(!kptsFrom3D.empty() && (_detectorFrom->getMinDepth() > 0.0f || _detectorFrom->getMaxDepth() > 0.0f))
{
//remove all keypoints/descriptors with no valid 3D points
UASSERT_MSG((int)kptsFrom.size() == descriptorsFrom.rows &&
kptsFrom3D.size() == kptsFrom.size(),
uFormat("kptsFrom=%d descriptorsFrom=%d kptsFrom3D=%d",
(int)kptsFrom.size(), descriptorsFrom.rows, (int)kptsFrom3D.size()).c_str());
std::vector<cv::KeyPoint> validKeypoints(kptsFrom.size());
std::vector<cv::Point3f> validKeypoints3D(kptsFrom.size());
cv::Mat validDescriptors(descriptorsFrom.size(), descriptorsFrom.type());
std::vector<int> validKeypointsIds;
if(orignalWordsFromIds.size())
{
validKeypointsIds.resize(kptsFrom.size());
}
}
int oi=0;
for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
{
if(util3d::isFinite(kptsFrom3D[i]))
{
validKeypoints[oi] = kptsFrom[i];
validKeypoints3D[oi] = kptsFrom3D[i];
if(orignalWordsFromIds.size())
{
validKeypointsIds[oi] = orignalWordsFromIds[i];
}
descriptorsFrom.row(i).copyTo(validDescriptors.row(oi));
++oi;
}
}
UDEBUG("Removed %d invalid 3D points", (int)kptsFrom3D.size()-oi);
validKeypoints.resize(oi);
validKeypoints3D.resize(oi);
kptsFrom = validKeypoints;
kptsFrom3D = validKeypoints3D;
if(orignalWordsFromIds.size())
{
validKeypointsIds.resize(oi);
orignalWordsFromIds = validKeypointsIds;
}
descriptorsFrom = validDescriptors.rowRange(0, oi).clone();
}
if(!kptsFrom3D.empty() &&
(_detectorFrom->getMinDepth() > 0.0f || _detectorFrom->getMaxDepth() > 0.0f) &&
(!fromSignature.sensorData().cameraModels().empty() || fromSignature.sensorData().stereoCameraModel().isValidForProjection())) // Ignore local map from OdometryF2M
{
_detectorFrom->filterKeypointsByDepth(kptsFrom, descriptorsFrom, kptsFrom3D, _detectorFrom->getMinDepth(), _detectorFrom->getMaxDepth());
}
if(kptsTo.size() == toSignature.getWords3().size())
@@ -708,34 +671,13 @@ Transform RegistrationVis::computeTransformationImpl(
(int)toSignature.sensorData().keypoints3D().size());
}
kptsTo3D = _detectorTo->generateKeypoints3D(toSignature.sensorData(), kptsTo);
if(kptsTo3D.size() && (_detectorTo->getMinDepth() > 0.0f || _detectorTo->getMaxDepth() > 0.0f))
{
UDEBUG("");
//remove all keypoints/descriptors with no valid 3D points
UASSERT((int)kptsTo.size() == descriptorsTo.rows &&
kptsTo3D.size() == kptsTo.size());
std::vector<cv::KeyPoint> validKeypoints(kptsTo.size());
std::vector<cv::Point3f> validKeypoints3D(kptsTo.size());
cv::Mat validDescriptors(descriptorsTo.size(), descriptorsTo.type());
}
int oi=0;
for(unsigned int i=0; i<kptsTo3D.size(); ++i)
{
if(util3d::isFinite(kptsTo3D[i]))
{
validKeypoints[oi] = kptsTo[i];
validKeypoints3D[oi] = kptsTo3D[i];
descriptorsTo.row(i).copyTo(validDescriptors.row(oi));
++oi;
}
}
UDEBUG("Removed %d invalid 3D points", (int)kptsTo3D.size()-oi);
validKeypoints.resize(oi);
validKeypoints3D.resize(oi);
kptsTo = validKeypoints;
kptsTo3D = validKeypoints3D;
descriptorsTo = validDescriptors.rowRange(0, oi).clone();
}
if(kptsTo3D.size() &&
(_detectorTo->getMinDepth() > 0.0f || _detectorTo->getMaxDepth() > 0.0f) &&
(!toSignature.sensorData().cameraModels().empty() || toSignature.sensorData().stereoCameraModel().isValidForProjection())) // Ignore local map from OdometryF2M
{
_detectorTo->filterKeypointsByDepth(kptsTo, descriptorsTo, kptsTo3D, _detectorTo->getMinDepth(), _detectorTo->getMaxDepth());
}
UASSERT(kptsFrom.empty() || descriptorsFrom.rows == 0 || int(kptsFrom.size()) == descriptorsFrom.rows);