allow resizing inputs when republishing (#28)

Author: nathanhhughes

hydra_ros/include/hydra_ros/active_window/reconstruction_visualizer.h

@@ -45,6 +45,8 @@
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
+#include "hydra_ros/utils/input_data_to_messages.h"
+
 namespace hydra {
 
 class ReconstructionVisualizer : public ActiveWindowModule::Sink {
@@ -68,6 +70,7 @@ class ReconstructionVisualizer : public ActiveWindowModule::Sink {
     visualizer::RangeColormap::Config colormap;
     visualizer::CategoricalColormap::Config label_colormap;
     config::VirtualConfig<MeshColoring> mesh_coloring;
+    DisplayConfig image_display;
   } const config;
 
   explicit ReconstructionVisualizer(const Config& config);

hydra_ros/include/hydra_ros/utils/input_data_to_messages.h

@@ -45,18 +45,34 @@ namespace hydra {
 struct InputData;
 using CmapFunc = std::function<spark_dsg::Color(uint32_t)>;
 
+struct DisplayConfig {
+  float width_scale = 1.0f;
+  float height_scale = 1.0f;
+};
+
+void declare_config(DisplayConfig& config);
+
 /**
  * @brief Make a colored image for the current labels in the input data
  */
 sensor_msgs::msg::Image::SharedPtr makeImage(const std_msgs::msg::Header& header,
                                              const InputData& sensor_data,
-                                             const CmapFunc& colormap);
+                                             const CmapFunc& colormap,
+                                             const DisplayConfig& config = {});
 
 /**
  * @brief Copy the current depth image to ros
  */
 sensor_msgs::msg::Image::SharedPtr makeDepthImage(const std_msgs::msg::Header& header,
-                                                  const InputData& sensor_data);
+                                                  const InputData& sensor_data,
+                                                  const DisplayConfig& config = {});
+
+/**
+ * @brief Copy the current range image to ros
+ */
+sensor_msgs::msg::Image::SharedPtr makeRangeImage(const std_msgs::msg::Header& header,
+                                                  const InputData& sensor_data,
+                                                  const DisplayConfig& config = {});
 
 /**
  * @brief Convert the input pointcloud to a ROS type

hydra_ros/src/active_window/reconstruction_visualizer.cpp

@@ -44,7 +44,6 @@
 #include <tf2_eigen/tf2_eigen.hpp>
 
 #include "hydra_ros/common.h"
-#include "hydra_ros/utils/input_data_to_messages.h"
 #include "hydra_ros/visualizer/voxel_drawing.h"
 
 namespace hydra {
@@ -102,6 +101,7 @@ void declare_config(ReconstructionVisualizer::Config& config) {
   field(config.filter_points_by_range, "filter_points_by_range");
   field(config.colormap, "colormap");
   field(config.label_colormap, "label_colormap");
+  field(config.image_display, "image_display");
   config.mesh_coloring.setOptional();
   field(config.mesh_coloring, "mesh_coloring");
 }
@@ -187,12 +187,18 @@ void ReconstructionVisualizer::call(uint64_t timestamp_ns,
 
     const auto sensor_name = output.sensor_data->getSensor().name;
     image_pubs_.publish(sensor_name + "/labels", [&]() {
-      return makeImage(header, *output.sensor_data, [this](uint32_t label) {
-        return label_colormap_(label);
-      });
+      return makeImage(
+          header,
+          *output.sensor_data,
+          [this](uint32_t label) { return label_colormap_(label); },
+          config.image_display);
+    });
+    image_pubs_.publish(sensor_name + "/depth", [&]() {
+      return makeDepthImage(header, *output.sensor_data, config.image_display);
+    });
+    image_pubs_.publish(sensor_name + "/range", [&]() {
+      return makeRangeImage(header, *output.sensor_data, config.image_display);
     });
-    image_pubs_.publish(sensor_name + "/depth",
-                        [&]() { return makeDepthImage(header, *output.sensor_data); });
     cloud_pubs_.publish(sensor_name + "/pointcloud", [&]() {
       return makeCloud(header, *output.sensor_data, config.filter_points_by_range);
     });

hydra_ros/src/utils/input_data_to_messages.cpp

@@ -34,6 +34,7 @@
  * -------------------------------------------------------------------------- */
 #include "hydra_ros/utils/input_data_to_messages.h"
 
+#include <config_utilities/config.h>
 #include <hydra/input/input_data.h>
 
 #include <cv_bridge/cv_bridge.hpp>
@@ -59,36 +60,70 @@ PointField makeField(const std::string& name,
   return field;
 }
 
+cv::Mat showImage(const cv::Mat& input, const DisplayConfig& config) {
+  if (config.width_scale == 1.0f && config.height_scale == 1.0f) {
+    return input;
+  }
+
+  cv::Mat output;
+  cv::resize(input,
+             output,
+             cv::Size(),
+             config.width_scale,
+             config.height_scale,
+             cv::INTER_NEAREST);
+  return output;
+}
+
 }  // namespace
 
+void declare_config(DisplayConfig& config) {
+  using namespace config;
+  name("DisplayConfig");
+  field(config.width_scale, "width_scale");
+  field(config.height_scale, "height_scale");
+}
+
 Image::SharedPtr makeImage(const std_msgs::msg::Header& header,
                            const InputData& sensor_data,
-                           const CmapFunc& colormap) {
+                           const CmapFunc& colormap,
+                           const DisplayConfig& config) {
   const auto& labels = sensor_data.label_image;
   cv_bridge::CvImagePtr msg(new cv_bridge::CvImage());
   msg->header = header;
   msg->encoding = "rgb8";
-  msg->image = cv::Mat(labels.rows, labels.cols, CV_8UC3);
+  cv::Mat img(labels.rows, labels.cols, CV_8UC3);
   for (int r = 0; r < labels.rows; ++r) {
     for (int c = 0; c < labels.cols; ++c) {
-      auto pixel = msg->image.ptr<uint8_t>(r, c);
+      auto pixel = img.ptr<uint8_t>(r, c);
       const auto color = colormap(labels.at<int>(r, c));
       *pixel = color.r;
       *(pixel + 1) = color.g;
       *(pixel + 2) = color.b;
     }
   }
 
+  msg->image = showImage(img, config);
   return msg->toImageMsg();
 }
 
 Image::SharedPtr makeDepthImage(const std_msgs::msg::Header& header,
-                                const InputData& sensor_data) {
-  const auto& depth = sensor_data.depth_image;
+                                const InputData& sensor_data,
+                                const DisplayConfig& config) {
+  cv_bridge::CvImagePtr msg(new cv_bridge::CvImage());
+  msg->header = header;
+  msg->encoding = "32FC1";
+  msg->image = showImage(sensor_data.depth_image, config);
+  return msg->toImageMsg();
+}
+
+Image::SharedPtr makeRangeImage(const std_msgs::msg::Header& header,
+                                const InputData& sensor_data,
+                                const DisplayConfig& config) {
   cv_bridge::CvImagePtr msg(new cv_bridge::CvImage());
   msg->header = header;
   msg->encoding = "32FC1";
-  msg->image = depth;
+  msg->image = showImage(sensor_data.range_image, config);
   return msg->toImageMsg();
 }