diff --git a/include/cloud_reprojector.hpp b/include/cloud_reprojector.hpp index e6c73b5..463e799 100644 --- a/include/cloud_reprojector.hpp +++ b/include/cloud_reprojector.hpp @@ -54,6 +54,10 @@ public: bool initialize(const CameraParams& cam_params, const ExtrinsicParams& ext_params); + pcl::PointCloud transformCloudToCamera( + const pcl::PointCloud& cloud_odom, + const OdomPose& odom_pose) const; + cv::Mat reprojectCloud(const pcl::PointCloud& cloud_odom, const OdomPose& odom_pose); diff --git a/script/record_dataset.sh b/script/record_dataset.sh new file mode 100755 index 0000000..5c06cbc --- /dev/null +++ b/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 + diff --git a/src/cloud_reprojection_ros.cpp b/src/cloud_reprojection_ros.cpp index a4fcd0b..71268ee 100644 --- a/src/cloud_reprojection_ros.cpp +++ b/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 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("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 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; diff --git a/src/cloud_reprojector.cpp b/src/cloud_reprojector.cpp index 784c1bf..22e029b 100644 --- a/src/cloud_reprojector.cpp +++ b/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& cloud_odom, - const OdomPose& odom_pose) +pcl::PointCloud CloudReprojector::transformCloudToCamera( + const pcl::PointCloud& cloud_odom, + const OdomPose& odom_pose) const { + pcl::PointCloud 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 // T_cam_odom: transforms points from odom frame to camera frame Eigen::Matrix4d T_cam_odom = T_cam_imu * T_imu_odom; - pcl::PointCloud cloud_in_cam; pcl::transformPointCloud(cloud_odom, cloud_in_cam, T_cam_odom); + return cloud_in_cam; +} +cv::Mat CloudReprojector::reprojectCloud(const pcl::PointCloud& cloud_odom, + const OdomPose& odom_pose) +{ + if (!initialized_) + { + return cv::Mat(); + } + + pcl::PointCloud cloud_in_cam = transformCloudToCamera(cloud_odom, odom_pose); return projectCloudToImage(cloud_in_cam); } @@ -94,14 +106,7 @@ cv::Mat CloudReprojector::reprojectCloudDepth(const pcl::PointCloud cloud_in_cam; - pcl::transformPointCloud(cloud_odom, cloud_in_cam, T_cam_odom); - + pcl::PointCloud cloud_in_cam = transformCloudToCamera(cloud_odom, odom_pose); return projectCloudToImageDepth(cloud_in_cam); } @@ -191,4 +196,4 @@ cv::Mat CloudReprojector::projectCloudToImageDepth(const pcl::PointCloud