加载和保存点云到硬盘

PCD是一种存储点云的格式。

这种格式非常简单，开始是一个包含关于点云中点类型和元素数目信息的数据头，然后是符合指定类型的列表。下面是一个PCD文件头的示例：

# .PCD v.5 - Point Cloud Data file format
FIELDS x y z intensity distance sid
SIZE 4 4 4 4 4 4
TYPE F F F F F F
COUNT 1 1 1 1 1 1
WIDTH 460400
HEIGHT 1
POINTS 460400
DATA ascii

目的1

加载PCD文件并将点云结果发布为ROS消息。

代码

源文件pcl_read.cpp：

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>

main(int argc, char **argv)
{
    ros::init (argc, argv, "pcl_read");

    ros::NodeHandle nh;
    ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("pcl_output", 1);

    sensor_msgs::PointCloud2 output;
    pcl::PointCloud<pcl::PointXYZ> cloud;

    pcl::io::loadPCDFile ("test_pcd.pcd", cloud);

    pcl::toROSMsg(cloud, output);
    output.header.frame_id = "odom";

    ros::Rate loop_rate(1);
    while (ros::ok())
    {
        pcl_pub.publish(output);
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

解析

1.添加头文件

#include <pcl/io/pcd_io.h>

2.从硬盘中加载点云

pcl::io::loadPCDFile ("test_pcd.pcd", cloud);

运行

roscd chapter6_tutorials/data
rosrun chapter6_tutorials pcl_read

打开rviz：

目的2

订阅一个 sensor_msgs/PointCloud2 主题，并且将接受的点云存储到一个文件中。

代码

源文件pcl_write.cpp：

#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>

void cloudCB(const sensor_msgs::PointCloud2 &input)
{
    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::fromROSMsg(input, cloud);
    pcl::io::savePCDFileASCII ("write_pcd_test.pcd", cloud);
}

main (int argc, char **argv)
{
    ros::init (argc, argv, "pcl_write");

    ros::NodeHandle nh;
    ros::Subscriber bat_sub = nh.subscribe("pcl_output", 10, cloudCB);

    ros::spin();

    return 0;
}

解析

void cloudCB(const sensor_msgs::PointCloud2 &input)
{
    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::fromROSMsg(input, cloud);
    pcl::io::savePCDFileASCII ("write_pcd_test.pcd", cloud);
}

运行

打开三个终端，分别运行：

roscore
roscd chapter6_tutorials/data && rosrun chapter6_tutorials pcl_read
roscd chapter6_tutorials/data && rosrun chapter6_tutorials pcl_write

这样就会创建一个名为 write_pcd_test.pcd 的文件。

注意：
如果不关闭pcl_read节点，pcl_write将一直存储点云，导致文件过大，因此演示成功后就关闭节点，节省硬盘存储空间。