From d3c625e8a386b797631c1a32178319b5e970ce57 Mon Sep 17 00:00:00 2001 From: Laura Lindzey Date: Fri, 27 Jan 2023 12:05:24 -0800 Subject: [PATCH 1/2] Minor cleanup of intensity->color calculation * Remove dead code * Rename variable for clarity * Add comment --- ros/src/sonar_postprocessor_nodelet.cpp | 37 ++++--------------------- 1 file changed, 6 insertions(+), 31 deletions(-) diff --git a/ros/src/sonar_postprocessor_nodelet.cpp b/ros/src/sonar_postprocessor_nodelet.cpp index 1ca7bfa..13164e9 100644 --- a/ros/src/sonar_postprocessor_nodelet.cpp +++ b/ros/src/sonar_postprocessor_nodelet.cpp @@ -54,47 +54,22 @@ class SonarPostprocessorNodelet : public nodelet::Nodelet { out.image.data.reserve(interface.ranges().size() * interface.azimuths().size()); - double logmin, logmax; - for (unsigned int r_idx = 0; r_idx < interface.nRanges(); ++r_idx) { for (unsigned int a_idx = 0; a_idx < interface.nAzimuth(); ++a_idx) { sonar_image_proc::AzimuthRangeIndices idx(a_idx, r_idx); - // const uint32_t pix = interface.intensity_uint32(idx); - // const auto range = interface.range(r_idx); - // Avoid log(0) auto intensity = interface.intensity_uint32(idx); - auto v = log(std::max((uint)1, intensity)) / log(UINT32_MAX); - - if ((r_idx == 0) && (a_idx == 0)) { - logmin = v; - logmax = v; - } else { - logmin = std::min(v, logmin); - logmax = std::max(v, logmax); - } + auto vv = log(std::max((uint)1, intensity)) / log(UINT32_MAX); + // The output image will look better if the full range of the colormap + // corresponds to a subset of the range of v. const float vmax = 1.0, threshold = 0.74; - - v = (v - threshold) / (vmax - threshold); - v = std::min(1.0, std::max(0.0, v)); - out.image.data.push_back(UINT8_MAX * v); - - // Just do the math in float for now - // float i = static_cast(pix)/UINT32_MAX; - - // //ROS_INFO_STREAM(a_idx << "," << r_idx << " : " << pix << " => " << - // i); - - // i *= gain_; - - // UINT8_MAX * i); + auto color = (vv - threshold) / (vmax - threshold); + color = std::min(1.0, std::max(0.0, vv)); + out.image.data.push_back(UINT8_MAX * color); } } - - // - float dr = exp(logmax - logmin); pubSonarImage_.publish(out); } From 55fc01f777d6fdb52daab99a45da2dcfa6a1b23b Mon Sep 17 00:00:00 2001 From: Laura Lindzey Date: Fri, 27 Jan 2023 12:12:59 -0800 Subject: [PATCH 2/2] Fix use of wrong variable in calculation --- ros/src/sonar_postprocessor_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros/src/sonar_postprocessor_nodelet.cpp b/ros/src/sonar_postprocessor_nodelet.cpp index 13164e9..e506c09 100644 --- a/ros/src/sonar_postprocessor_nodelet.cpp +++ b/ros/src/sonar_postprocessor_nodelet.cpp @@ -66,7 +66,7 @@ class SonarPostprocessorNodelet : public nodelet::Nodelet { // corresponds to a subset of the range of v. const float vmax = 1.0, threshold = 0.74; auto color = (vv - threshold) / (vmax - threshold); - color = std::min(1.0, std::max(0.0, vv)); + color = std::min(1.0, std::max(0.0, color)); out.image.data.push_back(UINT8_MAX * color); } }