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

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 //http://stackoverflow.com/questions/13299409/how-to-get-the-image-pixel-at-real-locations-in-opencv
rectified.at<unsigned short>(y,x) = 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) + (pLT * (1.f - a) + pRT * a) * (1.f - c) +
(raw.at<unsigned short>(yH, xL) * (1.f - a) + raw.at<unsigned short>(yH, xH) * a) * 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 //rectify depth
cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1); cv::Mat((int)depthFrame->height, (int)depthFrame->width, CV_32FC1, depthFrame->data).convertTo(depth, CV_16U, 1);
cv::flip(depth, depth, 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; bool registered = true;
if(registered) if(registered)

9
corelib/src/util3d.cpp
View File

@@ -443,6 +443,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
pt.r = v; pt.r = v;
} }
// 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)
{
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false); 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)) if (pcl::isFinite(ptXYZ) && ptXYZ.z >= minDepth && (maxDepth <= 0.0f || ptXYZ.z <= maxDepth))
{ {
@@ -460,6 +464,11 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN(); pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
} }
} }
else
{
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
}
}
} }
if(validIndices) if(validIndices)
{ {