add online cloud reprojection

include/cloud_reprojector.hpp

@@ -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

@@ -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

@@ -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

@@ -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,
+pcl::PointCloud<pcl::PointXYZRGB> CloudReprojector::transformCloudToCamera(
-                                          const OdomPose& odom_pose)
+    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);
+    pcl::PointCloud<pcl::PointXYZRGB> cloud_in_cam = transformCloudToCamera(cloud_odom, 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);
-
     return projectCloudToImageDepth(cloud_in_cam);
 }