【点云处理】图达通激光点云数据转换

    刚刚拿到一批图达通雷达的激光点云数据的rosbag，准备拿去做3D目标的标注。播出来看了一眼，发现点云的坐标轴的设定与我们平常使用的坐标轴设定还不太一样。下图(1)是我直接通过rviz看到的点云图像，这样看你很难看出来是个什么东西。真实的图像应该是图(2)的样子,这样路面和山坡就看的比较清晰了。

图1

图(2)

好了，现在就是要做一个坐标转换，转换到我们熟悉的坐标轴定义下。

【代码】

#include <ros/ros.h>
#include <pcl/point_cloud.h>
//msgs type and conversion
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <string>

ros::Publisher pub;
void call_back(const sensor_msgs::PointCloud2::ConstPtr input) {
    static int ch = 0;
    int point_bytes = input->point_step; //每个点的字节长度
    //点云数据以字节顺序存储在data容器中,offset是指在data中每个数据域的偏移
    int offset_x,offset_y,offset_z,offset_intensity;

    const auto& fields = input->fields;
    for (int f = 0; f < fields.size(); ++f) {
        if (fields[f].name == "x") offset_x = fields[f].offset;
        if (fields[f].name == "y") offset_y = fields[f].offset;
        if (fields[f].name == "z") offset_z = fields[f].offset;
        if (fields[f].name == "intensity") offset_intensity = fields[f].offset;
    }

    pcl::PointCloud<pcl::PointXYZI> cloud;//定义转换的点云数据类型
    for (int i=0; i<input->width; ++i) {
        pcl::PointXYZI point;
        point.x = *(float*)(&input->data[0] + point_bytes*i + offset_x);
        point.y = *(float*)(&input->data[0] + point_bytes*i + offset_y);
        point.z = *(float*)(&input->data[0] + point_bytes*i + offset_z);
        point.intensity = *(unsigned char*)(&input->data[0] + point_bytes*i + offset_intensity);

        //坐标轴的转换
        auto tmp = point.x;
        point.x = point.z;
        point.y = -point.y;
        point.z = tmp;

        cloud.push_back(point);
    }
    cloud.width = input->width;
    cloud.height = input->height;

    if (1) {
        //发布转换后的点云
        sensor_msgs::PointCloud2 msg_publish;
        pcl::toROSMsg(cloud, msg_publish);
        msg_publish.header.frame_id = "innovusion";
        pub.publish(msg_publish);
    }
}


int main(int argc,char** argv) {
    // Initialize ROS
    ros::init (argc, argv, "pcl2pcd");
    ros::NodeHandle nh;

    // Create publish
    pub = nh.advertise<sensor_msgs::PointCloud2>("iv_points2_convert",1);
    // Create a ROS subscriber for the input point cloud
    ros::Subscriber sub = nh.subscribe ("iv_points2", 1, call_back);

    // Spin
    ros::spin ();
}

这里我设计了一个节点，订阅从rosbag中发布出来的图达通激光雷达的点云数据，然后借助pcl纠正了点云的坐标，再重新发布出去。我们再看转换后的点云图像(图3)就正常了。

图3