From 70f2fbb2488ed11785d180b7161dc9ea5f41ab99 Mon Sep 17 00:00:00 2001 From: laurent-19 Date: Thu, 17 Apr 2025 15:23:32 +0300 Subject: [PATCH 1/2] include: Update DepthImageToLaserScan with distortion model Added distortion model case in the convert func Use rational polynomial, D coeffs and K coeffs for camera model Signed-off-by: laurent-19 --- .../DepthImageToLaserScan.hpp | 46 ++++++++++++++++--- 1 file changed, 39 insertions(+), 7 deletions(-) diff --git a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp index 4d4b827..a93260e 100644 --- a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp +++ b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp @@ -35,6 +35,7 @@ #include #include +#include #include "depthimage_to_laserscan/DepthImageToLaserScan_export.h" #include "depthimage_to_laserscan/depth_traits.hpp" @@ -173,20 +174,51 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final for (uint32_t u = 0; u < depth_msg->width; u++) { // Loop over each pixel in row T depth = depth_row[u]; - double r = depth; // Assign to pass through NaNs and Infs - // Atan2(x, z), but depth divides out - double th = -std::atan2(static_cast(u - center_x) * constant_x, unit_scaling); - int index = (th - scan_msg->angle_min) / scan_msg->angle_increment; + if (!depthimage_to_laserscan::DepthTraits::valid(depth)) { // Not NaN or Inf + continue; // Skip invalid depths + } - if (depthimage_to_laserscan::DepthTraits::valid(depth)) { // Not NaN or Inf + double r; + double th; + int index; + + if (cam_model.cameraInfo().distortion_model == "rational_polynomial" && + cam_model.cameraInfo().d.size() >= 8) + { + // Get the camera model coefficients + const auto & k = cam_model.cameraInfo().k; + const double z = depthimage_to_laserscan::DepthTraits::toMeters(depth); + + // Create the camera matrix + static const cv::Mat cameraMatrix = (cv::Mat_(3, 3) << + k[0], k[1], k[2], + k[3], k[4], k[5], + k[6], k[7], k[8]); + + // Undistort point + std::vector distorted_points{cv::Point2d(u, v)}; + std::vector undistorted_points; + cv::undistortPoints( + distorted_points, undistorted_points, + cameraMatrix, cam_model.distortionCoeffs()); + + const double x = undistorted_points[0].x * z; + th = -std::atan2(x, z); + r = z; + } else { + // Original common case // Calculate in XYZ - double x = (u - center_x) * depth * constant_x; - double z = depthimage_to_laserscan::DepthTraits::toMeters(depth); + const double x = (u - center_x) * depth * constant_x; + const double z = depthimage_to_laserscan::DepthTraits::toMeters(depth); + + // Atan2(x, z), but depth divides out + th = -std::atan2(static_cast(u - center_x) * constant_x, unit_scaling); // Calculate actual distance r = std::sqrt(std::pow(x, 2.0) + std::pow(z, 2.0)); } + index = static_cast((th - scan_msg->angle_min) / scan_msg->angle_increment); // Determine if this point should be used. if (use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)) { scan_msg->ranges[index] = r; From 045749f152a216a5e989979e65f9a16444242b8a Mon Sep 17 00:00:00 2001 From: laurent-19 Date: Thu, 12 Jun 2025 19:48:41 +0300 Subject: [PATCH 2/2] include: Fix testPos/NegInf failing. Handle invalid depth and angle th compute Signed-off-by: laurent-19 --- .../DepthImageToLaserScan.hpp | 77 +++++++++---------- 1 file changed, 37 insertions(+), 40 deletions(-) diff --git a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp index a93260e..a5a9624 100644 --- a/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp +++ b/include/depthimage_to_laserscan/DepthImageToLaserScan.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include "depthimage_to_laserscan/DepthImageToLaserScan_export.h" #include "depthimage_to_laserscan/depth_traits.hpp" @@ -174,54 +175,50 @@ class DEPTHIMAGETOLASERSCAN_EXPORT DepthImageToLaserScan final for (uint32_t u = 0; u < depth_msg->width; u++) { // Loop over each pixel in row T depth = depth_row[u]; - if (!depthimage_to_laserscan::DepthTraits::valid(depth)) { // Not NaN or Inf - continue; // Skip invalid depths - } - - double r; + double r = depth; // Assign to pass through NaNs and Infs double th; - int index; - if (cam_model.cameraInfo().distortion_model == "rational_polynomial" && - cam_model.cameraInfo().d.size() >= 8) - { - // Get the camera model coefficients - const auto & k = cam_model.cameraInfo().k; + if (depthimage_to_laserscan::DepthTraits::valid(depth)) { // Not NaN or Inf const double z = depthimage_to_laserscan::DepthTraits::toMeters(depth); - // Create the camera matrix - static const cv::Mat cameraMatrix = (cv::Mat_(3, 3) << - k[0], k[1], k[2], - k[3], k[4], k[5], - k[6], k[7], k[8]); - - // Undistort point - std::vector distorted_points{cv::Point2d(u, v)}; - std::vector undistorted_points; - cv::undistortPoints( - distorted_points, undistorted_points, - cameraMatrix, cam_model.distortionCoeffs()); - - const double x = undistorted_points[0].x * z; - th = -std::atan2(x, z); - r = z; + // Check distortion model and apply undistortion if necessary + if (cam_model.cameraInfo().distortion_model == "rational_polynomial" && + cam_model.cameraInfo().d.size() >= 8) + { + const auto & k = cam_model.cameraInfo().k; + static const cv::Mat cameraMatrix = (cv::Mat_(3, 3) << + k[0], k[1], k[2], + k[3], k[4], k[5], + k[6], k[7], k[8]); + std::vector distorted_points{cv::Point2d(u, v)}; + std::vector undistorted_points; + cv::undistortPoints( + distorted_points, undistorted_points, + cameraMatrix, cam_model.distortionCoeffs()); + + const double x = undistorted_points[0].x * z; + th = -std::atan2(x, z); // Overwrite default th value + r = z; + } else { + // Original common case + // Atan2(x, z), but depth divides out + th = -std::atan2(static_cast(u - center_x) * constant_x, unit_scaling); + // Calculate in XYZ + const double x = (u - center_x) * depth * constant_x; + r = std::sqrt(x * x + z * z); + } } else { - // Original common case - // Calculate in XYZ - const double x = (u - center_x) * depth * constant_x; - const double z = depthimage_to_laserscan::DepthTraits::toMeters(depth); - - // Atan2(x, z), but depth divides out + // Compute angle, even if depth is NaN or Inf th = -std::atan2(static_cast(u - center_x) * constant_x, unit_scaling); - - // Calculate actual distance - r = std::sqrt(std::pow(x, 2.0) + std::pow(z, 2.0)); } - index = static_cast((th - scan_msg->angle_min) / scan_msg->angle_increment); - // Determine if this point should be used. - if (use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)) { - scan_msg->ranges[index] = r; + int index = static_cast((th - scan_msg->angle_min) / scan_msg->angle_increment); + // Check if index is within bounds of the scan_msg + if (index >= 0 && index < static_cast(scan_msg->ranges.size())) { + // Determine if this point should be used. + if (use_point(r, scan_msg->ranges[index], scan_msg->range_min, scan_msg->range_max)) { + scan_msg->ranges[index] = r; + } } } }