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

Ignoring black pixels when creating clouds (recfification artifacts). Slightly improved depth retification speed.

Browse Source
This commit is contained in:
matlabbe
2016-09-13 12:10:14 -04:00
parent 44caa12fa5
commit e5977d6157
3 changed files with 22 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/CameraModel.cpp
View File

@@ -485,8 +485,8 @@ cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
//http://stackoverflow.com/questions/13299409/how-to-get-the-image-pixel-at-real-locations-in-opencv
rectified.at<unsigned short>(y,x) =
(raw.at<unsigned short>(yL, xL) * (1.f - a) + raw.at<unsigned short>(yL, xH) * a) * (1.f - c) +
(raw.at<unsigned short>(yH, xL) * (1.f - a) + raw.at<unsigned short>(yH, xH) * a) * c;
(pLT * (1.f - a) + pRT * a) * (1.f - c) +
(pLB * (1.f - a) + pRB * a) * c;
}
}
}

4
corelib/src/CameraRGBD.cpp
View File

@@ -1435,7 +1435,9 @@ SensorData CameraFreenect2::captureImage(CameraInfo * info)
//rectify depth
cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
cv::flip(depth, depth, 1);
depth = stereoModel_.left().rectifyDepth(depth);
//depth = stereoModel_.left().rectifyImage(depth, 0); // ~0.5/4 ms but is more noisy
depth = stereoModel_.left().rectifyDepth(depth); // ~16/25 ms
bool registered = true;
if(registered)

25
corelib/src/util3d.cpp
View File

@@ -443,17 +443,26 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
pt.r = v;
}
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
// Ignore pure black pixels, as they are generarly rectification artifacts on the contour. It is okay to
// assume this as normally depth would not be computed on pure black surfaces anyway.
if (pt.b > 0 && pt.g > 0 && pt.r > 0)
{
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
if(validIndices)
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
if (pcl::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth <= 0.0f || ptXYZ.z <= maxDepth))
{
validIndices->at(oi) = (h/decimation)*cloud->width + (w/decimation);
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
if (validIndices)
{
validIndices->at(oi) = (h / decimation)*cloud->width + (w / decimation);
}
++oi;
}
else
{
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
}
++oi;
}
else
{