头文件中自定义PCL点云数据类型并在项目中使用

具体操作

1、头文件中定义新的点类型

例如我想要自定义点类型PointXYZR，在XYZ属性的基础上增加ring属性用来包含点云中每个点所属的激光雷达哪根线束；

同时我还想定义点类型PointXYZIR，其在PointXYZR类型基础上还增加了点的强度intensity属性；

所以我新建一个头文件point_type_define.h，内容如下：

#ifndef PROJECT_POINT_TYPE_DEFINE_H
#define PROJECT_POINT_TYPE_DEFINE_H

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>

#define PCL_NO_PRECOMPILE

namespace perception {

struct EIGEN_ALIGN16 _PointXYZR
{
    PCL_ADD_POINT4D;                  // preferred way of adding a XYZ+padding
    uint32_t ring;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW   // make sure our new allocators are aligned
};

/**
 * @brief A point structure representing Euclidean xyz coordinates, and the ring value.
 */
struct EIGEN_ALIGN16 PointXYZR : public _PointXYZR
{
    inline PointXYZR (const _PointXYZR &p)
    {
        x = p.x; y = p.y; z = p.z; data[3] = 1.0f;
        ring = p.ring;
    }

    inline PointXYZR ()
    {
        x = y = z = 0.0f;
        data[3] = 1.0f;
        ring = 0;
    }

    inline PointXYZR (float _x, float _y, float _z, uint32_t ring)
    {
        x = _x; y = _y; z = _z;
        data[3] = 1.0f;
        ring = ring;
    }

    friend std::ostream& operator << (std::ostream& os, const PointXYZR& p)
    {
        os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.ring << ")";
        return (os);
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

struct EIGEN_ALIGN16 _PointXYZIR
{
    PCL_ADD_POINT4D;                  // preferred way of adding a XYZ+padding
    float intensity;
    uint32_t ring;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW   // make sure our new allocators are aligned
};

/**
 * @brief A point structure representing Euclidean xyz coordinates, and the ring value.
 */
struct EIGEN_ALIGN16 PointXYZIR : public _PointXYZIR
{
    inline PointXYZIR (const _PointXYZIR &p)
    {
        x = p.x; y = p.y; z = p.z; data[3] = 1.0f;
        intensity = p.intensity;
        ring = p.ring;
    }

    inline PointXYZIR ()
    {
        x = y = z = 0.0f;
        data[3] = 1.0f;
        ring = 0;
        ring = 0;
    }

    inline PointXYZIR (float _x, float _y, float _z, float _intensity, uint32_t _ring)
    {
        x = _x; y = _y; z = _z; 
        data[3] = 1.0f;
        intensity = _intensity;
        ring = _ring;
    }

    friend std::ostream& operator << (std::ostream& os, const PointXYZIR& p)
    {
        os << "(" << p.x << "," << p.y << "," << p.z << " - " << p.ring << ")";
        return (os);
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

} // namespace perception

POINT_CLOUD_REGISTER_POINT_STRUCT(perception::PointXYZR,
                                  (float, x, x)
                                  (float, y, y)
                                  (float, z, z)
                                  (uint32_t, ring, ring)
)

POINT_CLOUD_REGISTER_POINT_STRUCT(perception::PointXYZIR,
                                  (float, x, x)
                                  (float, y, y)
                                  (float, z, z)
                                  (float, intensity, intensity)
                                  (uint32_t, ring, ring)
)
#endif //PROJECT_POINT_TYPE_DEFINE_H

其中要特别注意如果你想要把你自定义的数据类型放在一个命名空间下，就像上面我的自定义类型放在perception命名空间下了，此时不能把POINT_CLOUD_REGISTER_POINT_STRUCT对自定义点类型进行注册的操作放在命名空间里，否则就会出现:

/usr/include/pcl-1.8/pcl/point_traits.h:176:12: error: invalid use of incomplete type ‘struct pcl::traits::fieldList<perception::PointXYZIR>’
或者
/usr/include/pcl-1.8/pcl/common/impl/io.hpp:83:3: error: no type named ‘type’ in ‘struct pcl::traits::fieldList<perception::PointXYZIR>’
   pcl::for_each_type<typename pcl::traits::fieldList<PointT>::type>(pcl::detail::FieldAdder<PointT>(fields));

等类似编译错误。

点云数据类型：
PCLPointField.h:

 enum PointFieldTypes { INT8 = 1,
                           UINT8 = 2,
                           INT16 = 3,
                           UINT16 = 4,
                           INT32 = 5,
                           UINT32 = 6,
                           FLOAT32 = 7,
                           FLOAT64 = 8 };

2、在PCL项目中使用自定义点云类型

在头文件里定义好了自定义的点数据类型之后，我们就可以使用自定义的点数据类型来存储我们想要的点云了。

新建一个main.cpp文件，内容如下：

#include "point_type_define.h"

int main(int argc, char** argv) {
    pcl::PointCloud<perception::PointXYZIR>::Ptr cloud(new pcl::PointCloud<perception::PointXYZIR>);
    cloud->header.frame_id = "base_link";
    cloud->width = 100;
    cloud->height = 1;
    cloud->points.resize(cloud->width * cloud->height);

    for (size_t i = 0; i < cloud->points.size (); ++i) {
        cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud->points[i].intensity = i;
        cloud->points[i].ring = 100 - i;
    }

    pcl::io::savePCDFile ("test.pcd", *cloud);

    return 0;
}

编译之后，运行就可以得到test.pcd点云文件，我们可以执行命名：

pcl_viewer test.pcd -ps 8

其中-ps选项指定了点的显示尺寸。

效果如下：

在选中该界面时，按下键盘L键，此时终端会显示：

Available dimensions: x y z intensity ring
List of available geometry handlers for actor test.pcd-0: xyz(1) 
List of available color handlers for actor test.pcd-0: [random](1) x(2) y(3) z(4) intensity(5) ring(6)

可以通过按数字来切换以不同点的属性值来显示点的颜色，从终端打印结果也能看出此时保存的点结果中确实有intensity和ring属性。

其实pcd格式也是一种文本文件格式，你也可以用编辑器打开，确认一下是否保存了点的intensity和ring属性：

3、在ROS项目中使用自定义点云类型

在使用ROS过程中会经常和点云打交道，ROS中点云的消息格式一般为sensor_msgs::PointCloud2，如果我们想要使用自定义点类型，如何进行发布和接收呢？

其实和正常点云操作大同小异，下面直接看代码。

// ROS
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>
// PCL
#include <pcl/point_cloud.h>
// Projects
#include "point_type_define.h"

ros::Publisher g_pub_cloud;

using namespace perception;
using std::cout;
using std::endl;

template <typename PointType>
void PublishCloud(const typename pcl::PointCloud<PointType>::Ptr msg)
{
//    pcl::PointCloud<pcl::PointXYZI>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZI>);
//    pcl::fromROSMsg(*msg, *temp_cloud);
//
//    temp_cloud->header.stamp = pcl_conversions::toPCL(msg->header.stamp);
    g_pub_cloud.publish(msg);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "cloud_publisher_node");
    ros::NodeHandle nh;
    g_pub_cloud = nh.advertise<sensor_msgs::PointCloud2>("/velody_cloud", 2);

    pcl::PointCloud<PointXYZIR>::Ptr cloud(new pcl::PointCloud<PointXYZIR>);
    cloud->header.frame_id = "base_link";
    cloud->width = 100;
    cloud->height = 1;
    cloud->points.resize(cloud->width * cloud->height);

    for (size_t i = 0; i < cloud->points.size (); ++i) {
        cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud->points[i].intensity = i;
        cloud->points[i].ring = 100 - i;
    }

    // pcl::PointCloud<PointXYZR>::Ptr cloud_xyz(new pcl::PointCloud<PointXYZR>);
    // pcl::copyPointCloud(*cloud, *cloud_xyz);

    ros::Rate rate(10);
    while (ros::ok()) {
        ROS_INFO("--------");
        ros::spinOnce();
        PublishCloud<PointXYZIR>(cloud);
        rate.sleep();
    }

    return 0;
}

运行节点后，打开rviz，显示PointCloud类型，就可以得到如下可视化结果：

注意此时如何将Color Transformer修改为Intensity属性，即可以根据点云中点的不同属性值来设置颜色显示模式，此时可以看到Channel Name中果然有x,y,z,ring,intensity五种可选属性，这也说明我们确实成功发布了自定义点云数据。

一切OK！

【参考】：
https://pointclouds.org/documentation/tutorials/adding_custom_ptype.html
https://blog.csdn.net/qq_33287871/article/details/106348507
https://blog.csdn.net/Linear_Luo/article/details/52723321
http://news.migage.com/articles/PCL+point%E7%9A%84%E7%B1%BB%E5%9E%8B%E8%A7%A3%E6%9E%90_4064560_csdn.html
https://blog.csdn.net/PengPengBlog/article/details/77374594

union用处：https://blog.csdn.net/xy010902100449/article/details/48292527
https://blog.csdn.net/u013566722/article/details/79430652

匿名union：
https://blog.csdn.net/u013566722/article/details/79430652