如何将geometry_msgs/Point转换为sensor_msgs/PointCloud2（C++实现）

关于width和height的设置说明 #

• 对于单个geometry_msgs/Point：直接将width设为1，height设为1即可。因为sensor_msgs/PointCloud2的总点数由width * height计算，单个点的总点数为1，这样设置完全符合点云的语义。
• 对于多个geometry_msgs/Point组成的集合：通常将height设为1（表示无序点云），width设为点的总数量；如果你的场景需要结构化点云（比如类似图像的行列排布），可以根据实际需求分配width和height，但前提是两者乘积等于总点数。

转换实现方法 #

方法1：ROS原生API（无额外依赖） #

不需要依赖PCL，直接用ROS提供的工具类构造PointCloud2：

#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Point.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <ros/ros.h>

// 单个Point转PointCloud2
sensor_msgs::PointCloud2 pointToCloud(const geometry_msgs::Point& input_point, const std::string& frame_id) {
    sensor_msgs::PointCloud2 output_cloud;
    output_cloud.header.frame_id = frame_id;
    output_cloud.header.stamp = ros::Time::now(); // 若原Point带时间戳，可替换为对应时间

    // 配置点云基础属性
    output_cloud.width = 1;
    output_cloud.height = 1;
    output_cloud.is_dense = true; // 单一点无无效数据，设为true

    // 设置点云字段（仅x、y、z）
    sensor_msgs::PointCloud2Modifier modifier(output_cloud);
    modifier.setPointCloud2FieldsByString(1, "xyz");
    modifier.resize(1);

    // 填充点数据
    sensor_msgs::PointCloud2Iterator<float> iter_x(output_cloud, "x");
    sensor_msgs::PointCloud2Iterator<float> iter_y(output_cloud, "y");
    sensor_msgs::PointCloud2Iterator<float> iter_z(output_cloud, "z");

    *iter_x = input_point.x;
    *iter_y = input_point.y;
    *iter_z = input_point.z;

    return output_cloud;
}

// 多个Point组成的vector转PointCloud2
sensor_msgs::PointCloud2 pointsToCloud(const std::vector<geometry_msgs::Point>& input_points, const std::string& frame_id) {
    sensor_msgs::PointCloud2 output_cloud;
    output_cloud.header.frame_id = frame_id;
    output_cloud.header.stamp = ros::Time::now();

    output_cloud.width = input_points.size();
    output_cloud.height = 1;
    output_cloud.is_dense = true;

    sensor_msgs::PointCloud2Modifier modifier(output_cloud);
    modifier.setPointCloud2FieldsByString(1, "xyz");
    modifier.resize(input_points.size());

    sensor_msgs::PointCloud2Iterator<float> iter_x(output_cloud, "x");
    sensor_msgs::PointCloud2Iterator<float> iter_y(output_cloud, "y");
    sensor_msgs::PointCloud2Iterator<float> iter_z(output_cloud, "z");

    for (const auto& p : input_points) {
        *iter_x = p.x;
        *iter_y = p.y;
        *iter_z = p.z;
        ++iter_x;
        ++iter_y;
        ++iter_z;
    }

    return output_cloud;
}

方法2：基于PCL库（适合已依赖PCL的项目） #

如果你的项目已经使用PCL，可以借助其转换工具简化代码：

#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Point.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>

sensor_msgs::PointCloud2 pointToCloud(const geometry_msgs::Point& input_point, const std::string& frame_id) {
    pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
    pcl::PointXYZ pcl_p;
    pcl_p.x = input_point.x;
    pcl_p.y = input_point.y;
    pcl_p.z = input_point.z;
    pcl_cloud.push_back(pcl_p);

    sensor_msgs::PointCloud2 output_cloud;
    pcl::toROSMsg(pcl_cloud, output_cloud);
    output_cloud.header.frame_id = frame_id;
    output_cloud.header.stamp = ros::Time::now();

    return output_cloud;
}

与LaserScan转PointCloud2的区别 #

你之前实现的LaserScan转换是从激光扫描的距离、角度信息生成点云数据，而本次场景是将已有的三维点数据打包成PointCloud2格式，逻辑更直接，核心是正确配置PointCloud2的元数据（width、height、字段）并填充已有坐标值。

内容的提问来源于stack exchange，提问作者YJMoon