(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211063510.4 (22)申请日 2022.09.01 (71)申请人 东南大学 地址 210096 江苏省南京市玄武区四牌楼 2 号 (72)发明人 潘树国　陈金晶　高旺　 (74)专利代理 机构 南京众联专利代理有限公司 32206 专利代理师 叶倩 (51)Int.Cl. G06T 7/73(2017.01) G06T 7/246(2017.01) G06T 7/269(2017.01) (54)发明名称 基于特征观测数和IMU预积分的自适应协方 差方法 (57)摘要 本发明公开了一种基于特征观测数和IMU预 积分的自适应协方差方法， 首先， 提取特征点并 将图像特征数据与IMU数据进行对齐； 通过使用 图像金字塔光流跟踪， 实现特征匹配； 再对特征 点跟踪次数进行计算， 在时间维度上获取当前帧 中每一个特征点的观测数， 通过特征点的观测 数， 计算当前帧中特征点观测数的均值和标准 差， 从而确定当前帧中特征点的静态观测权重； 通过IMU预积分对逆深度不确定性进行建模， 再 通过特征点观测数以及IMU预积分进行自适应协 方差的计算， 得到自适应的协方差矩阵， 最后将 得到的自适应协方差矩 阵与位姿求解的误差函 数进行融合构建不确定性加权的重投影误差， 有 效缓解了动态目标的影 响， 并且避免过度剔除导 致特征分布扭曲以及过少的视 觉测量。 权利要求书2页 说明书6页 附图2页 CN 115457127 A 2022.12.09 CN 115457127 A 1.基于特 征观测数和IMU预积分的自适应协方差方法， 其特 征在于,包括如下步骤： S1： 提取特征点并将图像特征数据与 IMU数据进行对齐， 所述对齐策略为： 第一个IMU数 据的时间戳小于上一帧图像结束的时间戳， 最后一个IMU数据的时间戳是第一个大于当前 帧图像结束的时间戳； S2： 通过使用图像金字塔光 流跟踪， 实现特 征匹配； S3： 对特征点跟踪次数进行计算， 在时间维度上获取当前帧中每一个特征点的观测数， 通过计算特征点的观测数， 计算当前帧中特征点观测数 的均值和标准差， 从而确定当前帧 中的特征点的静态 观测权重W(pi)； S4： 通过IMU预积分对逆深度不确定性进行建模， 所述特征点的逆深度不确定性与该特 征点在相机帧下归一向量的叉乘化坐标和帧间平移成反比， 与特征匹配不确定性δx成正 比， 用IMU的预积分项表示逆深度不确定性具体如下： 其中， xi＝K‑1pi， pi是特征点在图像平面上的齐次坐标， 表示xi的斜对称矩阵， 为 第i帧图像与第j帧图像之间IMU预积分得到的平移向量， δx＝f‑1δp为归一化平面上不确定 度形成的圆的半径， f为相机的焦距， δp为特征点特征匹配时产生的误差， δ d表 示第j帧中特 征点的逆深度的不确定性； S5： 通过步骤S3获得的特征点观测数以及步骤S4获得的IMU预积分进行自适应协方差 的计算， 得到自适应的协方差矩阵， 对于第l个特征点， 其静态观测权重为W(pl)， 其逆深度 不确定性 为 δ dl， 其自适应协方差矩阵Ql为： Ql＝WcW(pl)δ dlI2×2 其中， Wc为特征点原始协方差矩阵， I2×2为单位矩阵； S6： 将上述计算出的特征点的协方差矩阵与位姿求解的误差函数进行融合构建不确定 性加权的重投影误差， 所述 不确定性加权 重投影误差为： 其中， pl为第l个特征点在当前帧图像上的二维坐标， n为特征点总个数， 将特征点的协 方差矩阵进行分解可以得到Q‑1＝U∑‑1UT， 其中， 2.如权利要求1所述的基于特征观测数和IMU预积分的自适应协方差方法， 其特征在 于， 所述步骤S1中， 提取Shi ‑Tomasi角点， 使用掩码策略使提取到的特征点分布均匀， 所述 掩码策略为： 将已提取的特征点为圆心， r为半径的圆作为掩码， 剩余提取到的特征点坐标权　利　要　求　书 1/2 页 2 CN 115457127 A 2不落在掩码区域内。 3.如权利要求1或2所述的基于特征观测数和IMU预积分的自适应协方差方法， 其特征 在于， 所述 步骤S2进一 步包括： S21:在时间维度上获取当前帧中每一个特征点的观测数； 假设第k帧图像中的第i个特 征点用 表示， 从第一帧到当前帧， 如果特 征点pi被第k帧观察到， 则pi的观测数； Nk(pi)＝Nk‑1(pi)+1 其中， Nk(pi)是特征点pi在第k帧的观测数， 特 征点提取之后第一次被光 流跟踪到； 如果Nk(pi)大于观测数阈值Tv,则Nk(pi)＝Tv； S22： 计算当前帧中特 征点观测数的均值 μk和标准差σk， 具体为： 其中， nk为第k帧特 征点数量； S23： 根据步骤S2 2的计算结果， 计算当前帧的每 个特征点的静态 观测权重， 具体为： 其中， α 为大于零的实数。 4.如权利要求3所述的基于特征观测数和IMU预积分的自适应协方差方法， ， 其特征在 于： 所述步骤S21中， 如果特征点pi被观察到， 则特征点pi提取之后第一 次被光流跟踪到， 此 时该特征点pi已经经历了两帧图像， 设置初始值 为1； 如果特征点pi没有观测到， 则Nk(pi)＝Nk‑1(pi)， 在当前帧特征观测数计算完成后， 清除 该特征点的观测数信息， 并令Nk(pi)＝0。 5.如权利要求4所述的基于特征观测数和IMU预积分的自适应协方差方法， 其特征在 于： 所述步骤S3中， 特征跟踪的次数决定特征跟踪质量， 特征点在帧间跟踪的次数越多， 特 征点的质量越好。权　利　要　求　书 2/2 页 3 CN 115457127 A 3