具体实施方式

下面，将参考附图详细地描述根据本申请的示例性实施例。显然，所描述的实施例仅仅是本申请的一部分实施例，而不是本申请的全部实施例，应理解，本申请不受这里描述的示例实施例的限制。

申请概述

为了对可移动设备进行追踪及导航，以便于分析可移动设备的运动行为，同时提供行驶策略，通常需要确定可移动设备的位姿。目前，通过在移动设备上通常安装传感器(比如，相机、惯性测量单元等)，以采集可移动设备在行驶过程的运动数据及图像，根据图像进行相机标定(相机标定指的是利用相机所拍摄到的图像来还原空间中的物体)，根据相机标定的结果及运动数据，确定可移动设备的位姿。考虑到相机标定和运动数据的精度不够，具体地，相机标定的误差会体现在空间重建中，而运动数据得到的是相对位姿，前面某一帧的误差会一直传递到后面去，导致最后位姿误差有可能非常大，最终会限制系统整体的精度。因此，通常需要对位姿进行优化。目前的位姿优化方法中，通常需要恢复相机拍摄的图像中物体的空间信息，之后，基于恢复的图像中物体的空间信息，进而优化位姿。

但是，相机标定过程中恢复的图像中物体的空间信息的数据量是庞大的，会占据电子设备过多的存储空间，降低位姿优化效率。

本公开则充分考虑到上述缺点，通过获取当前帧图像的位姿，并确定位姿对应的可视空间区域，之后确定矢量地图在可视空间区域内的地图点，基于位姿及当前帧图像的特征信息，确定地图点投影到当前帧图像中的投影误差，之后，利用投影误差优化位姿，进而获取经过优化的位姿，该过程无需恢复图像中物体的空间信息，利用矢量地图中的地图点作为图像中物体的空间信息，从而节约电子设备的存储空间，提高位姿优化效率。

示例性方法

图1是本申请一种示例性实施例提供的位姿优化方法的流程示意图。

本实施例可应用在电子设备上，具体可以应用于智能设备、服务器或一般计算机上，智能设备包括但不限于自动驾驶汽车、无人机和智能机器人。

如图1所示，本申请一种示例性实施例提供的位姿优化方法至少包括如下步骤：

步骤101，获取图像采集设备在采集当前帧图像时对应的第一位姿。

图像采集设备固定安装在可移动设备上，随着可移动设备的运动而不断的改变位姿，并基于位姿不断的对可移动设备在行驶过程中的前方路面进行拍照，从而不断的采集图像，综上可得，图像与图像采集设备的位姿是对应的，因此，当前帧图像对应一个图像采集设备的位姿，图像采集设备的位姿可指示可移动设备的位姿。

需要说明的是，可移动设备中除了图像采集装置外还包括若干个传感器，这些传感器采集的数据能够反映出可移动设备的运动信息及位置信息，根据这些传感器采集的数据可粗略的估计图像采集设备的初始位姿，具体地，当前帧图像为第一帧图像时，获取若干个传感器采集的数据，对这些数据进行融合，从而确定由旋转矩阵及平移矩阵组成的第一位姿，第一位姿通常是粗略的，不能较为准确的反映图像采集设备的实际位姿，因此，以第一帧图像对应的第一位姿为初始位姿，进行位姿优化。平移矩阵指示了世界坐标系的原点和相机坐标系的原点之间的距离，旋转矩阵指示了世界坐标系分别绕横轴、纵轴和竖轴旋转到相机坐标系的效果之和，即世界坐标系绕横轴的旋转矩阵、纵轴的旋转矩阵和竖轴的旋转矩阵的乘积，旋转矩阵指示了世界坐标与相机坐标系的旋转关系。

需要说明的是，不同传感器采集数据的时间点往往是不同的，通常需要对各个传感器采集的数据进行时间同步，保证传感器采集数据的时间点与图像采集设备采集图像的时间点相同，从而保证传感器采集的数据的参考价值。

具体地，可移动设备指的是能够运动的物体，比如自动驾驶车辆、无人机和智能机器人。当前帧图像指的是图像采集设备当前时刻最新采集的图像，即图像采集设备采集的时间点最晚的图像。图像采集设备指的是具有拍照功能的设备，比如单目相机、双目相机。图像采集设备的位姿指的是图像采集设备在世界坐标系下的位置和姿态，位置主要通过在世界坐标系下的平移矩阵体现，姿态主要通过在世界坐标系下的旋转矩阵体现。

考虑到成像过程中的畸变、显示区域的大小及电子设备的存储空间，通常需要对采集的图像进行图像预处理，在这里，图像预处理包括但不限于去畸变和降采样。显而易见的，当前帧图像是图像预处理后的图像。

步骤102，确定所述第一位姿对应的可视空间区域。

图像采集设备能获取特定拍摄范围之内的信息，特定拍摄范围指示了图像采集设备的视距，视距指的是在可移动设备在正常行驶中，图像采集设备能连续拍摄的空间范围中各个方位对应的最远距离，这些方位包括上、下、左、右、前、后，视距通常是固定不变的，举例来说，图像采集设备的视距包括前视距离80米、左视距离20米，右视距离20米，上视距离15米，下视距离5米。可视空间区域以图像采集设备的位置为基准点，因此，基于图像采集设备对应的第一位姿和视距即可确定可视空间区域，可视空间区域指的是在世界坐标系下的图像采集设备的视距对应的空间坐标集合形成的区域，具体地，空间坐标集合包括但不限于前视距离和后视距离对应在横轴上的两个坐标点分别对应的坐标值、左视距离和右视距离对应在纵轴上的两个坐标点分别对应的坐标值、上视距离和下视距离对应在竖轴上的两个坐标点分别对应的坐标值和图像采集设备的空间坐标值。请参考图2，可视空间区域由左视距离、右视距离、上视距离和下视距离形成的平面上的4个顶点分别对应的空间坐标及图像采集设备的空间坐标形成的四棱锥，该平面与图像采集设备具有固定的相对位置关系，因此，基于前视距离和第一位姿能够确定该平面上的4个顶点分别对应的空间坐标，前视距离是图像采集设备与该平面的垂直距离。

步骤103，确定矢量地图中落入所述可视空间区域的地图点的空间坐标。

矢量物体由由点、线、矩形、多边形、圆和弧线等元素形成，这些元素都是通过数学公式计算获得的，由于矢量物体可通过公式计算获得，所以矢量物体的文件体积一般较小，矢量地图由矢量物体组成，所以矢量地图的文件体积通常较小，从而可节约电子设备的存储空间，进而提高电子设备的数据处理效率。矢量物体最大的优点是无论放大、缩小或旋转等不会失真，则矢量地图指的是能够任意扩大、缩小或旋转等不会失真的地图，同时可以根据需要调节矢量地图的比例尺的大小，以改变显示内容的详细程度。

考虑到矢量地图中矢量物体的轮廓的复杂性不同，为了简化计算并快速检索到矢量地图中的矢量物体，通常不选择去检索矢量物体本身，而是采用最小外接包围框作为检索对象，在最小外接包围框的形状确定后，最小外接包围框指示了矢量物体的最小区域边界，可更为准确的反映出矢量物体的区域范围，通过构建最小外接包围框的空间索引以实现对矢量物体的检索，从而可快速简单准确的检索出落入可视空间区域内的最小外接包围框，之后，需要对检索出的最小外接包围框内的矢量物体进行提取，基于提取的矢量物体确定地图点。在一种可能的实现方式中，通过确定矢量物体的最小外接包围框的若干个空间坐标，并确定最小外接包围框的若干个空间坐标形成的区域内的矢量物体的物体信息，物体信息包括物体类别、物体几何信息和物体关键节点的空间坐标，将这些物体信息作为一个物体元素存储起来，从而实现对矢量地图的解算，矢量地图的解算指的是对矢量地图中各个矢量物体的拆分，显而易见的，利用物体元素里的物体信息可生成对应的矢量物体，即恢复原有的矢量物体。举例来说，以矢量物体是标志牌为例，则物体类别为标志牌、物体几何信息为长方体、物体关键节点的空间坐标包括组成标记牌的八个顶点的空间坐标。之后，利用空间数据索引Rtree对矢量地图中的每个最小外接包围框的若干个空间坐标进行检索，确定位于可视空间区域中的最小外接包围框，显而易见的，当最小外接包围框的若干个空间坐标只有部分位于可视空间区域内时，该最小外接包围框也是位于可视空间区域内的，从而检索出部分落入可视空间区域内的矢量物体，之后，提取这些最小外接包围框对应的物体元素，利用物体元素中的物体信息生成矢量物体，从而得到由多个矢量物体形成的子图。

考虑到矢量物体通过公式计算，通常矢量地图中描绘矢量物体的关键结点相对较少(比如，柱子给出的是两端的顶点，路面箭头给出的是箭头顶点)，即子图中描绘矢量物体的关键节点相对较小，为了增加地图点的数量，提高位姿优化的准确性，需要对子图中的矢量物体进行升采样处理，以获取更多的采样点，并确定这些采样点的空间坐标，在这里，升采样的方式包括但不限于均匀采样，或靠近关键节点多采样，远离关键节点少采样的方式，需要说明的是采样点通常位于矢量物体的边缘轮廓上。考虑到检索最小外接包围框时，最小外接包围框的若干个空间坐标部分位于可视空间区域时，也会保留该最小外接包围框，并将该最小外接包围框对应的矢量物体加入子图，导致子图中的部分矢量物体不是完整地落入可视空间区域内，即不是所有的采样点都位于可视空间区域内，为了确保采样点的准确性，需要根据采样点的空间坐标对采样点进行筛选，以提取空间坐标落入可视空间区域内的采样点，并将其作为地图点，将采样点的空间坐标作为地图点的空间坐标，显而易见的，地图点位于可视空间区域内。矢量地图是基于世界坐标系建立的，因此，地图点的空间坐标位于世界坐标系。

需要说明的是，在三维重建过程中会不断的恢复图像中物体的空间信息，这些空间信息会占据电子设备大量的存储空间，同时降低电子设备的数据处理效率。但是对于矢量地图而言，仅需要不断从矢量地图中选择位于可视空间区域内的地图点，而矢量地图文件占据的存储空间较小，从而可节约电子设备的存储空间，提高电子设备的数据处理效率。

步骤104，确定所述当前帧图像的特征信息。

对当前帧图像进行语义分割，并根据语义分割结果进行特征提取，从而确定用于描述图像中物体的特征信息，特征信息包括但不限于颜色特征信息、纹理特征信息、形状特征信息及空间关系特征信息中的任意一种或多种，颜色特征信息及纹理特征信息均用于描述了图像中物体的表面性质，形状特征信息用于描述图像中物体的整体形状或轮廓形状，空间关系特征信息用于描述图像中多个物体之间的空间位置或相对方向关系。在具体的场景中，需要结合图像中物体的特征确定特征信息。

步骤105，根据所述第一位姿、所述地图点的空间坐标和所述特征信息，确定所述地图点在所述当前帧图像中的投影误差。

具体地，当前帧图像对应有图像坐标系，图像采集设备对应有相机坐标系，地图点的空间坐标对应有世界坐标系，世界坐标系通过刚体变换到达相机坐标系，然后相机坐标系通过透视投影变换到达图像坐标系。可以看出，世界坐标与图像坐标的关系建立在刚体变换和透视投影变换的基础上。刚体变换指的是在三维空间中，图像采集设备不发生形变时，把图像采集设备作旋转及平移的运动。透视投影指的是用中心投影法将形体投射到投影面上，从而获得的一种较为接近视觉效果的单面投影图，中心投影法指的是投射线汇交于投射中心。

具体地，通过图像采集设备的外部参数，外部参数是用来描述相机坐标系与世界坐标系的关系，由旋转矩阵和平移矩阵所构成的齐次矩阵，可以确定世界坐标系与相机坐标系的转换关系，之后，通过图像采集设备中的预设相机内参可以确定相机坐标系与图像坐标系之间的转换关系，预设相机内参只与图像采集设备中的相机内部结构有关，而与相机位置参数无关，主要包括图像主点(图像主点指的是摄影中心与图像平面的垂线，与图像平面的交点)坐标、单个像素高度及宽度、相机的有效焦距和透镜的畸变失真系数等，预设相机内参通常是不变的。

综上，基于第一位姿和预设相机内参可确定出世界坐标系与图像坐标系之间的转换关系，之后，根据该转换关系将地图点的空间坐标投影到当前帧图像中，以获取地图点在当前帧图像中的图像坐标，根据特征信息及地图点投影到当前帧图像中的图像坐标，确定地图点在当前帧图像的投影误差，考虑到地图点通常是位于矢量物体的边缘轮廓上，因此，投影误差指的是地图点投影在当前帧图像中的图像坐标与观测物体的边缘轮廓的图像坐标之间的距离，观测物体应当为地图点所属物体类别对应的物体，在保证误差在可接受范围内的前提下，观测物体也可以是当前帧图像中距离地图点投影在当前帧图像中的图像坐标最近的物体。需要说明的是，观测物体的边缘轮廓具有若干个图像坐标，而观测物体的边缘轮廓的图像坐标与地图点投影到当前帧图像中的图像坐标之间距离是最近的。图像坐标指的是图像坐标系下的坐标。观测物体的边缘轮廓的图像坐标是根据特征信息确定的。

步骤106，根据所述投影误差对所述第一位姿进行优化，以获得经过优化的第一位姿。

投影误差是基于第一位姿确定的，通过投影误差可以构造投影误差优化函数，投影误差优化函数指示了所有地图点的投影误差之和，当第一位姿变化时，投影误差也会发生变化，相应的，投影误差优化函数也会发生变化，最小化投影误差优化函数，从而优化第一位姿。投影误差指示了第一位姿与实际位姿之间的区别，从而可确保经过优化的第一位姿的准确性。

需要说明的是，基于时间序列，若干个经过优化的位姿之间的曲线通常是不完整的，因此，通常需要对经过优化的位姿与传感器采集的能够反映出可移动设备的运动信息及位置信息的数据进行融合，从而确定第一位姿，使得若干个第一位姿之间的曲线较为平滑，以更为准确的反映出图像采集设备的运动曲线。显而易见的，也可以不进行数据融合，直接将经过优化的位姿确定为第一位姿。

本实施例中通过确定矢量地图中落入可视范围内的地图点在当前帧图像中的投影误差，投影误差可反映出位姿与实际位姿之间的区别，基于投影误差优化位姿，可得到准确性相对较高的经过优化的位姿，同时考虑到矢量地图文件体积较小及无需恢复图像中物体的空间信息，从而避免引入其他误差，节省电子设备的存储空间，提高电子设备的数据处理效率，进而提高位姿优化效率。

图3示出了如图1所示的实施例中根据所述第一位姿、所述地图点的空间坐标和所述特征信息，确定所述地图点在所述当前帧图像中的投影误差步骤的流程示意图。

如图3所示，在上述图1所实施例的基础上，所述特征信息包括第一目标物体的轮廓信息，则本申请一种示例性实施例中，步骤105所示根据所述第一位姿、所述地图点的空间坐标和所述特征信息，确定所述地图点在所述当前帧图像中的投影误差步骤，具体可以包括如下步骤：

步骤1051，通过所述第一位姿，将所述地图点的空间坐标投影到所述当前帧图像中，以确定所述地图点的投影像素坐标。

图像坐标系包括图像物理坐标系及图像像素坐标系，相机坐标系通过透视投影变换到达图像物理坐标系，图像物理坐标系通过离散化变换到图像像素坐标系，离散化指的是不改变数据相对大小的条件下，对数据进行相应的缩小。图像像素坐标系和图像物理坐标系都在成像平面上，只是各自的原点和度量单位不一样。图像物理坐标系的原点通常情况下是图像主点，单位是毫米，属于物理单位。而图像像素坐标系以图像的顶点为坐标原点，单位是像素。考虑到图像采集设备中相机采集的图像为数字图像，数字图像包括若干个像素点，因此，通常是基于图像像素坐标系描述图像中每个像素点的位置，该位置即为像素坐标。

具体地，第一位姿指示了世界坐标系与相机坐标系之间的转换关系，预设相机内参指示了相机坐标系与图像像素坐标系之间的转换关系，基于第一位姿和预设相机内参，将地图点的空间坐标投影到当前帧图像中，确定该地图点投影到当前帧图像中的像素点，并确定像素点的像素坐标，该像素点的像素坐标即为地图点的投影像素坐标。

步骤1052，根据所述投影像素坐标及所述轮廓信息，确定投影误差。

第一目标物体为感兴趣的若干个物体，比如，交通标志元素(比如，柱子、标志牌、车道线、路面标注等)。轮廓信息包括但不限于感兴趣的物体的边缘像素点及边缘像素点的像素坐标。前述已知，投影误差是基于地图点与观测物体的边缘轮廓的最小距离确定的，在这里，观测物体为第一目标物体，因此，观测物体的边缘轮廓是从轮廓信息中确定的。

需要说明的是，对当前帧图像进行语义分割，可以获取当前帧图像中每个像素点的语义类别，根据语义类别提取第一目标物体对应的像素点，从而形成第一目标物体图，从第一目标物体图中提取第一目标物体边缘轮廓即可获取轮廓信息，轮廓信息包括第一目标物体对应的边缘轮廓图。

在第一种可能的实现方式中，不考虑边缘像素点所属物体类别与地图点所属物体类别是否相同，直接确定边缘轮廓图中距离投影像素坐标最近的边缘像素点的像素坐标，并将该像素坐标与投影像素坐标之间的距离值确定为投影误差，通过该投影误差最终确定的经过优化的位姿的准确性相对较低，但是操作简单快速，同时投影误差在可接受范围内，不会对确定的经过优化的位姿的准确性产生过大的影响。

在第二种可能的实现方式中，确定边缘轮廓图中距离投影像素坐标最近的边缘像素点的像素坐标，边缘像素点所属物体类别与地图点所属物体类别相同，并将该像素坐标与投影像素坐标之间的距离值确定为投影误差，该实现方式相对第一种可能的实现方式确定的经过优化的位姿的准确性相对较高。

在第三种可能的实现方式中，确定边缘轮廓图对应的残差图，残差图中每个像素点对应有一个倒角距离值，倒角距离值为像素点与距离该像素点最近的边缘像素点之间的距离值，在这里，不考虑边缘像素点所属物体类别与地图点所属物体类别是否相同，残差图与当前帧图像的大小及位置是对应的，确定投影像素坐标在残差图中的像素点，并将该像素点的倒角距离值确定为投影误差，该投影误差与第一种实现方式确定的投影误差相同，但是计算量相对较大。

在第四种可能的实现方式中，确定边缘轮廓图对应的若干个残差图，每个残差图中包括若干个第一目标物体，这些第一目标物体所属物体类别相同，每个残差图中的每个像素点对应有一个倒角距离值，倒角距离值为像素点与距离该像素点最近的边缘像素点的距离值。残差图的数量与第一目标物体所属物体类别的种类数相同，不同残差图中的若干个第一目标物体所属物体类别不同。具体地，通过确定与地图点所属物体类别相同的残差图，并确定投影像素坐标在该残差图中的像素点，将该像素点的倒角距离值确定为投影误差。该投影误差与第二种实现方式确定的投影误差相同，但是计算量相对较大，同时占据的存储资源相对较多。可以结合不同的场景，选择不同的实现方式。

需要说明的是，投影误差应当位于合理的范围内，不在合理范围内的投影误差应当直接滤除，使得投影误差可更为准确的反映出第一位姿与实际位姿的区别，从而提高确定的经过优化的位姿的准确性。

本实施例中，通过确定地图点在当前帧图像中的投影像素坐标，之后，根据投影像素坐标及轮廓信息，确定出可较为准确的反映第一位姿与实际位姿误差区别的投影误差，进而提高确定的经过优化的位姿的准确性

图4示出了如图3所示的实施例中通过所述第一位姿，将所述地图点的空间坐标投影到所述当前帧图像中，以确定所述地图点的投影像素坐标步骤的流程示意图。

如图4所示，在上述图3所示实施例的基础上，所述特征信息还包括第二目标物体的像素点，则本申请一种示例性实施例中，步骤1051所示通过所述第一位姿，将所述地图点的空间坐标投影到所述当前帧图像中，以确定所述地图点的投影像素坐标步骤，具体可以包括如下步骤：

步骤10511，通过所述第一位姿，将所述地图点的空间坐标投影到所述当前帧图像中，以获得投影点。

基于第一位姿和预设相机内参，将地图点的空间坐标投影到当前帧图像中，确定该地图点投影到当前帧图像中的像素点，该像素点即为投影点。

步骤10512，根据所述第二目标物体的像素点，确定掩膜区域。

由于矢量地图中不存在遮挡问题，而实际场景中的可移动设备在行驶过程中，可视空间区域内的第一目标物体往往会出现遮挡现象，导致图像采集设备采集的图像中的第一目标物体与矢量地图中的矢量物体往往不能很好的匹配。若地图点投影到被遮挡的区域，往往会获取一个错误的投影误差，从而影响到确定的经过优化的位姿的准确性。第二目标物体为不感兴趣的物体，即遮挡第一目标物体的遮挡物体，遮挡物体包括但不限于车辆、行人。因此，通过对第二目标物体的像素点进行掩膜处理，掩膜处理指的用选定的图像、图形或物体，对处理的像素点进行遮挡，之后，即可确定掩膜区域，从而屏蔽第二目标物体，使其不参加投影误差的计算，提高确定的经过优化的位姿的准确性。显而易见，当前帧图像之外的区域也是掩膜区域。

需要说明的是，对当前帧图像进行语义分割，可以获取当前帧图像中每个像素点的语义类别，之后，提取第二目标物体的语义类别对应的像素点即可确定第二目标物体的像素点。

步骤10513，根据所述投影点及所述掩模区域，确定投影像素坐标。

具体地，掩膜区域对应在当前帧图像中的区域设置标记，当投影点投影到当前帧图像中设置有标记的区域之外时，确定投影点的像素坐标为投影像素坐标。当投影点投影到当前帧图像中设置有标记的区域之内时，将预先设置的像素坐标确定为投影像素坐标，预先设置的像素坐标是无效的像素坐标，比如，可以预先设置一个无限大的像素坐标，此时，计算得到的投影误差非常大，直接滤除该投影误差即可。

本实施例中，通过对当前帧图像中遮挡第一目标物体的第二目标物体进行掩膜处理，使得投影到第二目标物体上的地图点的投影误差不参与位姿优化的过程，从而确保确定的经过优化的位姿的准确性。

图5示出了如图1所示的实施例中根据所述投影误差对所述第一位姿进行优化，以获得经过优化的第一位姿步骤的流程示意图。

如图5所示，在上述图1所示实施例的基础上，本申请一种示例性实施例中，步骤106所示根据所述投影误差对所述第一位姿进行优化，以获得经过优化的第一位姿步骤，具体可以包括如下步骤：

步骤1061，根据所述地图点与所述图像采集设备之间的距离，确定所述地图点的权重值。

考虑到地图点投影到图像上可能会出现分布不均匀的情况，可能会导致大多数距离图像采集设备较远的地图点往往集中在图像主点附近。而通常是距离图像采集设备较近的地图点更能反映第一位姿与实际位姿的区别，因此，需要对不同的地图点对应的投影误差增加一个权重值，以修正投影误差。权重值的大小与地图点到图像采集设备之间的距离负相关，或者与地图点到图像采集设备的距离，与可视空间区域中的最远可视距离的比值负相关，最远可视距离指的是距离图像采集装置最远的空间点到图像采集装置的距离。权重值的取值通常在0到1之间。当然，若投影误差的准确性较高，则可设置地图点的权重值为1。

步骤1062，根据所述地图点的权重值与所述地图点的投影误差对所述第一位姿进行优化，以获得经过优化的第一位姿。

利用权重值对投影误差进行修正，从而确保修正后的投影误差可更为准确的反映出第一位姿与实际位姿的区别，因此，根据权重值及投影误差构建投影误差优化函数，在这里，投影误差优化函数指示了经过权重值修正后的投影误差之和。具体地，投影误差优化函数C_chf的表达式如下：

其中，P_k表示当前帧图像的第一位姿；X表示地图点；ρ表示权重值；M_k表示可视空间区域；π表示投影；C_k(π(P_k，X))表示将地图点X基于第一位姿 P_k投影到当前帧图像中的投影误差。

需要说明的是，上述已知当前帧图像为第一帧图像时，第一位姿是传感器融合后的数据，之后，即可根据权重值及投影误差确定的投影误差优化函数对第一位姿进行优化，确定当前帧图像对应的经过优化的位姿。

本实施例中通过确定地图点的权重值，从而对投影误差进行修正，使得修正后的投影误差能够更为准确的反映第一位姿与实际位姿的区别，从而可更为准确的对位姿进行优化，得到更为准确的经过优化的位姿。

图6示出了本申请另一种示例性实施例提供的位姿优化方法的流程示意图。

如图6所示，本申请另一种示例性实施例中，在图1所示的步骤101至步骤105基础上，至少还包括如下步骤：

步骤601，对所述当前帧图像的第一特征点与先前帧图像的第二特征点进行特征匹配，以确定匹配特征点，所述匹配特征点对应同一语义类别信息。

对于图像采集设备运动时拍摄的相邻帧图像，相邻帧图像中均包含的若干个静止物体的位置和姿态的变化，与图像采集设备的位置和姿态的变化是相同的，即利用相邻帧图像的匹配特征点可以粗略的估计出图像采集设备的位姿。

具体地，对当前帧图像进行语义分割即可确定当前帧图像的第一特征点，对先前帧图像进行语义分割即可确定先前帧的第二特征点，第一特征点及第二特征点均携带语义类别信息，通过对当前帧图像及先前帧图像进行特征匹配，从而确定相邻帧图像的匹配特征点，匹配特征点携带的语义类别信息相同，同时得到的匹配特征点中已经滤除误差较大的匹配特征点，从而确保匹配特征点具有较大的参考价值，即可认为这些匹配特征点的准确性相对较高。

需要说明的是，图像中的特征点指的是图像中具有代表性及辨识度较高的区域中的像素点，特征点在图像中具有准确的位置、明确的物体属性及物体含义，通常包括关键点(Keypoint)及描述子(Descriptor)。关键点指的是特征点在图像中的位置，有些还具有方向、尺度信息；描述子通常是一个向量，按照人为的设计的方式，描述关键点周围像素的信息。在现有技术中，只要两个特征点的描述子在向量空间的距离相近，就可以认为这两个特征点是同一个特征点，为了进一步提高特征匹配的准确性，两个特征点的描述子在向量空间的距离相近同时两个特征点的语义类别相同，才可认为这两个特征点为同一特征点，即匹配特征点，从而确保匹配特征点的准确性。

步骤602，根据所述第一位姿、所述先前帧图像的第二位姿以及预设相机内参，确定基础矩阵。

用同一相机在不同位置拍摄同一物体，两张图像中的物体有重叠部分，那么理论上这两张图像会存在一定的对应关系，对于这样的两张图像，两张图像分别对应的射线中心(射线中心指的是图像成像时射线相交的点)与地图点的连线可形成对极平面，对极平面与两张图像相交的线为对极线，该过程仅和图像中的匹配的像素点有关系。综上可得，对于匹配特征点对应的两张图像，一张图像上的任意一个像素点，在另一张图像上对应有对极线，基础矩阵则指示了一个图像上的任意一个像素点与另一图像上的对极线的映射关系。

具体的，通过第一位姿及第二位姿可以确定图像采集设备的相对平移矩阵及相对旋转矩阵，之后，根据相对旋转矩阵、相对平移矩阵及预设相机内参即可确定基础矩阵。基础矩阵F的表达式如下：

F＝K^-Tt×RK^-1

其中，t表示相对平移矩阵；R表示相对旋转矩阵；K^-T表示预设相机内参的倒数的转置；K^-1预设相机内参的倒数。

步骤603，根据所述基础矩阵及匹配特征点，确定对极约束误差。

对极约束误差指示了所有匹配特征点代入对极约束表达式中的函数值之和，匹配特征点满足对极约束时，对极约束表达式的函数值等于0，匹配特征点不满足对极约束时，对极约束表达式的函数值不等于0。考虑到匹配特征点是不变的，且参考价值较高，因此，对极约束误差越小，越能说明基础矩阵的准确性越高，即优化的第一位姿的准确性越高，因此，不同的对极约束误差对应有不同的第一位姿。对极约束误差C_cpi的表达式如下：

其中，P_k表示当前帧图像的第一位姿；P_k-1表示先前帧图像的第二位姿；表示第i个匹配特征点在先前帧图像中的特征点的像素坐标的转置；x_i,k表示第i个匹配特征点当前帧图像中的特征点的像素坐标；F表示基础矩阵。

步骤604，根据所述投影误差、所述对极约束误差对所述第一位姿进行优化，以获得经过优化的第一位姿。

单靠投影误差来优化位姿，往往会出现收敛过度的情况，从而导致优化的位姿准确性相对较低。为了解决这种情况，需要加入匹配特征点的对极约束，从而避免收敛过度的情况，进而提高经过优化的位姿的准确性。

前述已知，当前帧图像为第一帧图像时，是通过投影误差对初始位姿进行优化，确定当前帧图像对应的优化的位姿，当前帧图像为第二帧图像时，该实施例中利用投影误差及对极约束误差对第二帧图像对应的第一位姿进行优化，确定第二帧图像对应的经过优化的第一位姿。

具体地，根据投影误差及对极约束误差构建误差优化函数，当第一位姿变化时，投影误差及对极约束误差也会发生变化，相应的，误差优化函数也会发生变化，最小化误差优化函数，从而优化第一位姿，进而提高位姿估算的准确性。误差优化函数C的表达式如下：

其中，σ表示投影误差修正系数；X表示地图点；M_k表示可视空间区域；π表示投影；C_k(π(P_k，X))表示将地图点X基于第一位姿P_k投影到当前帧图像中的投影误差；C_cpi表示对极约束误差；p₁表示投影误差对应的加权值；p₂表示对极约束误差对应的加权值。

投影误差修正系数主要是对投影误差进行修正，以确保修正后的投影误差的准确性，在这里，当投影误差的准确性较高时，投影误差修正系数可以为1，显而易见的，投影误差修正系数可以是前述已知的地图点的权重值，当然，投影误差修正系数也可以根据具体场景进行不同的设置，从而使得投影误差能够较为准确的反映出第一位姿与实际位姿之间的区别。

本实施例中，通过确定匹配特征点的对极约束误差，并利用对极约束误差及投影误差优化位姿，降低投影误差收敛过度的可能性，从而确保经过优化的位姿的准确性。

图7示出了本申请又一种示例性实施例提供的位姿优化方法。

如图7所示，本申请又一种示例性实施例中，在图1所示的步骤101至步骤105及图6所示的步骤601至603基础上，至少还包括如下步骤：

步骤701，通过绝对尺度传感器确定所述图像采集设备在采集所述当前帧图像与所述先前帧图像期间运动的第一平移增量。

绝对尺度传感器指的是能够测量可移动设备运动时的实际移动距离的传感器，绝对尺度传感器包括惯性测量单元(Inertial measurement unit，简称 IMU)及底盘控制传感器，惯性测量单元是测量可移动设备三轴姿态角(或角速率)以及加速度的装置，惯性测量单元时间长了会有累计误差，但是短时间内的平移增量是相对准确的。车底盘控制传感器用于测量可移动设备的运动信息。当可移动设备仅安装底盘控制传感器时，利用底盘控制传感器采集的当前帧图像与先前帧图像期间运动的平移增量为第一平移增量；当可移动设备安装底盘控制传感器及惯性测量单元时，融合底盘控制传感器及惯性测量单元采集的当前帧图像与先前帧图像期间运动的平移增量，并将融合后的平移增量确定为第一平移增量。第一平移增量指示了图像采集设备在采集先前帧图像与当前帧图像期间运动时所移动的距离值，距离值包括在横轴、纵轴和竖轴上对应的移动值。需要说明的是，绝对尺度传感器测量的是运动数据，运动数据可得到位姿增量。

步骤702，确定所述第一位姿与所述第二位姿之间的第二平移增量。

第二平移增量包括第一位姿在横轴上的坐标值与第二位姿在横轴上的坐标值的差值、第一位姿在纵轴上的坐标值与第二位姿在纵轴上的坐标值的差值和第一位姿在竖轴上的坐标值与第二位姿在竖轴上的坐标值的差值。

步骤703，根据所述投影误差、所述对极约束误差、所述第一平移增量与第二平移增量的差值对所述第一位姿进行优化，以获得经过优化的第一位姿。

对极约束是一个等式为零的约束，因此对极约束等式乘以非零常数后依然成立，这称为本质矩阵的尺度等价性，本质矩阵指的是相对旋转矩阵与相对平移矩阵的乘积。由于平移矩阵和旋转矩阵各有三个自由度，自由度具体指的是描述物体的独立参数，故本质矩阵共有六个自由度，但由于其尺度等价性，即本质矩阵与相机内参和两张图像之间的相对变换有关，而与图像之间的绝对位置没有关系，导致对极约束不能限制两张图像之间的实际距离，因此实际上本质矩阵具有5个自由度，增加第一平移增量与第二平移增量的差值的约束，使得位姿约束具有6个自由度，从而提高经过优化的位姿的准确性。

第一平移增量与第二平移增量的差值指示了图像采集设备在采集当前帧图像与先前帧图像期间运动的距离值，与第一位姿对应的图像采集设备运动到第二位姿对应的图像采集设备所移动的距离值之间的误差。

当前帧图像为第二帧图像时，第二帧图像的第一位姿为第一帧图像的优化的位姿与绝对尺度传感器采集的位姿增量之和，之后，利用投影误差、对极约束误差及第一平移增量与第二平移增量的差值对第一位姿进行优化，确定更为准确的经过优化的位姿。第二帧图像之后，当前帧图像的第一位姿均为先前帧图像的优化的位姿与绝对尺度传感器采集的位姿增量之和。

本发明实施例在投影误差、对极约束误差的基础上增加了第一平移增量与第二平移增量的差值的约束，使得位姿约束具有6个自由度，从而得到更为准确的经过优化的位姿。

图8示出了如图7所示的实施例中根据所述投影误差、所述对极约束误差、所述第一平移增量与第二平移增量的差值对所述第一位姿进行优化，以获得经过优化的第一位姿步骤的流程示意图。

如图8所示，在上述图7所示实施例的基础上，本申请又一种示例性实施例中，步骤703所示根据所述投影误差、所述对极约束误差、所述第一平移增量与第二平移增量的差值对所述第一位姿进行优化，以获得经过优化的第一位姿步骤，具体可以包括如下步骤：

步骤7031，基于所述投影误差、所述对极约束误差、所述第一平移增量与第二平移增量的差值确定目标函数。

在该实施例中，目标函数E的表达式可以为：

其中，σ表示投影误差修正系数；X表示地图点；M_k表示可视空间区域；π表示投影；C_k(π(P_k，X))表示将地图点X基于第一位姿P_k投影到当前帧图像中的投影误差；C_cpi表示对极约束误差；P_k表示当前帧图像的第一位姿；P_k-1表示先前帧图像的第二位姿；d_k表示第一平移增量；t_k表示第一位姿P_k与第二位姿P_k-1之间的第二平移增量；||d_k-t_k(P_k-1，P_k)||表示第一平移增量与第二平移增量的差值的二范数；p₃表示投影误差对应的加权值；p₄表示对极约束误差对应的加权值；p₅表示第一平移增量与第二平移增量的差值对应的加权值。需要说明的是，目标函数与误差优化函数不同，因此，投影误差和对极约束误差分别对应的加权值也可能会发生改变。

目标函数E的求解方法可以根据需要进行选择，例如可以包括高斯牛顿法或列文伯格-马夸尔特法等。

步骤7032，调整所述第一位姿，以调整所述目标函数。

对于目标函数E而言，投影误差、对极约束误差及第一平移增量与第二平移增量的差值均为自变量，通过对第一位姿的调整，可以使得目标函数E 的值发生变化。具体地，当第一位姿的发生变化时，意味着每一个地图点经过投影得到的投影误差、基础矩阵、第二平移增量也会发生变化，相应的，投影误差、对极约束误差及第一平移增量与第二平移增量的差值也发生变化，使得目标函数E也发生相应的改变。

步骤7033，确定所述目标函数满足预设条件时的第一位姿为经过优化的第一位姿。

在本实施例中，预设条件为目标函数E取最小值E_min。如上述步骤6032 所述，通过调整第一位姿，目标函数E的取值会发生变化。当目标函数E取最小值时，意味着投影误差、对极约束误差及第一平移增量与第二平移增量的差值的总和最小，此时获得的第一位姿为最佳值。

本实施例通过建立目标函数，并通过对第一位姿进行调整，使得目标函数的取值也相应调整，从而可以获得目标函数满足预设条件时的第一位姿，以实现对第一位姿的优化，进而可以获得更加精确的经过优化的第一位姿。

示例性装置

基于与本申请方法实施例相同的构思，本申请实施例还提供了一种位姿优化装置。

图9是本申请一种示例性实施例提供的位姿优化装置的结构示意图。

如图9所示，本申请一示例性实施例提供的位姿优化确定装置，包括：

获取模块901，用于获取图像采集设备在采集当前帧图像时对应的第一位姿；

区域确定模块902，用于确定所述第一位姿对应的可视空间区域；

地图点确定模块903，用于确定矢量地图中落入所述可视空间区域的地图点的空间坐标；

信息确定模块904，用于确定所述当前帧图像的特征信息；

第一误差确定模块905，用于根据所述第一位姿、所述地图点的空间坐标和所述特征信息，确定所述地图点在所述当前帧图像中的投影误差；

第一优化模块906，用于根据所述投影误差对所述第一位姿进行优化，以获得经过优化的第一位姿。

图10是本申请另一种示例性实施例提供的位姿优化装置的结构示意图。

如图10所示，在另一种示例性实施例中，所述特征信息包括第一目标物体的轮廓信息，所述第一误差确定模块905包括：

投影单元9051，用于通过所述第一位姿，将所述地图点的空间坐标投影到所述当前帧图像中，以确定所述地图点的投影像素坐标；

误差确定单元9052，用于根据所述投影像素坐标及所述轮廓信息，确定投影误差。

图11是本申请另一种示例性实施例提供的位姿优化装置中投影单元 9051的结构示意图。

如图11所示，在另一种示例性实施例中，所述特征信息还包括第二目标物体的像素点，所述投影单元9051包括：

投影子单元90511，用于通过所述第一位姿，将所述地图点的空间坐标投影到所述当前帧图像中，以获得投影点；

掩膜子单元90512，用于根据所述第二目标物体的像素点，确定掩膜区域；

坐标确定子单元90513，用于根据所述投影点及所述掩模区域，确定投影像素坐标。

如图10所示，在另一种示例性实施例中，所述第一优化模块906包括：

权重值确定单元9061，用于根据所述地图点与所述图像采集设备之间的距离，确定所述地图点的权重值；

第一优化单元9062，用于根据所述地图点的权重值与所述地图点的投影误差对所述第一位姿进行优化，以获得经过优化的第一位姿。

图12是本申请又一种示例性实施例提供的位姿优化装置的结构示意图。

如图12所示，在又一种示例性实施例中，在获取模块901、区域确定模块902、地图点确定模块903、信息确定模块904、第一误差确定模块905 基础上，还包括：

匹配模块1201，用于对所述当前帧图像的第一特征点与先前帧图像的第二特征点进行特征匹配，以确定匹配特征点，所述匹配特征点对应同一语义类别信息；

矩阵确定模块1202，用于根据所述第一位姿、所述先前帧图像的第二位姿以及预设相机内参，确定基础矩阵；

第二误差确定模块1203，用于根据所述基础矩阵及匹配特征点，确定对极约束误差；

第二优化模块1204，用于根据所述投影误差、所述对极约束误差对所述第一位姿进行优化，以获得经过优化的第一位姿。

图13是本申请再一种示例性实施例提供的位姿优化装置的结构示意图。

如图13所示，在再一种示例性实施例中，在获取模块901、区域确定模块902、地图点确定模块903、信息确定模块904、第一误差确定模块905、匹配模块1201、矩阵确定模块1202、第二误差确定模块1203基础上，还包括：

第一增量确定模块1301，用于通过绝对尺度传感器确定所述图像采集设备在采集所述当前帧图像与所述先前帧图像期间运动的第一平移增量；

第二增量确定模块1302，用于确定所述第一位姿与所述第二位姿之间的第二平移增量；

第三优化模块1303，用于根据所述投影误差、所述对极约束误差、所述第一平移增量与第二平移增量的差值对所述第一位姿进行优化，以获得经过优化的第一位姿。

图14是本申请再一种示例性实施例提供的位姿优化装置中第三优化模块1303的流程示意图。

如图14所示，在再一种示例性实施例中，所述第三优化模块1303包括：

函数确定单元13031，用于基于所述投影误差、所述对极约束误差、所述第一平移增量与第二平移增量的差值确定目标函数；

调整单元13032，用于调整所述第一位姿，以调整所述目标函数；

第二优化单元13033，用于确定所述目标函数满足预设条件时的第一位姿为经过优化的第一位姿。

示例性电子设备

图15图示了根据本申请实施例的电子设备的框图。

如图15所示，电子设备150包括一个或多个处理器151和存储器152。

处理器151可以是中央处理单元(CPU)或者具有数据处理能力和/或指令执行能力的其他形式的处理单元，并且可以控制电子设备150中的其他组件以执行期望的功能。

存储器152可以包括一个或多个计算机程序产品，所述计算机程序产品可以包括各种形式的计算机可读存储介质，例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器(RAM)和/或高速缓冲存储器(cache)等。所述非易失性存储器例如可以包括只读存储器 (ROM)、硬盘、闪存等。在所述计算机可读存储介质上可以存储一个或多个计算机程序指令，处理器151可以运行所述程序指令，以实现上文所述的本申请的各个实施例的位姿优化方法及/或者其他期望的功能。

在一个示例中，电子设备150还可以包括：输入装置153和输出装置154，这些组件通过总线系统和/或其他形式的连接机构(未示出)互连。

当然，为了简化，图15中仅示出了该电子设备150中与本申请有关的组件中的一些，省略了诸如总线、输入/输出接口等等的组件。除此之外，根据具体应用情况，电子设备150还可以包括任何其他适当的组件。

示例性计算机程序产品和计算机可读存储介质

除了上述方法和设备以外，本申请的实施例还可以是计算机程序产品，其包括计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的位姿优化方法中的步骤。

所述计算机程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本申请实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、C++等，还包括常规的过程式程序设计语言，诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。

此外，本申请的实施例还可以是计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的位姿优化方法中的步骤。

所述计算机可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。

以上结合具体实施例描述了本申请的基本原理，但是，需要指出的是，在本申请中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本申请的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本申请为必须采用上述具体的细节来实现。

本申请中涉及的器件、装置、设备、系统的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、系统。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“诸如但不限于”，且可与其互换使用。

还需要指出的是，在本申请的装置、设备和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本申请的等效方案。

提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本申请。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本申请的范围。因此，本申请不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。

为了例示和描述的目的已经给出了以上描述。此外，此描述不意图将本申请的实施例限制到在此公开的形式。尽管以上已经讨论了多个示例方面和实施例，但是本领域技术人员将认识到其某些变型、修改、改变、添加和子组合。