PCL 点云基础数据结构源码解析

Posted 2021-05-18 不如留个长发

PCL 点云基础数据结构源码解析

前文  提到两个常用点云小算法，感性地认识了下 PCL 用途。

本文开始介绍一下 pcl 的基本数据结构，并从基本的数据结构出发，实现一些关于点云数据结构更基础的操作。

PointT 基础数据结构

PCL 点云库中点云自然由点组成。首先我们来看他是如何表征点的数据结构的。

PCL 中用来表示点的类型有很多如(pcl::PointXYZ) (pcl::PointXYZI) (pcl::PointXYZRGBA)等，为了方便通常网络上 PCL 的例子中前面都会加一句这样的代码：

typedef pcl::PointXYZRGBA PointT;typedef pcl::PointCloud<PointT> PointCloudT;

这就是所谓的 PointT 数据结构 。

PCL’s PointT legacy goes back to the days where it was a library developed within ROS^[1].

阅读源码可以发现它写的其实并不那么直接，一起从 common/include/pcl/impl/point_types.hpp 一窥究竟。

以 PointXYZ 为例：

 struct _PointXYZ { PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4])
 PCL_MAKE_ALIGNED_OPERATOR_NEW };
 PCL_EXPORTS std::ostream& operator << (std::ostream& os, const PointXYZ& p); /** \brief A point structure representing Euclidean xyz coordinates. (SSE friendly) * \ingroup common */ struct EIGEN_ALIGN16 PointXYZ : public _PointXYZ { inline PointXYZ (const _PointXYZ &p): PointXYZ(p.x, p.y, p.z) {}
 inline PointXYZ (): PointXYZ(0.f, 0.f, 0.f) {}
 inline PointXYZ (float _x, float _y, float _z) { x = _x; y = _y; z = _z; data[3] = 1.0f; }
 friend std::ostream& operator << (std::ostream& os, const PointXYZ& p); PCL_MAKE_ALIGNED_OPERATOR_NEW };

我们可以看到 PointXYZ 派生自 _PointXYZ，自然第一步先去看看基类。虽然基类 _PointXYZ 中只有两句话 PCL_ADD_POINT4D 和 PCL_MAKE_ALIGNED_OPERATOR_NEW，但其实并不简单。

首先看 PCL_ADD_POINT4D：

#define PCL_ADD_POINT4D \ PCL_ADD_UNION_POINT4D \ PCL_ADD_EIGEN_MAPS_POINT4D

继续展开

#define PCL_ADD_UNION_POINT4D \ union EIGEN_ALIGN16 { \ float data[4]; \ struct { \ float x; \ float y; \ float z; \ }; \ };

可以看到其首先定义了一个内存对齐到 16-byte 的union，可以用 x，y，z 访问，也可以直接访问 float[4]。

其中 EIGEN_ALIGN16 并不是指定 union 的名字，而是定义在 Eigen/src/Core/util/Macros.h 中的一个宏用来做数据对齐：

 766 // Shortcuts to EIGEN_ALIGN_TO_BOUNDARY 767 #define EIGEN_ALIGN8 EIGEN_ALIGN_TO_BOUNDARY(8) 768 #define EIGEN_ALIGN16 EIGEN_ALIGN_TO_BOUNDARY(16) 769 #define EIGEN_ALIGN32 EIGEN_ALIGN_TO_BOUNDARY(32) 770 #define EIGEN_ALIGN64 EIGEN_ALIGN_TO_BOUNDARY(64) 771 #if EIGEN_MAX_STATIC_ALIGN_BYTES>0 772 #define EIGEN_ALIGN_MAX EIGEN_ALIGN_TO_BOUNDARY(EIGEN_MAX_STATIC_ALIGN_BYTES) 773 #else 774 #define EIGEN_ALIGN_MAX 775 #endif

它使用 EIGEN_ALIGN_TO_BOUNDARY(n) 强制数据 n-byte 对齐。

 664 #if (defined __CUDACC__) 665 #define EIGEN_ALIGN_TO_BOUNDARY(n) __align__(n) 666 #elif EIGEN_COMP_GNUC || EIGEN_COMP_PGI || EIGEN_COMP_IBM || EIGEN_COMP_ARM 667 #define EIGEN_ALIGN_TO_BOUNDARY(n) __attribute__((aligned(n))) 668 #elif EIGEN_COMP_MSVC 669 #define EIGEN_ALIGN_TO_BOUNDARY(n) __declspec(align(n)) 670 #elif EIGEN_COMP_SUNCC 671 // FIXME not sure about this one: 672 #define EIGEN_ALIGN_TO_BOUNDARY(n) __attribute__((aligned(n))) 673 #else 674 #error Please tell me what is the equivalent of __attribute__((aligned(n))) for your compiler 675 #endif

例如，如果使用的是 GCC 的话

struct __attribute__((aligned(16))) Vec4 { float x,y,z,w;};

可以将结构体对齐至16字节。

而 PCL_MAKE_ALIGNED_OPERATOR_NEW 可以在 common/include/pcl/memory.h 中找到其定义：

#include <Eigen/Core> // for EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW \ EIGEN_MAKE_ALIGNED_OPERATOR_NEW \ using _custom_allocator_type_trait = void;

可以看到它其实是对 Eigen 中EIGEN_MAKE_ALIGNED_OPERATOR_NEW 的一个 wrapper。再要深究就得去 Eigen/Core里翻了。

继续往下翻发现 Eigen/src/Core/util/Memory.h

690 #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(true)

继续往下

662 #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \663 void *operator new(std::size_t size) { \664 return Eigen::internal::conditional_aligned_malloc<NeedsToAlign>(size); \... ... // 下面省略了其他 new/delete 等内存操作

终于最后落到了 Eigen::internal::conditional_aligned_malloc 之类的函数，从函数名可以看出是处理了一些内存对齐的操作。

事实上也是如此，查询一些资料后可以发现这是因为使用如果自定义的结构中如果用到了固定大小的 Eigen 类型的话， 就得重载其 operator new 操作符来保障 16-bytes 对齐的指针，听起来很复杂，所以 Eigen 提供了 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 这个宏就达到效果。

如果我们自己使用 Eigen 用到了相同的情况，也要添加，不加这句的话，后面程序在分配内存会调用默认的构造函数，而默认构造分配内存的方式是不对齐，这会导致程序运行的时候出错。

If you define a structure having members of fixed-size vectorizable Eigen types^[2], you must overload its "operator new" so that it generates 16-bytes-aligned pointers. Fortunately, Eigen provides you with a macro EIGEN_MAKE_ALIGNED_OPERATOR_NEW that does that for you.

关于一些对齐的事项可以参照官网说明：Alignment issues^[3]

最后，我们来看下 PCL_ADD_EIGEN_MAPS_POINT4D 这个宏，

#define PCL_ADD_EIGEN_MAPS_POINT4D \ inline pcl::Vector3fMap getVector3fMap () { return (pcl::Vector3fMap (data)); } \ inline pcl::Vector3fMapConst getVector3fMap () const { return (pcl::Vector3fMapConst (data)); } \ inline pcl::Vector4fMap getVector4fMap () { return (pcl::Vector4fMap (data)); } \ inline pcl::Vector4fMapConst getVector4fMap () const { return (pcl::Vector4fMapConst (data)); } \ inline pcl::Array3fMap getArray3fMap () { return (pcl::Array3fMap (data)); } \ inline pcl::Array3fMapConst getArray3fMap () const { return (pcl::Array3fMapConst (data)); } \ inline pcl::Array4fMap getArray4fMap () { return (pcl::Array4fMap (data)); } \ inline pcl::Array4fMapConst getArray4fMap () const { return (pcl::Array4fMapConst (data)); }

这里其实也是对 Eigen 做了个封装：

 using Array3fMap = Eigen::Map<Eigen::Array3f>; using Array3fMapConst = const Eigen::Map<const Eigen::Array3f>; using Array4fMap = Eigen::Map<Eigen::Array4f, Eigen::Aligned>; using Array4fMapConst = const Eigen::Map<const Eigen::Array4f, Eigen::Aligned>; using Vector3fMap = Eigen::Map<Eigen::Vector3f>; using Vector3fMapConst = const Eigen::Map<const Eigen::Vector3f>; using Vector4fMap = Eigen::Map<Eigen::Vector4f, Eigen::Aligned>; using Vector4fMapConst = const Eigen::Map<const Eigen::Vector4f, Eigen::Aligned>;

可以看到这个宏其实添加了一组函数，这组函数为了可以使用各种 Eigen::Map 类型的引用来使用 data 数组。

template<typename PlainObjectType, int MapOptions, typename StrideType> 

class Eigen::Map< PlainObjectType, MapOptions, StrideType >

A matrix or vector expression mapping an existing array of data.

Eigen::Map 类本质上实现了一种高效的内存引用，也是为了能更好地使用其他 Eigen 函数。

到这里我们终于大致搞明白了其基类的组成，而派生出来的 PointXYZ 基本没有增加什么修改，就增加了一个 perator << 和几个初始化构造函数让我们可以指定 x，y，z 字段方便定义点。

值得注意的是：

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

我们看到这里把 data[3] 给成了1.0。这里可以大胆猜测，可能是这样处理的话 data 数组就表示一个齐次坐标了，可以方便变换矩阵 T 的运算。关于齐次坐标，也许将来会专门写一篇总结笔记 ;)。

可以看到 PointT 数据类型使用 Eigen 使用了很多对齐的特性，一般点云处理起来数据量都比较大，这样 SIMD 处理起来更方便，这也是为什么说它是 SSE friendly 的。

除了 PointXYZ 之外，还有很多衍生类型：

•PointXYZ - float x, y, z•PointXYZI - float x, y, z, intensity•PointXYZRGB - float x, y, z, rgb•PointXYZRGBA - float x, y, z, uint32_t rgba•Normal - float normal[3]，法线类型

这里摘取一部分参见 common/include/pcl/impl/point_types.hpp:

// Define all point types that include XYZ data#define PCL_XYZ_POINT_TYPES \ (pcl::PointXYZ) \ (pcl::PointXYZI) \ (pcl::PointXYZL) \ (pcl::PointXYZRGBA) \ (pcl::PointXYZRGB) \ (pcl::PointXYZRGBL) \ (pcl::PointXYZLAB) \ (pcl::PointXYZHSV) \ (pcl::InterestPoint) \ (pcl::PointNormal) \ (pcl::PointXYZRGBNormal) \ (pcl::PointXYZINormal) \ (pcl::PointXYZLNormal) \ (pcl::PointWithRange) \ (pcl::PointWithViewpoint) \ (pcl::PointWithScale) \ (pcl::PointSurfel) \ (pcl::PointDEM)

甚至你参照官方文档去拥有自己的自定义类型：【adding-custom-ptype】^[4]

点云

说完了点，我们终于来到了点云。

PCL 用来点云的的基本数据类型是PointCloud，PointCloud是一个C++模板类， 其定义在common/include/pcl/point_cloud.h 中，主要成员有

/** \brief The point cloud header. It contains information about the acquisition time. */pcl::PCLHeader header;

header - 记录了序列号，时间戳和帧 ID 信息，这个应该是从设备采集来的点云文件里会有相关的时序信息，一般的点云算法并不关心。

/** \brief The point data. */std::vector<PointT, Eigen::aligned_allocator<PointT> > points;

points - 数据列表，它是一个 pointT 类型的 vector，存取了所有构成点云的点数据，可以看到还使用了专属的对齐 allocator。

/** \brief The point cloud width (if organized as an image-structure). */std::uint32_t width = 0;

width - 指定点云数据集的宽度，这里的 width 和下面的 height 含义来自于有结构的点云，这里把数据组织成像图片矩阵那样的形式，数据被分为行和列，如一些从扫描仪得到的点云数据。有结构点云（或者说有组织点云）的一大好处就是能知道点云中点的相邻关系，最近邻操作效率就非常高，可以大大提高 PCL 中相应算法的效率。

对于无组织格式的数据集，width代表了所有点的总数；

对于有组织格式的数据集，width代表了一行中的总点数。

/** \brief The point cloud height (if organized as an image-structure). */std::uint32_t height = 0;

height - 指定点云数据集的高度（见上述 width 的解释）。

对于无组织格式的数据集，值为1；

对于有组织格式的数据集，表示数据总行数。

 /** \brief True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields). */ bool is_dense = true;

is_dense 作为标志位指代 points 中的数据是否都是有效的（有效为 true ）或者说是判断点云中的点是否包含 Inf/NaN这种值（包含为 false）。

除了这些成员变量外还有一些变量，比如说如下相应传感器的位置和姿态的变量，不过这些在常见的算法中都不会用到。

/** \brief Sensor acquisition pose (origin/translation). */Eigen::Vector4f sensor_origin_ = Eigen::Vector4f::Zero ();/** \brief Sensor acquisition pose (rotation). */Eigen::Quaternionf sensor_orientation_ = Eigen::Quaternionf::Identity ();

当然除此之外还有 ROS message 类型的 pcl::PCLPointCloud2，因为主要用于 ROS 的开发中这里不做赘述，参见【Remove-ROS】^[5] 和【ROS-Migration】^[6]。

文件

对于 pointcloud 数据类型，PCL 提供了 PCD 格式文件来进行存储，具体参见 【pcd_file_format】^[7]。这里摘取上述网页中最后提到的示例文件中部分做一些注释，同时示例文件本身也是官网准备的一个小彩蛋。

A snippet of a PCD file is attached below. It is left to the reader to interpret the data and see what it means. :) Have fun!:

# .PCD v.7 - Point Cloud Data file format - 文件格式标题VERSION .7 - 指定 PCD 文件版本FIELDS x y z rgb - 指定字段 SIZE 4 4 4 4 - 以字节为单位指定上述每个字段的大小TYPE F F F F - 每个字段的类型 F 为 floatCOUNT 1 1 1 1 - 指定每个字段有多少个元素WIDTH 213 - 点云数据的宽度（有组织点云）或个数（无组织点云）HEIGHT 1 - 点云数据的高度（有组织点云）或1（无组织点云）VIEWPOINT 0 0 0 1 0 0 0 - 数据集中的点采集的视点位姿(tx ty tz)+(qw qx qy qz)POINTS 213 - 点云总点数DATA ascii - 点云存储数据类型 ascii 和 binary 0.93773 0.33763 0 4.2108e+06 - 以下开始存储点云数据0.90805 0.35641 0 4.2108e+06 - 按上述字段 x y z rgb 开始存储0.81915 0.32 0 4.2108e+06 - x y z rgb 0.97192 0.278 0 4.2108e+06 - x y z rgb 0.944 0.29474 0 4.2108e+06 - x y z rgb 0.98111 0.24247 0 4.2108e+06 - x y z rgb 0.93655 0.26143 0 4.2108e+06 - x y z rgb ... - ...

可以看到 z 都是 0，所以点云在一个平面上，最后 rgb 字段也只有两个值，所以整个点云只有两种颜色。

将其复制后写入 pcd 文件，用 pcl visualizer 一探究竟，原来是 PCL 的 logo，平面版只有绿色和紫色，符合预期。

当然依据这些个规则，你可以构造自己的 pcd 文件，并用 pcl 来显示，比如说