一种基于视觉的集束优化方法、装置及电子设备与流程

1.本发明涉及无人驾驶技术领域，具体涉及一种基于视觉的集束优化方法、装置及电子设备。


背景技术：

2.无人驾驶技术作为当今世界最具潜力之一的技术，代表了汽车产业发展的战略方向，已成为国际国内新一轮科技革命和产业变革竞争的战略制高点。为满足无人驾驶的实时性和可靠性要求，无人驾驶车辆通常配备有视觉相机、惯性测量单元、激光雷达和轮速计等多种传感器。相机能够感知的外界信息最丰富，因此视觉相机是无人驾驶车辆的常用基本配置。为了对多相机成像系统采集的图像信息进行分析与运算，需要将多相机的图像信息转换到同一坐标系下进行描述.
3.在slam(simultaneous localization and mapping，即时定位与地图构建)流程中通常通过集束优化来对后端进行优化。集束优化通过最小化投影和观测点之间的几何距离所对应的重投影误差，并同时优化6-dof(6个自由度)相机位姿和地标的三维坐标。集束优化高度依赖于slam前端位姿求解器的初始化结果，然而，在实际应用中存在许多复杂和具有挑战性的场景，这使得前端求解器很可能得到较差的结果，会使得轨迹产生跳变等现象，从而对下游的规划与控车等模块产生很大影响。视觉slam框架中的轨迹通常由一系列离散的相机位姿来表示，每个位姿都与一个观测的图像相关联，这种离散的表示方法过于笼统，没有遵循车辆运动的运动学约束，因此目前基于视觉优化方法稳定性弱于基于激光雷达的优化方法。


技术实现要素：

4.有鉴于此，本发明实施例提供了一种基于视觉的集束优化方法、装置及电子设备，以解决基于纯视觉的车辆slam方法精度和稳定性不高的问题。
5.根据第一方面，本发明实施例提供了一种基于视觉的集束优化方法，包括：
6.获取车体的世界坐标系的位姿信息；
7.根据所述车体的世界坐标系的位姿信息构建所述车体的平滑约束函数，并计算重投影误差函数；
8.构造所述惯性测量单元的角速度观测值与所述车体的旋转之间的角速度约束函数；
9.根据所述车体的世界坐标系的位姿信息以及所述车体的平滑约束曲线，确定所述车体的运动方向，基于所述车体的运动方向和所述车体的世界坐标系的位姿信息，生成所述车体的运动约束函数；
10.根据所述车体的平滑约束曲线、重投影误差函数、运动约束函数以及角速度约束函数构建误差函数，并对所述误差函数进行联合优化，生成优化后的车体坐标系的位姿信息。
11.结合第一方面，在第一方面第一实施方式中，获取车体的世界坐标系的位姿信息，包括：
12.将从惯性测量单元识别的车体坐标系的第一相对旋转与相机识别的车体坐标系的第二相对旋转进行融合，得到相邻帧之间所述车体坐标系的相对旋转；
13.依据所述相邻帧之间所述车体坐标系的相对旋转以及相机识别的车体坐标系的相对位移，推算所述车体的世界坐标系的位姿信息。
14.结合第一方面第一实施方式，在第一方面第二实施方式中，从惯性测量单元中识别的车体坐标系的第一相对旋转，包括：
15.获取所述惯性测量单元的角速度观测值，并推算所述惯性测量单元的惯性相对旋转；
16.通过所述惯性测量单元到所述车体坐标系之间的外参，将所述惯性测量单元的惯性相对旋转转换为车体坐标系的第一相对旋转。
17.结合第一方面，在第一方面第三实施方式中，根据所述车体的世界坐标系的位姿信息构建所述车体的平滑约束曲线，包括：
18.基于所述车体的世界坐标系的位姿信息中的轨迹信息拟合三维b样条函数，通过所述车体的位移与所述三维b样条函数中一帧的位移的差异计算所述三维b样条轨迹与所述车体的离散位移之间的差异；
19.根据所述三维b样条轨迹与所述车体的离散位移之间的差异以及标量权重，构建所述车体的平滑约束曲线。
20.结合第一方面第三实施方式，在第一方面第四实施方式中，生成重投影误差函数，包括：
21.识别特征点在相机坐标系下的估计值和观测值，所述估计值为所述特征点在图像上的投影；
22.基于所述估计值与所述观测值之间的差异构建重投影误差函数。
23.结合第一方面，在第一方面第五实施方式中，构造所述惯性测量单元的角速度观测值与所述车体的旋转之间的角速度约束函数，包括：
24.基于所述惯性测量单元位姿信息中的旋转信息拟合所述惯性测量单元的四维b样条曲线；
25.所述角速度约束函数按照下方公式构建：
[0026][0027]
其中，ωd为标量权重，为所述车体的世界坐标系的旋转，为所述惯性测量单元到所述车体的外参数旋转，ωj为第j帧时所述惯性测量单元的角速度观测值，c2(t)为所述惯性测量单元位姿信息中的旋转信息拟合的四维b样条曲线。
[0028]
结合第一方面，在第一方面第六实施方式中，基于所述车体的运动方向和所述车体的世界坐标系的位姿信息，生成所述车体的运动约束函数包括：
[0029]
确定所述车体的运动方向，并对所述运动方向做归一化操作；
[0030]
生成所述车体的世界坐标系的旋转的旋转矩阵，结合所述归一化操作的结果生成
所述车体的运动约束函数，所述运动约束函数按照下方公式构建：
[0031][0032]
其中，为所述车体的旋转的旋转矩阵，c1(t)表示车体的世界坐标系的位姿信息中的轨迹信息拟合的三维b样条曲线，η(c
′1(t))表示对运动方向的归一化操作结果，ωc表示标量权重。
[0033]
结合第一方面，在第一方面第六实施方式中，所述误差函数按照以下公式构建：
[0034][0035]
其中，ρ表示减小异常数据影响的损失函数，表示车体坐标系到相机ck的外参数，表示第j帧时刻车体的旋转的旋转矩阵，表示车体的位移，xi表示特征点i在三维空间中特征点的位置，表示特征点i在第j帧图像上的二维观测值，ωs表示标量权重，表示离散的车体位移估计值，c1(tj)表示第j帧时刻的位移，c1(t)表示车体的世界坐标系的位姿信息中的轨迹信息拟合的三维b样条曲线，ωc表示标量权重，η(c
′1(t))表示对运动方向的归一化操作结果，ωd表示标量权重，表示惯性测量单元到车体的外参数旋转，ωj表示第j帧时惯性测量单元的角速度观测值，c2(t)表示惯性测量单元位姿信息中的旋转信息拟合的四维b样条曲线。
[0036]
根据第二方面，本发明实施例提供了一种基于视觉的集束优化装置，包括：
[0037]
车体位姿获取模块，用于获取车体的世界坐标系的位姿信息；
[0038]
平滑约束函数生成模块，用于根据所述车体的世界坐标系的位姿信息构建所述车体的平滑约束函数，并计算重投影误差函数；
[0039]
角速度约束函数生成模块，用于构造所述惯性测量单元的角速度观测值与所述车体的旋转之间的角速度约束函数；
[0040]
运动约束函数生成模块，用于根据所述车体的世界坐标系的位姿信息以及所述车体的平滑约束曲线，确定所述车体的运动方向，基于所述车体的运动方向和所述车体的世界坐标系的位姿信息，生成所述车体的运动约束函数；
[0041]
误差函数构建模块，用于根据所述车体的平滑约束曲线、重投影误差函数、运动约束函数以及角速度约束函数构建误差函数，并对所述误差函数进行联合优化，生成优化后的车体坐标系的位姿信息。
[0042]
根据第三方面，本发明实施例提供了一种电子设备，包括存储器和处理器，所述存储器和所述处理器之间互相通信连接，所述存储器中存储有计算机指令，所述处理器通过
执行所述计算机指令，从而执行第一方面或者第一方面的任意一种实施方式中所述的基于视觉的集束优化方法。
[0043]
本技术提供的技术方案，通过车体的世界坐标系的位姿信息构建车体的平滑约束函数，计算重投影误差函数，并基于平滑约束函数确定车体运动方向从而构建车体的运动约束函数。构造惯性测量单元的角速度观测值以及车体的旋转之间的角速度约束函数，最终根据得到的平滑约束函数、重投影误差函数、运动约束函数以及角速度约束函数构建误差函数，并对误差函数进行优化以得到车体坐标系的位姿信息。将车辆运动约束应用于集束优化中，并且基于平滑曲线使得连续时间的轨迹参数化，提升了集束优化的稳定性和精度。
附图说明
[0044]
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0045]
图1示出了本发明实施例中相机安装示意图；
[0046]
图2是本发明实施例中基于视觉的集束优化方法的流程图；
[0047]
图3是本发明实施例中获取车体的世界坐标系位姿信息方法的流程图；
[0048]
图4是本发明实施例中构建平滑约束函数和重投影误差函数的流程图；
[0049]
图5是根据本发明实施例的基于视觉的集束优化装置的结构框图；
[0050]
图6是根据本发明实施例电子设备的硬件结构示意图。
具体实施方式
[0051]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0052]
本技术提供的基于视觉的集束优化方法，由于前端的位姿求解器通常需要将多相机的信息转换到同一个坐标系下进行描述，因此需要对多相机相对于车体后轴中心的外参数进行标定以及各相机内参数提前进行标定，相机之间通过硬件触发进行同步采集。
[0053]
可应用于图1所示的系统中，矩形框表示车体，在车体的前后左右可以各安装一路单目车载相机(圆形所示)。当然，在实际应用中，根据车体配置的不同，也可以安装更多路的车载相机，本技术对此并不做限定，下文仅以四个车载相机为例进行阐述本技术的技术方案，更多路的车载相机对应的技术方案可以类推。
[0054]
在图1所示的系统中，可以先定义各个不同的坐标系。这些坐标系可以包括车体坐标系、相机原始坐标系和图像坐标系。这些坐标系都可以是右手系的坐标系。其中，车体坐标系的原点为车体后轴中心，y轴垂直车体后轴中心向下指向地面，x轴指向车体行进方向的右侧，z轴指向车体行进方向。相机原始坐标系的原点为车载相机的光心，z轴指向车载相机的朝向，x轴平行于图像平面且指向图像右侧，y轴垂直于图像平面且指向地面。图像坐标
系是一个二维直角坐标系，反映了相机ccd(charge coupled device，电荷耦合器件)芯片中像素的排列情况。其原点位于图像的左上角，其它两个坐标轴分别与图像的两条边重合。当然，在实际应用中，可以根据车辆的实际构造，灵活地调整相机的分布，并且相机的类型也可以根据实际应用场景的需求灵活调整。
[0055]
请参阅图2，图2是本发明实施例中基于视觉的集束优化方法的流程图，该方法包括如下步骤：
[0056]
s11，获取车体的世界坐标系的位姿信息。
[0057]
s12，根据车体的世界坐标系的位姿信息构建车体的平滑约束函数，并计算重投影误差函数。
[0058]
s13，构造惯性测量单元的角速度观测值与车体的旋转之间的角速度约束函数。
[0059]
s14，根据车体的世界坐标系的位姿信息以及车体的平滑约束曲线，确定车体的运动方向，基于车体的运动方向和车体的世界坐标系的位姿信息，生成车体的运动约束函数。
[0060]
s15，根据车体的平滑约束曲线、重投影误差函数、运动约束函数以及角速度约束函数构建误差函数，并对误差函数进行联合优化，生成优化后的车体坐标系的位姿信息。
[0061]
相机在进行图片采集时的帧率可控，通过计算机视觉领域中常用的图像处理方法，检测装在车体上每个相机获得的图像中的某一类型2d特征点并对其进行追踪，包括但不限于sift、surf、orb或深度学习方法提取的superpoint等类型的特征点。
[0062]
根据车体的平滑约束曲线、重投影误差函数、运动约束函数以及角速度约束函数构建误差函数，可以采用集束优化的方式对该误差函数进行联合优化，得到优化后的车体在车体坐标系的位姿，车体的位姿包括旋转和位移。
[0063]
本实施例提供的基于视觉的集束优化方法，通过车体的世界坐标系的位姿信息构建车体的平滑约束函数，计算重投影误差函数，并基于平滑约束函数确定车体运动方向从而构建车体的运动约束函数。构造惯性测量单元的角速度观测值以及车体的旋转之间的角速度约束函数，最终根据得到的平滑约束函数、重投影误差函数、运动约束函数以及角速度约束函数构建误差函数，并对误差函数进行优化以得到车体坐标系的位姿信息。将车辆运动约束应用于集束优化中，并且基于平滑曲线使得连续时间的轨迹参数化，提升了集束优化的稳定性和精度。
[0064]
请参阅图3，图3是本发明实施例中获取车体的世界坐标系位姿信息方法的流程图，该流程包括如下步骤：
[0065]
s111，获取惯性测量单元的角速度观测值，并推算惯性测量单元的惯性相对旋转。
[0066]
惯性测量单元是用于测量物体的三周姿态角或角速率以及加速度的装置，在车体上固定好惯性测量单元，并与车体坐标系进行外参数标定。获取惯性测量单元观测的车体的加速度和角速度数据，且惯性测量单元的观测数据与相机系统之间可以通过硬件触发实现数据同步。
[0067]
对惯性测量单元进行初始化，具体地，自相机拍摄的第二帧图片开始，对每个相机当前获得图片上的特征点与第一帧图片上的特征点进行匹配，当每个相机的匹配特征点的中位数距离小于某一特定阈值时，认为车体此时处于静止状态，特定阈值可依据实际情况进行设定。记录车体处于静止期间内的惯性测量单元的数据，直到判断相机开始运动的前一帧。利用静止期间惯性测量单元角速度真值为0且加速度方向为重力方向，计算静止期间
加速度计和陀螺仪的平均值，并计算偏置(biases)参数。
[0068]
定义第一帧图片中车体所处的位置为世界坐标系原点，可以利用计算机视觉中针对多相机系统的相对位姿估计算法计算相邻两帧之间车体坐标系的相对位移和旋转。利用惯性测量单元初始化期间得到的偏置参数，以及相邻帧图片之间同步获得的惯性测量单元的角速度观测值，通过视觉惯性里程计中常用的预积分模型计算惯性测量单元的惯性相对旋转。
[0069]
s112，通过惯性测量单元到车体坐标系之间的外参，将惯性测量单元的惯性相对旋转转换为车体坐标系的第一相对旋转。
[0070]
通过惯性测量单元的外参，将得到的惯性测量单元的惯性相对旋转转换为车体坐标系的第一相对旋转。
[0071]
s113，将从惯性测量单元识别的车体坐标系的第一相对旋转与相机识别的车体坐标系的第二相对旋转进行融合，得到相邻帧之间车体坐标系的相对旋转。
[0072]
相机识别的车体坐标系的第二相对旋转具体为，利用计算机视觉中针对多相机系统的相对位姿估计算法，计算相邻两帧之间车体坐标系的相对位姿，记为相机识别的车体坐标系的第二相对旋转。利用随机采样一致性的方法剔除出相邻帧匹配特征点对中的误匹配点对使后续建立的非齐次线性方程组不受误匹配点影响从而获得准确结果。
[0073]
利用计算机视觉中常用的旋转融合的方法将车体坐标系的第一相对旋转和第二相对旋转进行融合，获得融合后的旋转，进行通过航机推算生成相邻帧之间车体坐标系的相对旋转。
[0074]
s114，依据相邻帧之间车体坐标系的相对旋转以及相机识别的车体坐标系的相对位移，推算车体的世界坐标系的位姿信息。
[0075]
基于相邻帧之间车体坐标系的相对旋转，进一步推算出第j帧时刻车体在世界坐标系下的位姿信息位姿信息包含以四元数形式表示的旋转和位移记录第j帧时刻惯性测量单元观测的角速度值ωj，并利用随机采样一致性的方法筛选出相邻帧匹配特征点对中的误匹配点对。
[0076]
请参阅图4，图4是本发明实施例中构建平滑约束函数和重投影误差函数的流程图，该流程包括如下步骤：
[0077]
s121，基于车体的世界坐标系的位姿信息中的轨迹信息拟合三维b样条函数，通过车体的位移与三维b样条函数中一帧的位移的差异计算三维b样条轨迹与车体的离散位移之间的差异。
[0078]
其中，轨迹信息可以根据车体坐标系的位姿信息的位移分量得到。
[0079]
s122，根据三维b样条轨迹与所述车体的离散位移之间的差异以及标量权重，构建车体的平滑约束曲线。
[0080]
将平滑轨迹以一条p阶b样条曲线来表示，如下：
[0081][0082]
其中，u为时间参数，{pi}为控制平滑曲线形状的n+1个控制点，{n
i,p
(u)}是n+1个p
阶b样条基函数。b样条的形式和基函数一般是固定的，曲线的形状只受控制点的影响。b样条曲线可通过一组带时间戳的离散位姿完成初始化。利用车辆位姿的位移tb拟合出一条三维b样条c1(t)。
[0083]
离散的车体位移估计值与从拟合的三维b样条曲线中通过控制点和对应基函数获得第j帧时刻的位移c1(tj)的欧氏距离的平方，得到用于约束拟合的b样条与离散位移之间的差异，具体地，平滑约束曲线按照如下方式构建：
[0084][0085]
其中，ωs表示标量权重，表示离散的车体位移估计值，c1(tj)表示第j帧时刻的位移。
[0086]
s123，识别特征点在相机坐标系下的估计值和观测值。其中，估计值为特征点的三维位置在图像上的二维投影。观测值为相机在拍摄图像时直接在图像上检测的特征点位置，考虑到图像的测量误差以及单位矩阵的计算误差两者之间会存在一定的误差。
[0087]
s124，基于估计值与观测值之间的差异构建重投影误差函数。
[0088]
基于车体坐标系到当前相机ck的外参数以及第j帧时刻下车体在世界坐标系下的位姿在第j帧时刻下相机ck相对于世界坐标系的位姿可表示为利用计算机视觉中三角测量的方法输入相机ck的相邻帧筛选后匹配的特征点，以及相机在两帧时刻下的位置，可获得该匹配特征点在对应三维空间中特征点i在世界坐标系下的位置xi，该三维点在图像上的二维投影即为估计值，定义为三维空间中特征点i在第j帧中相机ck获得的图像上的二维观测，即为观测值。
[0089]
具体地，重投影误差函数按照如下方式构建：
[0090][0091]
其中，表示第j帧时刻车体的旋转的旋转矩阵，表示车体的位移，xi表示观测值，即三维空间中特征点的位置，表示估计值，即特征点i在第j帧图像上的二维观测值，表示将特征点从相机坐标系投影到图像平面的投影函数，表示车体坐标系到相机ck的外参数，ρ为减小异常数据影响的损失函数，例如huber loss。
[0092]
在一个实施方式中，基于惯性测量单元位姿信息中的旋转信息拟合惯性测量单元的四维b样条曲线，惯性测量单元的位姿信息包括位移和旋转，利用惯性测量单元的旋转的四元数形式拟合一条四维b样条曲线。角速度约束函数按照下方公式构建：
[0093][0094]
其中，ωd为标量权重，表示第j帧车体的世界坐标系的旋转，表示惯性测量单元到所述车体的外参数旋转，ωj为第j帧时惯性测量单元的角速度观测值，c2(t)表示惯性测量单元位姿信息中的旋转信息拟合的四维b样条曲线，c
′2(t)为c2(t)一阶导数，用于表示惯性测量单元的运动角速度方向。
[0095]
在一个实施方式中，确定车体的运动方向，并对运动方向做归一化操作。生成车体的世界坐标系的旋转的旋转矩阵，结合归一化操作的结果生成车体的运动约束函数，按照如下方式构建运动约束函数：
[0096][0097]
其中，表示车体的旋转的旋转矩阵，c1(t)表示车体的世界坐标系的位姿信息中的轨迹信息拟合的三维b样条曲线，η(c
′1(t))表示对运动方向的归一化操作结果，ωc表示标量权重。
[0098]
地面车辆运动学上的约束来源于ackermann转向原理，其瞬时运动是围绕瞬时旋转中心的旋转，该中心位于车辆后轮轴直线上。若以后轮轴上任意一点作为车体中心，则此时车体的朝向yb与运动方向平行，车体朝向yb可由车体位姿的旋转的y轴分量表示。运动方向可通过三维b样条曲线c1(t)的一阶导数c
′1(t)表示，表示对运动方向c
′1(t)做归一化操作。
[0099]
在一个实施方式中，误差函数按照以下公式构建：
[0100][0101]
其中，ρ表示减小异常数据影响的损失函数，表示车体坐标系到相机ck的外参数，表示第j帧时刻车体的旋转的旋转矩阵，表示车体的位移，xi表示特征点i在三维空间中特征点的位置，表示特征点i在第j帧图像上的二维观测值，ωs表示标量权重，表示离散的车体位移估计值，c1(tj)表示第j帧时刻的位移，c1(t)表示车体的世
界坐标系的位姿信息中的轨迹信息拟合的三维b样条曲线，ωc表示标量权重，η(c
′1(t))表示对运动方向的归一化操作结果，ωd表示标量权重，表示惯性测量单元到车体的外参数旋转，ωj表示第j帧时惯性测量单元的角速度观测值，c2(t)表示惯性测量单元位姿信息中的旋转信息拟合的四维b样条曲线。
[0102]
基于集束优化方法对该函数进行优化，最终待优化参数为车体的世界坐标系的位姿信息，包括位移和旋转最终得到优化后的车体坐标系的位姿。
[0103]
在本实施例中还提供了一种基于视觉的集束优化装置，该装置用于实现上述实施例及优选实施方式，已经进行过说明的不再赘述。如以下所使用的，术语“模块”可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的装置较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。本实施例提供一种基于视觉的集束优化装置，如图5所示，包括：
[0104]
车体位姿获取模块51，用于获取车体的世界坐标系的位姿信息；
[0105]
平滑约束函数生成模块52，用于根据车体的世界坐标系的位姿信息构建车体的平滑约束函数，并计算重投影误差函数；
[0106]
角速度约束函数生成模块53，用于构造惯性测量单元的角速度观测值与所述车体的旋转之间的角速度约束函数；
[0107]
运动约束函数生成模块54，用于根据车体的世界坐标系的位姿信息以及车体的平滑约束曲线，确定车体的运动方向，基于车体的运动方向和车体的世界坐标系的位姿信息，生成车体的运动约束函数；
[0108]
误差函数构建模块55，用于根据车体的平滑约束曲线、重投影误差函数、运动约束函数以及角速度约束函数构建误差函数，并对误差函数进行联合优化，生成优化后的车体坐标系的位姿信息。
[0109]
上述各个模块的更进一步的功能描述与上述对应实施例相同，在此不再赘述。
[0110]
请参阅图6，图6是本发明可选实施例提供的一种电子设备的结构示意图，如图6所示，该电子设备可以包括：至少一个处理器601，例如cpu(central processing unit，中央处理器)，至少一个通信接口603，存储器604，至少一个通信总线602。其中，通信总线602用于实现这些组件之间的连接通信。其中，通信接口603可以包括显示屏(display)、键盘(keyboard)，可选通信接口603还可以包括标准的有线接口、无线接口。存储器604可以是高速ram存储器(random access memory，易挥发性随机存取存储器)，也可以是非不稳定的存储器(non-volatile memory)，例如至少一个磁盘存储器。存储器604可选的还可以是至少一个位于远离前述处理器601的存储装置。其中处理器601可以结合图5所描述的装置，存储器604中存储应用程序，且处理器601调用存储器604中存储的程序代码，以用于执行上述任一方法步骤。
[0111]
其中，通信总线602可以是外设部件互连标准(peripheral component interconnect，简称pci)总线或扩展工业标准结构(extended industry standard architecture，简称eisa)总线等。通信总线602可以分为地址总线、数据总线、控制总线等。为便于表示，图6中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。
[0112]
其中，存储器604可以包括易失性存储器(英文：volatile memory)，例如随机存取
存储器(英文：random-access memory，缩写：ram)；存储器也可以包括非易失性存储器(英文：non-volatile memory)，例如快闪存储器(英文：flash memory)，硬盘(英文：hard disk drive，缩写：hdd)或固态硬盘(英文：solid-state drive，缩写：ssd)；存储器604还可以包括上述种类的存储器的组合。
[0113]
其中，处理器601可以是中央处理器(英文：central processing unit，缩写：cpu)，网络处理器(英文：network processor，缩写：np)或者cpu和np的组合。
[0114]
其中，处理器601还可以进一步包括硬件芯片。上述硬件芯片可以是专用集成电路(英文：application-specific integrated circuit，缩写：asic)，可编程逻辑器件(英文：programmable logic device，缩写：pld)或其组合。上述pld可以是复杂可编程逻辑器件(英文：complex programmable logic device，缩写：cpld)，现场可编程逻辑门阵列(英文：field-programmable gate array，缩写：fpga)，通用阵列逻辑(英文：generic array logic,缩写：gal)或其任意组合。
[0115]
可选地，存储器604还用于存储程序指令。处理器601可以调用程序指令，实现如本技术实施例的基于视觉的集束优化方法。
[0116]
本发明实施例还提供了一种非暂态计算机存储介质，所述计算机存储介质存储有计算机可执行指令，该计算机可执行指令可执行上述任意方法实施例中的基于视觉的集束优化方法。其中，所述存储介质可为磁碟、光盘、只读存储记忆体(read-only memory，rom)、随机存储记忆体(random access memory，ram)、快闪存储器(flash memory)、硬盘(hard disk drive，缩写：hdd)或固态硬盘(solid-state drive，ssd)等；所述存储介质还可以包括上述种类的存储器的组合。
[0117]
本技术提供的技术方案，通过车体的世界坐标系的位姿信息构建车体的平滑约束函数，计算重投影误差函数，并基于平滑约束函数确定车体运动方向从而构建车体的运动约束函数。构造惯性测量单元的角速度观测值以及车体的旋转之间的角速度约束函数，最终根据得到的平滑约束函数、重投影误差函数、运动约束函数以及角速度约束函数构建误差函数，并对误差函数进行优化以得到车体坐标系的位姿信息。引入b样条曲线以及车辆运动约束，对于集束优化的车辆轨迹进行建模，大幅提升基于视觉的slam系统的稳定性和精度。
[0118]
虽然结合附图描述了本发明的实施例，但是本领域技术人员可以在不脱离本发明的精神和范围的情况下做出各种修改和变型，这样的修改和变型均落入由所附权利要求所限定的范围之内。