CN110132242B - 多摄像机即时定位与地图构建的三角化方法及其运动体 - Google Patents

Abstract

本发明提供一种用于多摄像机即时定位与地图构建的三角化方法，包括如下步骤：将由k个摄像机中的同一摄像机和不同摄像机在相邻关键帧的拍摄结果作为n组摄像机组合，其中k和n为大于1的整数；通过对所述拍摄结果进行图像处理，得到用于预测各组摄像机组合的三角化成功率的评测结果；和基于各组摄像机组合的评测结果，选取m组摄像机组合进行三角化，其中，k≤m＜n。本发明还提供这样的多摄像机即时定位与地图构建方法及采用上述方法的运动体。

Description

多摄像机即时定位与地图构建的三角化方法及其运动体

技术领域

本发明总体涉及用于多摄像机即时定位与地图构建技术，具体地，涉及用于多摄像机即时定位与地图构建的三角化方法。

背景技术

即时定位与地图构建(Simultaneous Localization And Mapping，下文简称为SLAM)指的是运动物体在自身位置不确定的条件下，在完全未知环境中创建地图，同时利用地图进行自主定位和导航。换句话说，即时定位与地图构建的技术问题可以描述为：如何使运动物体在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和传感器数据进行自身定位，同时建造增量式地图。更具体地，SLAM是一种通过实时跟踪运动物体运动并在此过程中同时建立周围环境地图以达到定位导航等目标的技术。SLAM通过使用多种传感器如激光雷达、超声波传感器、GPS、惯性测量单元IMU以及摄像机等设备跟踪设备当前的位姿并利用采集到的激光雷达或者图像重构周围的环境以达到定位和地图构建的目标。SLAM以其实用性在无人机导航、自动驾驶等众多领域获得了广泛应用。其中摄像机以其低廉的成本获得了众多研究者和企业开发人员的青睐，而以摄像机为主要传感器的即时定位与地图构建技术——即视觉即时定位与地图构建技术(Visual即时定位与地图构建)也获得了前所未有的发展。

视觉SLAM根据所采用的摄像机的数量可以分为单目SLAM(单个摄像机)、双目SLAM(两个摄像机)、多摄像机SLAM；同时根据所采用摄像机的种类又可以分为透视摄像机SLAM(perspective camera SLAM)以及采用鱼眼镜头的鱼眼摄像机SLAM(fisheye cameraSLAM)。由于采用多个摄像机可以获取到周围环境中更加丰富的信息，多摄像机SLAM获得了越来越多的关注；同时鱼眼摄像机拥有普通透视摄像机无法相比的大视角，可以看到更宽范围内的信息，因此获得了更多的研究；而多鱼眼摄像机SLAM(multiple fisheye cameraSLAM)也渐渐走进了人们的视野。

围绕SLAM中定位与地图构建的两大目标，现有的视觉SLAM方案，例如ORB SLAM，一般将***划分为跟踪(tracking)和地图构建(mapping)两个线程。视觉SLAM技术首先通过特征点法等方法跟踪摄像机当前的位姿，在求解到当前摄像机的位姿之后，再通过三角化图像中新匹配的特征点将新的地图点加入到地图中。通过不断重复以上跟踪和地图构建的环节，视觉SLAM就可以成功地跟踪摄像机的运动并同时恢复周围的环境。

整个SLAM过程中，跟踪和地图构建相互依赖，互为支撑。具体而言，成功的跟踪能够保证地图构建过程中***地图点的正确性；而在成功更新地图之后，新获取的图片才能通过与地图匹配获得更准确的位姿。在SLAM算法过程中，跟踪与图片的处理速度直接相关，而地图构建负责处理后端优化等问题则较慢无法实时，现有的SLAM方案通过将跟踪与地图构建分离，由此缓解了***对于硬实时的需要，也就是说，跟踪线程在前端实时获取并处理获得的图像，地图构建线程则在后端以较慢的速度优化。

然而这种方案只是一种表面上的实时，在摄像机朝向的场景变化不大，可以重复利用的特征点较多的时候，比如前向摄像机，不需要频繁***关键帧，***可以稳定工作；如果摄像机朝向的视野变化很快，比如像车辆行进中的左视或者右视摄像机，新的关键帧必须被及时创建，而关键帧的创建依赖于地图构建线程，但如上所述地图构建线程的运行速率较慢，并不能达到实时或者准实时的速率，这就导致跟踪线程要求***新的关键帧时，地图构建线程仍然在忙于处理上一个关键帧，因此进一步导致关键帧无法***，进而导致地图无法更新，从而跟踪线程因为没有可以用于更新当前位姿的最新地图而跟踪失败。因此降低地图构建线程的计算量至关重要。

传统多摄像机三角化(triangulation)方法直接对当前关键帧与参考关键帧的多摄像机帧之间做全匹配并通过暴力搜索匹配特征点，需要的计算量会随着摄像机数量以指数激增。例如，如果车辆的多摄像机SLAM***中包含三个摄像机，则需要考虑9种摄像机三角化组合，而如果包含四个摄像机，就需要考虑16种摄像机三角化组合。当然，现在的一种效率更高的方法是仅对同一个词袋(BoW)节点附近的特征点做匹配。然而，这种方法能够在一定程度上减少同一个摄像机组合之间特征点匹配的计算量，但是仍然不能够满足多摄像机SLAM三角化的效率需求。

因此，需要一种能够提高多摄像机SLAM三角化效率的技术。

发明内容

鉴于上述情况，做出了本发明。

根据本发明的一个方面，提供一种用于多摄像机即时定位与地图构建的三角化方法，包括如下步骤：将由k个摄像机中的同一摄像机和不同摄像机在相邻关键帧的拍摄结果作为n组摄像机组合，其中k和n为大于1的整数；对由每一组摄像机组合所拍摄到的相邻关键图像帧进行图像处理，得到用于预测各组摄像机组合的本次三角化成功率的评测结果，其中，所述评测结果通过如下方式获得：从每一关键图像帧提取相关的特征数据；根据所述特征数据计算至少一项评测参数；和根据所述至少一项评测参数进行评估计算，以得到用于预测该组摄像机组合的三角化成功率的评测结果；以及基于各组摄像机组合的评测结果，通过判断评测结果是否满足预定阈值条件和/或通过评测结果的大小排序来筛选出m组摄像机组合，仅选取所述m组摄像机组合进行三角化，其中，k≤m＜n，从而实现基于实时信息选择部分摄像机组合进行三角化。

可选地，所述至少一项评测参数至少包括第一评测参数，所述第一评测参数为该组摄像机组合之间的共视面积A1，并且，通过如下公式(1)得到评测结果，

预测可三角化的点数P＝该组摄像机组合之间的共视面积A1×单位面积内预测可跟踪的特征点数B，其中B＞0…(1)，

其中，通过预测可三角化的点数P来预测摄像机组合的三角化成功率。

可选地，所述至少一项评测参数还包括第二至第四评测参数中的至少一个：该组摄像机组合中的各摄像机所拍摄的关键图像帧中的特征点数量A21和A22、纹理丰富程度A31和A32、以及场景遮挡程度A41和A42。

可选地，通过如下公式(2)得到单位面积内预测可跟踪特征点数B，

单位面积内预测可跟踪特征点数B＝k1*min{A21，A22}+k2*min{A31，A32}+k3*min{A41，A42}，其中，k1,k2,k3≥0……(2)。

可选地，所述特征数据包括第一特征数据，所述第一特征数据包括根据由所述相邻关键图像帧的特征点计算得到的各摄像机位姿、各摄像机视角、每一关键图像帧中的各特征点的场景深度的平均值、以及各特征点的场景深度的离散程度，并且所述第一评测参数通过根据所述第一特征数据计算得到的该组摄像机组合中的每一个摄像机所拍摄的场景的可视区域并且计算所述可视区域的重合程度而得到。

可选地，采用卡尔曼滤波来估算所述各特征点的场景深度的平均值以及各场景深度的离散程度。

可选地，对于透视摄像机，通过以当前摄像机中心为顶点，底边与当前摄像机成像平面平行的等腰三角形区域中到当前摄像机成像平面的垂直距离为场景深度的平均值±场景深度离散程度的梯形区域来描述所述可视区域，并且，对于鱼眼摄像机，通过以当前摄像机为中心、平均距离为半径且在场景深度的平均值±各场景深度的离散程度内分布的扇环区域来描述所述可视区域。

可选地，所述特征数据包括第二特征数据，所述第二特征数据包括每一关键图像帧中的特征点的数量，并且所述第二评测参数为所述第二特征数据；所述特征数据包括第三特征数据，所述第三特征数据包括每一关键图像帧中的各个网格的特征点的平均响应值，并且所述第三评测参数通过离散化统计所述第三特征数据作为该网格纹理丰富与否的判断条件来统计纹理丰富的网格的数量而获得；以及所述特征数据包括第四特征数据，所述第四特征数据包括每一关键图像帧中的各特征点的场景深度的平均值，并且所述第四评测参数通过判断所述第四特征数据是否低于预定阈值而获得。

可选地，所述阈值条件为β*avg_score，其中avg_score为同一摄像机组合的评测结果的平均值，β＞0.5。

根据本发明的一个方面，提供一种用于多摄像机即时定位与地图构建的三角化方法，包括如下步骤：将由k个摄像机中的同一摄像机和不同摄像机在相邻关键帧的拍摄结果作为n组摄像机组合，其中k和n为大于1的整数；对由每一组摄像机组合所拍摄到的相邻关键图像帧进行初始三角化，并获得初始三角化成功率的历史排序数据，其中，所述历史排序数据通过如下方式获得：对在相邻时刻中由n组摄像机组合所采集到的全部相邻关键图像帧，提取特征点并匹配该特征点，进行初始三角化；根据初始三角化结果获得每一组摄像机组合的初始三角化成功率；和按照初始三角化成功率对所述n组摄像机组合进行排序并将本次排序结果累积保存为历史排序数据；基于所述历史排序数据筛选出m个摄像机组合，仅选取m组摄像机组合进行三角化，其中，k≤m＜n，从而实现基于历史信息选择部分摄像机组合进行三角化。

可选地，所述三角化方法还包括以下步骤：根据跟踪情况判断所述m组摄像机组合是否需要重新选取；若需要重新选取，更新历史排序数据。

可选地，根据跟踪情况判断所述m组摄像机组合是否需要重新选取还包括：计算跟踪质量；判断跟踪质量是否符合阈值条件，若跟踪质量大于或等于预定阈值，则在后续过程中每次都选取所述m组摄像机组合进行三角化，若跟踪质量低于预定阈值，则重新选取m组摄像机组合进行三角化。

可选地，所述历史排序数据通过对在长期时间段内的长期排序数据和/或在短期时间段的短期排序数据以预定权重比例计算而得。

可选地，所述三角化成功率根据该组摄像机组合之间的成功三角化点数D获得。

可选地，还包括根据从所述特征点的相关特征数据中提取的至少一项评测参数作为调整参数，对按照三角化成功率累积保存的所述历史排序数据进行进一步调整。

可选地，所述至少一项评测参数还包括第二至第四评测参数中的至少一个：该组摄像机组合中的各摄像机所拍摄的关键图像帧中的特征点数量A21和A22、纹理丰富程度A31和A32、以及场景遮挡程度A41和A42。

可选地，通过如下公式(3)对所述历史排序数据进行调整，

k1*min{A21，A22}+k2*min{A31，A32}+k3*min{A41，A42}+k4*D，其中，k1,k2,k3≥0,k4>0……(3)。

可选地，所述特征数据包括第二特征数据，所述第二特征数据包括每一关键图像帧中的特征点的数量，并且所述第二评测参数为所述第二特征数据；所述特征数据包括第三特征数据，所述第三特征数据包括每一关键图像帧中的各个网格的特征点的平均响应值，并且所述第三评测参数通过离散化统计所述第三特征数据作为该网格纹理丰富与否的判断条件来统计纹理丰富的网格的数量而获得；并且所述特征数据包括第四特征数据，所述第四特征数据包括每一关键图像帧中的各特征点的场景深度的平均值，并且所述第四评测参数通过判断所述第四特征数据是否低于预定阈值而获得。

可选地，所述跟踪质量通过跟踪的地图点的数量以及在跟踪过程中地图点与成像平面的相应特征点的平均重投影误差来判断。

根据本发明，提供一种多摄像机即时定位与地图构建方法，其中多个摄像机安装于运动体上以协助运动体进行定位，所述方法包括：跟踪：跟踪并且获取摄像机当前的位姿；和地图构建：通过根据上述三角化方法自适应地选取所有摄像机组合中的部分摄像机组合对图像中新匹配的特征点进行三角化，以将新的地图点加入到地图中。

根据本发明，提供一种智能运动体，包括：多个摄像机，多个摄像机安装于运动体上以协助运动体进行定位；存储器，用于存储数据；和控制器，其中，所述控制器控制所述运动体采用根据上述多摄像机即时定位与地图构建方法进行定位。

如上所述，根据本发明的部分摄像机组合的方案并不需要在所有可能的摄像机组合之间做特征匹配与三角化，因此可以大大提高多摄像机SLAM在地图构建过程中的三角化的效率。

附图说明

从下面结合附图对本发明实施例的详细描述中，本发明的这些和/或其它方面和优点将变得更加清楚并更容易理解，其中：

图1示出了根据本发明的采用多个摄像机的SLAM***的三角化原理示意图。

图2示出了根据本发明的采用多个摄像机的SLAM***的同一摄像机和不同摄像机特征对应点的原理示意图。

图3示出了根据本发明的用于SLAM的三角化方法的基本原理框图。

图4示出了根据本发明的实时组合选择法的基本原理框图。

图5示出了根据本发明的历史组合选择法的基本原理框图。

具体实施方式

为了使本领域技术人员更好地理解本发明，下面结合附图和具体实施方式对本发明作进一步详细说明。

在详细描述各个实施例之前，给出本文中使用的术语的解释。

基于特征的SLAM方法(feature-based SLAM)：通过将输入的图像转化为一个图像帧(Frame)，然后对该图像均匀划分网格(grid)，形成一组图像块(image patches)，然后分别对每个图像块提取特征点(feature)，用于跟踪，如只采用一个摄像机的ORB-SLAM。

关键帧(Key Frame)：为了提高程序的运行效率和存储效率，SLAM不会保存所有的图像帧，而是选择其中关联地图点较多的图像帧保存下来，用于地图构建和优化，这样的帧称为关键帧。

跟踪(tracking)：通过特征点法等方法跟踪摄像机当前的位姿，对比采集到的图像与局部地图，求解到当前摄像机的位姿。

地图构建(mapping)：通过三角化图像中新匹配的特征点，将新的地图点加入到地图中。

采用多个摄像机的SLAM***：多个摄像机被固定在例如车辆上，在某个时刻，车辆将通过各个摄像机同时采集到多张图像，输入到程序中作为一个多图像帧。如图1，C1、C2、Ci分别为固定在车身上的各个摄像机自身的坐标系，坐标系的原点即为摄像机的光心，V为车辆的坐标系，摄像机与车辆之间固定连接，例如刚性连接。

多图像关键帧(Multi-Key Frame)：与只采用一个摄像机的ORB-SLAM类似，多摄像机SLAM***不会将所有的多图像帧存入***，而是只保留具有代表性的(例如关联地图点较多)的多图像帧保留下来。

特征对应点(feature correspondence)：在单摄像机SLAM***中，在时刻k-1与时刻k***所采集到的两帧图像都由同一个摄像机所采集，在三角化过程中创建新的地图点时，所使用的特征对应点分别来自两帧图像，因为两帧图像是同一个摄像机所拍摄，所以称为同一摄像机对应点；而在多摄像机SLAM中，如图2，特征对应点除了可能来自于同一摄像机中，如P1所指示的，也可能来自于不同的摄像机中，如P2所指示的。来自于同一摄像机的特征对应点称为同一摄像机对应点(intra-camera correspondence)，而来自于不同摄像机的特征对应点称为不同摄像机对应点(inter-camera correspondence)。而三角化既可以使用同一摄像机之间的对应特征点，也可以采用不同摄像机之间的对应特征点。

如背景技术所述，在多摄像机SLAM的地图构建过程中，若考虑所有摄像机组合，会导致三角化效率较低，则会因此而严重拖慢地图构建线程的工作效率，进而导致多摄像机SLAM在运动速度较快的平台(如无人机或自动驾驶车辆)上无法高效运行。实际上，若物体运动(尤其是自动驾驶场景中的运动)方向相对稳定，且摄像机是固定的，所有摄像机组合匹配会带来极大的冗余并带来了不必要的计算负担。以Oxford robotcar dataset中的摄像机布局为例，其中左面、右面和后面分别有一个鱼眼摄像机，车辆在向前运动时，除了必须要考虑的同种摄像机的组合(即左-左，右-右，后-后)之外，仅需要考虑当前帧(k时刻)后视摄像机与参考帧(k-1时刻)中左右摄像机之间的共视组合，即左-后，右-后两种组合。

如此，本发明提出一种仅通过部分摄像机组合进行地图构建的技术方案。这种方案的原理在于在自动驾驶等运动方向与速率相对恒定的应用场景中，多摄像机SLAM在地图构建过程中并不需要在所有可能的摄像机组合之间做特征匹配与三角化来创建新的地图点，因为车辆等平台的运动方向与速率相对稳定，所以当前位置与上一个关键帧位置的多摄像机之间的共视关系相对稳定，只需要选取共视较多的摄像机组合进行特征点匹配和三角化。因此，部分摄像机组合的方案可以大大提高多摄像机SLAM在地图构建过程中的三角化的效率。

本发明的总体技术构思原理，如图3所示，包括如下步骤：

a.摄像机分组步骤：将由k个摄像机中的同一摄像机和不同摄像机在相邻关键帧的拍摄结果作为n组摄像机组合，其中k和n为大于1的整数；

b.评测结果获得步骤：通过对所述拍摄结果进行图像处理或者对所述n组摄像机组合进行三角化，得到用于预测各组摄像机组合的三角化成功率的评测结果；和

c.选取组三角化步骤：基于各组摄像机组合的评测结果，选取m组摄像机组合进行三角化，其中，k≤m＜n。

在上述步骤中，根据参数来源于实时数据还是来源于历史数据，所述评测结果获得步骤可以采用下面的实时组合选择法和历史组合选择法。

一、实时组合选择法

如图4所示，在实时组合选择法中，申请人通过实时针对每组摄像机组合进行如下步骤来获得评测结果：

√对在相邻时刻中由该组摄像机组合所拍摄到的相邻关键图像帧，从每一关键图像帧提取相关的特征数据；

√从所述特征数据中提取至少一项评测参数；和

√根据所述至少一项评测参数进行评估计算，以得到用于预测该组摄像机组合的三角化成功率的评测结果。

如此，取评测结果排序在前的摄像机组合进行三角化。也就是说，每次三角化哪些摄像机组合，都是通过实时计算相关评测参数评估得到的。

特别地，申请人研究并选择三角化成功率的相关参数，以这些相关参数作为衡量预测三角化成功率的指标来选择摄像机组合。同时，由于可以根据每项参数对三角化成功率的影响重要性而对每项参数设定预定权重，然后通过一定的权重计算方式进行评测，可以更精确地将评测结果中满足预定阈值条件的部分组合将作为三角化的目标组合，进行多摄像机三角化。

下面将分为“获取参数”、“评测计算”和“组合选择”三个步骤对该方法进行描述。

一).获取参数

通过建模多摄像机的三角化过程，可以发现三角化成功率至少与以下参数相关：

a.相邻多关键帧的各摄像机之间的共视关系；

b.在车辆当前位置的多关键帧中，各个朝向的摄像机所拍摄的场景纹理的丰富程度；

c.在车辆当前位置不同朝向的摄像机的遮挡程度(被路边距离较近的车辆、行人或者建筑物等遮挡)；

d.摄像机跟踪的特征点数量。

下面的计算将综合考虑上面四种参数以给出三角化成功率的预测。也就是对在相邻时刻中由该组摄像机组合所拍摄到的相邻关键图像帧，从每一关键图像帧提取相关的特征数据，然后从所述特征数据中提取至少一项评测参数。当然，本领域技术人员可以理解，这四种参数仅仅是本发明给出的示例，也可以综合采用其他参数，例如可以组合一些其他参数。这些参数中某些必要参数可以分别单独使用或者组合使用，其他的参数可以作为扩充组合加入到指标的计算中。

下面对将这四种参数作为摄像机三角化成功率的预测指标，对各参数的建模过程解释如下：

1.相邻多关键帧的各摄像机之间的共视面积A1：

本参数需要首先对摄像机的可视区域进行建模。

例如以在街道中行驶的车辆为例，侧视摄像机面向街道的两侧，在城市街道中，由于绿化带以及两侧建筑物的存在，侧视摄像机的可视场景中物体的分布相对集中，因此可以通过测量特征点的平均深度(average depth)粗略估计场景中物体的位置；同时考虑到场景中物体的离散程度，可以通过方差σ描述。相比，对于前后视的摄像机，因为其可视方向指向道路延伸的方向，其可视区域的深度离散值(即方差σ)较大。

因此本方法提出通过摄像机位姿(camera pose)、摄像机视角(camera field-of-view)、视角中场景的平均深度、以及各深度的离散程度σ来刻画摄像机的可视区域。

特别说明的是，对于以上平均深度以及各深度的离散程度的估计可以采用卡尔曼滤波器(Kalman filter)估计的方法。每次三角化之前以当前摄像机所跟踪的特征点的平均深度作为初始值，以当前所跟踪的特征点的深度的方差作为方差估计，并在新的一帧图像中以得到的平均深度值与方差作为观测值更新卡尔曼滤波器。另外，因为在自动驾驶车辆场景中车辆摄像机的高度是相对固定的，因此可视区域可以只采用二维平面区域而不是三维空间区域刻画。更具体地，可视区域采用以相机为中心的三角形区域。对于视角较小的透视摄像机而言，通过以当前摄像机中心为顶点，底边与当前摄像机成像平面平行的等腰三角形区域中到当前摄像机成像平面的垂直距离为场景深度的平均值±场景深度离散程度(即average depth±σ)的梯形区域来描述所述可视区域。为了加快计算，在不损失算法精度的条件下，还可以采用该梯形的外接矩形等方式。对于视角较大的鱼眼摄像机(>180°)，可以采用特征点到摄像机的距离来描述，则可视区域描述为以当前摄像机为中心、平均距离为半径且在场景的平均深度±场景深度离散值(average depth±σ)内分布的扇环区域。为了加速计算，可以将该角度较大的扇环划分为多个角度相对较小的扇环，然后对每个扇环用一个外接梯形或者矩形替代。采用这种估计的好处在于在计算摄像机之间视野重合部分面积的时候更加方便。

基于以上对于可视区域的描述，不同时刻的摄像机共视可以描述为以上不同摄像机可视区域之间的交集，通过计算或者估计区域的重合程度计算得到。进一步地，通过计算交集区域面积并对结果进行排序，就可得到共视区域大小的排序。在仅采用此参数作为三角化成功率的唯一指标时，可依据共视区域大小的排序结果，优先三角化共视程度较高的摄像机组合。

以“左后组合”举例说明，对在k-1时刻由左摄像机以及在k时刻由后摄像机所拍摄的图像提取特征点，并根据以上所述的特征点平均深度±深度离散值分别给出两个摄像机可视区域，并计算两个区域的重合程度，以得到共视区域面积A1。

2.摄像机跟踪的特征点的数量A21和A22

本参数通过统计跟踪线程中的摄像机跟踪的特征点的数量获得。

以“左后组合”举例说明，对在k-1时刻由左摄像机以及在k时刻由后摄像机所拍摄的图像提取特征点，并根据以上所述的方法计算两幅图像中的特征点的数量。

3.物体所处场景中，各摄像机朝向的场景的纹理丰富程度A31和A32

在提取特征点时，因为每个网格提取的特征点数量是固定的，因此对于纹理丰富的区域，其平均响应值会高于纹理不丰富的区域。本参数的计算即以此为原理，通过离散化统计每个网格的中值响应值(或者平均响应值)，作为该网格纹理丰富与否的判断条件，并统计纹理丰富的网格的数量作为当前图像纹理是否丰富的标准。

网格纹理丰富程度的具体的计算例如可以通过如下方式：通过OpenCV等图像计算库计算特征点在图像中的位置、响应值、朝向等信息，然后从OpenCV提取的特征点数据中读取特征点的响应值，计算每个网格中所有特征点的平均响应值即可得到该网格纹理丰富程度。

以“左后组合”举例说明，对在k-1时刻由左摄像机以及在k时刻由后摄像机所拍摄的图像提取特征点，并根据以上所述的方法计算两幅图像中纹理丰富程度。

4.摄像机不同朝向的遮挡程度A41和A42

通过统计场景的各特征点的平均深度(距离)，将低于一定阈值认为出现遮挡，因此通过本参数，可以根据遮挡程度来改变该参数所代表的权重，比如低于预定阈值，表示遮挡程度较高，因此降低三角化的权值。因为在上面参数1的计算共视关系时，也需要计算平均深度，因此可以直接利用1中计算得到的平均深度。这里的遮挡程度根据是否出现遮挡设置为-1(出现遮挡)，否则取0(无遮挡)。

以“左后组合”举例说明，对在k-1时刻由左摄像机以及在k时刻由后摄像机所拍摄的图像提取特征点，并根据以上所述的方法计算两幅图像中的遮挡程度。

二).评测计算

在获取到以上的相关参数之后，根据所述至少一项评测参数进行评估计算，以得到用于预测该组摄像机组合的三角化成功率的评测结果。

通过以下公式(1)评测该摄像机组合三角化成功的概率：

预测可三角化的点数＝摄像机之间的共视面积A1×单位面积内预测可跟踪的特征点数B，其中B＞0……(1)，

其中，摄像机之间的共视面积A1即为上面所述参数1，单位面积内预测可跟踪的特征点数B通过上述参数A1、A21、A22、A31、A32、A41和A42的适当组合获取，其中特征点的数量为第二参数A21和A22、纹理丰富程度为第三参数A31和A32、遮挡程度为第四参数A41和A42。

下面给出一种可能的参数组合方式，假设被预测的摄像机组合中所包含的摄像机分别为C1、C2(例如k-1时刻的左摄像机以及k时刻的后摄像机两者)，则单位面积内预测可跟踪的特征点数B通过以下公式(2)获得：

单位面积内预测可跟踪特征点数B＝k1*min{A21，A22}+k2*min{A31，A32}+k3*min{A41，A42}，其中，k1,k2,k3均大于等于0

……(2)。

这里的单位面积内预测可跟踪的特征点数B实际是通过根据当前摄像机组合获取的若干参数作出的一种预测，因此通过适当设置k1、k2、k3，可使得上式能够正确反应单位面积中可追踪特征点的数量，继而通过将上式与共视面积相乘，即可获得所预测三角化地图点数。

如上所述，通过上述步骤“评测计算”，对所有摄像机组合(包括同一摄像机之间与不同摄像机之间)进行评测，之后对评测得到的结果作为预测值进行排序，作为三角化成功率的参考依据。

三).组合选择

在上述评测计算基础上，进一步以如下两种方式之一来选取合适的摄像机组合。

方式一：假定摄像机的数量为c而摄像机组合的数量为n，可将上面排序中所获得的排名靠前的c+α个组合作为三角化的目标组合。根据实际情况α可取0,1,…(n-c-1)等数值，这样以小于全部摄像机组合数量的组合作为三角化组合。

方式二：计算同一摄像机三角化评测值的平均值avg_score，以β*avg_score作为阈值判断进行筛选，将上面方式一中选取的c+α个摄像机组合中低于阈值的组合去掉。在筛选步骤中，如果最终满足条件的摄像机组合小于c，则取前c个摄像机组合做三角化。根据实际情况中，β可取适当的值，比如0.9。

最终，将满足以上条件的摄像机组合将作为三角化的目标组合，进行多摄像机三角化。

需要说明的是，在上面实时组合选择法的四种参数中，摄像机之间的共视程度为必要参数，其余参数可以根据不同应用场景的测试结果适当组合。或者，也可以仅使用共视程度这一参数时，而将单位面积内预测可跟踪特征点数设置为常数(例如置为1等)。另外，如果除了共视程度之外的三种参数中，仅使用遮挡程度时，则考虑到该参数取值为-1或0，总是小于或等于0，因此可以用一个正常数与其相加，比如使用10或者其他大于1的正数与其相加，使其取值为正数，以使得评测结果保持为正值。

二、历史组合选择法

为了提高效率，除了上述实时组合选择法，本发明还提出一种历史组合选择法。

在历史组合选择法中，通过在初始对所有摄像机组合进行三角化后，对所有摄像机组合的成功三角化的点数进行历史累积排名，并存储为动态队列；该动态队列中的排名可以作为后续三角化步骤中仅对部分摄像机组合进行三角化的参考，也就是取动态队列中排名靠前的组合进行三角化。这里以三角化成功点数作为三角化成功率的评测参数。

在跟踪稳定时，也就是跟踪质量高于阈值时，保持该组合不变，直到车辆行驶状况发生变化导致跟踪不稳定，也就是跟踪质量低于阈值，则重新采用全部组合，重新统计三角化成功率，并且选取成功率较高的组合。

具体方法如图5所示，包括步骤：

a.在初始时，针对所有摄像机组合，进行如下步骤：

对在相邻时刻中由n组摄像机组合所采集到的全部相邻关键图像帧，提取特征点并匹配该特征点，进行三角化；

依据三角化结果，获得每一组摄像机组合的三角化成功率；

根据三角化成功率对所述n组摄像机组合进行排序并将本次排序结果累积保存为历史排序数据，其中，所述历史排序数据作为用于预测各组摄像机组合的三角化成功率的评测结果。

b.根据历史排序数据，针对排名在前的部分摄像机组合进行后续三角化。

c.判断跟踪质量，若跟踪质量低于阈值，则重复步骤a-b，若跟踪质量高于阈值，则重复步骤b，如此一直循环进行。

优选地，历史累积排名可细分为全局窗口和当前本地窗口中的组合排名，采用排名靠前的组合进行三角化。这种历史组合选择法是考虑到物体或者车辆等平台的运动具有长期内相对稳定、但也有短期内变换的特点，提出采用长期数据和短期数据结合考虑的方式，即，同时维护长期历史数据与短期历史数据，其中长期历史数据包括较长时间内摄像机组合成功三角化的全部历史数据，而短期历史数据则只包括较短时间内的数据，以迅速适应弯道等短期内发生的变化。

相比实时组合选择法，历史组合选择法的目的在于根据历史数据快速选取摄像机组合，因此下面的示例中将主要采用三角化成功点数作为参数进行三角化成功率的评价，以快速累积历史数据并且综合历史数据地选取合适的三角化摄像机组合。

评价某个摄像机组合的三角化成功率的参数可以包括：

1)作为基础参数，统计该摄像机组合成功三角化点的数量，并以此为基础进行综合排序。具体地，通过比较点数，即三角化成功点数较多的组合，认为三角化成功率较高，这样的三角化成功点数是能够实时获取而不需要经过复杂计算的数据，因此能够提供高效便捷的评价途径。

2)进一步地，在需要进一步提高三角化成功率的预估精度的情况下，可以选择其他的参数作为调整参数，例如假设该组合包含C1,C2两个摄像机，则该两个摄像机的组合所跟踪的特征点数量的较小值min{c1所跟踪特征点数量,c2所跟踪特征点数量}，场景丰富程度的较小值min{c1纹理丰富程度,c2纹理丰富程度}，以及场景遮挡程度等参数。这些参数都类似于上面针对实时组合选择法中所描述的方式得到，即作为第二参数的特征点的数量A21和A22、作为第三参数的纹理丰富程度A31和A32、作为第四参数的遮挡程度A41和A42，在此不再赘述。这样，上面提及的成功三角化点的数量作为基础参数，摄像机所跟踪特征点的较小值等作为调整参数，后者可以根据不同应用场景的测试结果适当组合使用，以类似于实时组合选择法中的组合方法加入到预测三角化成功率的计算和选择中，对预测结果做进一步的优化。这种方法类似于历史选择法和实时选择法的组合。

对于跟踪的质量，一般是指跟踪的稳定程度和准确程度，可以通过跟踪的地图点的数量以及在跟踪过程中地图点与成像平面相应特征点的平均重投影误差来判断，跟踪的地图点数量能与之前跟踪的数量保持相当并且数量比较多，以及平均重投影误差比较小，都是跟踪质量高的体现。

当然，本领域技术人员可以理解，在场景稳定的情况下，并不需要反复选取不同的摄像机组合，因此，根据跟踪质量判断所述m组摄像机组合是否需要重新选取的步骤并不是必要的，可以在第一次选择后就固定使用m组摄像机组合。另外，判断方式也并不局限于符合跟踪质量的阈值条件，也可以以其他标准选择m组摄像机组合，例如是否达到预定跟踪时长等等。

另外，所述历史排序数据通过对在长期时间段内的长期排序数据和/或在短期时间段的短期排序数据以预定权重比例计算而得。例如，短期排序数据占比80％，长期排序数据占比20％。

这里注意的是，在历史组合选择法中，优先选取比较容易实时获取而不需要经过复杂计算的三角化成功点数作为基础参数，因为这样可以更迅速而稳定地维护长期数据和短期数据，以便在下一次三角化之前就将前面三角化的成功率评价完成，并以此为数据基础累积长期和短期数据，确定所选取的摄像机组合。这里，对实时组合选择法和历史组合选择法彼此的差异和优点进行横向比较：

实时组合选择法更重视三角化的实时成功率，依据当前摄像机组合的实时数据来预测三角化实时成功率从而选择合适的摄像机组合。具体地，实时组合选择法是在每次获取了各个组合的实时数据之后，在对该组合进行三角化之前，依据这些实时数据预测本次摄像机组合的三角化的成功率而筛选部分摄像机组合进行三角化，因此相比历史组合选择法，实时摄像机组合的选择结果更精确，适合精度优先的应用场景。

相比，历史组合选择法，更重视三角化的历史成功率，因此以积累三角化的成功率的历史数据为基础来选择合适的摄像机组合。具体地，历史组合选择法则是在每次对该组合进行三角化之前，就用之前短期和长期的摄像机组合三角化成功率来筛选摄像机组合进行三角化就可以了，因此，相比实时组合选择法，历史摄像机组合的选择结果相对粗糙，但是对资源的占用更少并且运算效率更高，适合效率优先的应用场景。

也就是说，两者在参数的选择、预测三角化成功率的方式以及预测三角化成功率的时间点方面均有差异，也各有各的优点。本领域技术人员可以根据具体应用场景选择合适的方法进行三角化，当然两者也可以综合使用，兼顾效率和精度两者。

需要说明的是，附图中按某顺序显示了各个步骤，并不表示这些步骤只能按照显示或者描述的顺序执行，只要不存在逻辑矛盾，步骤执行顺序可以不同于所显示的。

以上已经描述了本发明的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (21)

1.一种用于多摄像机即时定位与地图构建的三角化方法，包括如下步骤：

将由k个摄像机中的同一摄像机和不同摄像机在相邻关键帧的拍摄结果作为n组摄像机组合，其中k和n为大于1的整数；

对由每一组摄像机组合所拍摄到的相邻关键图像帧进行图像处理，得到用于预测各组摄像机组合的本次三角化成功率的评测结果，其中，所述评测结果通过如下方式获得：

从每一关键图像帧提取相关的特征数据；

根据所述特征数据计算至少一项评测参数；和

根据所述至少一项评测参数进行评估计算，以得到用于预测该组摄像机组合的三角化成功率的评测结果；以及

基于各组摄像机组合的评测结果，通过判断评测结果是否满足预定阈值条件和/或通过评测结果的大小排序来筛选出m组摄像机组合，仅选取所述m组摄像机组合进行三角化，其中，k≤m＜n。

2.根据权利要求1所述的三角化方法，其中，

所述至少一项评测参数至少包括第一评测参数，所述第一评测参数为该组摄像机组合之间的共视面积A1，并且，通过如下公式(1)得到评测结果，

预测可三角化的点数P＝该组摄像机组合之间的共视面积A1×单位面积内预测可跟踪的特征点数B，其中B＞0…(1)，

其中，通过预测可三角化的点数P来预测摄像机组合的三角化成功率。

3.根据权利要求2所述的三角化方法，其中，

所述至少一项评测参数还包括第二至第四评测参数中的至少一个：该组摄像机组合中的各摄像机所拍摄的关键图像帧中的特征点数量A21和A22、纹理丰富程度A31和A32、以及场景遮挡程度A41和A42。

4.根据权利要求3所述的三角化方法，其中，通过如下公式(2)得到单位面积内预测可跟踪特征点数B，

单位面积内预测可跟踪特征点数B＝k1*min{A21，A22}+k2*min{A31，A32}+k3*min{A41，A42}，其中，k1,k2,k3≥0……(2)。

5.根据权利要求2所述的三角化方法，其中，

所述特征数据包括第一特征数据，所述第一特征数据包括根据由所述相邻关键图像帧的特征点计算得到的各摄像机位姿、各摄像机视角、每一关键图像帧中的各特征点的场景深度的平均值、以及各特征点的场景深度的离散程度，并且

所述第一评测参数通过根据所述第一特征数据计算得到的该组摄像机组合中的每一个摄像机所拍摄的场景的可视区域并且计算所述可视区域的重合程度而得到。

6.根据权利要求5所述的三角化方法，其中，采用卡尔曼滤波来估算所述各特征点的场景深度的平均值以及各场景深度的离散程度。

7.根据权利要求5所述的三角化方法，其中，

对于透视摄像机，通过以当前摄像机中心为顶点，底边与当前摄像机成像平面平行的等腰三角形区域中到当前摄像机成像平面的垂直距离为场景深度的平均值±场景深度离散程度的梯形区域来描述所述可视区域，并且，

对于鱼眼摄像机，通过以当前摄像机为中心、平均距离为半径且在场景深度的平均值±各场景深度的离散程度内分布的扇环区域来描述所述可视区域。

8.根据权利要求4所述的三角化方法，其中，

所述特征数据包括第二特征数据，所述第二特征数据包括每一关键图像帧中的特征点的数量，并且所述第二评测参数为所述第二特征数据；

所述特征数据包括第三特征数据，所述第三特征数据包括每一关键图像帧中的各个网格的特征点的平均响应值，并且所述第三评测参数通过离散化统计所述第三特征数据作为该网格纹理丰富与否的判断条件来统计纹理丰富的网格的数量而获得；以及

所述特征数据包括第四特征数据，所述第四特征数据包括每一关键图像帧中的各特征点的场景深度的平均值，并且所述第四评测参数通过判断所述第四特征数据是否低于预定阈值而获得。

9.根据权利要求1所述的三角化方法，其中，

所述阈值条件为β*avg_score，其中avg_score为同一摄像机组合的评测结果的平均值，β＞0.5。

10.一种用于多摄像机即时定位与地图构建的三角化方法，包括如下步骤：

将由k个摄像机中的同一摄像机和不同摄像机在相邻关键帧的拍摄结果作为n组摄像机组合，其中k和n为大于1的整数；

对由每一组摄像机组合所拍摄到的相邻关键图像帧进行初始三角化，并获得初始三角化成功率的历史排序数据，其中，所述历史排序数据通过如下方式获得：

对在相邻时刻中由n组摄像机组合所采集到的全部相邻关键图像帧，提取特征点并匹配该特征点，进行初始三角化；

根据初始三角化结果获得每一组摄像机组合的初始三角化成功率；和

按照初始三角化成功率对所述n组摄像机组合进行排序并将本次排序结果累积保存为历史排序数据；

基于所述历史排序数据筛选出m个摄像机组合，仅选取m组摄像机组合进行三角化，其中，k≤m＜n。

11.根据权利要求10所述的三角化方法，其中，还包括以下步骤：

根据跟踪情况判断所述m组摄像机组合是否需要重新选取；

若需要重新选取，更新历史排序数据。

12.根据权利要求11所述的三角化方法，其中，根据跟踪情况判断所述m组摄像机组合是否需要重新选取还包括：计算跟踪质量并判断跟踪质量是否符合阈值条件，若跟踪质量大于或等于预定阈值，则在后续过程中每次都选取所述m组摄像机组合进行三角化，若跟踪质量低于预定阈值，则重新选取m组摄像机组合进行三角化。

13.根据权利要求10所述的三角化方法，其中，

所述历史排序数据通过对在长期时间段内的长期排序数据和/或在短期时间段的短期排序数据以预定权重比例计算而得。

14.根据权利要求10所述的三角化方法，其中，所述三角化成功率根据该组摄像机组合之间的成功三角化点数D获得。

15.根据权利要求10所述的三角化方法，其中，

还包括根据从所述特征点的相关特征数据中提取的至少一项评测参数作为调整参数，对按照三角化成功率累积保存的所述历史排序数据进行进一步调整。

16.根据权利要求15所述的三角化方法，其中，

所述至少一项评测参数还包括第二至第四评测参数中的至少一个：该组摄像机组合中的各摄像机所拍摄的关键图像帧中的特征点数量A21和A22、纹理丰富程度A31和A32、以及场景遮挡程度A41和A42。

17.根据权利要求16所述的三角化方法，其中，通过如下公式(3)对所述历史排序数据进行调整，

k1*min{A21，A22}+k2*min{A31，A32}+k3*min{A41，A42}+k4*D，其中，k1,k2,k3≥0,k4>0……(3)。

18.根据权利要求17所述的三角化方法，其中，

所述特征数据包括第二特征数据，所述第二特征数据包括每一关键图像帧中的特征点的数量，并且所述第二评测参数为所述第二特征数据；

所述特征数据包括第三特征数据，所述第三特征数据包括每一关键图像帧中的各个网格的特征点的平均响应值，并且所述第三评测参数通过离散化统计所述第三特征数据作为该网格纹理丰富与否的判断条件来统计纹理丰富的网格的数量而获得；并且

所述特征数据包括第四特征数据，所述第四特征数据包括每一关键图像帧中的各特征点的场景深度的平均值，并且所述第四评测参数通过判断所述第四特征数据是否低于预定阈值而获得。

19.根据权利要求12所述的三角化方法，其中，

所述跟踪质量通过跟踪的地图点的数量以及在跟踪过程中地图点与成像平面的相应特征点的平均重投影误差来判断。

20.一种多摄像机即时定位与地图构建方法，其中多个摄像机安装于运动体上以协助运动体进行定位，所述方法包括：

跟踪：跟踪并且获取摄像机当前的位姿；和

地图构建：通过根据权利要求1-19中的任一项所述的三角化方法自适应地选取所有摄像机组合中的部分摄像机组合对图像中新匹配的特征点进行三角化，以将新的地图点加入到地图中。

21.一种智能运动体，包括：

多个摄像机，多个摄像机安装于运动体上以协助运动体进行定位；

存储器，用于存储数据；和

控制器，

其中，所述控制器控制所述运动体采用根据权利要求20所述的多摄像机即时定位与地图构建方法进行定位。

Images