diff --git a/semantic_inference_ros/include/semantic_inference_ros/pointcloud_projection.h b/semantic_inference_ros/include/semantic_inference_ros/pointcloud_projection.h index aa21603..85386c5 100644 --- a/semantic_inference_ros/include/semantic_inference_ros/pointcloud_projection.h +++ b/semantic_inference_ros/include/semantic_inference_ros/pointcloud_projection.h @@ -43,6 +43,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 diff --git a/semantic_inference_ros/src/backprojection_nodelet.cpp b/semantic_inference_ros/src/backprojection_nodelet.cpp index 5905fc0..9142998 100644 --- a/semantic_inference_ros/src/backprojection_nodelet.cpp +++ b/semantic_inference_ros/src/backprojection_nodelet.cpp @@ -7,7 +7,6 @@ #include #include -#include #include #include @@ -28,7 +27,7 @@ using OptPose = std::optional; struct BackprojectionNode : public rclcpp::Node { public: - using SyncPolicy = ApproximateTime; + using SyncPolicy = ApproximateTime; using Sync = message_filters::Synchronizer; 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 info_sub_; message_filters::Subscriber cloud_sub_; std::unique_ptr sync_; @@ -80,7 +81,8 @@ void declare_config(BackprojectionNode::Config& config) { BackprojectionNode::BackprojectionNode(const rclcpp::NodeOptions& options) : Node("backprojection_node", options), config(config::fromCLI(options.arguments())), - image_sub_(*this), + label_sub_(*this), + color_sub_(*this), tf_buffer_(get_clock()), tf_listener_(tf_buffer_) { using namespace std::placeholders; @@ -102,11 +104,14 @@ BackprojectionNode::BackprojectionNode(const rclcpp::NodeOptions& options) 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(SyncPolicy(10), image_sub_, info_sub_, cloud_sub_); - sync_->registerCallback(std::bind(&BackprojectionNode::callback, this, _1, _2, _3)); + sync_ = std::make_unique( + 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,13 +129,14 @@ OptPose BackprojectionNode::getTransform(const std::string& parent_link, return tf_double.cast(); } -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) { @@ -138,9 +144,17 @@ void BackprojectionNode::callback(const Image::ConstSharedPtr& image_msg, } // 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); } catch (const cv_bridge::Exception& e) { SLOG(ERROR) << "cv_bridge exception: " << e.what(); return; @@ -149,10 +163,11 @@ void BackprojectionNode::callback(const Image::ConstSharedPtr& image_msg, auto output = std::make_unique(); 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 +176,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)); } diff --git a/semantic_inference_ros/src/pointcloud_projection.cpp b/semantic_inference_ros/src/pointcloud_projection.cpp index 8b1529d..e5558da 100644 --- a/semantic_inference_ros/src/pointcloud_projection.cpp +++ b/semantic_inference_ros/src/pointcloud_projection.cpp @@ -1,6 +1,7 @@ #include "semantic_inference_ros/pointcloud_projection.h" #include +#include #include @@ -9,10 +10,10 @@ #include #include -#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 x_iter_; - sensor_msgs::PointCloud2Iterator y_iter_; - sensor_msgs::PointCloud2Iterator z_iter_; + PointCloud2Iterator x_iter_; + PointCloud2Iterator y_iter_; + PointCloud2Iterator 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 x_iter_; - sensor_msgs::PointCloud2ConstIterator y_iter_; - sensor_msgs::PointCloud2ConstIterator z_iter_; + PointCloud2ConstIterator x_iter_; + PointCloud2ConstIterator y_iter_; + PointCloud2ConstIterator z_iter_; }; struct InputLabelIterBase { @@ -93,7 +94,7 @@ struct InputLabelIter : InputLabelIterBase { uint32_t get() const override { return *iter_; } private: - sensor_msgs::PointCloud2ConstIterator iter_; + PointCloud2ConstIterator iter_; }; std::unique_ptr 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(output, "label"); - auto colors = sensor_msgs::PointCloud2Iterator(output, "rgba"); + auto labels = PointCloud2ConstIterator(output, "label"); + auto colors = PointCloud2Iterator(output, "rgba"); while (labels != labels.end()) { const auto unknown = static_cast(*labels) == unknown_label; const auto& color = unknown ? recolor.default_color : recolor.getColor(*labels); @@ -274,22 +275,27 @@ std::optional 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(output, "label"); + auto label_out_iter = PointCloud2Iterator(output, "label"); + std::unique_ptr> color_out_iter; + if (!color_image.empty()) { + color_out_iter = std::make_unique>(output, "rgba"); + } while (pos_in_iter) { const Eigen::Vector3f p_cloud = *pos_in_iter; @@ -304,13 +310,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(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] = 255u; + } } if (!in_view && config.discard_out_of_view) { @@ -323,9 +344,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); } diff --git a/semantic_inference_ros/test/test_pointcloud_projection.cpp b/semantic_inference_ros/test/test_pointcloud_projection.cpp index 6129dc2..a70f680 100644 --- a/semantic_inference_ros/test/test_pointcloud_projection.cpp +++ b/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 expected; expected.push_back(makePoint(1.0, 2.0, 1.0, 0, 255, 0, 2));