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

add online cloud reprojection

Browse Source
This commit is contained in:
hjy
2026-04-08 14:27:30 +08:00
parent 0ec1300ab5
commit 6864f1114c
4 changed files with 63 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/cloud_reprojector.hpp
+4
View File
@@ -54,6 +54,10 @@ public:
bool initialize(const CameraParams& cam_params, const ExtrinsicParams& ext_params);
pcl::PointCloud<pcl::PointXYZRGB> transformCloudToCamera(
const pcl::PointCloud<pcl::PointXYZRGB>& cloud_odom,
const OdomPose& odom_pose) const;
cv::Mat reprojectCloud(const pcl::PointCloud<pcl::PointXYZRGB>& cloud_odom,
const OdomPose& odom_pose);
script/record_dataset.sh
Executable
+7
View File
@@ -0,0 +1,7 @@
ros2 bag record \
/odin1/sync/cloud_in_cam \
/odin1/sync/image/compressed \
/odin1/sync/odometry \
/odin1/sync/wiwc \
/odin1/combined_image/compressed
src/cloud_reprojection_ros.cpp
+34 -4
View File
@@ -147,7 +147,7 @@ void CloudReprojectionRosNode::loadParameters()
combined_jpeg_quality_ = this->get_parameter("register_keys.combined_jpeg_quality").as_int();
combined_jpeg_quality_ = std::max(1, std::min(100, combined_jpeg_quality_));
sync_cloud_topic_ = sync_topic_prefix_ + "/cloud_slam";
sync_cloud_topic_ = sync_topic_prefix_ + "/cloud_in_cam";
sync_odom_topic_ = sync_topic_prefix_ + "/odometry";
sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
sync_image_topic_ = sync_topic_prefix_ + "/image/compressed";
@@ -235,7 +235,6 @@ void CloudReprojectionRosNode::syncCallback(
const Odometry::ConstSharedPtr& wiwc_msg,
const CompressedImage::ConstSharedPtr& image_compressed_msg)
{
sync_cloud_pub_->publish(*cloud_msg);
sync_odom_pub_->publish(*odom_msg);
sync_wiwc_pub_->publish(*wiwc_msg);
sync_image_pub_->publish(*image_compressed_msg);
@@ -276,6 +275,22 @@ void CloudReprojectionRosNode::syncCallback(
odom_msg->pose.pose.position.z
);
pcl::PointCloud<pcl::PointXYZRGB> cloud_cam =
reprojector_->transformCloudToCamera(cloud_odom, odom_pose);
if (cloud_cam.empty())
{
RCLCPP_WARN(this->get_logger(), "Camera-frame cloud is empty after reprojection");
return;
}
PointCloud2 cloud_cam_msg;
pcl::toROSMsg(cloud_cam, cloud_cam_msg);
cloud_cam_msg.header = image_compressed_msg->header;
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);
cv::Mat depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
if (depth_vis.empty()) {
return;
@@ -428,7 +443,7 @@ void CloudReprojectionRosNode::loadParameters()
pnh_.param<int>("register_keys/combined_jpeg_quality", jq, 85);
combined_jpeg_quality_ = std::max(1, std::min(100, jq));
sync_cloud_topic_ = sync_topic_prefix_ + "/cloud_slam";
sync_cloud_topic_ = sync_topic_prefix_ + "/cloud_in_cam";
sync_odom_topic_ = sync_topic_prefix_ + "/odometry";
sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
sync_image_topic_ = sync_topic_prefix_ + "/image/compressed";
@@ -491,7 +506,6 @@ void CloudReprojectionRosNode::syncCallback(
const nav_msgs::OdometryConstPtr& wiwc_msg,
const sensor_msgs::CompressedImageConstPtr& image_compressed_msg)
{
sync_cloud_pub_.publish(cloud_msg);
sync_odom_pub_.publish(odom_msg);
sync_wiwc_pub_.publish(wiwc_msg);
sync_image_pub_.publish(image_compressed_msg);
@@ -531,6 +545,22 @@ void CloudReprojectionRosNode::syncCallback(
odom_msg->pose.pose.position.z
);
pcl::PointCloud<pcl::PointXYZRGB> cloud_cam =
reprojector_->transformCloudToCamera(cloud_odom, odom_pose);
if (cloud_cam.empty())
{
ROS_WARN("Camera-frame cloud is empty after reprojection");
return;
}
sensor_msgs::PointCloud2 cloud_cam_msg;
pcl::toROSMsg(cloud_cam, cloud_cam_msg);
cloud_cam_msg.header = image_compressed_msg->header;
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);
cv::Mat depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
if (depth_vis.empty()) {
return;
src/cloud_reprojector.cpp
+17 -12
View File
@@ -60,12 +60,14 @@ Eigen::Matrix4d CloudReprojector::odomPoseToMatrix(const OdomPose& odom) const
return T;
}
cv::Mat CloudReprojector::reprojectCloud(const pcl::PointCloud<pcl::PointXYZRGB>& cloud_odom,
const OdomPose& odom_pose)
pcl::PointCloud<pcl::PointXYZRGB> CloudReprojector::transformCloudToCamera(
const pcl::PointCloud<pcl::PointXYZRGB>& cloud_odom,
const OdomPose& odom_pose) const
{
pcl::PointCloud<pcl::PointXYZRGB> cloud_in_cam;
if (!initialized_)
{
return cv::Mat();
return cloud_in_cam;
}
// T_odom_imu: imu pose in odom frame
@@ -80,9 +82,19 @@ cv::Mat CloudReprojector::reprojectCloud(const pcl::PointCloud<pcl::PointXYZRGB>
// T_cam_odom: transforms points from odom frame to camera frame
Eigen::Matrix4d T_cam_odom = T_cam_imu * T_imu_odom;
pcl::PointCloud<pcl::PointXYZRGB> cloud_in_cam;
pcl::transformPointCloud(cloud_odom, cloud_in_cam, T_cam_odom);
return cloud_in_cam;
}
cv::Mat CloudReprojector::reprojectCloud(const pcl::PointCloud<pcl::PointXYZRGB>& cloud_odom,
const OdomPose& odom_pose)
{
if (!initialized_)
{
return cv::Mat();
}
pcl::PointCloud<pcl::PointXYZRGB> cloud_in_cam = transformCloudToCamera(cloud_odom, odom_pose);
return projectCloudToImage(cloud_in_cam);
}
@@ -94,14 +106,7 @@ cv::Mat CloudReprojector::reprojectCloudDepth(const pcl::PointCloud<pcl::PointXY
return cv::Mat();
}
Eigen::Matrix4d T_odom_imu = odomPoseToMatrix(odom_pose);
Eigen::Matrix4d T_imu_odom = T_odom_imu.inverse();
Eigen::Matrix4d T_cam_imu = extrinsic_params_.Tic.inverse();
Eigen::Matrix4d T_cam_odom = T_cam_imu * T_imu_odom;
pcl::PointCloud<pcl::PointXYZRGB> cloud_in_cam;
pcl::transformPointCloud(cloud_odom, cloud_in_cam, T_cam_odom);
pcl::PointCloud<pcl::PointXYZRGB> cloud_in_cam = transformCloudToCamera(cloud_odom, odom_pose);
return projectCloudToImageDepth(cloud_in_cam);
}