wanngwe/odin_ros_driver1
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add overlay image on c++ for debug; unify synced sensor output

Browse Source
This commit is contained in:
jingyang-huang
2026-04-09 15:51:26 +08:00
parent 77fac3f793
commit cdfe249111
5 changed files with 109 additions and 156 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/cloud_reprojection_ros.cpp
+103 -23
View File
@@ -51,6 +51,67 @@ cv::Mat decodeCompressedToBgr(const uint8_t* data, size_t len)
return cv::imdecode(raw, cv::IMREAD_COLOR);
}
cv::Mat overlayProjectedCloudOnImage(const cv::Mat& camera_bgr,
const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam,
const CloudReprojector::CameraParams& cam_params,
int point_radius = 2,
float min_depth = 0.5f,
float max_depth = 30.0f)
{
if (camera_bgr.empty()) {
return cv::Mat();
}
cv::Mat overlay = camera_bgr.clone();
mini_vikit::PolynomialCamera camera_model(
cam_params.image_width, cam_params.image_height,
cam_params.A11, cam_params.A22,
cam_params.u0, cam_params.v0,
cam_params.A12,
cam_params.k2, cam_params.k3, cam_params.k4,
cam_params.k5, cam_params.k6, cam_params.k7);
std::vector<cv::Point> pixels;
std::vector<float> depths;
pixels.reserve(cloud_in_cam.size());
depths.reserve(cloud_in_cam.size());
for (const auto& pt : cloud_in_cam) {
if (pt.z <= min_depth || pt.z >= max_depth) {
continue;
}
const Eigen::Vector2d uv = camera_model.world2cam(Eigen::Vector3d(pt.x, pt.y, pt.z));
const int u = static_cast<int>(std::round(uv.x()));
const int v = static_cast<int>(std::round(uv.y()));
if (u < 0 || u >= overlay.cols || v < 0 || v >= overlay.rows) {
continue;
}
pixels.emplace_back(u, v);
depths.emplace_back(static_cast<float>(pt.z));
}
if (pixels.empty()) {
return overlay;
}
cv::Mat depth_values(static_cast<int>(depths.size()), 1, CV_32F, depths.data());
cv::Mat clipped, norm_u8, colors;
cv::min(cv::max(depth_values, min_depth), max_depth, clipped);
clipped = (clipped - min_depth) * (255.0f / (max_depth - min_depth + 1e-6f));
clipped.convertTo(norm_u8, CV_8U);
cv::applyColorMap(norm_u8, colors, cv::COLORMAP_JET);
for (int i = 0; i < colors.rows; ++i) {
const cv::Vec3b color = colors.at<cv::Vec3b>(i, 0);
cv::circle(overlay, pixels[static_cast<size_t>(i)], point_radius,
cv::Scalar(color[0], color[1], color[2]), -1);
}
return overlay;
}
} // namespace
#ifdef ROS2
@@ -101,7 +162,7 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
<< "\n wiwc_topic: " << wiwc_topic_
<< "\n camera_image_topic: " << camera_image_topic_
<< "\n sync_* topics under: " << sync_topic_prefix_
<< "\n reprojected_image_topic (depth z grayscale): " << reprojected_image_topic_
<< "\n overlay_img_topic: " << sync_overlay_image_topic_
<< "\n combined_compressed_topic: " << combined_compressed_topic_
<< "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off"));
@@ -121,12 +182,12 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
sync_wiwc_pub_ = this->create_publisher<Odometry>(sync_wiwc_topic_, 10);
sync_image_pub_ = this->create_publisher<CompressedImage>(sync_image_topic_, 10);
reprojected_image_pub_ = image_transport::create_publisher(this, reprojected_image_topic_);
overlay_image_pub_ = image_transport::create_publisher(this, sync_overlay_image_topic_);
if (publish_combined_compressed_) {
combined_pub_ = this->create_publisher<CompressedImage>(combined_compressed_topic_, 10);
}
RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + depth + combined)");
RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + overlay + combined)");
}
void CloudReprojectionRosNode::loadParameters()
@@ -135,7 +196,7 @@ void CloudReprojectionRosNode::loadParameters()
this->declare_parameter<std::string>("cloud_slam_topic", "/odin1/cloud_slam");
this->declare_parameter<std::string>("odometry_topic", "/odin1/odometry");
this->declare_parameter<std::string>("wiwc_topic", "/odin1/wiwc");
this->declare_parameter<std::string>("reprojected_image_topic", "/odin1/reprojected_image");
this->declare_parameter<std::string>("overlay_image_topic", "/odin1/overlay_img");
this->declare_parameter<std::string>("register_keys.sync_camera_topic", "/odin1/image/compressed");
this->declare_parameter<std::string>("register_keys.sync_topic_prefix", "/odin1/sync");
this->declare_parameter<std::string>("register_keys.combined_compressed_topic", "/odin1/combined_image/compressed");
@@ -145,7 +206,7 @@ void CloudReprojectionRosNode::loadParameters()
cloud_slam_topic_ = this->get_parameter("cloud_slam_topic").as_string();
odometry_topic_ = this->get_parameter("odometry_topic").as_string();
wiwc_topic_ = this->get_parameter("wiwc_topic").as_string();
reprojected_image_topic_ = this->get_parameter("reprojected_image_topic").as_string();
// sync_overlay_image_topic_ = this->get_parameter("overlay_image_topic").as_string();
camera_image_topic_ = this->get_parameter("register_keys.sync_camera_topic").as_string();
sync_topic_prefix_ = this->get_parameter("register_keys.sync_topic_prefix").as_string();
combined_compressed_topic_ = this->get_parameter("register_keys.combined_compressed_topic").as_string();
@@ -159,6 +220,7 @@ void CloudReprojectionRosNode::loadParameters()
sync_odom_topic_ = sync_topic_prefix_ + "/odometry";
sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
sync_image_topic_ = sync_topic_prefix_ + "/image/compressed";
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img";
// Load camera parameters from calib.yaml file directly
std::string package_path = get_package_source_directory();
@@ -243,10 +305,20 @@ void CloudReprojectionRosNode::syncCallback(
const Odometry::ConstSharedPtr& wiwc_msg,
const CompressedImage::ConstSharedPtr& image_compressed_msg)
{
sync_cloud_slam_pub_->publish(*cloud_msg);
sync_odom_pub_->publish(*odom_msg);
sync_wiwc_pub_->publish(*wiwc_msg);
sync_image_pub_->publish(*image_compressed_msg);
const auto sync_stamp = image_compressed_msg->header.stamp;
PointCloud2 sync_cloud_slam_msg = *cloud_msg;
sync_cloud_slam_msg.header.stamp = sync_stamp;
Odometry sync_odom_msg = *odom_msg;
sync_odom_msg.header.stamp = sync_stamp;
Odometry sync_wiwc_msg = *wiwc_msg;
sync_wiwc_msg.header.stamp = sync_stamp;
CompressedImage sync_image_msg = *image_compressed_msg;
sync_image_msg.header.stamp = sync_stamp;
sync_cloud_slam_pub_->publish(sync_cloud_slam_msg);
sync_odom_pub_->publish(sync_odom_msg);
sync_wiwc_pub_->publish(sync_wiwc_msg);
pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
pcl::fromROSMsg(*cloud_msg, cloud_odom);
@@ -295,28 +367,36 @@ void CloudReprojectionRosNode::syncCallback(
PointCloud2 cloud_cam_msg;
pcl::toROSMsg(cloud_cam, cloud_cam_msg);
cloud_cam_msg.header = image_compressed_msg->header;
cloud_cam_msg.header.stamp = sync_stamp;
if (cloud_cam_msg.header.frame_id.empty()) {
cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id;
}
sync_cloud_pub_->publish(cloud_cam_msg);
sync_image_pub_->publish(sync_image_msg);
cv::Mat depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
if (depth_vis.empty()) {
return;
}
auto depth_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", depth_vis).toImageMsg();
reprojected_image_pub_.publish(*depth_msg);
cv::Mat cam_bgr = decodeCompressedToBgr(
image_compressed_msg->data.data(), image_compressed_msg->data.size());
if (cam_bgr.empty()) {
RCLCPP_ERROR(this->get_logger(), "cv::imdecode failed (sync compressed image, format=%s)",
image_compressed_msg->format.c_str());
return;
}
cv::Mat overlay_vis = overlayProjectedCloudOnImage(
cam_bgr, cloud_cam, reprojector_->getCameraParams());
if (overlay_vis.empty()) {
return;
}
auto overlay_msg = cv_bridge::CvImage(image_compressed_msg->header, "bgr8", overlay_vis).toImageMsg();
overlay_image_pub_.publish(*overlay_msg);
if (combined_pub_) {
cv::Mat cam_bgr = decodeCompressedToBgr(
image_compressed_msg->data.data(), image_compressed_msg->data.size());
if (cam_bgr.empty()) {
RCLCPP_ERROR(this->get_logger(), "cv::imdecode failed (sync compressed image, format=%s)",
image_compressed_msg->format.c_str());
return;
}
const int H = std::max(depth_vis.rows, cam_bgr.rows);
cv::Mat left = resizeToHeight(depth_vis, H);
cv::Mat right = resizeToHeight(cam_bgr, H);
@@ -420,7 +500,7 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::Nod
<< "\n wiwc_topic: " << wiwc_topic_
<< "\n camera_image_topic: " << camera_image_topic_
<< "\n sync_prefix: " << sync_topic_prefix_
<< "\n reprojected_image_topic (depth z): " << reprojected_image_topic_
<< "\n overlay_image_topic (depth z): " << sync_overlay_image_topic_
<< "\n combined_compressed_topic: " << combined_compressed_topic_
<< "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off"));
@@ -438,7 +518,7 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::Nod
sync_wiwc_pub_ = nh_.advertise<nav_msgs::Odometry>(sync_wiwc_topic_, 1);
sync_image_pub_ = nh_.advertise<sensor_msgs::CompressedImage>(sync_image_topic_, 1);
reprojected_image_pub_ = nh_.advertise<sensor_msgs::Image>(reprojected_image_topic_, 1);
overlay_image_pub_ = nh_.advertise<sensor_msgs::Image>(sync_overlay_image_topic_, 1);
if (publish_combined_compressed_) {
combined_pub_ = nh_.advertise<sensor_msgs::CompressedImage>(combined_compressed_topic_, 1);
}
@@ -451,7 +531,7 @@ void CloudReprojectionRosNode::loadParameters()
pnh_.param<std::string>("cloud_slam_topic", cloud_slam_topic_, std::string("/odin1/cloud_slam"));
pnh_.param<std::string>("odometry_topic", odometry_topic_, std::string("/odin1/odometry"));
pnh_.param<std::string>("wiwc_topic", wiwc_topic_, std::string("/odin1/wiwc"));
pnh_.param<std::string>("reprojected_image_topic", reprojected_image_topic_, std::string("/odin1/reprojected_image"));
pnh_.param<std::string>("overlay_image_topic", sync_overlay_image_topic_, std::string("/odin1/reprojected_image"));
pnh_.param<std::string>("register_keys/sync_camera_topic", camera_image_topic_, std::string("/odin1/image/compressed"));
pnh_.param<std::string>("register_keys/sync_topic_prefix", sync_topic_prefix_, std::string("/odin1/sync"));
pnh_.param<std::string>("register_keys/combined_compressed_topic", combined_compressed_topic_, std::string("/odin1/combined_image/compressed"));
@@ -588,7 +668,7 @@ void CloudReprojectionRosNode::syncCallback(
}
sensor_msgs::ImagePtr depth_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", depth_vis).toImageMsg();
reprojected_image_pub_.publish(depth_msg);
overlay_image_pub_.publish(depth_msg);
if (publish_combined_compressed_) {
cv::Mat cam_bgr = decodeCompressedToBgr(