CN105869136A - 一种基于多摄像机的协作式视觉slam方法 - Google Patents

Abstract

本发明提供了一种基于多摄像机的协作式视觉SLAM方法，具体涉及一种在动态环境中使用多个摄像机的协作式视觉SLAM方法。本发明所述方法允许摄像机间的相对位置和方向随时间变化，使得多个摄像机可独立地移动并可以安装在不同的平台上，解决了关于摄像机姿态估计、地图点分类和摄像机群组管理等问题，使得该方法可以在动态场景中稳健地工作，并且可以重建移动物体的三维轨迹。与现有基于单摄像机的SLAM方法相比，本发明的方法更加精确和健壮，可以适用于微型机器人和可穿戴的增强现实。

Description

一种基于多摄像机的协作式视觉SLAM方法

技术领域

本发明涉及一种同步定位与地图创建(SLAM，simultaneous localization andmapping)方法，尤其涉及一种在动态环境中带有多个移动摄像机的协作式视觉SLAM方法。

背景技术

传统的视觉SLAM方法主要有两种，基于单摄像机的视觉SLAM方法和基于多摄像机的视觉SLAM方法。

基于单摄像机的视觉SLAM方法：主要有基于structure-from-motion(SFM)技术和一个贝叶斯推理方法，可通过扩展的卡尔曼滤波来解决这两种类型的方法。基于SFM的方法在单位计算时间内能产生更加精确的结果，而基于滤波的方法在处理资源有限的情况下会更加有效。然而，这些方法通常不考虑动态场景。近期提出的一些方法，应用多体的SFM来处理动态的环境。然而，这种方法只适用于刚性移动物体，并且移动点的3D重建是一个模棱两可的扩展。

基于多摄像机的视觉SLAM方法：Nister提出了一种带立体声钻机的视觉测程系统。他们的系统更像是一个在基于SFM的单摄像机SLAM系统的基础上额外加了一个摄像机，这个额外的摄像机能在每帧生成地图点。为了解决这个问题，帕斯等将近处和远处的三维点进行分离，只用远处的点去评估摄像机转动。Kaess和Dellaert为了结合全向视觉和单目视觉的优势安装了多个面向不同方向的摄像机。Castle将多个摄像机自由的分布在一个静态环境下，其中每个摄像机由一个独立的基于单摄像机的SLAM系统处理。可通过将地图的特征点与其他地图点进行校准得到的不同地图来对一个摄像机进行定位。然而，这些方法仍然只是关注于静态场景，而没有充分利用多摄像机的优势，此外摄像机的相对位置通常是固定的。

现有的基于计算机视觉的SLAM系统，主要关注于静态环境下带有一个摄像机的导航。然后事实上，真实的世界都是移动的物体。尽管有比较好的方法能通过将动态的点视为离散值进行检测和丢弃。然而，当移动点的部分比较大时，传统的SLAM方法往往会失败。另外在动态环境下，对于碰撞检测和路径规划等任务，重建移动物体的三维轨迹往往很重要。

发明内容

本发明的目的在于针对上述技术存在的缺陷，提出了一种在动态环境中使用多个摄像机的协作式视觉SLAM方法，该方法中，允许摄像机间的相对位置和方向随时间变化，满足多个摄像机可独立地移动并可以安装在不同的平台上，解决关于姿态估计、映射和摄像机群组管理等问题，使得该方法可以在动态场景中稳健地工作，并且可以重建移动物体的三维轨迹。与现有基于单摄像机的SLAM方法相比，本发明的方法更加精确和健壮，可以适用于微型机器人和可穿戴的增强现实。

本发明是通过如下方式实现的：一种基于多摄像机的协作式SLAM方法，包括以下步骤：

1、一种基于多摄像机的协作式视觉SLAM方法，其特征在于：包括以下步骤：

1)初始化系统时假定所有的摄像机观测相同的初始场景，使用Kanade-Lucas-Tomasi(KLT)跟踪器对系统中摄像机的每一帧进行特征点的监测和跟踪。

2)将上述经过监测和跟踪的特征点送到系统四大组件：

2.1)摄像机姿态估计组件：该组件通过在每一帧将3D地图点与2D图像特征进行校准来计算摄像机的姿态，过程如下：

2.1.1)摄像机内参姿态估计：

a)如果摄像头的内部参数是已知的，通过最小化重投影误差，来计算摄像头姿态Θ＝(R，t)，公式如下：

其中，是三维点M_i的图像投影，m_i是与M_i校准的图像特征点，||·||计算两个图像点的距离，i是图像点的索引；M估计是Tukey biweight函数，定义如下：

$\mathrm{\ρ}\left(x\right)=\left\{\begin{array}{cc}\frac{t^2}{6(1-{[1-{\left(\frac{x}{t}\right)}^2]}^3)},& \mathrm{if}\left|x\right|\≤t,\\ \frac{t^2}{6},& \mathrm{otherwise}.\end{array}\right.$

b)如果特征点的监测和跟踪的误差服从高斯分布摄像头姿态公式通过迭代加权最小二乘法进行最小化，其中Θ根据摄像机在前一帧的姿态进行初始化。

2.1.2)摄像机外参姿态估计：每个摄像头独立工作，同时使用静态和动态的点来同步获取所有摄像机的姿态，公式如下：

此处，c表示摄像机索引，S和D表示“静态”和“动态”地图点集合；表示c摄像机的第i个可见的地图点。

2.2)三维地图点构建，用一个高斯函数表示三维地图点位置的一个可能的分布，协方差矩阵表示每个三维地图点位置的不确定性，M_i表示三角化的位置，通过测量每个三维地图点位置的不确定性来生成三维地图点并协助进行三维地图点的校准和迭代更新。

2.3)“点的分类”组件根据步骤(2.2)分析三维地图点位置的一致性，把每一帧的地图点分为动态和静态的点，具体方法如下：用“错误”点表示由不正确的关系生成的映射点，需要进一步观察的点标记为中间状态“不确定”。

2.3.1)首先把所有的点都视为静态的，检查每一帧里所有“静态”点的重投影误差：一个静态地图点的投影位置服从高斯分布m_i和其相应特征点间的马氏距离应该小于θ；如果所跟踪的特征点的马氏距离比较大，那么这个地图点可能是“动态的”或者“错误的”，标记为“不确定”点并在下一步继续分类。

2.3.2)对不同摄像机的同一帧的三维位置和跟踪特征点重新进行三角测量：如果所有特征点跟M_i的投影的马氏距离都小于θ，则认为这个地图点是“动态的”；否则，认为它是一个摄像机外参离群点，视为由于不正确的特征匹配引起的“错误”点。

2.3.3)由于动态点的三维位置在点的分类时是随着时间自然变化的，因此，本方法能够产生移动点的三维轨迹；如果物体停止运动，动态的点就会成为静态的点，将一个动态点当前的三维位置投影到前一帧，如果对于连续帧的最小值N_min接近它的轨迹特征点(马氏距离＜θ)，则认为这个点是“静态的”。

2.4)“摄像机分组”组件使用步骤(2.3)中经过分类的动态和静态的点来估计所有具有视角重叠的摄像机的姿态，将具有视角重叠的摄像机分成同一组并协同工作来进行映射和定位；当摄像机相遇或分离时，摄像机组可以合并和拆分。

2.4.1)合并：如果两个摄像机组群相遇并且具有相同的视角重叠，这两个组群将会被合并；由于漂移误差，在群组合并的过程中，通过纠正摄像机姿态和地图点来形成一个单一的全局的一致的地图。

2.4.2)拆分：根据为每一个映射特征点存储的一个指向相应三维位置的指针，快速的计算出摄像机i，j之间共同地图点N_ij的数量；建立一个无向图，无向图中的节点表示摄像机，如果N_ij＞0，以N_ij为边缘加权来连接摄像机i和j，在无向图中的连接组件形成了一个摄像机组群，摄像机外参操作仅应用于同一组群内的摄像机；用最大权重为每个摄像机组群提取一个生成树，只匹配在所选择的生成树的边所连接的摄像机间的特征点；当组群中任一个摄像机移走并且与其他摄像机间没有视角重叠时，这个摄像机组群会将其分离出去，完成摄像机组的拆分。

3)经过上述四个组件协作，实现摄像机的姿态估计、静态点的地图构建及动态点的三维轨迹跟踪。

其中，步骤2.2)中测量三维地图点位置的不确定性的步骤如下：

1)测量三维地图点位置的不确定性时，只考虑特征监测和三角测量的不确定性；假设特征监测的误差服从高斯分布一个三维地图点的位置的不确定性通过协方差表示，公式如下：

${\mathrm{\Σ}}_i={\left(J_i^T{J}_i\right)}^{-1}{\mathrm{\σ}}^2$

其中是在所有视图上将三维地图点映射到二维图像的摄像机投影函数的雅可比，k表示用于三角化的视图的数目；

1.1)当有一个新的地图点的图像监测时，可以通过Kalman增益快速的更新它的三维位置：

其中，计算第n+1帧的的图像投影，Kalman增益计算如下：

$K_i={\mathrm{\Σ}}_i^{\left(n\right)}{\hat{J}}_i^T{({\mathrm{\σ}}^{2}I+{\hat{J}}_i^T{\mathrm{\Σ}}_i^{\left(n\right)}{\hat{J}}_i)}^{-1}$

其中，在处估计出的的雅克比；

1.2)三角测量的不确定性可通过以下公式进行更新：

${\mathrm{\Σ}}_i^{(n+1)}={\mathrm{\Σ}}_i^{\left(n\right)}-K_i{\hat{J}}_i{\mathrm{\Σ}}_i^{\left(n\right)}$

三维地图点生成包括两种方法：“摄像机内参映射”和“摄像机外参映射”。

1)“摄像机内参映射”，根据每个独立的摄像机的特征轨道重建静态地图点；

1.1)如果有足够长的未映射的特征追踪，用这个跟踪的开始帧和结束帧来对一个三维地图点进行三角测量，计算出三维位置；

1.2)根据步骤1.1)计算出的三维位置，通过协方差公式计算协方差；

1.3)检查特征追踪的所有帧的重投影误差，如果投影和特征点间的马氏距离小于所有的帧，一个静态地图点生成；

2)“摄像机外参映射”，通过来自同一个组群内的摄像机的相应的点来生成动态地图点，摄像机外参映射仅被应用到没有映射的特征点；

2.1)通过ZNCC(零均值归一化互相关)在不同的摄像机间匹配图像特征；

2.2)进一步利用现有的地图点的关系作为种子去引导匹配：如果一个点与它最近的种子点之间的视差矢量有很大的不同，那么这对特征点被认为是不匹配的；

2.3)在摄像机间匹配完特征点之后，三角化相应的点生成新的地图点。

三维地图点的校准包括以下步骤：

1)在每一帧，将每个地图点与来自不同摄像机的新的检测图像的特征点连接起来，这些特征点通过特征跟踪与地图点进行校准；

2)进一步处理保留的未映射的特征点，用有效的静态的且在最近的帧N_rec内有相应特征点的地图点对这些未映射的特征点进行校准；将上述有效的地图点投影到图像中，通过ZNCC比较位于投影中心的图像区域和特征点；一旦一个未映射的特征点与一个三维地图点校准，就对这个特征点的三维位置和所有的观测重新进行三角测量，重新计算协方差。

由于漂移误差，当摄像机再次相遇时，不同摄像机组群的三维地图重建是不一致的，因此，在群组合并的过程中，需要纠正摄像机姿态和地图点来形成一个单一的全局的一致的地图；摄像机姿态和地图点纠正方法如下：假设两个摄像机在第一帧分开在第F帧合并，将调整从第2帧到第F帧的所有摄像机的姿态，并且调整这些帧内生成的所有地图点；

1)首先估计在F帧中摄像机间正确的相对姿态：

1.1)在不同组群的摄像机间监测和匹配SURF特征，计算它们的相对姿态；

1.2)使用步骤1.1)中所述相对姿态来引导特征点的匹配，对于每对匹配的特征点，取他们位置的平均值来合并它们的相应的三维地图点；

1.3)对所有在第F帧中的地图点和它们相应的特征点进行BA(光束调整)来完善所有摄像机的姿态；

2)使用在第F帧中更新后的相对摄像机姿态作为硬约束来完善所有的摄像机姿态：

2.1)首先生成一个无向图，其中每个摄像机姿态是一个顶点，同时每个边遵从一个相对的姿态约束；对于每个摄像机，在相邻帧中它的姿态是连接的；对于同一组群中的摄像机，如果摄像机在生成树中是相邻的，那么它们在同一帧中的姿态是连接的；

让p＝1，.....，P，q＝1，...，Q表示不同组群的摄像机；用表示摄像机p在第i帧的姿态，用表示在第i帧中摄像机p和q的相对姿态；其中，表示旋转矩阵和平移向量；α用来计算两个摄像机组群间的全局尺度差异，公式如下：

$T_p^i=\left(\begin{array}{cc}{R}_p^i& t_p^i\\ O^T& 1\end{array}\right),T_{\mathrm{pq}}^i=\left(\begin{array}{cc}{R}_{\mathrm{pq}}^i& {\mathrm{\αt}}_{\mathrm{pq}}^i\\ O^T& 1\end{array}\right)$

2.2)将第F帧的相对姿态视为硬约束，公式如下：

$T_p^F-T_{\mathrm{pq}}^F{T}_p^F=O_{4\×4}$

相当于：

$R_p^F-R_{\mathrm{pq}}^F{R}_p^F=O_{3\×3}$

$t_p^F-R_{\mathrm{pq}}^F{t}_p-{\mathrm{\αt}}_{\mathrm{pq}}^F=O_{3\×1}$

得到(P+Q)×(P+Q-1)/2作为第F帧的相对姿态，本发明中只选用(P+Q-1)作为第F帧的相对姿态。

2.3)将所有的硬约束放在一起，根据下面的公式得到两个线性系统：

U_r ^F＝0 V_t ^F＝0

其中，是在第F帧中所有旋转矩阵的元素的矢量叠加，是一个由第F帧中的所有平移元素和比例因子组成的向量。

3)在第一帧固定摄像机的姿态，除了在第F帧中的相对姿态，将所有其它相对姿态视为软约束： $T_m-T_{\mathrm{mn}}^{\mathrm{old}}{T}_n\≈0$

此处，是根据合并前的SLAM程序得到的m和n之间的相对姿态。

3.1)将所有的软约束放到一起，可以得到两个类似的线性系统：

A_r≈α≠0 Bt≈b≠0

此处，和是所有帧的所有旋转和平移的元素的矢量叠加；注意两个线性系统的右侧不等于0，因为在第一帧中的摄像机姿态是固定的。

4)结合步骤2.3)中硬约束和步骤3.1)中软约束公式，可以得到更新后的摄像机姿态和解决两个比例因子约束线性最小二乘问题，公式如下：

$\begin{array}{ccc}\arg \underset{r}{\min }{\left|\right|A_r-a\left|\right|}^2& s.t.& {\hat{U}}_r=0\end{array}$

$\begin{array}{ccc}\arg \underset{\hat{t}}{\min }{\left|\right|\hat{B}\hat{t}-\hat{b}\left|\right|}^2& s.t.& \hat{V}\hat{t}=0\end{array}$

其中，是t附加了一个尺度因子α是添加零元素之后的增广矩阵和向量。

5)对步骤4)中两个方程的结果进行优化，进一步通过SVD寻找与初始矩阵最相近的旋转矩阵，优化问题转化为一组稀疏矩阵的线性方程；使用CSparse库来解决上述稀疏矩阵的线性方程；摄像机姿态更新之后，地图点的三维位置通过三角化它们相应的特征点跟着更新。

为了提高效率，该方法还采用BA(光束调整)方法对摄像机姿态和重现的地图点进行增量细化：

1)为了提高效率，BA只细化一些被选定的帧的摄像机姿态和从这些帧中重建的地图点，BA方法在一个独立的线程中运行，这个线程对最近的K关键帧进行操作；当连续插入K-1关键帧时，将会调用BA方法。

2)细化其他关键帧姿态时，采取在关键帧固定摄像机姿态，同时使用BA之前的连续帧间的相对姿态作为软约束；然后在保持关键帧姿态不变的同时更新所有摄像机的姿态；姿态细化后，通过对相应特征点进行三角测量来更新其他地图点的三维位置。

本发明有益效果是：

1、该方法允许摄像机间的相对位置和方向随时间变化，使得多个摄像机可独立地移动并可以安装在不同的平台上，这能使得并行计算获得更高的效率，解决关于摄像机姿态估计、地图点的分类和摄像机群组管理等问题，使得该系统可以在动态场景中稳健地工作，并且可以重建移动物体的三维轨迹。

2、与现有基于单摄像机的SLAM方法相比，本发明的方法更加精确和健壮，可以适用微型机器人和可穿戴的增强现实。

3、维持位置的不确定性对于像点的分类和校准这样的地图操作是非常重要的，由于三角测量的不确定性，它们的重建位置也经常会随着时间的变化而改变；利用位置的协方差矩阵，可以更好区分静态点和动态点。

附图说明

图1是本发明所述基于多摄像机的协作式视觉SLAM方法流程图。

具体实施方式

实施例中将特征检测不确定性的标准偏差σ设定为3.0像素；马氏距离的阈值θ设为2.0，以此决定一个特征点是在内围层还是一个孤立点(根据高斯分布有95％的把握)；将用来衡量图像块之间的相似性的ZNCC阈值T_ncc设为0.7；三角化一个特征轨迹的最小帧数N_min设为60；有效地图点缓存的帧率为200；寻找匹配摄像机外参映射的附近种子点的半径设为图像宽度和图像高度中的较大值的10％，并且

下面结合附图对本发明作进一步的描述。

如图1所示，一种基于多摄像机的协作式视觉SLAM方法，包括下步骤：

1)初始化系统时假定所有的摄像机观测相同的初始场景，使用Kanade-Lucas-Tomasi(KLT)跟踪器对系统中摄像机的每一帧进行特征点的监测和跟踪；

2)将上述经过监测和跟踪的特征点送到系统四大组件：

2.1)摄像机姿态估计：该组件通过在每一帧将3D地图点与2D图像特征进行校准来计算摄像机的姿态，过程如下：

2.1.1)摄像机内参姿态估计：

a)如果摄像头的内部参数是已知的，通过最小化重投影误差，来计算摄像头姿态Θ＝(R，t)，公式如下：

其中，是三维点M_i的图像投影，m_i是与M_i校准的图像特征点，||·||计算两个图像点的距离，i是图像点的索引；M估计是Tukey biweight函数，定义如下：

$\mathrm{\ρ}\left(x\right)=\left\{\begin{array}{cc}\frac{t^2}{6(1-{[1-{\left(\frac{x}{t}\right)}^2]}^3)},& \mathrm{if}\left|x\right|\≤t,\\ \frac{t^2}{6},& \mathrm{otherwise}.\end{array}\right.$

b)如果特征点的监测和跟踪的误差服从高斯分布设置ρ(·)中的阈值t为3σ，摄像头姿态公式通过迭代加权最小二乘法进行最小化，其中Θ根据摄像机在前一帧的姿态进行初始化；

2.1.2)摄像机外参姿态估计：每个摄像头独立工作，同时使用静态和动态的点来同步获取所有摄像机的姿态，公式如下：

此处，c表示摄像机索引，S和D表示“静态”和“动态”地图点集合；表示c摄像机的第i个可见的地图点(1为可见，0为不可见)；

2.2)三维地图点构建，用一个高斯函数表示三维地图点位置的一个可能的分布，协方差矩阵表示每个三维地图点位置的不确定性，M_i表示三角化的位置，通过测量每个三维地图点位置的不确定性来生成三维地图点并协助进行三维地图点的校准；

为了方便计算：对于每个特征点，维持一个指针指向它相应的地图点；类似的，对于每一个地图点，也维持一个指针指向其在每个视图中对应的图像特征点，并且存储位于这些特征点中心的局部图像区域；将原始输入图像裁剪为原来的30％，选取一个11*11像素的区域；当一个新的三维地图点生成时或者变得可见时进一步保持跟踪帧的数目，迭代的改进三维地图点的位置；

2.2.1)测量三维地图点位置的不确定性时，只考虑特征监测和三角测量的不确定性；假设特征监测的误差服从高斯分布一个三维地图点的位置的不确定性通过协方差表示，公式如下：

${\mathrm{\Σ}}_i={\left(J_i^T{J}_i\right)}^{-1}{\mathrm{\σ}}^2$

其中是在所有视图上将三维地图点映射到二维图像的摄像机投影函数的雅可比，k表示用于三角化的视图的数目；当有一个地图点的新的图像监测时，可以通过Kalman增益快速的更新它的三维位置：

其中，计算第n+1帧的的图像投影，Kalman增益计算如下：

$K_i={\mathrm{\Σ}}_i^{\left(n\right)}{\hat{J}}_i^T{({\mathrm{\σ}}^{2}I+{\hat{J}}_i^T{\mathrm{\Σ}}_i^{\left(n\right)}{\hat{J}}_i)}^{-1}$

其中，在处估计出的的雅克比；

三角测量的不确定性可通过以下公式进行更新：

${\mathrm{\Σ}}_i^{(n+1)}={\mathrm{\Σ}}_i^{\left(n\right)}-K_i{\hat{J}}_i{\mathrm{\Σ}}_i^{\left(n\right)}$

2.2.2)三维地图点生成包括以下两种方法：a)“摄像机内参映射”，根据每个独立的摄像机的特征轨道重建静态地图点；如果有足够长的(＞M_min帧)未映射的特征追踪(指向地图点的指针为NULL)，用这个跟踪的开始帧和结束帧来对一个三维地图点进行三角测量，计算出三维位置，通过协方差公式计算协方差；检查特征追踪的所有帧的重投影误差，如果投影和特征点间的马氏距离小于所有的帧，一个静态地图点生成；

b)“摄像机外参映射”，通过来自同一个组群内的摄像机的相应的点来生成动态地图点，摄像机外参映射仅被应用到没有映射的特征点；通过ZNCC(零均值归一化互相关)在不同的摄像机间匹配图像特征：为了避免模棱两可的匹配，相应的点仅在距离极线为3σ的区间内被搜索，只考虑匹配；进一步利用现有的地图点的关系作为种子去引导匹配：如果一个点与它最近的种子点之间的视差矢量有很大的不同，那么这对特征点被认为是不匹配的；在摄像机间匹配完特征点之后，三角化相应的点生成新的地图点；所有可能的摄像机对间详细的特征匹配是非常低效的；选择一个图的生成树并且只匹配通过生成树的边连接起来的摄像机间的特征；本方法中，当监测动态点时，每五帧调用一次摄像机外参映射；

2.2.3)三维地图点的校准包括以下步骤：

1)在每一帧中，需要在内部检测中将来自不同摄像机的新的检测图像的特征点连接起来，许多特征点通过特征跟踪与地图点进行校准；

2)进一步处理保留的没有映射的特征点：只考虑有效的静态的且在最近的帧N_rec内有相应特征点的映射点；对于每一个在当前帧中检测到的未映射的特征点，这些有效的映射点来进行校准。将上述有效的映射点投影到图像中，通过ZNCC比较位于投影中心的图像区域和特征点；考虑到地图点位置和特征监测的不确定性，一个地图点M_i的投影位置应该满足高斯分布，其中协方差是在M_i处估计出的图像投影p(·)的雅克比，马氏距离计算如下：

只考虑与m_i的马氏距离最小的特征点m_j，然后检查M_i和m_j之间的ZNCC得分；

为了减少透视变形引起的问题，当为M_i选择图像区域时，挑选一个最近的摄像机的存储；如果mj的ZNCC得分＜J_ncc则丢弃m_j；进一步沿着m_j的特征轨迹穿越回去检查它之前的位置是否也在M_i的投影附近，如果在所有帧中的它们之间的马氏距离都小于θ，那么m_j被M_i校准；

一旦一个未映射的特征点与一个三维地图点校准，就对这个特征点的三维位置和所有的观测重新进行三角测量，重新计算协方差；为了降低计算成本，我们从每个摄像机的特征跟踪中选择其中拥有最大的视角变化的两个观测者来进行三角测量。

2.3)“点的分类”组件通过分析地图点的三角测量的一致性，把每一帧的地图点分为动态和静态的点，用“错误”点表示由不正确的关系生成的映射点，需要进一步观察的点标记为中间状态“不确定”；首先把所有的点都视为静态的，检查每一帧里所有“静态”点的重投影误差：一个静态地图点的投影位置服从高斯分布m_i和其相应特征点间的马氏距离应该小于θ；如果所跟踪的特征点的马氏距离比较大，那么这个地图点可能是“动态的”或者“错误的”，标记为“不确定”点并在下一步继续分类；

然后对不同摄像机的同一帧的三维位置和跟踪特征点重新进行三角测量：如果所有特征点跟M_i的投影的马氏距离都小于θ，则认为这个地图点是“动态的”；否则，认为它是一个摄像机外参离群点，即对于摄像机外参三角测量来说是一个离散点；把外参摄像机离群点视为由于不正确的特征匹配引起的“错误”点；

由于动态点的三维位置在点的分类时是随着时间自然变化的，因此，本方法能够产生移动点的三维轨迹；如果物体停止运动，动态的点就会成为静态的点，将一个动态点当前的三维位置投影到前一帧，如果对于连续帧的最小值N_min接近它的轨迹特征点(马氏距离<θ)，则认为这个点是“静态的”；

2.4)“摄像机分组”组件使用上述动态和静态的点来同步估计所有具有视角重叠的摄像机的姿态，将具有视角重叠的摄像机分成同一组；所述具有视角重叠的摄像机姿态协同工作来进行映射和定位；当摄像机相遇或分离时，摄像机组可以合并和拆分；

其中，拆分过程：根据为每一个映射特征点存储的一个指向相应三维位置的指针，快速的计算出摄像机i，j之间共同地图点N_ij的数量；建立一个无向图，无向图中的节点表示摄像机，如果N_ij＞0，以N_ij为边缘加权来连接摄像机i和j，在无向图中的连接组件形成了一个摄像机组群，摄像机外参操作仅应用于同一组群内的摄像机；用最大权重为每个摄像机组群提取一个生成树，只匹配在所选择的生成树的边所连接的摄像机间的特征点；当组群中任一个摄像机移走并且与其他摄像机间没有视角重叠时，这个摄像机组群会将其分离出去，完成摄像机组的拆分；

合并过程：如果两个摄像机组群相遇并且具有相同的视角重叠，这两个组群将会被合并；为了检测不同组群的摄像机是否有视角重叠，将一个摄像机生成的地图点投影到其他组群中的摄像机的平面图像，如果可见点的数量>我们试验中的摄像机中的所有地图点的30％，并且这些点所跨越的区域比较大>图像区域的70％，则这两个有视角重叠的摄像机所在的组群将会被合并；当摄像机彼此分离，那么每个摄像机组群中的映射和定位将会独立进行；由于漂移误差，当摄像机再次相遇时，不同摄像机组群的三维地图重建是不一致的，因此，在群组合并的过程中，通过纠正摄像机姿态和地图点来形成一个单一的全局的一致的地图。

其中，纠正摄像机姿态和地图点方法如下：假设两个摄像机在第一帧分开在第F帧合并，将调整从第2帧到第F帧的所有摄像机的姿态，并且调整这些帧内生成的所有地图点；

1)首先估计在F帧中摄像机间正确的相对姿态：在不同的组群的摄像机间监测和匹配SURF特征，计算它们的相对姿态；然后使用这些相对姿态来引导特征点的匹配(即在距离极限3σ的区间内寻找相应的点)；对于每对匹配的特征点，取他们位置的平均值来合并它们的相应的三维地图点；

2)使用在第F帧中更新后的相对摄像机姿态作为硬约束来完善所有的摄像机姿态：首先生成一个无向图，其中每个摄像机姿态是一个顶点，同时每个边遵从一个相对的姿态约束；对于每个摄像机，在相邻帧中它的姿态是连接的；对于同一组群中的摄像机，如果摄像机在生成树中是相邻的，那么它们在同一帧中的姿态是连接的；

让p＝1，.....，P，q＝1，...，Q表示不同组群的摄像机；用表示摄像机p在第i帧的姿态，用表示在第i帧中摄像机p和q的相对姿态；其中，表示旋转矩阵和平移向量；α用来计算两个摄像机组群间的全局尺度差异；

$T_p^i=\left(\begin{array}{cc}{R}_p^i& t_p^i\\ O^T& 1\end{array}\right),T_{\mathrm{pq}}^i=\left(\begin{array}{cc}{R}_{\mathrm{pq}}^i& {\mathrm{\&alpha;t}}_{\mathrm{pq}}^i\\ O^T& 1\end{array}\right)$

3)将第F帧的相对姿态视为硬约束，公式如下：

$T_p^F-T_{\mathrm{pq}}^F{T}_p^F=O_{4\&times;4}$

相当于：

$R_p^F-R_{\mathrm{pq}}^F{R}_p^F=O_{3\&times;3}$

$t_p^F-R_{\mathrm{pq}}^F{t}_p-{\mathrm{\&alpha;t}}_{\mathrm{pq}}^F=O_{3\&times;1}$

得到(P+Q)×(P+Q-1)/2作为第F帧的相对姿态，本发明中只选用(P+Q-1)作为第F帧的相对姿态，其中，任意一个都位于摄像机组群的生成树中或者连接两个生成树；将所有的约束放在一起，根据下面的公式得到两个线性系统：

U_r ^F＝0 V_t ^F＝0

其中，是在第F帧中所有旋转矩阵的元素的矢量叠加，是一个由第F帧中的所有平移元素和比例因子组成的向量；

4)在第一帧固定摄像机的姿态，除了在第F帧中的相对姿态，将所有其他的相对姿态视为软约束，对于任意被虚线连接的摄像机m和n，希望通过调整，使得它们的相对姿态的变化比较小，因此，满足如下公式：

$T_m-T_{\mathrm{mn}}^{\mathrm{old}}{T}_n\&ap;0$

此处，是根据合并前的SLAM程序得到的m和n之间的相对姿态；将所有的软约束放到一起，可以得到两个类似的线性系统：

A_r≈α≠0 Bt≈b≠0

此处，和是所有帧的所有旋转和平移的元素的矢量叠加；注意两个线性系统的右侧不等于0，因为在第一帧中的摄像机姿态是固定的；

5)结合硬约束和软约束，可以得到更新后的摄像机姿态和解决两个比例因子约束线性最小二乘问题，公式如下：

$\begin{array}{ccc}\arg \underset{r}{\min }{\left|\right|A_r-a\left|\right|}^2& s.t.& {\hat{U}}_r=0\end{array}$

$\begin{array}{ccc}\arg \underset{\hat{t}}{\min }{\left|\right|\hat{B}\hat{t}-\hat{b}\left|\right|}^2& s.t.& \hat{V}\hat{t}=0\end{array}$

其中，是t附加了一个尺度因子α是添加零元素之后的增广矩阵和向量；两个方程取得结果后，进一步通过SVD(即，将所有的奇数置为1)寻找与初始矩阵最相近的旋转矩阵，优化问题转化为一组稀疏矩阵的线性方程；使用CSparse库)来解决步骤9)中所述稀疏矩阵的线性方程；摄像机姿态更新之后，地图点的三维位置通过三角化它们相应的特征点也会跟着更新。

经过上述四个组件协作，实现摄像机的摄像机姿态估计、静态点的地图构建及动态点的三维轨迹跟踪。

最后为了提高效率，采用BA(光束调整)方法对摄像机姿态和重现的地图点进行增量细化，每当在任何摄像机中被跟踪的特征点的数量有显著的下降时(30％)，为所有的摄像机插入关键帧；为了提高效率，BA只细化一些被选定的帧的摄像机姿态和从这些帧中重建的地图点，BA方法在一个独立的线程中运行，这个线程对最近的K关键帧进行操作；当连续插入K-1关键帧时，将会调用BA方法(即，在两个连续的BA调用过程中，将会有一个共同的关键帧)。

细化其他关键帧姿态时，采取在关键帧固定摄像机姿态，同时使用BA之前的连续帧间的相对姿态作为软约束；即，执行其中，是BA之前摄像机m和n间的相对姿态。然后，在保持关键帧姿态不变的同时更新所有摄像机的姿态；姿态细化后，通过对相应特征点进行三角测量来更新其他地图点的三维位置。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内的所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种基于多摄像机的协作式视觉SLAM方法，其特征在于：包括以下步骤：

1)初始化系统时假定所有的摄像机观测相同的初始场景，使用Kanade-Lucas-Tomasi(KLT)跟踪器对系统中摄像机的每一帧进行特征点的监测和跟踪；

2)将上述经过监测和跟踪的特征点送到系统四大组件：

2.1)摄像机姿态估计组件：该组件通过在每一帧将3D地图点与2D图像特征进行校准来计算摄像机的姿态，过程如下：

2.1.1)摄像机内参姿态估计：

a)如果摄像头的内部参数是已知的，通过最小化重投影误差，来计算摄像头姿态Θ＝(R，t)，公式如下：

其中，是三维点M_i的图像投影，m_i是与M_i校准的图像特征点，||·||计算两个图像点的距离，i是图像点的索引；M估计是Tukey biweight函数，定义如下：

b)如果特征点的监测和跟踪的误差服从高斯分布摄像头姿态公式通过迭代加权最小二乘法进行最小化，其中Θ根据摄像机在前一帧的姿态进行初始化；

2.1.2)摄像机外参姿态估计：每个摄像头独立工作，同时使用静态和动态的点来同步获取所有摄像机的姿态，公式如下：

此处，c表示摄像机索引，S和D表示“静态”和“动态”地图点集合；表示c摄像机的第i个可见的地图点；

2.2)三维地图点构建，用一个高斯函数表示三维地图点位置的一个可能的分布，协方差矩阵表示每个三维地图点位置的不确定性，M_i表示三角化的位置，通过测量每个三维地图点位置的不确定性来生成三维地图点并协助进行三维 地图点的校准和迭代更新；

2.3)“点的分类”组件根据步骤(2.2)分析三维地图点位置的一致性，把每一帧的地图点分为动态和静态的点，具体方法如下：用“错误”点表示由不正确的关系生成的映射点，需要进一步观察的点标记为中间状态“不确定”；

2.3.1)首先把所有的点都视为静态的，检查每一帧里所有“静态”点的重投影误差：一个静态地图点的投影位置服从高斯分布m_i和其相应特征点间的马氏距离应该小于θ；如果所跟踪的特征点的马氏距离比较大，那么这个地图点可能是“动态的”或者“错误的”，标记为“不确定”点并在下一步继续分类；

2.3.2)对不同摄像机的同一帧的三维位置和跟踪特征点重新进行三角测量：如果所有特征点跟M_i的投影的马氏距离都小于θ，则认为这个地图点是“动态的”；否则，认为它是一个摄像机外参离群点，视为由于不正确的特征匹配引起的“错误”点；

2.3.3)由于动态点的三维位置在点的分类时是随着时间自然变化的，因此，本方法能够产生移动点的三维轨迹；如果物体停止运动，动态的点就会成为静态的点，将一个动态点当前的三维位置投影到前一帧，如果对于连续帧的最小值N_min接近它的轨迹特征点(马氏距离<θ)，则认为这个点是“静态的”；

2.4)“摄像机分组”组件使用步骤(2.3)中经过分类的动态和静态的点来估计所有具有视角重叠的摄像机的姿态，将具有视角重叠的摄像机分成同一组并协同工作来进行映射和定位；当摄像机相遇或分离时，摄像机组可以合并和拆分；

2.4.1)合并：如果两个摄像机组群相遇并且具有相同的视角重叠，这两个组群将会被合并；由于漂移误差，在群组合并的过程中，通过纠正摄像机姿态和地图点来形成一个单一的全局的一致的地图；

2.4.2)拆分：根据为每一个映射特征点存储的一个指向相应三维位置的指针，快速的计算出摄像机i,j之间共同地图点N_ij的数量；建立一个无向图，无向图中的节点表示摄像机，如果N_ij＞0，以N_ij为边缘加权来连接摄像机i和j，在无向图中的连接组件形成了一个摄像机组群，摄像机外参操作仅应用于同一组群内的摄像机；用最大权重为每个摄像机组群提取一个生成树，只匹配在所选择的生成树的边所连接的摄像机间的特征点；当组群中任一个摄像机移走并且与其他摄像机间没有视角重叠时，这个摄像机组群会将其分离出去，完成摄像机组的拆分；

3)经过上述四个组件协作，实现摄像机的姿态估计、静态点的地图构建及动态点的三维轨迹跟踪。

2.根据权利要求1所述的基于多摄像机的协作式视觉SLAM方法，其特征在于：步骤2.2)中所述的测量三维地图点位置的不确定性，步骤如下：

1)测量三维地图点位置的不确定性时，只考虑特征监测和三角测量的不确定性； 假设特征监测的误差服从高斯分布一个三维地图点的位置的不确定性通过协方差表示，公式如下：

其中是在所有视图上将三维地图点映射到二维图像的摄像机投影函数的雅可比，k表示用于三角化的视图的数目；

1.1)当有一个新的地图点的图像监测时，可以通过Kalman增益快速的更新它的三维位置：

其中，计算第n+1帧的的图像投影，Kalman增益计算如下：

其中，在处估计出的的雅克比；

1.2)三角测量的不确定性可通过以下公式进行更新：

3.根据权利要求1所述的基于多摄像机的协作式视觉SLAM方法，其特征在于：步骤2.2.)中所述的三维地图点生成包括两种方法：“摄像机内参映射”和“摄像机外参映射”；

1)“摄像机内参映射”，根据每个独立的摄像机的特征轨道重建静态地图点；

1.1)如果有足够长的未映射的特征追踪，用这个跟踪的开始帧和结束帧来对一个三维地图点进行三角测量，计算出三维位置；

1.2)根据步骤1.1)计算出的三维位置，通过协方差公式计算协方差；

1.3)检查特征追踪的所有帧的重投影误差，如果投影和特征点间的马氏距离小于所有的帧，一个静态地图点生成；

2)“摄像机外参映射”，通过来自同一个组群内的摄像机的相应的点来生成动态地图点，摄像机外参映射仅被应用到没有映射的特征点；

2.1)通过ZNCC(零均值归一化互相关)在不同的摄像机间匹配图像特征；

2.2)进一步利用现有的地图点的关系作为种子去引导匹配：如果一个点与它最近的种子点之间的视差矢量有很大的不同，那么这对特征点被认为是不匹配的；

2.3)在摄像机间匹配完特征点之后，三角化相应的点生成新的地图点；

4.根据权利要求1所述的基于多摄像机的协作式视觉SLAM方法，其特征在于：步骤2.2)中所述的三维地图点的校准包括以下步骤：

1)在每一帧，将每个地图点与来自不同摄像机的新的检测图像的特征点连接起来，这些特征点通过特征跟踪与地图点进行校准；

2)进一步处理保留的未映射的特征点，用有效的静态的且在最近的帧N_rec内有相应特征点的地图点对这些未映射的特征点进行校准；将上述有效的地图点投影到图像中，通过ZNCC比较位于投影中心的图像区域和特征点；一旦一个未映射的特征点与一个三维地图点校准，就对这个特征点的三维位置和所有的观测重新进行三角测量，重新计算协方差。

5.根据权利要求1所述的基于多摄像机的协作式视觉SLAM方法，其特征在于：所述的步骤2.4.1)中所述摄像机姿态和地图点纠正方法如下：假设两个摄像机在第一帧分开在第F帧合并，将调整从第2帧到第F帧的所有摄像机的姿态，并且调整这些帧内生成的所有地图点；

1)首先估计在F帧中摄像机间正确的相对姿态：

1.1)在不同组群的摄像机间监测和匹配SURF特征，计算它们的相对姿态；

1.2)使用步骤1.1)中所述相对姿态来引导特征点的匹配，对于每对匹配的特征点，取他们位置的平均值来合并它们的相应的三维地图点；

1.3)对所有在第F帧中的地图点和它们相应的特征点进行BA(光束调整)来完善所有摄像机的姿态；

2)使用在第F帧中更新后的相对摄像机姿态作为硬约束来完善所有的摄像机姿态：

2.1)首先生成一个无向图，其中每个摄像机姿态是一个顶点，同时每个边遵从一个相对的姿态约束；对于每个摄像机，在相邻帧中它的姿态是连接的；对于同一组群中的摄像机，如果摄像机在生成树中是相邻的，那么它们在同一帧中的姿态是连接的；

让p＝1，.....，P，q＝1，...，Q表示不同组群的摄像机；用表示摄像机p在第i帧的姿态，用表示在第i帧中摄像机p和q的相对姿态；其中， 表示旋转矩阵和平移向量；α用来计算两个摄像机组群间的全局尺度差异，公式如下：

2.2)将第F帧的相对姿态视为硬约束，公式如下：

相当于：

得到(P+Q)×(P+Q-1)/2作为第F帧的相对姿态，本发明中只选用(P+Q-1)作为第F帧的相对姿态；

2.3)将所有的硬约束放在一起，根据下面的公式得到两个线性系统：

U_r ^F＝0 V_t ^F＝0

其中，是在第F帧中所有旋转矩阵的元素的矢量叠加，是一个由第F帧中的所有平移元素和比例因子组成的向量；

3)在第一帧固定摄像机的姿态，除了在第F帧中的相对姿态，将所有其它相对姿态视为软约束：

此处，是根据合并前的SLAM程序得到的m和n之间的相对姿态；

3.1)将所有的软约束放到一起，可以得到两个类似的线性系统：

A_r≈a≠0 Bt≈b≠0

此处，和是所有帧的所有旋转和平移的元素的矢量叠加；注意两个线性系统的右侧不等于0，因为在第一帧中的摄像机姿态是固定的；

4)结合步骤2.3)中硬约束和步骤3.1)中软约束公式，可以得到更新后的摄像机姿态和解决两个比例因子约束线性最小二乘问题，公式如下：

其中，是t附加了一个尺度因子α，是添加零元素之后的增广矩阵和向量；

5)对步骤4)中两个方程的结果进行优化，进一步通过SVD寻找与初始矩阵最相近的旋转矩阵，优化问题转化为一组稀疏矩阵的线性方程；使用CSparse库来解决上述稀疏矩阵的线性方程；摄像机姿态更新之后，地图点的三维位置通过三角化它们相应的特征点跟着更新。

6.根据权利要求1所述的基于多摄像机的协作式视觉SLAM方法，其特征在于： 该方法还包括采用BA(光束调整)方法对摄像机姿态和重现的地图点进行增量细化：

1)为了提高效率，BA只细化一些被选定的帧的摄像机姿态和从这些帧中重建的地图点，BA方法在一个独立的线程中运行，这个线程对最近的K关键帧进行操作；当连续插入K-1关键帧时，将会调用BA方法；

2)细化其他关键帧姿态时，采取在关键帧固定摄像机姿态，同时使用BA之前的连续帧间的相对姿态作为软约束；然后在保持关键帧姿态不变的同时更新所有摄像机的姿态；姿态细化后，通过对相应特征点进行三角测量来更新其他地图点的三维位置。