CN113158816A - 面向室外场景物体的视觉里程计二次曲面路标构建方法 - Google Patents

Abstract

本发明涉及一种面向室外场景物体的视觉里程计二次曲面路标构建方法，包括：视觉里程计实时的进行实例分割，统计更新地图点观测频数，构建亲和力矩阵，采用匈牙利分配算法完成关联或者初始为新物体；对于满足初始化条件的物体采用参数分离的切平面线性重构方法完成二次曲面初始化并进行参数解算，构建观测误差函数进行非线性优化不断对二次曲面路标更新与优化，获得最终的估计效果。本发明考虑了室外场景下运动物体的情况，并克服了二次曲面切平面直接线性重构容易受噪声影响的问题，提升了室外场景下移动机器人对环境物体估计的准确性以及环境理解能力，有助于智能驾驶中导航与避障等目标导向的工作。

Description

面向室外场景物体的视觉里程计二次曲面路标构建方法

技术领域

本发明涉及计算机视觉及机器人领域，具体涉及一种面向室外场景物体的视觉里程计二次曲面路标构建方法。

背景技术

物体视觉里程计是指在利用传统视觉里程计对相机进行跟踪时，加入语义信息获取方法，构建对环境物体描述。相比于传统视觉里程计，物体视觉里程计不仅能完成相机跟踪的功能，同时也能为移动机器人提供更强的感知能力，支持其完成如避障、导航等目标导向的任务。物体视觉里程计的关键问题在于物体路标的构建，在多种路标构建方法中二次曲面由于数学表达简单但表示准确是合适的物体表示方法，但目前物体视觉里程计中二次曲面路标构建存在以下挑战：1)物体观测噪声较大，除了本身视觉里程计相机跟踪时相机位姿估计以及几何特征提取和匹配存在误差，各种语义信息获取方法由于遮挡甚至误检漏检同样存在噪声，这为二次曲面路标的重构带来了困难。2)物体位姿估计计算代价大，为了获得更准确的物体表示，部分二次曲面路标构建方法会引入基于深度学习的物体位姿估计方法或者点云处理方法，这些方法实时性差且需要较高计算性能的平台。3)室外场景物体数据关联困难，室外场景存在不少移动物体，如行人移动车辆等，运动物体不能简单利用物体位置分布进行关联，且容易导致遮挡从而导致错误的数据关联难以在实际移动机器人平台应用。

因此，如何设计能够满足实际应用于室外场景物体视觉里程计中二次曲面路标构建方法成为当前亟需解决的问题。

发明内容

针对以往物体视觉里程计二次曲面路标构建方法噪声敏感，数据关联不能对应运动物体的不足，本发明提出了一种新的面向室外场景物体的视觉里程计二次曲面路标构建方法。

一种面向室外场景物体的视觉里程计二次曲面路标构建方法包括：

首先将检测结果分配至已有物体或者创建新物体完成数据关联，之后对于满足初始化条件的物体进行直接线性重构完成二次曲面的初始化，在视觉里程计有新的关键帧检测加入时，应用非线性优化不断对二次曲面路标进行更新和优化。

该方法能够在存在运动物体的室外场景下对于静态物体稳定关联，在二次曲面路标重构时对于相机跟踪和物体检测噪声有较好的鲁棒性，提升机器人环境感知能力，智能驾驶的导航、避障等目标导向的应用场景。

本发明的技术方案是这样实现的：1)首先视觉里程计远程调用YOLACT网络完成实时实例分割，统计更新地图点观测频数。2)对于每个检测结果与历史物体计算多种跟踪线索，构建亲和力矩阵，采用匈牙利分配算法完成关联或者初始为新物体。3)对于满足初始化条件的物体采用参数分离的切平面线性重构方法完成二次曲面初始化。4)在初始化完成或新关键帧检测到来时，对二次曲面矩阵进行参数解算，并构建观测误差函数进行非线性优化不断对二次曲面路标更新与优化，获得最终的估计效果。

室外场景物体的视觉里程计二次曲面路标构建方法，包括如下步骤：

步骤一、视觉里程计接收相机采集图像帧并进行处理，远程调用服务端YOLACT网络完成实例分割，服务端采用编码并回传当前帧检测实例集合，视觉里程计对检测实例掩码范围内投影地图点观测统计进行更新；

步骤二、将步骤一获得的检测实例与历史物体实例进行关联，结合卡尔曼滤波预测检测框、范围投影地图点观测频数分布以及已初始化二次曲面投影重叠率多信息加权计算每个检测结果与物体之间的距离，并构建亲和力矩阵采用匈牙利分配算法完成关联，对于没有成功关联的检测结果初始化为新的物体实例；

步骤三、对于步骤二中获得新分配但未初始化的物体实例进行初始化，以所有关联检测实例的检测框有效边缘反投影平面为切平面，根据对偶二次曲面定义构建齐次线性约束，利用中心三角化近似平移分量，并结合纯偏航角假设对齐次线性约束降维，剩余目标参数使用SVD分解求解，获得二次曲面路标初始值；

步骤四、对步骤三获得的线性重构的二次曲面矩阵物体实例以及步骤二中关键帧获得新分配的已初始化物体实例进行多自由度(如9自由度)参数解算，并对二次曲面路标进行非线性优化，获得最终的估计结果。

可选地，步骤一包括：

视觉里程计程序在在接受到摄像机采集得到的图像帧后，排队至检测线程进行处理，并远程调用服务端YOLACT网络完成实例分割，服务端完成推理后将压缩回传当前图像帧检测实例集合D＝{D_j|j∈[0,J]}，

其中，J表示实例分割推理得到当前帧检测实例数量，j代表当前帧检测集合中第j个检测实例，每一个检测实例D_j主要包含检测框向量B_i以及物体对应二值掩码M_j；

对于每一个检测实例D_j，视觉里程计统计其掩码范围内地图点X投影集合X_j：

X_j＝{X|In(M_j,PX)} (2-1)

其中In(*)为判断投影点是否位于检测掩码内操作，P为相机内参矩阵。

可理解的是，检测实例和对应的投影地图点集合是步骤一的输出，下一步的输入。

可选地，步骤二包括：

多跟踪线索的亲和力矩阵构建，采用匈牙利算法实现步骤一获得的检测实例到视觉里程计已有物体实例的分配，完成数据关联。

对应于检测实例为图像平面的物体表示，物体实例定义为对于物体在三维空间中的表示，二次曲面路标即是物体实例具体的表示方法，如果物体实例已经创建，但是二次曲面路标仍未初始化完成，则称未初始化物体实例，反之称为初始化物体实例；

在步骤一获得当前图像帧检测集合D后，视觉里程计在已创建的物体实例中过滤出候选匹配物体实例集合O＝{O_i|i∈[0,I]}，其中I表示候选匹配物体实例数量，i表示集合内第i个物体实例，如果O为空集，则D所有检测实例创建为新的未初始化物体实例；否则对每个检测实例D_j与每个物体O_i之间计算距离，作为分配时评价的依据；

计算距离时依据了以下跟踪线索进行计算：

1)卡尔曼滤波预测距离，对于历史物体存在已有检测信息，根据历史检测信息对当前帧检测框位置进行预测以及更新；

检测框滤波状态向量x以及观测向量z数学表示为：

其中，u，v与

分别表示检测框中心坐标以及其移动速度，s与

表示检测框面积及其变化速率，r表示检测框纵横比；

利用检测框的状态更新方程代入卡尔曼滤波器进行预测，以O_i对应的预测检测框B_i与D_j实例分割检测框B_j重叠率作为计算距离，记为

2)地图点观测频数分布距离，对于视觉里程计每一地图点都会统计自其初始化之后所属检测框被分配物体实例标签O_i的概率分布P(O＝O_i|X)，记地图点X被物体实例观测总频数为N_X，根据观测D_j地图点集合X_j中统计所有地图点对物体实例O_i观测频数分布律作为地图点观测分布距离，记为

其中，分母

统计了D_j内地图点对所有物体实例标签的观测总频数，分子

统计了D_j对于物体实例O_i标签观测总频数。

3)二次曲面投影距离，如果参与计算的物体实例O_i已经完成初始化时，计算二次曲面路标投影检测框，对偶二次曲面

在相机内参矩阵P作用下投影为对偶二次曲面

后对应的外切检测框表达式为：

其中，(x₁,y₁)和(x₂,y₂)分别表示检测框的左上和右下顶点坐标。最后根据式(2-3)计算出重叠率距离，记为

最后根据上述三种跟踪线索，利用匈牙利算法完成当前帧每一个检测实例D_j至物体实例O_i的分配，并输出获得新分配检测实例的所有物体实例。可理解的是，数据关联步骤的输出即为步骤三的输入。

可选地，步骤三包括：

参数分离的切平面直接线性初始化对偶二次曲面路标；

1)检测框边缘反投影切平面约束构建，

对于步骤二中每一个获得新分配的未初始化物体实例，通过初始化条件判断后会尝试调用所有历史分配的检测实例构建切平面约束，对于每一个检测实例所属的相机帧c，对应相机内参矩阵记为P_c，其中关联的检测框第k个边缘记为

计算反投影平面记为

并根据由当前相机c坐标系至参考相机r坐标系的欧式变换T_rc变换到同一坐标系表示：

其中，normalize为对计算得到的平面向量规范化操作。

以反投影平面为切平面构建关于求解目标参考相机r坐标系下对偶二次曲面参数

的线性约束：

其中，

为对对偶二次曲面矩阵

的上三角拉直，m_*k表示为第k个边缘提供的线性约束向量。

2)平移分量独立估计；

采用中心三角化近似的方法对对偶二次曲面的平移量t_rq进行独立估计：

其中T_wr为参考相机的位姿，

为检测框中心非齐次坐标值，[i]表示取向量第i个元素，[i,*]表示取矩阵第i行向量；

3)纯偏航角参数分离，

基于纯偏航角旋转假设，对二次曲面参数进行化简：

其中，R_rq为待求纯偏航角旋转矩阵，θ_y为

待求偏航角角度，[a_x,a_y,a_z]^T为

待求轴长长度向量，

已在此前平移分量独立估计中求出。此时式(2-9)解目标向量中

已为常数项，与齐次项

合并，整理后获得最后降维的新线性约束：

根据式(2-10)线性方程约束可以求解出

剩余参数

结合此前已知常数项

即可求得最终的对偶二次曲面参数矩阵

这里即为步骤三的输出。

可选地，步骤四包括：

二次曲面非线性优化；

1)对偶二次曲面参数解算；

步骤三求解得到对偶二次曲面参数矩阵

后，需要从矩阵中解算出9自由度参数，平移分量t_rq独立求解且引入纯偏航角假设，待求参数仅有轴长[a_x,a_z]以及偏航角θ_y，待求参数根据式(2-9)联立相关方程并求解得到：

根据解算得到的9自由度参数作为初始值，并构建以下三个优化误差函数：预测检测框误差、轴长先验误差和纹理平面误差；

2)预测检测框误差；

再一次采用式(2-5)，获得待优化二次曲面O_i在第k个关键帧下的投影检测框

与关键帧k上的关联检测实例D_ik的检测框

构建距离代数误差e_b：

3)轴长先验误差，

实例分割能够给出物体类别cls(O_i)，根据统计方法确定同一类别物体的先验尺寸a_prior，与估计二次曲面轴长采用长短轴对应后直接计算代数误差获得轴长先验误差：

4)纹理平面误差；

在公式(2-1)中统计了检测D_j范围内地图点观测集合；在平面范围内对于地图点投影采用三角剖分，划分出物体纹理平面投影，

在三维空间下，根据三角片面顶点X₁，X₂，X₃可以计算出平面的数学表示：

其中，

是点X_i的非齐次表达式

表示为检测实例D_ik构建的纹理平面，

为纹理平面的法向量，

为纹理平面原点距离；根据

的法向量，计算出和

平行的世界坐标系二次曲面

的切面

为

的上三维块矩阵，t_wq为

世界坐标系下的平移分量，最后以纹理片面

与相平行的二次曲面切平面

之间正交距离作为最后的纹理平面距离e_p：

最后结合预测检测框误差、轴长先验误差和纹理平面误差对对偶二次曲面路标

进行非线性优化，提升路标表示的准确性。

根据本发明的另一方面，一种电子设备，包括存储器和处理器，其中，存储器中存储有计算机程序，处理执行所述存储器中存储的计算机程序，具体为执行上述第一方面任一所述的面向室外场景物体的视觉里程计二次曲面路标构建方法。

有益效果

本发明算法考虑了室外场景下运动物体的情况，设计了多跟踪信息融合的数据关联方法。并根据二次曲面切平面直接线性重构容易受噪声影响的问题，设计了纯偏航假设下平移分量参数分离的线性重构方法对二次曲面矩阵进行初始化。最后在初始化完成或新关键帧观测到来时，对二次曲面矩阵进行参数解算并结合多种优化误差函数进行非线性优化，构建二次曲面路标，提升了室外场景下移动机器人对环境物体估计的准确性以及环境理解能力，有助于智能驾驶中导航与避障等目标导向的工作。

附图说明

图1为本发明一实施例的地图点观测频数示意图；

图2为本发明一实施例检测框边缘反投影切平面示意图；

图3为本发明检测框中心三角化近似平移分量示意图；

图4为本发明纹理平面投影划分示意图；

图5为本发明具体实施方式的算法执行流程示意图；

图6为本发明具体实施方式的检测线程通信流程示意图；

图7为本发明具体实施方式的室外物体数据关联效果图；

图8为本发明具体实施方式的二次曲面路标创建效果图。

具体实施方式

为了更好的解释本发明，以便于理解，下面结合附图，通过具体实施方式，对本发明作详细描述。

为了更好的理解上述技术方案，下面将参照附图更详细地描述本发明的示例性实施例。虽然附图中显示了本发明的示例性实施例，然而应当理解，可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更清楚、透彻地理解本发明，并且能够将本发明的范围完整的传达给本领域的技术人员。

本发明实施例的方法关键在于，计算轻量且对噪声鲁棒的二次曲面路标重构方法，以及应对运动物体稳定关联和检测的物体数据关联方法。为此，结合图1至图8，对本发明实施例的一种面向室外场景物体的视觉里程计二次曲面路标构建方法进行详细说明。

在本实施例中，图像实例分割后通过结合多种物体跟踪线索信息计算关联权重，利用匈牙利分配算法完成数据关联。采用切平面直接线性重构方法初始化二次曲面，应用平移分量独立估计以及纯偏航角假设提高初始化鲁棒性，构建二次曲面观测的误差函数非线性优化提升估计效果。

在本实施例中，一种面向室外场景物体的视觉里程计二次曲面路标构建方法，首先将检测结果分配至已有物体或者创建新物体完成数据关联，之后对于满足初始化条件的物体进行直接线性重构完成二次曲面的初始化，在视觉里程计有新的关键帧检测加入时，应用非线性优化不断对二次曲面路标进行更新和优化。

该方法能够在存在运动物体的室外场景下对于静态物体稳定关联，在二次曲面路标重构时对于相机跟踪和物体检测噪声有较好的鲁棒性，提升机器人环境感知能力，智能驾驶的导航、避障等目标导向的应用场景。

本发明的技术方案是这样实现的：1)首先视觉里程计远程调用YOLACT网络完成实时实例分割，统计更新地图点观测频数。2)对于每个检测结果与历史物体计算多种跟踪线索，构建亲和力矩阵，采用匈牙利分配算法完成关联或者初始为新物体。3)对于满足初始化条件的物体采用参数分离的切平面线性重构方法完成二次曲面初始化。4)在初始化完成或新关键帧检测到来时，对二次曲面矩阵进行参数解算，并构建观测误差函数进行非线性优化不断对二次曲面路标更新与优化，获得最终的估计效果。

室外场景物体的视觉里程计二次曲面路标构建方法，包括如下步骤：

步骤一：视觉里程计远程调用实例分割，统计更新地图点观测频数。

视觉里程计在视觉里程计对每帧输入图像进行压缩，远程调用服务端YOLACT网络完成实例分割，服务端完成推理后将结果压缩回传。根据返回结果，视觉里程计统计检测结果D_j掩码范围内地图点X投影集合X_j：

X_j＝{X|In(D_j,PX)} (3-1)

其中In(*)为判断投影点是否位于检测掩码内操作，P为相机内参矩阵。

步骤二：多跟踪线索的亲和力矩阵构建，采用匈牙利算法实现检测实例D_j到物体实例O_i的分配，完成数据关联。

对应于检测实例为图像平面的物体表示，物体实例定义为对于物体在三维空间中的表示，二次曲面路标即是物体实例具体的表示方法，如果物体实例已经创建，但是二次曲面路标仍未初始化完成，则称未初始化物体实例，反之称为初始化物体实例。

在获得当前相机检测结果D_j以及对应的地图点投影集合后X_j，与参与分配每个的物体O_i之间计算距离，作为分配时评价的依据。计算距离时依据了以下跟踪线索进行计算：

1)卡尔曼滤波预测距离。对于历史物体存在已有检测信息，可以根据历史检测信息对当前帧检测框位置进行预测以及更新。检测框滤波状态向量以及观测向量数学表示为：

其中，u，v与

分别表示检测框中心坐标以及其移动速度，s与

表示检测框面积及其变化速率，r表示检测框纵横比。检测框的位置与面积随相机运动不断变化而纵横比一般保持不变，进而可以利用检测框的状态更新方程代入卡尔曼滤波器进行预测，以预测O_i检测框B_i与D_j检测框B_j重叠率作为计算距离：

2)地图点观测频数分布距离。对于视觉里程计每一地图点都会统计自其初始化之后所属检测框被分配物体实例标签的概率分布P(O|X)，记地图点被物体实例观测总频数为N_X，由于同一帧下地图点可能位于重叠的多个检测掩码，所以N_X有可能大于地图点实际观测帧数。具体地图观测频数的示意如附图1所示。根据观测D_j地图点集合X_j中统计所有地图点对物体实例O_i观测频数分布律作为地图点观测分布距离：

3)二次曲面投影距离。如果对应计算的物体实例已经完成初始化，可以计算二次曲面路标投影检测框，对偶二次曲面

在相机内参矩阵P作用下投影为对偶二次曲面

后对应的外切检测框表达式为：

根据式(3-3)可以计算出重叠率距离，记为

步骤三：参数分离的切平面直接线性初始化对偶二次曲面路标。

1)检测框边缘反投影切平面约束构建。对于相机c下关联检测框第k个边缘

可以计算其反投影平面并欧式变换到参考相机坐标系r下，具体检测框边缘反投影操作如附图2所示：

以反投影平面为切平面可以构建关于对偶二次曲面参数的线性约束：

2)平移分量独立估计。为解决参数耦合的问题，采用中心三角化近似的方法对对偶二次曲面的平移量t_rq进行独立估计，中心三角化近似平移分量估计过程如附图3所示：

其中T_wr为参考相机的位姿，

为检测框中心非齐次坐标值。

3)纯偏航角参数分离。再引入纯偏航角旋转假设，对二次曲面参数进行化简：

其中，R_rq为待求纯偏航角旋转，此时式(3-9)解目标向量中

已为常数项，可以与齐次项

合并，整理后获得最后降维的新线性约束：

步骤四：二次曲面非线性优化。

1)对偶二次曲面参数解算。步骤三求解得到对偶二次曲面参数矩阵后，需要从矩阵中解算出9自由度参数，平移分量t_rq独立求解且引入纯偏航角假设，待求参数仅有轴长[a_x,a_z]以及偏航角θ_y，待求参数根据式(3-9)可以联立相关方程并求解得到：

根据解算得到的9自由度参数可以作为初始值，并构建以下优化误差函数。

2)预测检测框误差。再一次采用式(3-5)，可以获得待优化二次曲面O_i在第k个关键帧下的投影检测框

与关键帧k上的检测结果

可以构建距离代数误差e_b：

3)轴长先验误差。实例分割能够给出物体类别cls(O_i)。根据统计方法可以确定同一类别物体的先验尺寸a_prior，与估计二次曲面轴长采用长短轴对应后直接计算代数误差可以获得轴长先验误差：

4)纹理平面误差。在式(3-1)中统计了检测D_j范围内地图点观测集合。在平面范围内对于地图点投影采用三角剖分，划分出物体纹理平面投影，纹理平面投影划分的示意如附图4所示。在三维空间下，根据三角片面顶点X₁，X₂，X₃可以计算出平面的数学表示：

其中，

是点X_i的非齐次表达式。根据

的法向量，可以计算出

和二次曲面平行切面

最后以纹理片面

与相平行的二次曲面切平面

之间正交距离作为最后的纹理平面距离e_p：

另外，本实施方式的算法的具体流程如附图5所示：

步骤一：视觉里程计首先与服务端建立通信连接，输入采集双目图象对后，视觉里程计检测线程对图像分辨率进行降采样并采用JPGE压缩算法进行图像压缩，Protobuf预定义消息进行数据压缩以及二进制序列化，gRPC远程调用服务端深度网络模块的接口传输图像信息，检测线程开始等待服务端应答，应答结果到达后，客户端对数据掩码结果进行RLE解码，从跟踪线程中统计地图点投影，并与实例检测信息一同构建检测实例D_j，完成检测结果的解析操作。

服务端的深度网络模块监听客户端连接并等待客户端请求，当有新的请求到达时，服务端对图像解压缩并调整至网络输入分辨率，调用YOLACT实例分割网络进行推理，推理完成后对掩码结果采用RLE编码，与检测结果一同封装为Protobuf消息并序列化压缩，向请求客户端发起应答，检测线程与服务端通信流程图如附图6所示。

步骤二：在检测线程处理检测实例信息同时，跟踪线程同时对输入图像进行特征提取、立体匹配、相机跟踪与关键帧筛选，完成上述视觉里程计的相机跟踪任务后，跟踪线程等待检测实例的到达。获得当前帧检测结果后，将进行数据关联操作。首先根据当前视觉里程计跟踪的相机位姿选择候选匹配的物体集合，具体选择范围为相机视角范围内50m以内所有未关联失败的物体。对于候选物体，使用卡尔曼滤波预测步骤更新预测检测框状态：

其中P_t为t时刻状态的协方差矩阵，Q_t为系统状态转移方程协方差矩阵，F为系统状态转移矩阵。随后对于每一组检测实例D_j和物体实例O_i，可以根据式(3-3)计算卡尔曼预测重叠率距离

式(3-4)计算地图点观测视频分布距离

如果物体已经初始化，还需要计算二次曲面投影距离

最终检测实例D_j相对物体实例O_i的亲和力矩阵A通过距离加权求和的方式构建，调用匈牙利算法完成A矩阵的行列规约，实现检测实例到物体的分配：

N为参与分配所有检测实例的数量，M为参与分配所有物体实例数量。对于每一类距离都会有最大距离限制，当计算的亲和距离超过对应阈值时，整体亲和力距离a_ij将指定为inf，拒绝D_j和物体实例O_i的关联。对于每一物体实例O_i，都会选择与其对应最小有效距离a_ij的检测实例D_j作为关联。对式(3-1)集合的地图点更新物体观测频数，并调用卡尔曼滤波更新步骤使用D_j对检测框状态进行更新校正：

其中H_t为系统观测矩阵，R_t为系统观测协方差矩阵。对于未被分配的检测实例D_j，会使用其数据直接初始化新的物体实例，对于违背分配的物体实例O_i，会更新其连续关联失败计数器，如果该物体实例未初始化且连续关联失败超过指定阈值，将该物体标记为错误物体并从系统中剔除，室外场景下物体跟踪效果如附图7所示。

步骤三：参数分离的二次曲面切平面直接线性重构。完成步骤二的数据关联后，对于所有获得新关联检测的物体实例，按照以下三个条件判断是否满足二次曲面初始化条件：

(1)判断条件的当前帧为视觉里程计关键帧；

(2)已关联至少5个检测结果；

(3)检测结果之间最大视差大于阈值。

对于满足初始化条件的物体实例，统计所有检测框的边缘，过滤出距离图像边缘距离超过阈值，且不与任何其余检测框有重叠的边缘参与初始化。每一条边缘根据式(3-6)、式(3-8)、式(3-10)计算线性约束，所有边缘构建的线性约束联立构成齐次线性方程组：

m′_K为式(3-10)单个线性约束向量，K为应用于初始化的切平面总数，线性约束矩阵M′采用SVD方法对目标参数向量求规范解，最后求解参数如下构建最终对偶二次曲面矩阵参数

步骤四：对于完成初始化物体实例，或者是有新的关键帧检测关联的物体实例，系统在局部地图线程中执行视觉里程计地图点筛选、局部光束平差、关键帧过滤等后端优化操作之后，均会调用非线性优化进一步估计二次曲面路标，对于初始化物体实例首先调用式(3-11)进行参数解算获得优化变量，随后根据式(3-12)、式(3-13)、式(3-16)构建优化目标函数：

Ω_b，Ω_p和Ω_s分别为投影检测误差、纹理平面误差以及先验轴长误差的信息矩阵。式(3-22)的非线性优化函数采用G²o库包含的Levenberg-Marquardt算法迭代优化求解，完成优化的二次曲面路标与视觉里程计生成的三维地图点一同保存，构成地图数据库，最后完成优化后的二次曲面构建效果如附图8所示。

本实施例的构建方法，首先视觉里程计远程调用YOLACT网络完成实时实例分割，统计更新地图点观测频数。对于每个检测结果与历史物体计算多种跟踪线索，构建亲和力矩阵，采用匈牙利分配算法完成关联或者初始为新物体。对于满足初始化条件的物体采用参数分离的切平面线性重构方法完成二次曲面初始化。在初始化完成或新关键帧检测到来时，对二次曲面矩阵进行参数解算，并构建观测误差函数进行非线性优化不断对二次曲面路标更新与优化，获得最终的估计效果。本发明考虑了室外场景下运动物体的情况，并克服了二次曲面切平面直接线性重构容易受噪声影响的问题，在视觉里程计运行同时构建准确的二次曲面路标表示，提升了室外场景下移动机器人对环境物体估计的准确性以及环境理解能力，有助于智能驾驶中导航与避障等目标导向的工作。

本实施例的方法可以应用在任一电子设备或任一智能驾驶车辆的车载控制器中，本实施例不对其限定，根据实际需要设置。任一设备可包括存储器和处理器，其中，存储器中存储有计算机程序，处理执行所述存储器中存储的计算机程序，具体为执行上述任意实施例所述的面向室外场景物体的视觉里程计二次曲面路标构建方法。

应当注意的是，在权利要求中，不应将位于括号之间的任何附图标记理解成对权利要求的限制。词语“包含”不排除存在未列在权利要求中的部件或步骤。位于部件之前的词语“一”或“一个”不排除存在多个这样的部件。本发明可以借助于包括有若干不同部件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的权利要求中，这些装置中的若干个可以是通过同一个硬件来具体体现。词语第一、第二、第三等的使用，仅是为了表述方便，而不表示任何顺序。可将这些词语理解为部件名称的一部分。

此外，需要说明的是，在本说明书的描述中，术语“一个实施例”、“一些实施例”、“实施例”、“示例”、“具体示例”或“一些示例”等的描述，是指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管已描述了本发明的优选实施例，但本领域的技术人员在得知了基本创造性概念后，则可对这些实施例作出另外的变更和修改。所以，权利要求应该解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明进行各种修改和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也应该包含这些修改和变型在内。

Claims (8)

1.一种面向室外场景物体的视觉里程计二次曲面路标构建方法，其特征在于，包括：

步骤一、视觉里程计接收相机采集图像帧并进行处理，远程调用服务端YOLACT网络完成实例分割，服务端采用编码并回传当前帧检测实例集合，视觉里程计对检测实例掩码范围内投影地图点观测统计进行更新；

步骤二、将步骤一获得的检测实例与历史物体实例进行关联，通过多信息加权计算方式获取每个检测结果与物体之间的距离，并构建亲和力矩阵采用匈牙利分配算法完成关联，对于没有成功关联的检测结果初始化为新的物体实例；

步骤三、对于步骤二中获得新分配但未初始化的物体实例进行初始化，以所有关联检测实例的检测框有效边缘反投影平面为切平面，根据对偶二次曲面定义构建齐次线性约束，利用中心三角化近似平移分量，并结合纯偏航角假设对齐次线性约束降维，剩余目标参数使用SVD分解求解，获得二次曲面路标初始值；

步骤四、对步骤三获得的线性重构的二次曲面矩阵物体实例以及步骤二中关键帧获得新分配的已初始化物体实例进行多自由度参数解算，并对二次曲面路标进行非线性优化，获得最终的估计结果。

2.根据权利要求1所述的方法，其特征在于，步骤二包括：

将步骤一获得的检测实例与历史物体实例进行关联，结合卡尔曼滤波预测检测框、范围投影地图点观测频数分布以及已初始化二次曲面投影重叠率多信息加权计算每个检测结果与物体之间的距离，并构建亲和力矩阵采用匈牙利分配算法完成关联，对于没有成功关联的检测结果初始化为新的物体实例。

3.根据权利要求1所述的方法，其特征在于，步骤一包括：

视觉里程计在在接收到摄像机采集得到的图像帧后，排队至检测线程进行处理，并远程调用服务端YOLACT网络完成实例分割，服务端完成推理后将压缩回传当前图像帧检测实例集合D＝{D_j|j∈[0,J]}，

其中，J表示实例分割推理得到当前帧检测实例数量，j代表当前帧检测集合中第j个检测实例，每一个检测实例D_j主要包含检测框向量B_i以及物体对应二值掩码M_j；

对于每一个检测实例D_j，视觉里程计统计其掩码范围内地图点X投影集合X_j：

X_j＝{X|In(M_j,PX)} (1-1)

In(*)为判断投影点是否位于检测掩码内操作，P为相机内参矩阵。

4.根据权利要求1至3任一所述的方法，其特征在于，步骤二包括：

多跟踪线索的亲和力矩阵构建，采用匈牙利算法实现步骤一获得的检测实例到视觉里程计已有物体实例的分配，完成数据关联；

在步骤一获得当前图像帧检测集合D后，视觉里程计在已创建的物体实例中过滤出候选匹配物体实例集合O＝{O_i|i∈[0,I]}，其中I表示候选匹配物体实例数量，i表示集合内第i个物体实例，如果O为空集，则D所有检测实例创建为新的未初始化物体实例；否则对每个检测实例D_j与每个物体O_i之间计算距离，作为分配时评价的依据。

5.根据权利要求4所述的方法，其特征在于，步骤二还包括：

计算距离时依据了以下跟踪线索进行计算：

1)卡尔曼滤波预测距离，对于历史物体存在已有检测信息，根据历史检测信息对当前帧检测框位置进行预测以及更新；

检测框滤波状态向量x以及观测向量z数学表示为：

其中，u，v与

分别表示检测框中心坐标以及其移动速度，s与

表示检测框面积及其变化速率，r表示检测框纵横比；

利用检测框的状态更新方程代入卡尔曼滤波器进行预测，以O_i对应的预测检测框B_i与D_j实例分割检测框B_j重叠率作为计算距离，记为

下标i、j表示了物体实例O_i和检测实例D_j；

2)地图点观测频数分布距离，对于视觉里程计每一地图点都会统计自其初始化之后所属检测框被分配物体实例标签O_i的概率分布P(O＝O_i|X)，记地图点X被物体实例观测总频数为N_X，根据观测D_j地图点集合X_j中统计所有地图点对物体实例O_i观测频数分布律作为地图点观测分布距离，记为

其中，分母

统计了D_j内地图点对所有物体实例标签的观测总频数，分子

统计了D_j对于物体实例O_i标签观测总频数；

3)二次曲面投影距离，如果参与计算的物体实例O_i已经完成初始化时，计算二次曲面路标投影检测框，对偶二次曲面

在相机内参矩阵P作用下投影为对偶二次曲面

后对应的外切检测框表达式为：

其中，(x₁,y₁)和(x₂,y₂)分别表示检测框的左上和右下顶点坐标；

最后计算出重叠率距离，记为

根据上述三种跟踪线索，利用匈牙利算法完成当前帧每一个检测实例D_j至物体实例O_i的分配，并输出获得新分配检测实例的所有物体实例。

6.根据权利要求5所述的方法，其特征在于，步骤三包括：

参数分离的切平面直接线性初始化对偶二次曲面路标；

1)检测框边缘反投影切平面约束构建，

对于步骤二中每一个获得新分配的未初始化物体实例，通过初始化条件判断后会尝试调用所有历史分配的检测实例构建切平面约束，对于每一个检测实例所属的相机帧c，对应相机内参矩阵记为P_c，其中关联的检测框第k个边缘记为

计算反投影平面记为

并根据由当前相机c坐标系至参考相机r坐标系的欧式变换T_rc变换到同一坐标系表示：

其中，normalize为对计算得到的平面向量规范化操作。

以反投影平面为切平面构建关于求解目标参考相机r坐标系下对偶二次曲面参数

的线性约束：

其中，

为对对偶二次曲面矩阵

的上三角拉直，m_*k表示为第k个边缘提供的线性约束向量；

2)平移分量独立估计；

采用中心三角化近似的方法对对偶二次曲面的平移量t_rq进行独立估计：

其中T_wr为参考相机的位姿，

为检测框中心非齐次坐标值，[i]表示取向量第i个元素，[i,*]表示取矩阵第i行向量；

3)纯偏航角参数分离，

基于纯偏航角旋转假设，对二次曲面参数进行化简：

其中，R_rq为待求纯偏航角旋转矩阵，θ_y为

待求偏航角角度，[a_x,a_y,a_z]^T为

待求轴长长度向量，

已在此前平移分量独立估计中求出；

另外，解目标向量中

已为常数项，与齐次项

合并，整理后获得最后降维的新线性约束：

根据式(1-10)线性方程约束可以求解出

剩余参数

结合此前已知常数项

即可求得最终的对偶二次曲面参数矩阵

7.根据权利要求6所述的方法，其特征在于，步骤四包括：

二次曲面非线性优化；

1)对偶二次曲面参数解算；

步骤三求解得到对偶二次曲面参数矩阵

后，需要从矩阵中解算出9自由度参数，平移分量t_rq独立求解且引入纯偏航角假设，待求参数仅有轴长[a_x,a_z]以及偏航角θ_y，待求参数根据联立相关方程并求解得到：

根据解算得到的9自由度参数作为初始值，并构建以下三个优化误差函数：预测检测框误差、轴长先验误差和纹理平面误差；

2)预测检测框误差；

获得待优化二次曲面O_i在第k个关键帧下的投影检测框

与关键帧k上的关联检测实例D_ik的检测框

构建距离代数误差e_b：

3)轴长先验误差，

实例分割能够给出物体类别cls(O_i)，根据统计方法确定同一类别物体的先验尺寸a_prior，与估计二次曲面轴长采用长短轴对应后直接计算代数误差获得轴长先验误差：

4)纹理平面误差；

在公式(1-1)中统计了检测D_j范围内地图点观测集合；在平面范围内对于地图点投影采用三角剖分，划分出物体纹理平面投影，

在三维空间下，根据三角片面顶点X₁，X₂，X₃可以计算出平面的数学表示：

其中，

是点X_i的非齐次表达式

表示为检测实例D_ik构建的纹理平面，

为纹理平面的法向量，

为纹理平面原点距离；根据

的法向量，计算出和

平行的世界坐标系二次曲面

的切面

为

的上三维块矩阵，t_wq为

世界坐标系下的平移分量，最后以纹理片面

与相平行的二次曲面切平面

之间正交距离作为最后的纹理平面距离e_p：

最后结合预测检测框误差、轴长先验误差和纹理平面误差对对偶二次曲面路标

进行非线性优化，提升路标表示的准确性。

8.一种电子设备，其特征在于，包括存储器和处理器，其中，存储器中存储有计算机程序，处理执行所述存储器中存储的计算机程序，具体为执行上述权利要求1至7任一所述的面向室外场景物体的视觉里程计二次曲面路标构建方法。

Images