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

Depth16 set invalid for values = 65535

Browse Source
This commit is contained in:
matlabbe
2016-02-19 18:10:32 -05:00
parent 9b16d93481
commit 081be68616
2 changed files with 54 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
corelib/src/Features2d.cpp
View File

@@ -502,7 +502,20 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
mask = cv::Mat::zeros(maskIn.rows, maskIn.cols, CV_8UC1); mask = cv::Mat::zeros(maskIn.rows, maskIn.cols, CV_8UC1);
for(int i=0; i<(int)mask.total(); ++i) for(int i=0; i<(int)mask.total(); ++i)
{ {
float value = maskIn.type()==CV_16UC1?float(((unsigned short*)maskIn.data)[i])/1000.0f:((float*)maskIn.data)[i]; float value = 0.0f;
if(maskIn.type()==CV_16UC1)
{
if(((unsigned short*)maskIn.data)[i] > 0 &&
((unsigned short*)maskIn.data)[i] < std::numeric_limits<unsigned short>::max())
{
value = float(((unsigned short*)maskIn.data)[i])*0.0001f;
}
}
else
{
value = ((float*)maskIn.data)[i];
}
if(value>_minDepth && if(value>_minDepth &&
(_maxDepth == 0.0f || value <= _maxDepth)) (_maxDepth == 0.0f || value <= _maxDepth))
{ {

43
corelib/src/util2d.cpp
View File

@@ -994,7 +994,19 @@ float getDepth(
int u_end = std::min(u+1, depthImage.cols-1); int u_end = std::min(u+1, depthImage.cols-1);
int v_end = std::min(v+1, depthImage.rows-1); int v_end = std::min(v+1, depthImage.rows-1);
float depth = isInMM?(float)depthImage.at<unsigned short>(v,u)*0.001f:depthImage.at<float>(v,u); float depth = 0.0f;
if(isInMM)
{
if(depthImage.at<unsigned short>(v,u) > 0 &&
depthImage.at<unsigned short>(v,u) < std::numeric_limits<unsigned short>::max())
{
depth = float(depthImage.at<unsigned short>(v,u))*0.001f;
}
}
else
{
depth = depthImage.at<float>(v,u);
}
if((depth==0.0f || !uIsFinite(depth)) && estWithNeighborsIfNull) if((depth==0.0f || !uIsFinite(depth)) && estWithNeighborsIfNull)
{ {
@@ -1007,7 +1019,19 @@ float getDepth(
{ {
if((uu == u && vv!=v) || (uu != u && vv==v)) if((uu == u && vv!=v) || (uu != u && vv==v))
{ {
float d = isInMM?(float)depthImage.at<unsigned short>(vv,uu)*0.001f:depthImage.at<float>(vv,uu); float d = 0.0f;
if(isInMM)
{
if(depthImage.at<unsigned short>(vv,uu) > 0 &&
depthImage.at<unsigned short>(vv,uu) < std::numeric_limits<unsigned short>::max())
{
depth = float(depthImage.at<unsigned short>(vv,uu))*0.001f;
}
}
else
{
d = depthImage.at<float>(vv,uu);
}
if(d!=0.0f && uIsFinite(d)) if(d!=0.0f && uIsFinite(d))
{ {
if(tmp == 0.0f) if(tmp == 0.0f)
@@ -1042,7 +1066,20 @@ float getDepth(
{ {
if(!(uu == u && vv == v)) if(!(uu == u && vv == v))
{ {
float d = isInMM?(float)depthImage.at<unsigned short>(vv,uu)*0.001f:depthImage.at<float>(vv,uu); float d = 0.0f;
if(isInMM)
{
if(depthImage.at<unsigned short>(vv,uu) > 0 &&
depthImage.at<unsigned short>(vv,uu) < std::numeric_limits<unsigned short>::max())
{
depth = float(depthImage.at<unsigned short>(vv,uu))*0.001f;
}
}
else
{
d = depthImage.at<float>(vv,uu);
}
// ignore if not valid or depth difference is too high // ignore if not valid or depth difference is too high
if(d != 0.0f && uIsFinite(d) && fabs(d - depth) < maxZError) if(d != 0.0f && uIsFinite(d) && fabs(d - depth) < maxZError)
{ {