点云三维重建

慧拓

1. 分类后点云簇的处理

点云三维重建的内容主要包括以下3个方面：
1. 车辆的三维模型替换
2. 障碍物的三维网格化
3. 地面（包括操作面）的三维网格化

1.1 车辆

因为车辆部分不需要根据点云的细节进行三维模型生成，而是根据点云簇的几何特征，估计出车辆的位姿，替换成预先制作好的3D车辆模型，因此不需要经过三角网格化（即三维重建）的过程。
要想正确的放置车辆的3D模型，我们需要确定车辆在场景中的位姿：
位置 - 车辆的位置，可以通过求得车辆有向包围盒的中心点来作为车辆本地坐标系的原点。
朝向 - 对于车辆本地坐标系的3个轴的朝向，我们需要确定车辆的方向向量：LOOK和UP，得到这两个向量后，与这两个向量正交的向量方向即为车辆的LEFT方向，这样就确定了车辆本地坐标系在激光雷达坐标系（世界坐标系）中的几何关系。

大体的过程可简要概括为以下步骤：
1. 得到前述步骤中的车辆点云簇
2. 计算该点云簇的有向包围盒（OBB），这样可以得到2个确定位姿的向量所在的直线方向。
3. 通过车辆自身几何特性和车辆包围盒与激光雷达的几何关系，得出UP方向。（见1.1.2.2）
4. 继续通过生成OBB层次树来细化车辆的包围盒，进而得到LOOK向量（见1.1.2.3）。

1.1.1 生成OBB包围盒层次树

1.1.1.1 有向包围盒（OBB - Oriented Bounding Box）

常用的包围盒有球体，轴对齐包围盒（AABB - Axis-aligned bounding box），有向包围盒（OBB - Oriented Bounding Box）以及凸包包围盒等。我们的需求是估计车辆的姿态，综合考虑准确性和运算速度，使用有向包围盒（OBB）是最合适的。
各种包围盒的简要示意如下图所示：

左：包围球；中：轴对齐包围盒；右：有向包围盒

有向包围盒生成算法参考：
https://hewjunwei.wordpress.com/2013/01/26/obb-generation-via-principal-component-analysis/
通过PCA（Principal Component Analysis 主成分分析）来生成OBB的算法是目前主流的快速算法，相比传统的OBB生成，PCA方法通过计算点云簇的特征向量来得到OBB的主轴，避免了大量的两点之间的距离运算。

1.1.1.2 OBB包围盒层次树

生成代表车辆的点云簇的OBB，只能得到车辆的大体姿态，但无法确定车辆的具体朝向（如车头和车尾各在OBB的哪一边），因此，我们需要进一步细化点云簇的几何特征，用以判断车头的位置。
为此，我们需要生成多个层级的OBB，即对包围盒进行递归细化，既可以用单个的OBB来得到车辆的大致姿态，又可以用足够细化的OBB层次来得到车头的位置（以及更多的判断内容）。这样就得到了车辆的姿态，再通过最上级OBB中心点与激光雷达坐标原点的相对关系得到车辆的位置，即可以摆放事先做好的3D模型了。

OBB包围盒层次树，即在生成了点云簇的OBB之后，继续针对点云簇的几何特征，对其进行分割，对分割后的每一部分继续计算其OBB，如此递归执行，直到满足某个结束条件为止。其基本思想如下图所示：

左一：生成的OBB层次树示意
左二：第一层OBB
右二：第一次分割的OBBs
右一：进一步分割的OBBs

对于我们的需求：矿卡而言，大致递归到3-4层得到的OBB层次树，即可以满足判断车头位置的需求。

1.1.2 车辆位姿判断

1.1.2.1 车辆位置

通过激光雷达坐标系的原点与第一层OBB的中心点之间的几何关系可以得出。

1.1.2.2 UP方向

步骤：
1. 将最长边所在的4个面作为候选面。
2. 根据车辆高度和宽度的大小关系，将上述4个面中其中2个排除，剩余2个候选面。
3. 2个候选面的法向量中，与激光雷达坐标Y轴夹角小的，即为车辆的UP方向。

UP方向也可以在下面“LOOK方向”内描述的分类器中直接与LOOK方向一同得出。

1.1.2.3 LOOK方向

步骤：
1. 将最长边所在的4个面排除，剩余2个候选面（车头和车尾对应面）
2. 用 1.1.2.2 步骤中得到的UP方向，以及过第一层OBB的中心点并与最长边平行的直线确定的平面，对第一层OBB进行切割。得到的切割面为第一层OBB在车辆本地坐标XOY平面的投影面。
3. 第一层OBB中所有点在投影面上的投影作为当前参考点集。计算该点集的凸包。
4. 根据预先知道的车辆几何特征，以及步骤3得到的凸包，得出车头的位置属于2个候选面中的一个（面H）。
5. 步骤4得出的面H的法向量，即为车辆的LOOK方向。

车辆的LOOK和UP方向，也可以通过训练一个对细化的OBB层次树进行分类的分类器得出，分类结果包括确定车辆位姿需要的LOOK和UP向量。

得到的凸包即为判断车头朝向的数据。可以用简单直接的几何方法判断，也可以用事先用上述步骤得到的凸包训练的分类器来做判断。
此步骤因为最终得到的是一组2D点，且判别所在坐标系为车辆自身本地坐标系的2D投影，因此计算量比较小。

1.1.2.4 摆放3D模型

此步骤需要的关键数据：车辆位置，车辆的LOOK朝向，UP朝向已经在前述步骤得出。

1.2 其他障碍物

经过聚类后得到的点云簇中，再经过分类过程，得出非车辆的分类结果后，可暂时作为其他障碍物处理。
此步骤只需将点云簇进行三角网格化处理即可。

1.2.1 三角网格化

点云三角网格化的大体步骤分为以下步：

• 得出点云的凸包
• 对凸包进行三角剖分
• 法线估计
• 将上述结果导出成3D网格数据
• 赋予材质，进行渲染

此步骤可使用PCL库进行处理，PCL内封装了包括贪婪投影三角化法在内的三角网格化算法，直接得出包含每个三角面片的法线向量的最优化三角剖分结果，可以直接使用。

1.2.2 放置

可以对上述步骤生成的凸包计算一层OBB，位姿计算方法同“车辆”部分的描述。

1.3 地面

地面的处理分为2部分：
1. 地面3D网格生成
2. 工作面判断

1.3.1 三角网格化

三角网格化步骤同“其他障碍物”部分的“三角网格化”内容。

1.3.2 工作面判断

步骤：
1. 读取已经生成的地面3D网格的每个三角面的法线
2. 计算所有法线朝向的中位数向量
3. 计算所有法向量与中位数向量的夹角
4. 夹角大于某个阈值的向量对应的三角面，即视为工作面
5. 将统计得出的工作面赋予特定的材质，令其颜色有别于普通地面

实际使用中，可以通过预先对施工场地的了解，对步骤中涉及的参数进行调整和过滤。