CN112669354A - 一种基于车辆非完整约束的多相机运动状态估计方法 - Google Patents

Abstract

本发明请求保护一种基于车辆非完整约束的多相机运动状态估计方法，包括步骤：1.多相机统一坐标系，获取相机间的刚性约束；2.单目SLAM的构建；3.车辆运动模型的建立，对车辆在直线运动和转弯状态下分别进行建模，根据Ackerman转向原理可以计算出车辆和相机的旋转角θ，平移角

和绝对偏移量ρ和λ的数学关系，从而为多相机***提供运动学约束求取尺度信息。4.多相机***和车辆的运动估计，完成初步的位姿估计及局部点云地图生成。5.相机的帧间位姿恢复和特征点匹配的重投影误差采用光束法平差进行优化，获取车辆及车载多相机***的准确位姿估计及全局点云。本发明适用于汽车高精度定位和状态估计，具有更广的适应性、更高的准确度且较好的鲁棒性。

Description

一种基于车辆非完整约束的多相机运动状态估计方法

技术领域

本发明属于汽车自动驾驶与视觉定位领域，具体涉及到一种基于车辆非完整约束的多相机运动状态估计方法。

背景技术

随着人工智能在近些年的高速发展，自动驾驶、人脸识别、机器视觉成为了未来的重要发展趋势。其中自动驾驶汽车的自我运动估计、定位、地图构建、路径规划及行人、障碍物检测是当前研究的重点。当前，自动驾驶汽车在一些地图先验信息已知的场景中已取得车道级的定位精度，在环境未知的非结构化道路且GPS、北斗、伽利略等有源定位传感器无法使用的情况下，如何进行车辆的自我运动估计及环境地图的构建是当前研究的重点和难点之一。

车辆智能感知是指汽车依赖自身携带的雷达、相机、超声波等传感器，感知汽车自身及行驶的周围环境，为智能汽车安全行驶提供及时、准确、可靠的决策依据。当前自动驾驶汽车严重依赖雷达和GPS等传感器进行车辆位置的定位及地图构建，相机在自动驾驶汽车只起了很小的作用。视觉传感器具有具有检测范围广、观测范围远、成本低、能耗小、重量轻和环境表达能力强等诸多优点，其中基于视觉的多相机全向SLAM***能够获取周围环境的全覆盖信息，相比于单双目视觉SLAM视差狭窄，局部成像区域信息较少的缺点，具有巨大的优势，是未来自动驾驶定位及地图构建发展的方向。同时，大多数轮式车辆在运动学中都具有瞬时旋转中心，即在某一点上，车辆每个轮子都绕着这个点进行圆周运动。因此，可以为车载多相机***添加车辆的运动学约束来进行更高效、准确、鲁棒的车辆运动状态估计。

所以，综上所述基于车辆非完整约束的多相机运动状态估计将运动学与多相机全向SLAM结合，既简化了广义多相机模型中的极线约束，还可以获取周围环境的全覆盖信息，在车辆出现大视角变化时，跟踪定位也能保持稳定，具有更高的精度且较好的鲁棒性，适用场景更加广泛。

发明内容

本发明旨在解决以上现有技术的问题。提出了一种基于车辆非完整约束的多相机运动状态估计方法。本发明的技术方案如下：

一种基于车辆非完整约束的多相机运动状态估计方法，其包括以下步骤：

步骤1、多相机统一坐标系：完成多相机***的联合标定，确定多相机***中相机的相对位置关系，获取相机间的刚性约束；

步骤2、单目SLAM的构建：用于获得初步的相机帧间位姿变换并通过三角化恢复出初始地图点；

步骤3、车辆运动模型的建立：车辆在平面运动具有三个自由度，对车辆在直线运动和转弯状态下分别进行建模，根据Ackerman转向原理计算出车辆和相机的旋转角θ，平移角

和绝对偏移量ρ和λ的数学关系，获得多相机的运动学约束；

步骤4、多相机***和车辆的运动估计：根据相机间的刚性约束方程和运动学约束方程，采用帧间的对极约束求解尺度信息和位姿变换矩阵，并将各个方向点云利用相机间的刚体约束配准，完成初步的位姿估计及局部点云地图生成；

步骤5、全局非线性优化：相机的帧间位姿恢复和特征点匹配的重投影误差采用光束法平差进行优化，获取全局点云地图，完成车辆及车载多相机***的位姿估计。

进一步的，所述步骤1的多相机统一坐标系是采用四台广角相机固定在车辆的前向、左向、后向、左向的四个方向上，取前向相机坐标系为车辆坐标系，利用张正友的平面模板标定法建立多相机***中每两两相机间的R、T关系，并将其统一在同一车辆坐标系下。

进一步的，所述步骤1具体包括以下步骤：

步骤1-1.固定相机传感器到车辆上，其中每个相机均为广角相机。取前向相机坐标系为车辆坐标系，利用张正友的平面模板标定法建立多相机***中每两两相机间的旋转矩阵R和平移矩阵T关系，记其相互之间的约束关系为[R₁|T₁]、[R₂|T₂]、[R₃|T₃]；

步骤1-2.采用100mm*100mm的棋盘格对广角相机进行标定，拍摄多张棋盘格角点图片，获取相机内参f_u、f_v、u₀、v₀以及相机外参R、T矩阵，可表示为如下所示公式。其中f_u、f_v分别表示相机的横、纵焦距，u₀、v₀表示相机光心,s为坐标轴倾斜参数，理想情况下为0；

步骤1-3.根据相机标定获得的外参[R|T]矩阵，将其统一到车辆坐标系下；

步骤1-4.由步骤1-2获取相机的畸变系数k₁，k₂，k₃，p₁，p₂，广角相机的畸变主要是径向桶形畸变，采用Opencv中的initUndistortRectifyMap和remap函数建立畸变映射到图像像点的映射关系；

步骤1-5.重复上述步骤1-2至步骤1-4，完成车左、车右、车后广角相机标定和畸变矫正工作。

进一步的，所述步骤2单目SLAM的构建具体包括：首先选定一个ORB特征大于100的图像作为参考帧，匹配当前帧与参考帧的ORB特征，采用多线程同步计算参考帧到当前帧的Homography模型和Fundamental模型，采用RANCAC框架进行迭代，选择迭代分数最高的记为S_H、S_F，则有

当R_H>0.45时，表示二维平面和低视差的情况，选择单应矩阵求解帧间R、T矩阵；否则选择基础矩阵，最后，以第一帧为世界坐标系采用直接线性变换DLT三角化测量匹配点对应的三维信息，生成局部点云。

进一步的，所述步骤3中，若前向相机和后向相机的位姿变换矩阵R_f＝R_b，

则其为直线运动(其中R_b、R_f、T_f、T_b分别表示前向相机和后向相机的旋转矩阵和平移矩阵，

表示归一化数组的(欧几里德)L2-范数)；否则为转弯状态，车辆在行驶过程中转弯可以近似为Ackerman转向模型；根据微分几何关系，获取相邻帧的车辆偏航角θ、相机偏航角

和车辆绝对偏移量ρ和相机绝对偏移量λ的数学关系，为多相机***提供运动学约束，将刚性约束和运动学约束联立求取车辆及多相机***的尺度信息。

进一步的，所述步骤3车载多相机***运动模型的建立具体包括：汽车在平面上运动，有三个自由度，即它在平面上的位置和方向，车辆通过Ackerman转向原理对左右前轮在转弯时施加不同的转向角度来确保了车辆的平稳运动，固定在车辆上的多相机***和车辆都在局部进行着平面圆周运动。运动学约束推导步骤如下：

步骤3-1.车辆平面运动主要涉及到三个参数，分别为旋转角θ，偏航角

和车辆偏移量ρ，取前向相机相机坐标系和车辆坐标系一致，则后向相机在车辆坐标系下的坐标可以表示为C₃＝[0 -e 0]，e表示后向相机和前向相机的距离，则其位置1的位置矩阵P₁为：

步骤3-2.若汽车前向相机与后向相机的位姿R_f＝R_b，

则其为直线运动，遵循直线运动模型；否则则为转弯状态，遵循Ackerman转向原理，其在t时刻和t+1时刻的相对运动R、T可以表示为：

其中

表示车辆偏航角，θ表示车辆运动圆周角。

步骤3-3.由此可得，在车辆坐标系中后向相机在位置2的位置矩阵为式3.3，为了描述后向相机从位置1到位置2的相对运动，将P₂在车辆左边系下的矩阵转换到后向相机坐标系C₃。将其在坐标系C₃下的位置矩阵定义为P'₂，则P'₂可以表示为式3.4，因为车辆上的每一点都随着车辆进行圆周运动，所以旋转矩阵R₂和R′₂是等价的，因此T′₂可以表示为式3.5，因此P₁到P′₂的本质矩阵E＝[T′₂]×R′₂可以表示为式3.6；

步骤3-4.根据后向相机从位置1运动到位置2在相机坐标系C₃的T′₂、E及可以表示为式3.7；

步骤3-5.由步骤3-3和步骤3-4可知，式3.5和式3.7是等价的，由此可得式3.8，进而求得ρ、λ和

θ角的数学关系。

其中ρ代表前向相机偏移量、λ代表后向相机偏移量，

表示车辆偏航角，θ表示车辆运动圆周角。

进一步的，所述步骤4车辆及车载多相机***运动状态估计具体包括：

步骤4-1.根据车辆运动过程中车载多相机***在相邻两帧的对极约束关系，设前一帧k地图点在其相机坐标系的位置为p＝(x,y,z)^T，后一帧同一地图点在其坐标系的坐标p'＝(x',y',z)^T，则有：

p'^TEp＝0(4.1)

代入式3.7的本质矩阵，可得：

给定n个地图点，可以直接利用系数矩阵[xz′ zx′ zy′ yz′]奇异值分解来间接计算旋转角θ和偏航角

从而获得车辆运动的偏移量；

步骤4-2.由相机间相互关系可知，相机间的刚性约束分别[R₁|T₁]、[R₂|T₂]、[R₃|T₃]，由步骤2可得多相机***在四个方向的点云，首先以相同标准提取点云集中的关键点，计算其在当前点云集中3D特征描述子和坐标的位置，利用相机间的变换矩阵进行点云的配准生成周围环境的地图点信息，将相机间刚性约束其和运动学模型提供的尺度信息结合，从而完成车辆及多相机***的初步位姿估计及局部点云地图生成。

进一步的，所述步骤5具体包括：

步骤5-1.利用光束法平差BA对空间地图点进行极大似然估计，通过迭代方式逐渐收敛到目标函数的最小值，求取最优的估计，消除特征点匹配中的错误估计；

其中q_ij代表观测像素坐标，P(C_i,X_j)代表投影值，即根据位姿计算得到的像素坐标。w_ij代表指标变量，若点j在相机i中可见则为1，否则为0。

本发明的优点及有益效果如下：

本发明提出了一种基于车辆非完整约束的多相机运动状态估计方法，用于自动驾驶汽车在行驶过程中的高精度定位与状态估计，具有检测范围广、观察距离远，环境表达能力强的优点。将车辆的运动学模型与多相机全向SLAM***进行结合，简化了广义多相机模型中的极线约束，提高了运行计算过程中的效率。并通过四个广角相机相互补充视野，实现了对车辆行驶过程中周围360°环境的观测，解决了现有SLAM***视场狭窄、局部成像区域信息较少或出现大的视角变化，跟踪定位会频繁丢失、鲁棒性差等问题。其优越和创新性主要体现在：

(1)本发明采用了车辆运动学和多相机全向SLAM结合的方式，与单目SLAM方式相比，通过为SLAM***添加车辆的运动学约束，增加了SLAM***的尺度信息，简化了广义多相机***的极线约束与点对应关系，具有更高效的运行速度。

(2)本发明采用的全向SLAM***利用多相机的成像与结构特点完成了全向SLAM***的地图构建和定位，拥有更大的视场和更多的特征匹配点，因此跟踪的稳定性和精度更高。

(3)本发明的提出解决了在没有环境先验信息且GPS等有源定位传感器信号缺失或无法使用的情况下车辆的定位及地图构建，进行自我运动估计的问题，为车辆在更广泛环境的安全行驶提供了准确、可靠的决策依据。

附图说明

图1是本发明提供优选实施例方法的流程简图；

图2是本发明中车载多相机***位置示意图；

图3是本发明中车载多相机***相机坐标系相互位置关系；

图4是本发明中车辆运动学几何关系示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。

本发明解决上述技术问题的技术方案是：

如图1所示是本发明所要解决问题的实施例方法的流程简图，不失一般性。本专利采用四台广角相机分别固定于车辆的前后左右四个方向(如图2所示)，图3中的C_i分别代表多相机***中的第i个相机坐标系。通过建立车辆运动学模型为多相机***添加运动学约束，车辆运动学约束数学关系见图4。为了对自动驾驶汽车进行高精度的定位和位姿估计，本专利将SLAM与运动学结合提出了此技术方案，包括如下步骤：

1.建立多相机***的相互位置关系，完成其联合标定，获取每个相机的内外参数：

(1)如图2所示，采用四台广角相机固定在车辆的前向、左向、后向、左向的四个方向上，利用张正友的平面模板标定法建立多相机***中每两两相机间的R、T关系及相机内参数，并将其统一在同一车辆坐标系下；

(2)采用100mm*100mm的棋盘格对广角相机进行标定，拍摄多张棋盘格角点图片，获取相机内参f_u、f_v、u₀、v₀以及相机外参R、T矩阵。

(3)由步骤(2)可获取相机的畸变系数k₁，k₂，k₃，p₁，p₂。采用Opencv中的initUndistortRectifyMap和remap函数建立畸变映射到图像像点的映射关系。

(4)重复上述步骤1-2至步骤1-4，完成车左、车右、车后广角相机标定和畸变矫正工作。

2.单目初始化，选定一个ORB特征大于100的图像作为参考帧，匹配当前帧与参考帧的ORB特征，采用多线程同步计算参考帧到当前帧的Homography模型和Fundamental模型，采用RANCAC框架进行迭代，选择迭代分数最高的记为S_H、S_F。则有

当R_H>0.45时，表示二维平面和低视差的情况，选择单应矩阵求解帧间R、T矩阵；否则选择基础矩阵。最后，以第一帧为世界坐标系采用直接线性变换(DLT)三角化测量匹配点对应的三维信息，生成局部点云。

3.如图3所示，将多相机***建模为广义相机模型，建立车载多相机***的运动学模型，简化了GEC算法，其具体操作步骤如下：

(1)车辆平面运动主要涉及到三个参数，分别为旋转角θ，偏航角

和车辆偏移量ρ。其在t时刻和t+1时刻的相对运动R、T可以表示为：

(2)如图4所示，车辆后向相机在车辆坐标系平移运动矩阵和在C₃坐标系的平移运动矩阵可以表示为：

如图4所示，后向相机运动都为从位置1运动到位置2，所以式(4)的两个式子等价，由此可以得到ρ、λ和

θ角的数学关系：

4.将建立的车辆运动学模型和SLAM***进行结合，提取每个相机相邻帧的ORB特征描述子利用对极约束进行特征匹配求解，从而为多相机***提供尺度信息约束，获得车辆和多相机***的初步位姿估计。其具体操作步骤如下：

(1)根据车辆运动过程中车载多相机***在相邻两帧的对极约束关系,取空间点p，则有：

p'Ep＝0(7)

代入式(7)的本质矩阵，可得：

给定n个地图点，可以直接利用系数矩阵[xy′ yx′ zy′ yz′]奇异值分解来间接计算旋转角θ和偏航角

(2)由相机间的相互位置关系可知，相机间的刚性约束分别[R₁|T₁]、[R₂|T₂]、[R₃|T₃]。由步骤2可得多相机***在四个方向的点云，利用相机间的变换矩阵进行点云的配准生成周围环境的地图点信息，将相机间的刚性约束和运动学模型提供的尺度信息结合，从而完成车辆及多相机***的初步位姿估计及局部点云地图生成。

5.全局非线性优化包括车辆运动模型和SLAM***中获得的特征点匹配的重投影误差采用光束法平差进行优化，获取车辆及车载多相机***的准确位姿估计。利用光束法平差(BA)对空间地图点进行极大似然估计，通过迭代方式逐渐收敛到目标函数的最小值，求取最优的估计，消除特征点匹配中的错误估计。

还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。

以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。

Claims (8)

1.一种基于车辆非完整约束的多相机运动状态估计方法，其特征在于，包括以下步骤：

步骤1、多相机统一坐标系：完成多相机***的联合标定，确定多相机***中相机的相对位置关系，获取相机间的刚性约束；；

步骤2、单目SLAM的构建：用于获得初步的相机帧间位姿变换并通过三角化恢复出初始地图点；

步骤3、车辆运动模型的建立：车辆在平面运动具有三个自由度，对车辆在直线运动和转弯状态下分别进行建模，根据Ackerman转向原理计算出车辆和相机的旋转角θ，平移角

和绝对偏移量ρ和λ的数学关系，获得多相机的运动学约束；

步骤4、多相机***和车辆的运动估计：根据相机间的刚性约束和运动学约束，采用帧间的对极约束求解尺度信息和位姿变换矩阵，并将各个方向点云利用相机间的刚体约束配准，完成初步的位姿估计及局部点云地图生成；

步骤5、全局非线性优化：相机的帧间位姿恢复和特征点匹配的重投影误差采用光束法平差进行优化，获取全局点云地图，完成车辆及车载多相机***的位姿估计。

2.根据权利要求1所述的一种基于车辆非完整约束的多相机运动状态估计方法，其特征在于，所述步骤1的多相机统一坐标系是采用四台广角相机固定在车辆的前向、左向、后向、左向的四个方向上，取前向相机坐标系为车辆坐标系，利用张正友的平面模板标定法建立多相机***中每两两相机间的R、T关系，并将其统一在同一车辆坐标系下。

3.根据权利要求2所述的一种基于车辆非完整约束的多相机运动状态估计方法，其特征在于，所述步骤1具体包括以下步骤：

步骤1-1.固定相机传感器到车辆上，其中每个相机均为广角相机。取前向相机坐标系为车辆坐标系，利用张正友的平面模板标定法建立多相机***中每两两相机间的旋转矩阵R和平移矩阵T的关系，记其相互之间的约束关系为[R₁|T₁]、[R₂|T₂]、[R₃|T₃]；

步骤1-2.采用100mm*100mm的棋盘格对广角相机进行标定，拍摄多张棋盘格角点图片，获取相机内参f_u、f_v、u₀、v₀以及相机外参R、T矩阵，可表示为如下所示公式。其中f_u、f_v分别表示相机的横、纵焦距，u₀、v₀表示相机光心,s为坐标轴倾斜参数，理想情况下为0；

步骤1-3.根据相机标定获得的外参[R|T]矩阵，将其统一到车辆坐标系下；

步骤1-4.由步骤1-2获取相机的畸变系数k₁，k₂，k₃，p₁，p₂，广角相机的畸变主要是径向桶形畸变，采用Opencv中的initUndistortRectifyMap和remap函数建立畸变映射到图像像点的映射关系；

步骤1-5.重复上述步骤1-2至步骤1-4，完成车左、车右、车后广角相机标定和畸变矫正工作。

4.根据权利要求1-3任一项所述的一种基于车辆非完整约束的多相机运动状态估计方法，其特征在于，所述步骤2单目SLAM的构建具体包括：首先选定一个ORB特征大于100的图像作为参考帧，匹配当前帧与参考帧的ORB特征，采用多线程同步计算参考帧到当前帧的Homography模型和Fundamental模型，采用RANCAC框架进行迭代，选择迭代分数最高的记为S_H、S_F，则有

当R_H>0.45时，表示二维平面和低视差的情况，选择单应矩阵求解帧间R、T矩阵；否则选择基础矩阵，最后，以第一帧为世界坐标系采用直接线性变换DLT三角化测量匹配点对应的三维信息，生成局部点云。

5.根据权利要求4所述的一种基于车辆非完整约束的多相机运动状态估计方法，其特征在于，所述步骤3中，若前向相机和后向相机的位姿变换矩阵R_f＝R_b，

则其为直线运动(其中R_b、R_f、T_f、T_b分别表示前向相机和后向相机的旋转矩阵和平移矩阵，

表示归一化数组的(欧几里德)L2-范数)；否则为转弯状态，车辆在行驶过程中转弯可以近似为Ackerman转向模型；根据微分几何关系，获取相邻帧的车辆偏航角θ、相机偏航角

和车辆绝对偏移量ρ和相机绝对偏移量λ的数学关系，为多相机***提供运动学约束，将刚性约束和运动学约束联立求取车辆及多相机***的尺度信息。

6.根据权利要求5所述的一种基于车辆非完整约束的多相机运动状态估计方法，其特征在于，所述步骤3车载多相机***运动模型的建立具体包括：汽车在平面上运动，有三个自由度，即它在平面上的位置和方向，车辆通过Ackerman转向原理对左右前轮在转弯时施加不同的转向角度来确保了车辆的平稳运动，固定在车辆上的多相机***和车辆都在局部进行着平面圆周运动。运动学约束推导步骤如下：

步骤3-1.车辆平面运动主要涉及到三个参数，分别为旋转角θ，偏航角

和车辆偏移量ρ，取前向相机相机坐标系和车辆坐标系一致，则后向相机在车辆坐标系下的坐标可以表示为C₃＝[0 -e 0]，e表示后向相机相对前向相机的距离。则其位置1的位置矩阵为，记为P₁：

步骤3-2.若汽车前向相机与后向相机的位姿R_f＝R_b，

则其为直线运动，遵循直线运动模型；否则则为转弯状态，遵循Ackerman转向原理，其在t时刻和t+1时刻的相对运动R、T可以表示为：

其中

表示车辆偏航角，θ表示车辆运动圆周角。

步骤3-3.由此可得，在车辆坐标系中后向相机在位置2的位置矩阵为式3.3，为了描述后向相机从位置1到位置2的相对运动，将P₂在车辆左边系下的矩阵转换到后向相机坐标系C₃。将其在坐标系C₃下的位置矩阵定义为P₂'，则P₂'可以表示为式3.4，因为车辆上的每一点都随着车辆进行圆周运动，所以旋转矩阵R₂和R′₂是等价的，因此T′₂可以表示为式3.5，因此P₁到P′₂的本质矩阵E＝[T′₂]_×R′₂可以表示为式3.6；

步骤3-4.根据后向相机从位置1运动到位置2在相机坐标系C₃的T′₂、E及可以表示为式3.7；

步骤3-5.由步骤3-3和步骤3-4可知，式3.5和式3.7是等价的，由此可得式3.8，进而求得ρ、λ和

θ角的数学关系。

其中ρ代表前向相机偏移量、λ代表后向相机偏移量，

表示车辆偏航角，θ表示车辆运动圆周角。

7.根据权利要求6所述的一种基于车辆非完整约束的多相机运动状态估计方法，其特征在于，所述步骤4车辆及车载多相机***运动状态估计具体包括：

步骤4-1.根据车辆运动过程中车载多相机***在相邻两帧的对极约束关系，设前一帧k地图点在其相机坐标系的位置为p＝(x,y,z)^T，后一帧同一地图点在其坐标系的坐标p'＝(x',y',z)^T，则有：

p'^TEp＝0(4.1)

代入式3.7的本质矩阵，可得：

给定n个地图点，可以直接利用系数矩阵[xz′ zx′ zy′ yz′]奇异值分解来间接计算旋转角θ和偏航角

从而获得车辆运动的偏移量；

步骤4-2.由相机间相互关系可知，相机间的刚性约束分别[R₁|T₁]、[R₂|T₂]、[R₃|T₃]，由步骤2可得多相机***在四个方向的点云，首先以相同标准提取点云集中的关键点，计算其在当前点云集中3D特征描述子和坐标的位置，利用相机间的变换矩阵进行点云的配准生成周围环境的地图点信息，将相机间刚性约束其和运动学模型提供的尺度信息结合，从而完成车辆及多相机***的初步位姿估计及局部点云地图生成。

8.根据权利要求7所述的一种基于车辆非完整约束的多相机运动状态估计方法，其特征在于，所述步骤5)具体包括：

步骤5-1.利用光束法平差BA对空间地图点进行极大似然估计，通过迭代方式逐渐收敛到目标函数的最小值，求取最优的估计，消除特征点匹配中的错误估计；

其中q_ij代表观测像素坐标，P(C_i,X_j)代表投影值，即根据位姿计算得到的像素坐标。w_ij代表指标变量，若点j在相机i中可见则为1，否则为0。

Images