From 4b8c45ff79c5b3db0f68567b889c674546615d41 Mon Sep 17 00:00:00 2001 From: Lucas Walter Date: Sun, 16 Jan 2022 09:37:49 -0800 Subject: [PATCH] don't pass through negative depth values for now --- depth_image_proc/src/nodelets/register.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/depth_image_proc/src/nodelets/register.cpp b/depth_image_proc/src/nodelets/register.cpp index 563acd0f8..02e6e26be 100644 --- a/depth_image_proc/src/nodelets/register.cpp +++ b/depth_image_proc/src/nodelets/register.cpp @@ -224,10 +224,17 @@ bool transform_depth( // Transform to RGB camera frame Eigen::Vector4d xyz_rgb = depth_to_rgb * xyz_depth; - // TODO(lucasw) return false if xyz_rgb.z() < 0.0 - or is that okay for some < 0 fx/fy? + new_depth = static_cast(xyz_rgb.z()); + // TODO(lucasw) is the intent to simulate what a real depth camera would see? If so reject negative depth + // but if want to preserve as much data as possible it may make sense to pass through negative values + // (though don't overwrite positive values, use abs in the z buffer test) + if (new_depth < 0.0) + { + return false; + } // Project to (u,v) in RGB image - const double inv_Z = 1.0 / xyz_rgb.z(); + const double inv_Z = 1.0 / new_depth; u_rgb = (rgb_fx*xyz_rgb.x() + rgb_Tx)*inv_Z + rgb_cx + 0.5; if (u_rgb < 0 || u_rgb >= width) @@ -237,8 +244,6 @@ bool transform_depth( if (v_rgb < 0 || v_rgb >= height) return false; - new_depth = static_cast(xyz_rgb.z()); - return true; }