点云操作处理

格式转换

参考文档

// sensor_msgs::PointCloud2转pcl::PointXYZI
void toPCL(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud<pcl::PointXYZI> &pcl_cloud) {
    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(cloud, pcl_pc2);
    pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);
}

// pcl::PointXYZI转sensor_msgs::PointCloud2
void fromPCL(const pcl::PointCloud<pcl::PointXYZI> &pcl_cloud, sensor_msgs::PointCloud2 &cloud) {
    pcl::PCLPointCloud2 pcl_pc2;
    pcl::toPCLPointCloud2(pcl_cloud, pcl_pc2);
    pcl_conversions::moveFromPCL(pcl_pc2, cloud);
}

---

合并点云

合并点云分为两种类型(参考)：

1. 第一种是两个点云数据集的字段类型和维度相同，合并之后点云只是点的数量增加了；

pcl::PointCloud<pcl::PointXYZI> pcl_cloud_1, pcl_cloud_2, pcl_cloud_add;
pcl_cloud_add = pcl_cloud_1 + pcl_cloud_2;

2. 第二种是两个点云数据集的字段类型或维度不同，但是点的数量相同，合并之后相当于扩展了字段或维度； 例如点云A是N个点的XYZ点集，点云B是N个点的RGB点，则连接两个字段形成的点云C是N个XYZRGB类型。

// 一个 PointXYZ 类型的点云和一个 Normal 类型的点云合并为一个 PointNormal 类型点云
// 注意：要确定 cloud1_in 和 cloud2_in 点云数目相同，并且 cloud_merge 的字段包含两个输入点云的所有字段
pcl::PointCloud<pcl::PointXYZ> cloud1_in;
pcl::PointCloud<pcl::Normal> cloud2_in;
pcl::PointCloud<pcl::PointNormal> &cloud_merge;
pcl::concatenateFields(cloud1_in, cloud2_in, cloud_merge);

一般将同一个激光发布的每一帧点云叠加，用的就是第一种，如果不叠加，那么在Rviz里看到的就只有每一帧的数据。

当然，如果没有对叠加的点云进行坐标变换，即转到世界坐标系下，叠加起来的点云就是每帧点云在同一个位置叠加，看起来就是一团，因为这是在激光lidar的坐标系下的。

要想看起来是正常的地图点云，就要进行坐标变换。

---

过滤点云

有时我们获得激光点云太多，想把上面或者左右两边的点云去掉，很简单

首先将 sensor_msgs::PointCloud2 转为 PCL 格式，这里用 pcl::PointXYZI，转换之前讲过；

下面举的例子是将激光点云左右5m、前后20m、上面-1.5m以及强度小于50的去掉

pcl::PointCloud<pcl::PointXYZI> pcl_cloud, pcl_cloud_filt;
for (int i = 0; i < pcl_cloud.points.size(); i++) {
  pcl::PointXYZI point = pcl_cloud.points[i];
  if (point.x < 20 && point.x > -20 && point.y < 5 && point.y > -5 &&
      point.z < -1.5 && point.intensity > 50) {
    pcl_cloud_filt.points.push_back(point);
  }
}

注意，激光是前面为x，左边为y，上面为z，点云越深的（如红色）intensity就越大，点云越浅的（如黄色）intensity就越小。