Skip to content

add color image to backprojection #24

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
nathanhhughes wants to merge 1 commit into
base: main
Choose a base branch
from
Open

add color image to backprojection #24

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions semantic_inference_ros/include/semantic_inference_ros/pointcloud_projection.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
43 changes: 29 additions & 14 deletions semantic_inference_ros/src/backprojection_nodelet.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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 {
Expand All @@ -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_;
Expand Down Expand Up @@ -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;
Expand All @@ -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<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,
Expand All @@ -124,23 +129,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);
} catch (const cv_bridge::Exception& e) {
SLOG(ERROR) << "cv_bridge exception: " << e.what();
return;
Expand All @@ -149,10 +163,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;
Expand All @@ -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));
}

Expand Down
58 changes: 41 additions & 17 deletions semantic_inference_ros/src/pointcloud_projection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#include "semantic_inference_ros/pointcloud_projection.h"

#include <config_utilities/config.h>
#include <semantic_inference/logging.h>

#include <optional>

Expand All @@ -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;
Expand Down Expand Up @@ -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 {
Expand All @@ -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 {
Expand All @@ -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,
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -274,22 +275,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;
Expand All @@ -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<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] = 255u;
}
}

if (!in_view && config.discard_out_of_view) {
Expand All @@ -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);
}

Expand Down
3 changes: 2 additions & 1 deletion semantic_inference_ros/test/test_pointcloud_projection.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand Down