前言

自动驾驶激光感知模块，常常需要求得目标物体的最小凸多边形，以判断是否会发生碰撞；以及目标车辆的航向角，以预测目标可能的前进方向。

一、点云凸包

最小凸多边形也叫作凸包，就是将最外层的点连接起来，它能包含点集中所有的点。

最流行的凸包算法是Graham扫描(Graham scan)算法和Preparata & Hong提出的“分而治之”(Divide-and-Conquer)算法。这两者的时间复杂度都是O(nlogn)，但是Graham算法的常数因子更小，实际运行要快得多。不过“分而治之”算法的优点在于可以方便自然地扩展到3D，乃至更多维度，而Graham算法则不能。对于自动驾驶工况，我们只需要获得2D平面凸包即可，因此选择Graham扫描算法。

格雷厄姆扫描算法流程

把所有点放在二维坐标系中，则纵坐标最小的点一定是凸包上的点，如下图中的P0。
把所有点的坐标平移一下，使 P0 作为原点，如下图。
计算各个点相对于 P0 的幅角 α ，按从小到大的顺序对各个点排序。当 α 相同时，距离 P0 比较近的排在前面。例如上图得到的结果为 P1，P2，P3，P4，P5，P6，P7，P8。我们由几何知识可以知道，结果中第一个点 P1 和最后一个点 P8 一定是凸包上的点。
以上，我们已经知道了凸包上的第一个点 P0 和第二个点 P1，我们把它们放在栈里面。现在从步骤3求得的那个结果里，把 P1 后面的那个点拿出来做当前点，即 P2 。接下来进入扫描模式开始找剩余的点：
连接P0和栈顶的那个点，得到直线 L 。看当前点是在直线 L 的右边还是左边。如果在直线的右边就执行步骤6；如果在直线上，或者在直线的左边就执行步骤7。
如果在右边，则栈顶的那个元素不是凸包上的点，把栈顶元素出栈。回到步骤5。
当前点是凸包上的点，把它压入栈。检查当前点是不是步骤3那个结果的最后一个元素。是最后一个元素的话就结束。如果不是的话就把当前点后面那个点做新的当前点，返回步骤5。

最后，栈中的元素就是凸包上的点了。

代码

struct Node
{double x,y;
}p[MAX],S[MAX];//p储存点云的位置，S是凸包的栈
inline bool cmp(Node a,Node b)//比较函数，对点的极角进行排序
{double A=atan2((a.y-p[1].y),(a.x-p[1].x));double B=atan2((b.y-p[1].y),(b.x-p[1].x));if(A!=B)return A<B;else    return a.x<b.x; //这里注意一下，如果极角相同，优先放x坐标更小的点
}
double Cross(Node a,Node b,Node c)//计算叉积
{return (b.x-a.x)*(c.y-a.y)-(b.y-a.y)*(c.x-a.x);
}
void GetConvexHull()//求出凸包
{for(int i=1;i<Num;i++)//找到最靠近左下的点if(p[0].y>p[i].y||(p[0].y==p[i].y&&p[i].x<p[0].x))swap(p[0],p[i]);sort(p+1,p+Num,cmp);//对于剩余点按照极角进行排序 S[0]=p[0],S[1]=p[1];//提前在栈中放入节点 int top=1;for(int i=2;i<Num;)//枚举其他节点 {if(top&&Cross(S[top-1],p[i],S[top])>=0)top--;//如果当前栈顶不是凸包上的节点则弹出 elseS[++top]=p[i++];//加入凸包的栈中 }//输出//for(int i=0;i<=top;i++)//    cout<<S[i].x<<S[i].y<<endl;
}

二、车辆航向角

1. PCA算法

由于车辆是矩形，可采用PCA算法确定航向角，通过计算单个车辆点云坐标数据(xk,yk)的协方差矩阵，然后取最大特征值，其对应的特征向量即为车辆航向方向。

但需要注意的是PCA算法适用于车辆点云遮挡较小的情况，如果车辆点云被大量遮挡，特征向量跟实际朝向会存在不小的偏差。

2. 凸多边形最小外接矩形算法

凸多边形最小外接矩形算法比PCA更具有普适性，在上面我们得到了目标的凸多边形，有一条十分重要的数学推导：

对于凸多边形的一个外接矩形存在一条边与原凸多边形的边共线。

故对凸多边形的每一条边，计算其最小矩形，最后取面积最小的矩形为凸多边形最小外接矩形。

知道了矩形坐标，便能计算出车辆的航向角。

3. L-shape拟合算法

对于大多数定义良好的目标对象的点云簇，凸多边形最小外接矩形算法已经足够用了；但是它却不能完全正确地包含部分遮挡的对象，如下图：凸多边形最小外接矩形算法与L-Shape拟合算法的比较。

由于聚类原因，其中稀疏点(红色圆圈)常常被认为是孤立点，可以注意到这种情况下凸多边形最小外接矩形算法会生成不正确的矩形框；为解决这个问题，有了基于特征的L-Shape拟合算法，利用这一方法，可以正确的推导出yaw方向上的偏航。

L-Shape拟合过程如下：首先，选择两个最远的离群点x1和x2，它们位于面向Lidar传感器的障碍物边界上的两侧，然后在x1、x2两点间绘制一条直线Ld，以这两个点为上下区间，从障碍物的所有点向Ld做投影，得到正交线L0簇。然后可以从L0簇中获得带有最大距离Dmax和接近90度角度的那个唯一的正交线段L0。然后，L0不与Ld相交的点记为角点X3。闭合连接X1、X2和X3得到L形折线；又由于大多数交通工具（小车，公交车，卡车）的航向与其最长维度的边平行，所以BoundingBox的方向（障碍物航向）即是最长的那条线（本例是线段x2x3）。过程见下图所示：

激光感知（八）：计算目标最小凸多边形及航向角相关推荐

关于车路协同目标定位（含通过定位计算速度、加速度、航向角）
关于车路协同目标定位(含通过定位计算速度.加速度.航向角) 文章目录关于车路协同目标定位(含通过定位计算速度.加速度.航向角) 1 视觉目标定位 1.1 方案原理 1.2 从算法端考虑方案测试方法 ...
感知融合（六）：运动补偿算法计算目标绝对速度
目录前言 1. 运动补偿 1) 位移补偿 2) 角度补偿 2. 附录定位(东北天)坐标系 atan2 坐标旋转前言在自动驾驶场景中,常常需要计算目标的绝对速度以区分静止目标和运动目标,从而进行 ...
无人驾驶感知篇之目标跟踪（一）
基本的感知任务有针对动态道路参与者的目标识别.检测和跟踪,之前的文章基本把无人驾驶感知篇之目标检测写的差不多了,这篇开始写一写目标的跟踪. 1.什么是目标跟踪目标跟踪是目标检测在时间轴上的延续,获得 ...
BASNET：边界感知的显著目标检测
BASNET:边界感知的显著目标检测摘要采用深卷积神经网络进行显著目标检测,取得了较好的效果.然而,以前的工作大多侧重于区域精度,而不是边界质量.在本文中,我们提出了一种预测-细化体系结构Basn ...
最小凸多边形（凸包）
描述给出平面上n个点的坐标,计算最远两点间的距离,以及包含所有点的最小凸多边形(凸包) 输入第一行一个整数n,接下来是n行的实数对,表示n个点坐标.2<=n<=10000 n x1 y ...
C++ —— (两个经纬度计算距离、方位角)、(经纬度A+距离+方位，计算目标经纬度)、(多个经纬度计算面积)
顺看编码不易,觉得文章好,请给作者点赞关注.一键三连.谢谢! * 计算两经纬度之间的距离.方位角 * 已知A经纬度.根据距离.方位,计算目标经纬度测试了三组数 ...
根据起始点经纬度、距离、方位角计算目标点经纬度的方法
主要目的: 在处理卡口数据的过程中,遇到了一个问题:对于每个交叉口只知道其中心点的经纬度,而不知道每个进口道停车线的经纬度,对不同的进口道,难以将轨迹数据分开处理. 因此,采用了一种简化的方法,假设了 ...
睿智的目标检测20——利用mAP计算目标检测精确度
睿智的目标检测20--利用mAP计算目标检测精确度学习前言 GITHUB代码下载知识储备 1.IOU的概念 2.TP TN FP FN的概念 3.precision(精确度)和recall(召回率 ...
android计算心率方法,计算目标心率最简单的方法 | 跑者
摘要当我们在目标心率区跑步或进行其它锻炼时,我们的身体将会从中获得最大的益处. 当我们在目标心率区跑步或进行其它锻炼时,我们的身体将会从中获得最大的益处.以每分钟心跳次数(bpm)为单位,目标心率是 ...

激光感知（八）：计算目标最小凸多边形及航向角

目录

前言