一种基于多相机模型的车载环视在线SLAM系统及方法与流程

一种基于多相机模型的车载环视在线slam系统及方法
技术领域
1.本发明属于计算机视觉检测的技术领域，具体涉及一种基于多相机模型的车载环视在线slam系统及方法。


背景技术：

2.随着人工智能的飞速发展，机器人、无人车等领域成为众多科研技术人员的研究热点，尤其是在未知环境和无法使用gps信号的情况下，自主实现精准定位和创建地图是该领域的研究前提条件。同步定位与建图(simultaneous localization and mapping，slam)是指移动平台在姿态未知的情况下，利用自身或外部传感器感知并构建一致的环境地图，同时确定自身相对姿态的方法。slam作为实现移动平台自主的关键，它在许多领域都显示出巨大的应用价值。为了实现应用场景的标准化和通用化，需要一个鲁棒的实时slam系统。
3.自动驾驶汽车是一个复杂的智能系统，要实现自动驾驶在技术上面临许多挑战，运动估计是其中一项关键技术。在运动估计中，经常会使用到各种传感器(如相机、激光雷达、惯性测量单元imu、轮速计等)，其中，主要依赖于车载的相机提供的图像信息这种基于视觉的方法由于依赖的传感器(一般是相机、惯性测量单元、轮速计等)重量轻、成本低和能提供的信息充足等优点，该方法在运动估计技术中被广泛运用。此外，多相机系统由于能够覆盖更广的视野，提高运动估计的性能，尤其是在纹理较差的环境中有较多的优势。到目前为止，也出现了一些针对多相机系统的运动估计方法，如有人将现有的主流slam算法(如orb-slam2和dso)扩充到多相机系统，但这些方法的问题是需要巨大的计算成本。然而，现有的位姿算法，要么适用于单相机系统，作出过分简化的假设，计算昂贵，要么在非完整车辆运动下简单地退化。故而仅使用计算机视觉技术通常不够鲁棒，由于现有技术的限制，自动驾驶的车载传感器对周边环境信息的感知和获取不够充分(如对于白色背景的物体无法识别等情况)，这对自动驾驶汽车来仍然存在较大的安全隐患。
4.因此，如何提供一种稳定可靠的视觉slam系统，以解决现有技术中环境感知方法获取信息的不充分、保证视觉slam系统的精度和稳定性，仍然是自动驾驶领域中需要研究的一个重点问题。


技术实现要素：

5.本发明的目的是针对现有技术的缺陷，提供一种基于多相机模型的车载环视在线slam系统及方法，通过在车辆上安装的四个相机，对接收图像进行处理，构建四个方向的视觉模型，通过klt光流算法跟踪角点，惯性测量单元imu、轮速计融合视觉信息，在紧耦合的滑动窗口优化框架中估计传感器的偏置，来实现在线实时的环视相机视觉slam与空间运动跟踪，用于解决现有技术中装配环视相机的车辆运动估计不鲁棒、由于需要大量的计算资源导致车载计算中心不能够实时处理从而导致用户不能在驾驶过程中在线实时使用的缺点，为自动驾驶汽车对地面环境感知及定位导航提供有效准确的环境信息，保证自动驾驶的安全性。
6.为实现上述目的及其他相关目的，本发明提供了如下的技术方案：一种基于多相机模型的车载环视在线slam系统，包括多相机模块、数据输入模块、多相机角点跟踪模块、融合imu和轮速计的快速位姿计算模块、多相机闭环检测模块，其中，多相机模块，采用多个相机安装于车辆不同方向，用于采集车辆周围环境的图像信息；数据输入模块，输入根据输入的图像信息和车辆的运动参数建立多相机模型，将场景点投影到多相机模型的坐标系，利用多相机slam算法进行车辆运动估计和定位；多相机角点跟踪模块，对数据输入模块中输入的图像信息进行角点跟踪，并进行车辆的初始姿态估计和重定位，跟踪车辆的局部地图，以及分析判断是否插入新的关键帧；融合imu和轮速计的快速位姿计算模块，将车辆的运动参数及图像信息在滑动窗口中进行联合优化得到车辆的位姿；多相机闭环检测模块，根据车辆的局部地图及车辆的位姿信息得到车辆的轨迹，检测车辆的轨迹是否出现闭环，并对当前的检测结果进行修正，得到最接近真值的车辆轨迹。
7.进一步的，所述车辆运动参数包括惯性测量单元imu采集的车辆的实时三轴线加速度、三轴角速度和轮速计采集的车辆的速度。
8.更进一步的，还包括外参优化模块，根据接收到的车辆运动参数和图像信息优化相机外参。
9.更进一步的，一个关键帧包含多个相机同时获取的图像。
10.进一步的，所述的相机模型包括：针孔相机模型、opencv鱼眼相机模型、鱼眼相机模型、斯卡拉穆扎相机模型和堪娜拉勃兰特相机模型，不同的相机模型采取不同的去畸变和投影。
11.本发明还提供了一种基于多相机模型的车载环视在线slam方法,包括以下步骤：步骤一、在多相机模块中初始化，建立环视相机模型，将场景点投影到多相机坐标系中，将车载环视相机中各个相机以固定方向配置到多相机坐标系中；
12.步骤二、通过数据输入模块接收车载环视相机采集到的图像信息，使用多关键帧融合不同相机的图片；
13.步骤三、融合imu和轮速计的快速位姿计算模块接收数据输入模块中车辆的惯性测量单元imu和轮速计采集的数据信息，并在滑动窗口中将图像信息和上述数据信息进行联合优化计算，最终获得车辆的位姿，车辆的位姿的集合组成车辆轨迹；
14.步骤四、在多相机闭环检测模块中检测车辆轨迹是否出现闭环，并对当前结果进行修正，得到最接近真实值的车辆轨迹。
15.进一步的，所述步骤一中，根据轮速计采集到的速度信息，在前面十帧，每一帧的位姿都使用预积分得到位姿，然后使用这十帧的位姿互相三角化前面的特征点，形成局部的地图点，完成初始化。
16.进一步的，所述步骤二中，当相机采集新的图像后，调用光流算法跟踪之前的角点到当前图像中，根据跟踪状态把跟踪失败的角点剔除，利用边缘判断把跟踪到的图像边缘的角点剔除。
17.进一步的，所述步骤二中，一个多关键帧上包含了多个相机同时获取的图像，且将多个相机同时获得的图像作为一帧放在关键帧中，即一帧关键帧中包含了四个相机的信息。
18.进一步的，在所述步骤四中，采用dbow2检测车辆轨迹是否出现闭环，通过dbow2从
词包数据库中找到最相似的帧，将闭环帧加入到滑动窗口中，使用联合优化来计算重定位结果。
19.采用本发明的技术方案，具有的有益效果为：通过采用多相机来扩大感知范围来进行当前车辆的定位，引入了多种传感器(如多个相机、惯性测量单元和轮速计)来提高系统的鲁棒性，能够解决现有视觉slam方法视场角小、视野受限的问题，增加鲁棒性，提高计算速度，为机器人的自主性提供可靠保障，从而保证机器人自主导航的充分性，在实际应用中更加安全可靠。
附图说明
20.图1是本发明一种基于多相机模型的车载环视在线slam系统框架示意图。
21.图2是本发明运行过程中的节点通讯关系示意图。
具体实施方式
22.下面结合附图，对本发明的实施例作进一步详细的描述，使得本技术方案更加清晰易懂。通过本说明书所揭露的内容本领域技术人员可轻易地了解本发明的其他优点与功效。本发明还可以同另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。
23.如图1所示为一种基于多相机模型的车载环视在线slam系统的框架示意图。本系统包括了包括数据输入模块、多相机角点跟踪模块、融合imu和轮速计的快速位姿计算模块以及多相机闭环检测模块。
24.其中的多相机闭环检测模块中，采用多个相机安装于车辆不同方向，用于采集周围环境的图像信息。采用4个相机,在车辆上前视、左视、后视、右视方向配置，用于环绕车身360
°
的实时视觉的采集，对车辆的周围环境进行实时检测和识别。
25.数据输入模块中，输入到数据输入模块的信息有：通过多相机模块采集到的车辆周围环境的图像信息，惯性测量单元imu采集到的角速度、线加速度信息、轮速计采集到的速度信息。通过输入到数据输入模块的信息建立多相机模型，将场景点投影到多相机模型的坐标系，利用多相机slam算法进行车辆运动估计和定位。
26.多相机角点跟踪模块，对数据输入模块中输入的多个相机的图像信息进行角点跟踪，并进行车辆的初始姿态估计和重定位，跟踪车辆的局部地图，以及根据新来的一帧是否是关键帧来决定是否往滑动窗口内插入新的关键帧，如果新来的一帧是关键帧，则往滑动窗口内插入新的关键帧；如果新来的一帧不是关键帧，则不往滑动窗口内插入。判断是否是关键帧的方法：每当采集到新的一帧图像时，首先判断该帧与滑动窗口里存的前一帧的相对位移，即与第十帧的角点的相对位置，用角点的相对位移来表示。如果角点的平均相对位移大于设定的阈值，则判定该帧是一个新的关键帧，把这个新的关键帧压入到滑动窗口中的最新位置，其它的关键帧都往前移动，首位的关键帧被边缘化；如果不是新的关键帧，就把之前的第十帧边缘化掉，这个新的一帧替换成为第十帧。在slam系统中，地图分为局部地图和全局地图两种，在此处使用到的局部地图描述的是车体附近特征点信息，在特征点的选取过程中，只保留离相机当前位置较近的特征点，而把较远的或视野外的特征点去除，从
而建立一个小范围的局部地图。
27.融合imu和轮速计的快速位姿计算模块，用于惯性测量单元imu、轮速计采集车辆在运动过程中的角速度、线加速度、速度，多相机模块采集到的图像信息，通过数据输入模块输入到程序中，将imu、轮速计及图像信息在滑动窗口中进行联合优化得到车辆的位姿。
28.多相机闭环检测模块，由多相机角点跟踪模块得到的车辆的局部地图及融合imu和轮速计的快速位姿计算模块中得到车辆的位姿信息可以得到车辆的轨迹，该模块的作用是用于检测车辆轨迹是否出现闭环，并对当前的检测结果进行修正，得到最接近真值的车辆轨迹。
29.本实施例所述车辆运动参数包括惯性测量单元imu采集的车辆的实时三轴线加速度、三轴角速度和轮速计采集的车辆的速度。
30.本系统还包括了一个外参优化模块，根据接收到的数据信息(车辆运动参数和图像信息)优化外部参数(即相机外参)。相机外参具体指相机的旋转和平移参数。
31.本实施例的相机模型可以选用以下类型：针孔相机模型、opencv鱼眼相机模型、鱼眼相机模型、斯卡拉穆扎相机模型和堪娜拉勃兰特相机模型，不同的相机模型采取不同的去畸变和投影。
32.与系统对应的，本实施例还涉及一种基于多相机模型的车载环视在线slam方法,包括以下步骤：
33.步骤一、在多相机模块中初始化，建立环视相机模型，将场景点投影到多相机坐标系中，将车载环视相机中各个相机以固定方向配置到多相机坐标系中；
34.步骤二、通过数据输入模块接收车辆的多相机模块输入进来的环视相机的图像信息，使用多关键帧融合不同相机的图片；
35.步骤三、融合imu和轮速计的快速位姿计算模块接收数据输入模块中车辆的惯性测量单元imu和轮速计采集的数据信息，并在滑动窗口中将图像信息和上述数据信息进行联合优化计算，最终获得车辆的位姿，车辆的位姿的集合组成车辆轨迹；
36.步骤四、在多相机闭环检测模块中检测车辆轨迹是否出现闭环，并对当前结果进行修正，得到最接近真实值的车辆轨迹。
37.在所述步骤一中，根据轮速计采集到的较为准确的速度信息，在前面十帧，每一帧的位姿都使用预积分得到位姿，然后使用这十帧的位姿互相三角化前面的特征点，形成局部的地图点，从而完成初始化。
38.在所述步骤二中，当相机采集新的图像后，调用光流算法跟踪之前的角点到当前图像中，根据跟踪状态把跟踪失败的角点剔除，利用边缘判断把跟踪到的图像边缘的角点剔除。一个关键帧上包含了多个相机同时获取的图像，且将多个相机同时获得的图像作为一帧放在关键帧中。
39.在所述步骤四中，采用dbow2(用于图像序列中快速位置识别的二进制词包，为了简便，下文统一用dbow2代替)检测车辆轨迹是否出现闭环，对于每一帧关键帧，我们提取500个fast角点和brief描述子,并将描述子转换为词包向量，加入到词包数据库中。通过dbow2从词包数据库中找到最相似的帧，将闭环帧加入到滑动窗口中，使用联合优化来计算重定位结果。
40.本实施例的一种基于多相机模型的车载环视在线slam方法，根据使用的不同的相
机，确定对应的相机模型，建立环视相机模型，对车辆进行位姿估计，在该环视相机在线slam算法中使用多关键帧融合不同相机的图片；在系统运行过程中，可以通过人为调整外参优化模块开启或关闭参数(比如说我想要开启这个模块，就把标志位置为true，关闭就置为false，这里的参数就是这个标志位)，来开启或者关闭外参优化模块，决定是否使用外参优化模块，如果开启外参优化，则在处理线程中，系统不仅使用给定的外部参数(即相机外参)，还将根据接收到的信息来不断优化外部参数(即相机外参)；如果关闭外参优化模块，则系统会一直使用给定的外参。在实验车辆行进的过程中，多相机模块采集车辆周围环境的图像信息，数据输入模块接受四个相机的图像信息、惯性测量单元采集到的的三轴线加速度和三轴角速度信息以及轮速计采集到的速度信息，再输入给融合imu和轮速计的快速测量模块，通过融合惯性测量单元imu和轮速计的快速位姿计算模块，将imu、轮速计和环视相机获得的图像信息进行融合，在滑动窗口中进行联合优化，获得车辆的轨迹后，还要进行多相机的闭环检测，检测轨迹是否出现闭环，如果出现闭环，则需要在多相机闭环检测模块进行轨迹优化，最终获得实验车辆的位姿。具体步骤如下：
41.(1)根据不同的相机模型和不同的畸变模型，建立不同的多相机模型，将场景点投影到多相机坐标系，车载的各个相机以前视、左视、后视、右视方向配置在该多相机坐标系中；利用本文提出的多相机slam算法进行车辆运动估计和定位，在算法中进行多相机角点跟踪，使用多关键帧融合不同相机的图片。具体包括以下过程：
42.(1.1)建立相机模型过程：系统中可选择的相机模型包括pinhole针孔相机模型、opencv鱼眼相机模、鱼眼相机模型、斯卡拉穆扎相机模型和堪娜拉勃兰特相机模型，根据车载环视相机的类型，选取对应的相机模型，不同的相机模型采取不同的去畸变和投影方式。如：如果使用的是鱼眼相机，可以选择opencv鱼眼相机模型或者鱼眼相机模型，这类鱼眼相机模型是先将3d点投影到单位球面，再将单位球面上的点投影到归一化平面上。
43.选择采用鱼眼相机模型时，由于图像只有中间圆部分是有效图像，所以使用遮罩进行剔除。在没有被剔除的区域，提取新的角点，这种做法，使得不管之前跟踪了多少角点，后续都可以将新的角点加入进去，使得每一帧的图像都有满足要求的角点个数。
44.(1.2)多相机角点跟踪线程：用于车辆运行过程中获取车辆周围环境的图像信息，所述图像信息通过四个相机以环视(指前视、左视、后视、右视)实现。该线程对多个相机的图像进行角点跟踪，而不采用提取特征点的方法，从而可以提高前端角点跟踪的速度。通过角点跟踪进行初始姿态估计或重定位，同时跟踪局部地图，最后判断插入新的多关键帧。多关键帧指一个关键帧上包含了多个相机同时获取的图像的关键帧，在算法中将同一时刻获得的四帧图像视为一帧。该多相机角点跟踪线程的步骤如下。
45.步骤一：线程中设置回调函数接收每个相机的图像，再对每个相机的图像做直方图均衡化，使得图像整体曝光更加均衡，不会出现某处过于亮或者过于暗的情况。
46.步骤二：如果建立的相机模型为鱼眼相机模型，则给图像添加椭圆形遮罩。由于图像只有中间部分区域的图像是有效部分，所以多相机角点跟踪模块会给四个相机接收到的图像添加遮罩，来剔除鱼眼相机采集到的图像中的外围无效部分，这里的椭圆形遮罩是人为设置的椭圆形遮罩；如不采用鱼眼相机模型，则不会添加相应的椭圆形遮罩。
47.步骤三：对每个相机进行步骤一处理后的图像进行执行klt算法跟踪角点，并根据opencv中的基于五点法的本质矩阵算法计算出的跟踪状态(利用opencv的内置的函数计算
得到的)，剔除跟踪失败的角点，通过提前设置的二值化椭圆形遮罩，剔除掉跟踪到遮罩内像素值为0的部分即处于图像边缘的角点。
48.步骤四：使用ransac和八点法计算步骤二得到的跟踪匹配对(利用opencv内置函数得到的匹配对)，将外点进行剔除。
49.步骤五：对每个角点的跟踪次数进行判断，先对跟踪的角点按跟踪次数降排序，然后依次选角点，选一个角点，在遮罩中将以该角点为中心的预设半径内的区域设为0，后面不再选取该区域内的角点，这种做法可以保持跟踪到的角点呈一种较为均匀分布的状态。
50.步骤六：在遮罩中不为0的区域，提取新的角点，该提取的角点个数为初始设定的个数减去上一次跟踪的个数，确保在本次操作之前不管跟踪了多少个角点，后续都可以添加新的角点，使得普通的单个图像的帧都有满足要求的角点个数。
51.步骤七：在一步处理过程完成后，更新每个相机跟踪到的角点的位置信息和跟踪状态，跟踪状态指的是被跟踪到和没有被跟踪到，如果有被跟踪到，被跟踪到的次数。
52.另外，在该线程中，可以通过调整参数，(比如说我想要开启这个模块，就把标志位置为true，关闭就置为false，这里的参数就是这个标志位)来观测去畸变之后的图像是否正常(指可视化的图像用肉眼看上去是没有畸变的)，如果开启显示去畸变后的图像节点，则在程序运行过程中可以看到不同相机模型根据不同的畸变模型去完畸变后的图像，如果关闭该节点，则不能看到去完畸变后的图像，但相应地，角点跟踪模块的处理速度会有一定的提升。
53.(2)在接收低频的相机图像时，系统同时接收高频的惯性处理单元imu和轮速计的信息，系统中设置了一个imu和轮速计融合快速位姿计算模块，用于接收车辆行驶过程中的运动参数，所述运动参数包括车辆配备的imu采集到的实时三轴线加速度和三轴角速度，轮速计采集到的的速度信息。在融合imu和轮速计的位姿计算中，初始化后，为了减少计算量，直接将惯性测量单元采集到的角速度、线加速度信息，轮速计采集到的速度信息和多相机模块采集到的车辆周围环境的图像信息进行联合优化计算，使用滑动窗口，将滑动窗口内的上述变量全部进行优化，并进行外参优化。在跟踪过程处理线程接收新的带有已跟踪角点的图片，对这些角点计算坐标、创建地图点，在滑动窗口内判断是否需要边缘化，新进来的一帧图像是否是关键帧，维护滑动窗口。上述过程中的一些方法与具体内容如下：
54.(2.1)初始化方法：因为轮速计可以在短时间内获得较为准确的尺度信息，所以在数据输入模块接收到的最开始的十帧，我们每一帧都使用预积分得到位姿，然后使用这十帧的位姿互相三角化前面的角点，形成局部的地图点，从而完成初始化过程。
55.(2.2)跟踪过程：在跟踪过程中，处理线程每接收到一张新的带有已跟踪的角点的图片，对于已跟踪到的角点中的那些还没有深度的角点，用奇异值分解算法计算出该角点的坐标，使得它在它被观察到的每帧图像上的重投影误差最小。该点的坐标采用观察到该角点的第一帧图像的相机坐标系的深度来表示。再结合各个关键帧的位姿、各个相机的外参、边缘化的信息、与预积分的误差、每个角点的重投影误差、回环或闭环误差来进行优化。在滑动窗口中，判断边缘化的条件是，是否进来一个新的关键帧。如果有边缘化的话，则把窗口中最前面的一个关键帧滑掉。然后把第一次被这个关键帧观察到的角点，都转移到新的第0个关键帧上。如果没有边缘化的话，则把之前最新的这一帧用进来的最新的这一帧替换掉。
56.(2.3)判断是否是关键帧的方法：每当采集到新的一帧图像时，首先判断该帧与滑动窗口里存的前一帧的相对位移，即与第十帧的角点的相对位置，用角点的相对位移来表示。如果角点的平均相对位移大于设定的阈值，则判定该帧是一个新的关键帧，把这个新的关键帧压入到滑动窗口中的最新位置，其它的关键帧都往前移动，首位的关键帧被边缘化；如果不是新的关键帧，就把之前的第十帧边缘化掉，这个新的一帧替换成为第十帧。
57.(2.4)创建地图点的方法：对于已跟踪到角点中的那些还没有深度的角点，如果它被之前两帧以上的关键帧观察到过，用奇异值分解计算出一个它的坐标，使它在被观察到的每帧图像上的重投影误差最小。点的坐标用在它被观察到的第一帧图像的相机坐标系的深度表示。
58.(2.5)外参优化模块：优化的目标是各帧的位置、姿态、偏移、相机外参及每一个角点在它被观察到的第一帧的深度。优化的残差有四项，分别是先验残差、imu和轮速计残差、重投影残差和闭环优化残差。上述四项残差项相加，得到最终的残差项，然后使用列文伯格优化方法进行优化。优化之后，得到滑动窗口中每一帧的位姿和每一个局部地图中的角点的逆深度，从而更新当前的位姿和角点的逆深度。优化残差的方法是：在每次非线性迭代后，根据残差和雅克比调整自变量的值，以及调整后的自变量计算出新的残差，再计算出新的雅克比。最后使用列文伯格优化方法对最终的残差项进行优化。
59.(2.6)在车辆运动的过程中，可能会出现车辆轨迹闭环的情况，系统中设置了一个多相机闭环检测模块，用于检测轨迹是否出现闭环，并对当前结果进行修正，使轨迹更接近真实值。在该模块中，首先用dbow2(用于图像序列中快速位置识别的二进制词包，为了简便，下文统一用dbow2代替)进行闭环检测，检测出闭环帧，再进行重定位，最后进行全局位姿优化，在全局位姿优化过程中，需要对位姿图进行管理。
60.(2.7)闭环检测方法：闭环检测模块采用dbow2进行闭环检测，通过dbow2从数据中找到最相似的帧，找到的最相似的十帧为闭环候选帧，其中最相似的帧即为闭环帧，将闭环帧加入到滑动窗口中，使用联合优化来计算重定位结果。在闭环检测过程中，通过角点验证当前帧与闭环候选帧之间的关联。首先通过brief描述子进行这两帧的角点匹配，通过汉明距离选择距离最短的匹配对。直接的匹配会造成许多外点匹配，为了移除外点匹配，采取以下两步进行几何验证:a.2d-2d:使用ransac(随机抽样一致性算法)计算基础矩阵，来剔除2d-2d之间的匹配；b.3d-2d:使用ransac和pnp(perspective-n-point的缩写，是求解3d到2d点对运动的方法)进行3d和2d点之间的匹配验证。通过上面两次验证的匹配对加入到候选关键帧中。
61.(2.8)重定位方法：将闭环帧加入到局部的滑动窗口中，固定闭环帧的位姿和它观察到的3d点。在重定位时，我们需要估计闭环帧v帧和当前帧的相对位姿。联合优化此时滑动窗口中的所有状态变量，包括点和位姿和imu和轮速计的信息。此时能轻易的写出视觉观测模型中闭环帧与当前滑动窗口中的观测方程，非线性误差如下：
[0062][0063]
使用这种联合优化计算出来的重定位结果更加精确。然后在使用这种结果计算闭环帧与当前帧之间的相对位姿，即当前的偏移误差。
[0064]
(2.9)全局位姿优化方法：在重定位之后，得到了一条能代表全局位姿的轨迹，使
用轻量级4自由度全局姿态图优化方法来进行全局位姿优化，可以使可能出现漂移或者跳跃变化的车辆轨迹更加平滑一致。因为imu受重力方向影响，所以滚转和俯仰角完全在可视惯性系统中观察到，又因为在车体运动中滚转和俯仰角运动是非常小的，所以只估计x,y,z和偏航角，即可优化全局位姿。每帧关键帧被边缘化后添加到位姿图中，一个关键帧用作一个关键帧位姿图中的顶点。通过位姿图的优化使我们调整顶点，使配置与边缘匹配为尽可能多。只调整3d定位顶点的x,y,z和偏航角ψ，并设置滚转角和俯仰角作为常量变量。这样的优化在无漂移方向不会发生修正。残差边如下：
[0065][0066]
(2.10)位姿图的管理方法：当行驶距离增大后，位姿图的大小可能无限增大，从而限制系统的实时性能，所以需要对(3.3)中的位姿图进行管理。为此，实施一个降采样过程来保持位姿图大小，将位姿图为有限的大小。带闭环的所有关键帧关闭约束将保留，而其他关键帧要么太近，要么方向非常相似的临近帧被移除。
[0067]
(2.11)本发明按照附图1的框架搭建基于多相机模型的车载环视在线slam系统，输入的高频惯性测量单元imu、轮速计信息在融合后进行快速位姿计算，同时，低频的图像信息通过回调函数接收，再进入多相机角点跟踪模块进行klt光流跟踪(本系统及方法的角点跟踪和imu轮速计信息融合是多线程中并行的)。
[0068]
在首次接收到相机的图像信息、惯性测量单元imu的三轴线加速度和三轴角速度信息以及轮速计采集到的车辆的速度信息后，系统准备进入初始化过程，初始化后在滑动窗口中进行多传感器的联合优化。系统接收到的低频图像信息通过转换为词包向量存入词包，在闭环检测模块检测轨迹是否发生了闭环，并对当前结果进行修正，使轨迹更接近真实值。闭环的结果与滑窗中的优化结果一起进行位姿计算，计算得到的位姿送入位姿图优化模块，优化得到最终的全局位姿。
[0069]
图2是本发明实施过程中的通讯节点示意图，数据接收模块接收四个相机的图像话题：/camera/0/0/image、/camera/0/1/image、/camera/0/2/image、/camera/0/3/image和融合惯性测量单元imu和轮速计的话题：/odom_imu，四个相机图像进入/multi_feature_tracker线程进行多相机角点跟踪，输出跟踪结果到/camera/0/0/feature，与/odom_imu一起在滑动窗口中/odometry_kl_test节点进行联合优化，再进入/pose_graph节点进行闭环检测以及位姿图的管理，最终在/pose_graph/pose_graph话题内输出轨迹。
[0070]
上述实施例为本发明最佳的实施方式，但是本发明的实施方式不受上述实施例的限制，其他任何未背离本发明的精神实质与原理下所做的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本方面的保护范围之内。更具体地说，在本技术公开、附图和权利要求的范围内，可以对主题组合布局的组成部件和/或布局进行多种变型和改进。除了对组成部件和/或布局进行的变型和改进外，对于本领域技术人员来说，其他的用途也将是明显的。