- 双目视觉定位总体方案设计
主要步骤说明：
1）双目相机标定，获取左右摄像头内参、外参，得到图像坐标到世界坐标的映射模型。
2）图像预处理，根据标定得到畸变参数对采集到的图像去畸变，根据测试图像数据特征对畸变后图像做增强、滤波等，使图像更便于后续特征检测；
3）图像特征点匹配：采用ORB算法对左右相机图像作特征检测，检测后的特征点根据匹配算法做匹配，记录匹配后的特征点对；
4）根据匹配后的特征点对计算视差，根据双目视觉测距原理得到匹配点对对应摄像机坐标系下的三维坐标；
5）根据匹配点对三维坐标和图像坐标采用PnP或ICP算法计算相机位姿；
6）根据相机与车辆相对位置关系确定车辆当前位置；
7）对借鉴视觉SLAM和信标识别定位思想对定位误差做修正；
8）车辆连续运动实时获取位置信息得到车辆全局位姿信息。
2.各模块详细说明
2.1双目相机标定
2.1.1 标定原理
相机成像模型中涉及四个基础坐标系：世界坐标系、相机坐标系、像素坐标系、成像平面坐标系。空间上的一点由世界坐标系转换到摄像机坐标系，然后再将其投影到成像平面坐标系，最后再将成像平面上的数据转化到图像像素坐标系。空间一点P(Xw,Yw,Zw)经相机小孔投影到像平面生成倒立虚像，进行等效，如图2-1所示：
图2-1 相机模型中各坐标系关系
在不考虑畸变时，各坐标系关系转换为数学模型如图2-2所示，其中(u,v)是图像中的像素坐标，(u0,v0)是相机镜头光心在像平面的坐标，dx和dy为感光芯片像素点对应的实际物理尺寸，f/dx、f/dy、u0 、v0为相机内参，R,T 为旋转和平移矩阵，为相机外参，S为比例缩放因子。
图2-2 各坐标系转换数学模型
在考虑畸变时通过非线性估计求解标定模型参数。由于畸变的影响，线性模型中的点 (u,v)会发生偏移。因此线性标定得到的参数误差较大，引入畸变参数后，对相机模型进行极大似然估计，得到得到相机内参（含畸变系数：径向畸变系数s1,s2,s3,切向畸变系数p1,p2）和外参，为保证标定的稳定性，一般取大于 14 张图片完成标定，减少由测量误差带来的精度损失。
双目相机的标定，在以上标定基础上还需考虑两摄像机之间的旋转平移关系，可推导出
两相机坐标系之间的旋转矩阵R和平移向量T分别为R=RrRlT,T=Tr-RTl
2.1.2 标定实验设计
基本思想，采用打印的棋盘格图像（已知各角点的世界坐标），通过角点检测图像算法求解出棋盘格图像角点坐标，根据标定模型求解出个标定参数，步骤如下：
- 打印棋盘格图像；
- 双目相机从多个角度拍摄不少于14张图片；
- 角点检测和参数计算（通过matlab工具箱或OpenCV库函数实现）
- 验证标定结果精确度，对畸变图像通过标定模型中的畸变系数作校正，查看校正后效果是否可接受；将棋盘格图像根据标定模型重投影计算出世界坐标，并与真实的世界坐标做对比，评估误差。
2.2 图像预处理
2.2.1畸变校正
摄像机采集到的的图像可能发生形变，主要可分为径向畸变和切向畸变。
径向畸变是指像素点以图像中心为圆心，顺着径向产生的变形，通常由于镜头厚薄不均匀导致，依据图像在径向上的变化方向，分为枕形畸变和桶形畸变两种。
2-3枕形畸变 2-4桶形畸变
切向畸变是由摄像机的光轴未垂直于成像平面引起的，与径向畸变类似，图像的切
向畸变也可以通过畸变参数进行校正。
畸变校正过程即是根据畸变系数模型还原图像。
2.2.2 Bouguet 算法极线校正
校正前的左右相机的光心并不是平行的，两个光心的连线就叫基线，像平面与基线的交点就是极点，像点与极点所在的直线就是极线，左右极线与基线构成的平面就是空间点对应的极平面。
校正后，极点在无穷远处，两个相机的光轴平行。像点在左右图像上的高度一致。这也就是极线校正的目标。校正后做后续的立体匹配时，只需在同一行上搜索左右像平面的匹配点即可，能使效率大大提高。
（a）校正前 （b）校正后
2-4 极线校正前后对比图
2.2.3 图像增强和滤波
在实际场景中，可能由于光线变化、阴影遮挡等因素导致图像特征点难检出，因此考虑根据场景特征制定合理的图像增强方案，常用的图像增强算法如ACE（自适应对比度增强算法）、Clahe(限制对比度的自适应直方图均衡)等。此外，由于实际场景复杂，噪点干扰性强，可根据场景图像特征做滤波处理，便于后续特征检测，常用滤波算法如中值滤波、高斯滤波等。
2.3 空间点三维重建
图2-5 双目视觉定位坐标关系图
如图2.5所示，由小孔成像和相似三角形原理可推断出空间点P对应在相机坐标系下的坐标，图中PL和PR分别为左右相机在左右成像平面匹配的特征点坐标，b为两相机光心距离，f为相机焦距，则空间点P对应在相机坐标系下的坐标为：
X’=bxuL/(xuL-xuR) , Y’=byvL/(xuL-xuR), Z’=bf/(xuL-xuR)
由此可确定空间点在相机坐标系下的三维坐标，根据相机坐标系和世界坐标系的旋转平移关系即可确定该点的世界坐标。上述坐标求解关键是获取左右相机拍摄的图像中特征匹配点PL和PR，即定位的关键在于匹配特征点的准确性。
2.4图像特征检测及匹配
该部分多以传统图像算法实现，但现有算法可能仍存在部分场景匹配率低以及算法设计较复杂时耗时长等问题；部分学者采用深度学习卷积神经网络得到特征视差，通过传统匹配算法实现匹配。深度学习方案受限于训练数据的完备性及网络运行时间。KITTI数据集和Sceneflow数据集中部分数据可供使用，模型网络运行时间问题可考虑在尽可能少的牺牲检测精度的前提下裁剪网络分支。本方案设计初步考虑先尝试传统算法实现再根据实际测试问题优化迭代的思路。
根据特征的种类，特征图像匹配算法点特征匹配、边缘特征匹配、面特征匹配算法，本方案的主要目标是提取特征点并匹配，因此仅考虑高效的点特征提取及匹配算法，如下图2-6所示为常用的特征点匹配算法：
2-6 常用点特征图像匹配算法
综合考虑算法的实时性与特征点提取的准确性，本方案拟采用适用于双目实时定位的ORB算法，如图2-7所示，采用灰度质心法在Fast角点检测算法的基础上加入方向特征，将特征转换为Brief描述符，并加入旋转矩阵，使其具有局部不变性，通过计算汉明距离实现匹配，最后通过RANSAC 匹配优化算法实现误匹配剔除。在算法实现时考虑到实时性可根据实际测试结果对原图像取一定的ROI区域，在匹配时可根据距离与视差的关系确定大致匹配范围以减少匹配时间。
2-7 ORB算法图像匹配过程
针对本项目应用场景，匹配点对主要用来计算同一像素点在不同相机上的对应特征点的视差，以确定该点对应相机坐标系的坐标。对得到的匹配点对计算视差，根据图2-8确定是否采用该点对计算得到的视差值，如果不满足条件，则考虑当前帧不计算，直接考虑历史帧位置信息跟踪当前车位置。
2-8 根据匹配点视差计算相机坐标系下的坐标
2.6 相机位姿计算
1.PnP算法：n个空间3D点真实坐标与空间点在图像上的投影2D点对应的点对，计算相机位姿。
由上述步骤分别计算出了n个左右图像匹配的空间点图像坐标及其对应的相机坐标下的三维坐标，根据PnP原理,当n>3时，即可求解出相机位姿，即相机相对世界坐标系的旋转和平移矩阵。算法可能存在的问题：
1）P3P 利用点数较少，当给定的配对点多于 3 组时,难以利用更多的信息。
2）如果 3D 点或 2D 点受噪声影响,或者存在误匹配,则算法失效。
针对PnP利用点数少问题可通过EPnP改进，基本思想是借鉴主成分分析的思想，先选所有点的质心点作为中心点，再选3个主方向上的点作为其余点。
2.ICP算法：多个匹配的特征点得到相机坐标系下的三维坐标，得到三维点云，通过ICP算法迭代求解出相机位姿，即相机相对世界坐标系的旋转和平移矩阵。
2.7车辆定位
根据相机位姿和车的相对位置关系，即可得到车的三维坐标，进而实现定位。
2.8定位误差修正
- 视觉SLAM方式
借鉴视觉SLAM思想，在后端优化和回环检测模块对误差进行修正。
2-9 视觉SLAM模块图
1）视觉里程计 (Visual Odometry)：仅有视觉输入的姿态估计，上述定位过程可视为视觉里程计获取相机位姿的过程
2）后端优化 (Optimization): 后端接受不同时刻视觉里程计测量的相机位姿，可设计滤波器（如卡尔曼滤波器）对位姿优化
3）闭环检测 (Loop Closing): 如果发生了轨迹闭环, 即可判断自身是否进入历史同一地点，如进入同一地点，则可根据当前位置的历史信息修正当前位姿;
4）建图 (Mapping): 根据估计的轨迹，建立与任务要求对应的地图，便于回环检测时获取对应信息。
2.信标辅助定位
1）建立高精度驾驶地图，如包含有不同特征的车道线、交通标志牌、红绿灯等信息，地图中含有这些路标信息的较为准确的位置（精度与定位精度相匹配）；
2）在实时定位过程中通过检测并识别地图中含有的路标信息，与地图中已有的路标做匹配，若匹配上则当前位置路标为高精度地图中的路标位置，进而推算出当前车的位移。
其中上述见图和定位过程均可通过深度学习和传统目标检测算法模板匹配实现。
