(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202111651540.2 (22)申请日 2021.12.3 0 (71)申请人 杭州电子科技大 学 地址 310018 浙江省杭州市下沙高教园区2 号大街 (72)发明人 洪慧　单珂珂　 (74)专利代理 机构 杭州天昊专利代理事务所 (特殊普通 合伙) 33283 专利代理师 董世博 (51)Int.Cl. G06T 7/00(2017.01) G06T 7/80(2017.01) G06V 10/44(2022.01) (54)发明名称 一种基于半直接法的单目视觉里程计的设 计方法 (57)摘要 本发明公开了一种基于半直接法视觉里程 计的设计方法， 包括如下步骤： S1、 通过相机采集 图像数据， 获得图像序列； S2、 对单目相机初始 化， 确定其尺度； S3、 采用稀 疏图像对齐最小化两 个像素块之间的光度误差， 获取粗略的相机位 姿， 以及像素点之间的对应关系； S4、 进行ORB特 征提取和BRIEF描述子的计算， 优化相机的位姿； S5、 将三维地图点与当前帧的特征点进行匹配， 利用最小化重投影误差进一步优化当前位姿； S6、 输出相机位姿； S7、 判断是否有新 图像加入， 若是则执行步骤S3， 进行算法的循环， 否则结束 流程； 本发明的方法 处理速度快， 定位精度高， 能 够处理稀疏纹理、 光照变化以及移动物体等难 题。 权利要求书3页 说明书7页 附图1页 CN 114494150 A 2022.05.13 CN 114494150 A 1.一种基于半直接法的单目视 觉里程计的设计方法， 其特 征在于， 包括如下步骤： S1： 通过相机采集图像数据， 获得图像序列， 并传输到分析设备上； 其中分析设备为电 脑， 相机为单目相机； S2： 通过单目初始化 流程， 确定图像的尺度； S3： 基于直接法估计相机的初始位姿； S4： 基于特征点法优化相机的初始位姿； 特征点法包括对图像进行ORB 特征提取和特征 匹配的过程； S5： 通过将三维地图点与当前帧的特征点进行匹配， 利用最小化重投影误差进一步优 化当前相机位姿； S6： 输出相机位姿， 包括 三维坐标点、 平 移向量和旋转向量； S7： 判断是否有新图像加入； 若是则返回S3， 进入算法的循环； 否则结束流 程。 2.根据权利要求1所述的一种基于半直接法的单目视觉里程计的设计方法， 其特征在 于， 所述S2中的单目初始化 流程包括如下步骤： S21： 移动相机， 并获取图像， 组成图像序列； 其中， 移动相机时， 要求在设定方向上移动 超过设定值的距离； S22： 选取图像序列中的前一帧作为初始帧， 后一帧作为 参考帧； S23： 通过对初始帧图像和参考帧图像进行ORB特征点提取和特征匹配， 建立两帧图像 之间的数据关联； S24： 利用三角测量得到初始帧中特征点的深度； 其中三角测量表示通过两处观察同一 个点的夹角， 从而确定该点的距离 。 3.根据权利要求1所述的一种基于半直接法的单目视觉里程计的设计方法， 其特征在 于， 所述S23中的数据关联的建立， 首先需要设定任意参考空间点P， 空间点P在两帧图像中 对应的投影点像素坐标分别为p1， p2； K为相机的内参， P为空间点P的世界坐标， 设定初始帧 到参考帧的相机运动变换为旋转矩阵R， 平移向量t； 然后通过对极几何约束求出本质矩阵 E； 最后使用八点法对求得的本质矩阵E进行奇异值分解， 获得旋转矩阵R， 平 移向量t。 4.根据权利要求3所述的一种基于半直接法的单目视觉里程计的设计方法， 其特征在 于， 所述对极几何表达式如下： p2TK‑Tt^RK‑1p1＝0          (1) E＝t^R               (2) 在已经获得R和t的基础上， 设定两个特征点的深度为s1， s2； 其中深度 s1， s2满足如下公 式： s1x1＝s2Rx2+t            (3) 其中， x1和x2表示两个特征点R和t归一化后的坐标； 最后通过最小二乘法进行求解出s1 和s2。 5.根据权利要求3所述的一种基于半直接法的单目视觉里程计的设计方法， 其特征在 于， 所述S3中的初始位姿估计过程为采用稀疏图像对齐， 最小化两个像素块之间的光度误 差获取粗略的相机位姿， 以及像素点之间的对应关系。 6.根据权利要求5所述的一种基于半直接法的单目视觉里程计的设计方法， 其特征在 于， 所述初始位姿估计过程具体包括：权　利　要　求　书 1/3 页 2 CN 114494150 A 2首先设定参考空间点P， 参考空间点P在初始帧图像和参考帧图像 中对应的投影点像素 坐标分别为p1， p2； 则空间点P在两帧图像中的像素光度误差e为： e＝I1(p1)‑I2(p2)           (3) 其中， I1(p1)表示初始帧图像中像素坐标为p1的像素光度； I2(p2)表示参考帧图像中像 素坐标为p2的像素光度； 通过像素光度误差e获得优化一个空间点P的光度误差的二范数 为： minξJ( ξ )＝||e||2           (4) 其中， ξ表示相机位姿的李 代数； J( ξ )表示相机位姿； 考虑空间中N个像素点的光度误差ei＝I1(p1， i)‑I2(p2， i)， i∈(1， N)， 则N个像素点的相 机的位姿估计表示 为： 求式(5)中的优化问题， 需要通过分析像素光度误差e和相机位姿 的李代数ξ的导数关 系； 其中通过使用李 代数的扰动模型， 给 exp( ξ )左乘一个设定的小扰动exp( δ ξ )获得： 其中， K为相 机的内参； P为空间点P的世界坐标； Z1、 Z2分别表示； 设定q为扰动分量在第 二个相机坐标系的三维坐标， u为扰动分量的像素坐标， 获得： q＝ δ ξ ^exp( ξ ^)P             (7) 将式(7)、 (8)代入式(6)， 并对式(6)进行一阶泰勒公式展开， 获得： 其中， 表示u处的像素梯度， 表示投影方程关于相机坐标系下的三维空间点的导 数， 表示变换后的三维点对的导数； δ ξ 通过设定的小扰动exp( δ ξ )获得； 最后推导出单个点误差相对于李代数ξ 的雅克比矩阵； 并通过列文伯格 ‑马夸尔特方法 进行增量计算， 获得N个 像素点的光度误差， 迭代求 解相机位姿J， 表示 为： 7.根据权利要求4所述的一种基于半直接法的单目视觉里程计的设计方法， 其特征在 于， 所述S4中， 基于特 征点法优化相机的初始位姿， 包括如下步骤： S41： 首先需要将图像分成若干大小相等的网格， 对每个网格进行Oriented  FAST角点 提取， 使用四叉树分配算法均匀化特 征点； S42： 构建直方图， 筛 选出与主流方向一 致的特征点作为 候选匹配点； S43： 对保留下来的特征点进行BRIEF描述子的计算； 其 中， 首先找出所有描述子之间的 最小距离和最大距离， 当描述子之间的距离小于两倍最小距离时， 则匹配成功， 否则为误匹权　利　要　求　书 2/3 页 3 CN 114494150 A 3