PCL 通过相机外参标定 将点云坐标转换成世界坐标 C++ 详细代码

PCL (Point Cloud Library) 是一个开源的计算机视觉库，主要用于处理3D点云数据。在利用相机外参(通常指摄像机的内参矩阵和姿态信息)对点云进行标定并将点云坐标从相机坐标系转换到世界坐标系的过程中，你需要做的是将摄像头观测到的三维像素坐标转换为世界的XYZ坐标。

以下是一个简化的C++示例，使用pcl_ros库，它封装了ROS（Robot Operating System）与PCL的交互：

#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_to_ros.h>
#include <pcl_ros/point_cloud_registered.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>

// 假设已经获取到了imu和camera的信息
Eigen::Matrix4f camera_extrinsics; // 外部参数矩阵
tf::Transform tf_world_to_camera; // TF坐标变换

void callback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
    cv::Mat cv_image(image_msg->height, image_msg->width, CV_8UC3, image_msg->data);
    
    // 转换点云数据到RGB格式
    pcl::PointCloud<pcl::PointXYZRGBA> pointcloud;
    fromROSMsg(*cloud_msg, pointcloud);

    // 应用外部参数
    for (const auto &pt : pointcloud.points) {
        Eigen::Vector3f pixel = {pt.x, pt.y, 1.0}; // 三维像素坐标加上深度信息
        Eigen::Vector3f world_point = camera_extrinsics * tf_world_to_camera * pixel; // 转换到世界坐标
        // 将world_point保存回点云中，注意这里只是简单地展示思路，实际操作需要适配点云的数据结构
        pointcloud.points.push_back(world_point);
    }

    // 现在pointcloud已经是世界坐标下的点云
}

int main()
{
    // 初始化Subscriber并设置回调函数
    ros::NodeHandle nh;
    ros::Subscriber sub_image = nh.subscribe("camera/image", 1, callback);
    ros::Subscriber sub_cloud = nh.subscribe("camera/depth_registered", 1, callback); // 如果是注册过的点云，订阅深度图像对应的点云

    // 等待ros节点启动并开始监听消息
    ros::spin();

    return 0;
}