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

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

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

@@ -138,6 +138,8 @@ public:
static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
int getMaxFeatures() const {return maxFeatures_;}
float getMinDepth() const {return _minDepth;}
float getMaxDepth() const {return _maxDepth;}
public:
virtual ~Feature2D();

16
corelib/include/rtabmap/core/util3d_features.h
View File

@@ -47,23 +47,31 @@ namespace util3d
std::vector<cv::Point3f> RTABMAP_EXP generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
const CameraModel & cameraModel);
const CameraModel & cameraModel,
float minDepth = 0,
float maxDepth = 0);
std::vector<cv::Point3f> RTABMAP_EXP generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
const std::vector<CameraModel> & cameraModels);
const std::vector<CameraModel> & cameraModels,
float minDepth = 0,
float maxDepth = 0);
std::vector<cv::Point3f> RTABMAP_EXP generateKeypoints3DDisparity(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & disparity,
const StereoCameraModel & stereoCameraMode);
const StereoCameraModel & stereoCameraModel,
float minDepth = 0,
float maxDepth = 0);
std::vector<cv::Point3f> RTABMAP_EXP generateKeypoints3DStereo(
const std::vector<cv::Point2f> & leftCorners,
const std::vector<cv::Point2f> & rightCorners,
const StereoCameraModel & model,
const std::vector<unsigned char> & mask = std::vector<unsigned char>());
const std::vector<unsigned char> & mask = std::vector<unsigned char>(),
float minDepth = 0,
float maxDepth = 0);
std::map<int, cv::Point3f> RTABMAP_EXP generateWords3DMono(
const std::map<int, cv::KeyPoint> & kpts,

52
corelib/src/Features2d.cpp
View File

@@ -592,7 +592,7 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
{
imageMono = data.imageRaw();
}
//generate a disparity map
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
std::vector<unsigned char> status;
@@ -604,60 +604,22 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
leftCorners,
status);
if(_maxDepth > 0.0f || _minDepth > 0.0f)
{
UASSERT(status.size() == leftCorners.size() && status.size() == rightCorners.size());
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i] != 0)
{
float d = data.stereoCameraModel().computeDepth(leftCorners[i].x - rightCorners[i].x);
if((_minDepth > 0.0f && d < _minDepth) ||
(_maxDepth > 0.0f && d > _maxDepth))
{
status[i] = 0;
}
}
}
}
keypoints3D = util3d::generateKeypoints3DStereo(
leftCorners,
rightCorners,
data.stereoCameraModel(),
status);
status,
_minDepth,
_maxDepth);
}
else if(!data.depthRaw().empty() && data.cameraModels().size())
{
keypoints3D = util3d::generateKeypoints3DDepth(
keypoints,
data.depthOrRightRaw(),
data.cameraModels());
if(_maxDepth > 0.0f || _minDepth > 0.0f)
{
UASSERT(keypoints3D.size() == keypoints.size());
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(unsigned int i=0; i<keypoints.size(); ++i)
{
float d = util2d::getDepth(
data.depthRaw(),
keypoints[i].pt.x/float((data.imageRaw().cols/data.depthRaw().cols)),
keypoints[i].pt.y/float((data.imageRaw().rows/data.depthRaw().rows)),
false);
bool reject = true;
if(uIsFinite(d) && d>_minDepth && (_maxDepth <= 0.0f || d < _maxDepth))
{
reject = false;
}
if(reject)
{
keypoints3D[i].x = bad_point;
keypoints3D[i].y = bad_point;
keypoints3D[i].z = bad_point;
}
}
}
data.cameraModels(),
_minDepth,
_maxDepth);
}
return keypoints3D;

73
corelib/src/Memory.cpp
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)

57
corelib/src/RegistrationVis.cpp
View File

@@ -320,7 +320,6 @@ Transform RegistrationVis::computeTransformationImpl(
if(!toSignature.sensorData().imageRaw().empty())
{
std::vector<cv::Point2f> cornersFrom;
cv::KeyPoint::convert(kptsFrom, cornersFrom);
std::vector<cv::Point2f> cornersTo;
@@ -533,6 +532,34 @@ Transform RegistrationVis::computeTransformationImpl(
"is maybe a problem with the logic above (getWords3() should be null or equal to kptsfrom).");
}
kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
if(detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f)
{
UDEBUG("");
//remove all keypoints/descriptors with no valid 3D points
UASSERT((int)kptsFrom.size() == descriptorsFrom.rows &&
kptsFrom3D.size() == kptsFrom.size());
std::vector<cv::KeyPoint> validKeypoints(kptsFrom.size());
std::vector<cv::Point3f> validKeypoints3D(kptsFrom.size());
cv::Mat validDescriptors(descriptorsFrom.size(), descriptorsFrom.type());
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];
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;
descriptorsFrom = validDescriptors.rowRange(0, oi).clone();
}
}
else
{
@@ -546,6 +573,34 @@ Transform RegistrationVis::computeTransformationImpl(
"is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo).");
}
kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
if(detector->getMinDepth() > 0.0f || detector->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();
}
}
else
{

14
corelib/src/Stereo.cpp
View File

@@ -128,6 +128,8 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
UDEBUG("util2d::calcOpticalFlowPyrLKStereo() end");
UASSERT(leftCorners.size() == rightCorners.size() && status.size() == leftCorners.size());
int countFlowRejected = 0;
int countDisparityRejected = 0;
for(unsigned int i=0; i<status.size(); ++i)
{
if(status[i]!=0)
@@ -136,9 +138,21 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
if(disparity < float(this->minDisparity()) || disparity > float(this->maxDisparity()))
{
status[i] = 0;
++countDisparityRejected;
}
}
else
{
++countFlowRejected;
}
}
UDEBUG("total=%d countFlowRejected=%d countDisparityRejected=%d", (int)status.size(), countFlowRejected, countDisparityRejected);
if(countFlowRejected + countDisparityRejected > (int)status.size()/2)
{
UWARN("A large number (%d/%d) of stereo correspondences are rejected! Optical flow may have failed, images are not calibrated or the background is too far (no disparity between the images).", countFlowRejected+countDisparityRejected, (int)status.size());
}
return rightCorners;
}

55
corelib/src/util3d_features.cpp
View File

@@ -51,18 +51,22 @@ namespace util3d
std::vector<cv::Point3f> generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
const CameraModel & cameraModel)
const CameraModel & cameraModel,
float minDepth,
float maxDepth)
{
UASSERT(cameraModel.isValidForProjection());
std::vector<CameraModel> models;
models.push_back(cameraModel);
return generateKeypoints3DDepth(keypoints, depth, models);
return generateKeypoints3DDepth(keypoints, depth, models, minDepth, maxDepth);
}
std::vector<cv::Point3f> generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
const std::vector<CameraModel> & cameraModels)
const std::vector<CameraModel> & cameraModels,
float minDepth,
float maxDepth)
{
UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1));
UASSERT(cameraModels.size());
@@ -74,6 +78,7 @@ std::vector<cv::Point3f> generateKeypoints3DDepth(
keypoints3d.resize(keypoints.size());
float rgbToDepthFactorX = 1.0f/(cameraModels[0].imageWidth()>0?cameraModels[0].imageWidth()/subImageWidth:1);
float rgbToDepthFactorY = 1.0f/(cameraModels[0].imageHeight()>0?cameraModels[0].imageHeight()/depth.rows:1);
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(unsigned int i=0; i<keypoints.size(); ++i)
{
float x = keypoints[i].pt.x*rgbToDepthFactorX;
@@ -93,12 +98,17 @@ std::vector<cv::Point3f> generateKeypoints3DDepth(
cameraModels.at(cameraIndex).fy()*rgbToDepthFactorY,
true);
cv::Point3f pt(ptXYZ.x, ptXYZ.y, ptXYZ.z);
if(util3d::isFinite(pt) &&
!cameraModels.at(cameraIndex).localTransform().isNull() &&
!cameraModels.at(cameraIndex).localTransform().isIdentity())
cv::Point3f pt(bad_point, bad_point, bad_point);
if(pcl::isFinite(ptXYZ) &&
(minDepth <= 0.0f || ptXYZ.z >= minDepth) &&
(maxDepth <= 0.0f || ptXYZ.z <= maxDepth))
{
pt = util3d::transformPoint(pt, cameraModels.at(cameraIndex).localTransform());
pt = cv::Point3f(ptXYZ.x, ptXYZ.y, ptXYZ.z);
if(!cameraModels.at(cameraIndex).localTransform().isNull() &&
!cameraModels.at(cameraIndex).localTransform().isIdentity())
{
pt = util3d::transformPoint(pt, cameraModels.at(cameraIndex).localTransform());
}
}
keypoints3d.at(i) = pt;
}
@@ -109,24 +119,33 @@ std::vector<cv::Point3f> generateKeypoints3DDepth(
std::vector<cv::Point3f> generateKeypoints3DDisparity(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & disparity,
const StereoCameraModel & stereoCameraModel)
const StereoCameraModel & stereoCameraModel,
float minDepth,
float maxDepth)
{
UASSERT(!disparity.empty() && (disparity.type() == CV_16SC1 || disparity.type() == CV_32F));
UASSERT(stereoCameraModel.isValidForProjection());
std::vector<cv::Point3f> keypoints3d;
keypoints3d.resize(keypoints.size());
float bad_point = std::numeric_limits<float>::quiet_NaN ();
for(unsigned int i=0; i!=keypoints.size(); ++i)
{
cv::Point3f pt = util3d::projectDisparityTo3D(
cv::Point3f tmpPt = util3d::projectDisparityTo3D(
keypoints[i].pt,
disparity,
stereoCameraModel);
if(util3d::isFinite(pt) &&
!stereoCameraModel.left().localTransform().isNull() &&
!stereoCameraModel.left().localTransform().isIdentity())
cv::Point3f pt(bad_point, bad_point, bad_point);
if(util3d::isFinite(tmpPt) &&
(minDepth <= 0.0f || tmpPt.z >= minDepth) &&
(maxDepth <= 0.0f || tmpPt.z <= maxDepth))
{
pt = util3d::transformPoint(pt, stereoCameraModel.left().localTransform());
pt = tmpPt;
if(!stereoCameraModel.left().localTransform().isNull() &&
!stereoCameraModel.left().localTransform().isIdentity())
{
pt = util3d::transformPoint(pt, stereoCameraModel.left().localTransform());
}
}
keypoints3d.at(i) = pt;
}
@@ -137,7 +156,9 @@ std::vector<cv::Point3f> generateKeypoints3DStereo(
const std::vector<cv::Point2f> & leftCorners,
const std::vector<cv::Point2f> & rightCorners,
const StereoCameraModel & model,
const std::vector<unsigned char> & mask)
const std::vector<unsigned char> & mask,
float minDepth,
float maxDepth)
{
UASSERT(leftCorners.size() == rightCorners.size());
UASSERT(mask.size() == 0 || leftCorners.size() == mask.size());
@@ -159,7 +180,9 @@ std::vector<cv::Point3f> generateKeypoints3DStereo(
disparity,
model);
if(util3d::isFinite(tmpPt))
if(util3d::isFinite(tmpPt) &&
(minDepth <= 0.0f || tmpPt.z >= minDepth) &&
(maxDepth <= 0.0f || tmpPt.z <= maxDepth))
{
pt = tmpPt;
if(!model.localTransform().isNull() &&