add color image to backprojection (#24)

* add color image to backprojection

* force input image encoding to match assumed ordering

* add new field to signal out-of-view colors

Author: nathanhhughes

semantic_inference_ros/include/semantic_inference_ros/pointcloud_projection.h

@@ -29,6 +29,8 @@ struct ProjectionConfig {
   std::set<uint32_t> allowed_labels;
   //! Input label remapping for input pointcloud
   std::unordered_map<uint32_t, uint32_t> input_remapping;
+  //! Alpha value for out-of-view point colors
+  uint16_t out_of_view_alpha = 0;
 
   bool isOverride(uint32_t label) const { return override_labels.count(label); }
   bool isAllowed(uint32_t label) const { return allowed_labels.count(label); }
@@ -43,6 +45,7 @@ bool projectSemanticImage(const ProjectionConfig& config,
                           const sensor_msgs::msg::PointCloud2& cloud,
                           const Eigen::Isometry3f& image_T_cloud,
                           sensor_msgs::msg::PointCloud2& output,
+                          const cv::Mat& color_image = cv::Mat(),
                           const ImageRecolor* recolor = nullptr);
 
 }  // namespace semantic_inference

semantic_inference_ros/src/backprojection_nodelet.cpp

@@ -7,7 +7,6 @@
 #include <message_filters/synchronizer.h>
 #include <tf2_ros/transform_listener.h>
 
-#include <cstdint>
 #include <string>
 
 #include <cv_bridge/cv_bridge.hpp>
@@ -28,7 +27,7 @@ using OptPose = std::optional<Eigen::Isometry3f>;
 
 struct BackprojectionNode : public rclcpp::Node {
  public:
-  using SyncPolicy = ApproximateTime<Image, CameraInfo, PointCloud2>;
+  using SyncPolicy = ApproximateTime<Image, Image, CameraInfo, PointCloud2>;
   using Sync = message_filters::Synchronizer<SyncPolicy>;
 
   struct Config {
@@ -48,11 +47,13 @@ struct BackprojectionNode : public rclcpp::Node {
                        const std::string& child_link,
                        const rclcpp::Time& stamp);
 
-  void callback(const Image::ConstSharedPtr& image_msg,
+  void callback(const Image::ConstSharedPtr& label_msg,
+                const Image::ConstSharedPtr& color_msg,
                 const CameraInfo::ConstSharedPtr& info_msg,
                 const PointCloud2::ConstSharedPtr& cloud_msg);
 
-  ianvs::ImageSubscription image_sub_;
+  ianvs::ImageSubscription label_sub_;
+  ianvs::ImageSubscription color_sub_;
   message_filters::Subscriber<CameraInfo> info_sub_;
   message_filters::Subscriber<PointCloud2> cloud_sub_;
   std::unique_ptr<Sync> sync_;
@@ -80,7 +81,8 @@ void declare_config(BackprojectionNode::Config& config) {
 BackprojectionNode::BackprojectionNode(const rclcpp::NodeOptions& options)
     : Node("backprojection_node", options),
       config(config::fromCLI<Config>(options.arguments())),
-      image_sub_(*this),
+      label_sub_(*this),
+      color_sub_(*this),
       tf_buffer_(get_clock()),
       tf_listener_(tf_buffer_) {
   using namespace std::placeholders;
@@ -99,14 +101,16 @@ BackprojectionNode::BackprojectionNode(const rclcpp::NodeOptions& options)
 
   pub_ = create_publisher<PointCloud2>("labeled_cloud", config.output_queue_size);
 
+  // these default to the same namespaces as the input to the inference node
   const rclcpp::QoS qos(config.input_queue_size);
-  // these are designed to default to the same namespaces as the input to the inference
-  // node
-  image_sub_.subscribe("semantic/image_raw", config.input_queue_size);
+  label_sub_.subscribe("semantic/image_raw", config.input_queue_size);
+  color_sub_.subscribe("color/image_raw", config.input_queue_size);
   info_sub_.subscribe(this, "color/camera_info", qos.get_rmw_qos_profile());
   cloud_sub_.subscribe(this, "cloud", qos.get_rmw_qos_profile());
-  sync_ = std::make_unique<Sync>(SyncPolicy(10), image_sub_, info_sub_, cloud_sub_);
-  sync_->registerCallback(std::bind(&BackprojectionNode::callback, this, _1, _2, _3));
+  sync_ = std::make_unique<Sync>(
+      SyncPolicy(10), label_sub_, color_sub_, info_sub_, cloud_sub_);
+  sync_->registerCallback(
+      std::bind(&BackprojectionNode::callback, this, _1, _2, _3, _4));
 }
 
 OptPose BackprojectionNode::getTransform(const std::string& parent_link,
@@ -124,23 +128,32 @@ OptPose BackprojectionNode::getTransform(const std::string& parent_link,
   return tf_double.cast<float>();
 }
 
-void BackprojectionNode::callback(const Image::ConstSharedPtr& image_msg,
+void BackprojectionNode::callback(const Image::ConstSharedPtr& label_msg,
+                                  const Image::ConstSharedPtr& color_msg,
                                   const CameraInfo::ConstSharedPtr& info_msg,
                                   const PointCloud2::ConstSharedPtr& cloud_msg) {
   // Find transform from cloud to image frame
   const rclcpp::Time stamp(cloud_msg->header.stamp);
   const auto image_T_cloud = getTransform(
-      !config.camera_frame.empty() ? config.camera_frame : image_msg->header.frame_id,
+      !config.camera_frame.empty() ? config.camera_frame : info_msg->header.frame_id,
       !config.lidar_frame.empty() ? config.lidar_frame : cloud_msg->header.frame_id,
       stamp);
   if (!image_T_cloud) {
     return;
   }
 
   // Convert image
-  cv_bridge::CvImageConstPtr img_ptr;
+  cv_bridge::CvImageConstPtr label_ptr;
   try {
-    img_ptr = cv_bridge::toCvShare(image_msg);
+    label_ptr = cv_bridge::toCvShare(label_msg);
+  } catch (const cv_bridge::Exception& e) {
+    SLOG(ERROR) << "cv_bridge exception: " << e.what();
+    return;
+  }
+
+  cv_bridge::CvImageConstPtr color_ptr;
+  try {
+    color_ptr = cv_bridge::toCvShare(color_msg, "bgr8");
   } catch (const cv_bridge::Exception& e) {
     SLOG(ERROR) << "cv_bridge exception: " << e.what();
     return;
@@ -149,10 +162,11 @@ void BackprojectionNode::callback(const Image::ConstSharedPtr& image_msg,
   auto output = std::make_unique<PointCloud2>();
   const auto valid = projectSemanticImage(config.projection,
                                           *info_msg,
-                                          img_ptr->image,
+                                          label_ptr->image,
                                           *cloud_msg,
                                           image_T_cloud.value(),
                                           *output,
+                                          color_ptr->image,
                                           recolor_.get());
   if (!valid) {
     return;
@@ -161,7 +175,7 @@ void BackprojectionNode::callback(const Image::ConstSharedPtr& image_msg,
   output->header = cloud_msg->header;
   output->header.frame_id = config.projection.use_lidar_frame
                                 ? cloud_msg->header.frame_id
-                                : image_msg->header.frame_id;
+                                : info_msg->header.frame_id;
   pub_->publish(std::move(output));
 }
 

semantic_inference_ros/src/pointcloud_projection.cpp

@@ -1,6 +1,7 @@
 #include "semantic_inference_ros/pointcloud_projection.h"
 
 #include <config_utilities/config.h>
+#include <semantic_inference/logging.h>
 
 #include <optional>
 
@@ -9,10 +10,10 @@
 #include <rclcpp/node.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
 
-#include "semantic_inference_ros/ros_log_sink.h"
-
 namespace semantic_inference {
 
+using sensor_msgs::PointCloud2ConstIterator;
+using sensor_msgs::PointCloud2Iterator;
 using sensor_msgs::msg::CameraInfo;
 using sensor_msgs::msg::Image;
 using sensor_msgs::msg::PointCloud2;
@@ -49,9 +50,9 @@ struct OutputPosIter {
  private:
   // techinically we could probably just have one iterator, but (small) chance that
   // someone sends a non-contiguous pointcloud
-  sensor_msgs::PointCloud2Iterator<float> x_iter_;
-  sensor_msgs::PointCloud2Iterator<float> y_iter_;
-  sensor_msgs::PointCloud2Iterator<float> z_iter_;
+  PointCloud2Iterator<float> x_iter_;
+  PointCloud2Iterator<float> y_iter_;
+  PointCloud2Iterator<float> z_iter_;
 };
 
 struct InputPosIter {
@@ -71,9 +72,9 @@ struct InputPosIter {
  private:
   // techinically we could probably just have one iterator, but (small) chance that
   // someone sends a non-contiguous pointcloud
-  sensor_msgs::PointCloud2ConstIterator<float> x_iter_;
-  sensor_msgs::PointCloud2ConstIterator<float> y_iter_;
-  sensor_msgs::PointCloud2ConstIterator<float> z_iter_;
+  PointCloud2ConstIterator<float> x_iter_;
+  PointCloud2ConstIterator<float> y_iter_;
+  PointCloud2ConstIterator<float> z_iter_;
 };
 
 struct InputLabelIterBase {
@@ -93,7 +94,7 @@ struct InputLabelIter : InputLabelIterBase {
   uint32_t get() const override { return *iter_; }
 
  private:
-  sensor_msgs::PointCloud2ConstIterator<T> iter_;
+  PointCloud2ConstIterator<T> iter_;
 };
 
 std::unique_ptr<InputLabelIterBase> iterFromFields(const PointCloud2& cloud,
@@ -222,8 +223,8 @@ struct LabelImageAdapter {
 void recolorCloud(PointCloud2& output,
                   const ImageRecolor& recolor,
                   uint32_t unknown_label) {
-  auto labels = sensor_msgs::PointCloud2ConstIterator<uint32_t>(output, "label");
-  auto colors = sensor_msgs::PointCloud2Iterator<uint8_t>(output, "rgba");
+  auto labels = PointCloud2ConstIterator<uint32_t>(output, "label");
+  auto colors = PointCloud2Iterator<uint8_t>(output, "rgba");
   while (labels != labels.end()) {
     const auto unknown = static_cast<uint32_t>(*labels) == unknown_label;
     const auto& color = unknown ? recolor.default_color : recolor.getColor(*labels);
@@ -259,6 +260,9 @@ void declare_config(ProjectionConfig& config) {
   field(config.override_labels, "override_labels");
   field(config.allowed_labels, "allowed_labels");
   field(config.input_remapping, "input_remapping");
+  field(config.out_of_view_alpha, "out_of_view_alpha");
+
+  checkInRange<uint16_t>(config.out_of_view_alpha, 0, 255, "out_of_view_alpha");
 }
 
 std::optional<uint32_t> ProjectionConfig::remapInput(
@@ -274,22 +278,27 @@ std::optional<uint32_t> ProjectionConfig::remapInput(
 
 bool projectSemanticImage(const ProjectionConfig& config,
                           const CameraInfo& intrinsics,
-                          const cv::Mat& image,
+                          const cv::Mat& labels,
                           const PointCloud2& cloud,
                           const Eigen::Isometry3f& image_T_cloud,
                           PointCloud2& output,
+                          const cv::Mat& color_image,
                           const ImageRecolor* recolor) {
   image_geometry::PinholeCameraModel model;
   model.fromCameraInfo(intrinsics);
 
   auto pos_in_iter = InputPosIter(cloud);
-  const LabelImageAdapter img_wrapper(image);
+  const LabelImageAdapter img_wrapper(labels);
   // iterator over label field in input pointcloud if it exists
   InputLabelAdapter label_in_iter(cloud, config.input_label_fieldname);
 
-  initOutput(cloud, output, recolor != nullptr);
+  initOutput(cloud, output, recolor != nullptr || !color_image.empty());
   auto pos_out_iter = OutputPosIter(output);
-  auto label_out_iter = sensor_msgs::PointCloud2Iterator<uint32_t>(output, "label");
+  auto label_out_iter = PointCloud2Iterator<uint32_t>(output, "label");
+  std::unique_ptr<PointCloud2Iterator<uint8_t>> color_out_iter;
+  if (!color_image.empty()) {
+    color_out_iter = std::make_unique<PointCloud2Iterator<uint8_t>>(output, "rgba");
+  }
 
   while (pos_in_iter) {
     const Eigen::Vector3f p_cloud = *pos_in_iter;
@@ -304,13 +313,28 @@ bool projectSemanticImage(const ProjectionConfig& config,
       v = std::round(pixel.y);
     }
 
-    const auto in_view = u >= 0 && u < image.cols && v >= 0 && v < image.rows;
+    const auto in_view = u >= 0 && u < labels.cols && v >= 0 && v < labels.rows;
     const uint32_t label_in =
         config.remapInput(*label_in_iter).value_or(config.unknown_label);
     if (in_view) {
       *label_out_iter = config.isOverride(label_in) ? label_in : img_wrapper(v, u);
+      if (color_out_iter) {
+        auto& curr_color_out = *color_out_iter;
+        const auto& pixel = color_image.at<cv::Vec3b>(v, u);
+        curr_color_out[0] = pixel[0];
+        curr_color_out[1] = pixel[1];
+        curr_color_out[2] = pixel[2];
+        curr_color_out[3] = 255u;
+      }
     } else {
       *label_out_iter = config.isAllowed(label_in) ? label_in : config.unknown_label;
+      if (color_out_iter) {
+        auto& curr_color_out = *color_out_iter;
+        curr_color_out[0] = 0;
+        curr_color_out[1] = 0;
+        curr_color_out[2] = 0;
+        curr_color_out[3] = config.out_of_view_alpha;
+      }
     }
 
     if (!in_view && config.discard_out_of_view) {
@@ -323,9 +347,12 @@ bool projectSemanticImage(const ProjectionConfig& config,
     ++pos_out_iter;
     ++label_in_iter;
     ++label_out_iter;
+    if (color_out_iter) {
+      ++(*color_out_iter);
+    }
   }
 
-  if (recolor) {
+  if (recolor && color_image.empty()) {
     recolorCloud(output, *recolor, config.unknown_label);
   }
 

semantic_inference_ros/test/test_pointcloud_projection.cpp

@@ -135,7 +135,8 @@ TEST(PointcloudProjection, ColorCorrect) {
   config.unknown_label = 5;
 
   PointCloud2 labeled;
-  projectSemanticImage(config, info, img, cloud, image_T_cloud, labeled, &recolor);
+  projectSemanticImage(
+      config, info, img, cloud, image_T_cloud, labeled, cv::Mat(), &recolor);
 
   pcl::PointCloud<pcl::PointXYZRGBL> expected;
   expected.push_back(makePoint(1.0, 2.0, 1.0, 0, 255, 0, 2));