A visual SLAM algorithm based on adaptive inertial navigation assistant feature matching

基于自适应惯导辅助特征匹配的视觉SLAM算法 A visual SLAM algorithm based on adaptive inertial navigation assistant feature matching first-author 贾晓雪（1997-），女，河南郑州人，硕士研究生，2020年于河南理工大学获得学士学位，主要从事视觉惯性同步定位与建图方面的研究。E-mail： jia_xiao_xue@163.com http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467128&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467132&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467130&type= 22.01332855 32.00399780 贾晓雪 （1997-），女，河南郑州人，硕士研究生，2020年于河南理工大学获得学士学位，主要从事视觉惯性同步定位与建图方面的研究。E-mail： jia_xiao_xue@163.com jia_xiao_xue@163.com corresp E-mail： dongqingtree@qq.com E-mail： dongqingtree@qq.com 赵冬青（1976-），男，湖北十堰人，教授，硕士生导师，2007年于解放军信息工程大学获得博士学位，主要从事卫星导航，室内定位以及多传感器融合定位技术的研究。E-mail：dongqingtree@qq.com http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467135&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467156&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467154&type= 22.01333618 32.00399780 赵冬青 （1976-），男，湖北十堰人，教授，硕士生导师，2007年于解放军信息工程大学获得博士学位，主要从事卫星导航，室内定位以及多传感器融合定位技术的研究。E-mail： dongqingtree@qq.com dongqingtree@qq.com 信息工程大学 地理空间信息研究所，河南 郑州 450001 信息工程大学 地理空间信息研究所，河南 郑州 450001 Geography Space Information Institute， Information Engineering University， Zhengzhou 450001， China Geography Space Information Institute， Information Engineering University， Zhengzhou 450001， China 为提高视觉同步定位与建图系统的定位、建图精度，克服传统算法中特征匹配搜索半径为固定值导致视觉里程计在高动态运动时特征误匹配率高的问题，本文提出一种搜索半径自适应的惯导辅助图像特征匹配方法。该方法首先对双目相机左右两帧图像进行特征提取与匹配，并获取地图点三维坐标，然后通过预积分惯性测量单元的量测值预测相机位姿，再根据误差传播定律计算预测位姿的协方差，最后利用预测位姿将地图点投影至图像得到对应像素坐标，从而根据像素坐标中误差确定地图点最有可能出现的区域半径。实验结果表明，该方法可有效缩小特征匹配的搜索半径，显著提高图像特征匹配的准确度，使跟踪线程位姿精度提高约38.09%，系统整体位姿精度提高约16.38%。该方法可为每个特征点提供自适应区域约束，提高特征点匹配的准确度，进而提升系统位姿估计精度，构建更高精度的稠密地图。 This paper proposes a feature-matching algorithm based on an adaptive search radius to improve the accuracy of SLAM localization and mapping. This method can overcome the problem in which the search radius of feature matching is fixed in the traditional algorithm， leading to a high mismatching rate of the visual odometer in high dynamic motion. The algorithm first extracts and matches the features of the left and right images of the binocular camera and obtains the three-dimensional coordinates of the map points. Second， the camera pose is predicted by the measured values of the pre-integral inertial measurement unit. Then， the covariance of the predicted pose is calculated according to the error propagation law. Finally， the predicted pose is used to project the map points to the image to get the corresponding pixel coordinates. According to the error in pixel coordinates， the most likely radius of the map point is determined. Experimental results show that this method can effectively reduce the search radius of feature matching and significantly improve the accuracy of image feature matching. The position and pose accuracy of the tracking thread in the ORB-SLAM3 system is improved by approximately 38.09%， and the system's whole position and pose accuracy is improved by approximately 16.38%. This method can provide an adaptive region constraint for each feature point， improve the accuracy of feature point matching， improve the precision of position and pose estimation of the whole SLAM system， and build a more accurate dense map. 计算机视觉 视觉里程计 特征匹配 自适应阈值 惯性导航 computer vision vision odometry feature matching adaptive threshold inertial navigation 1 引 言 1 视觉同步定位与建图技术（Simultaneous Localization and Mapping， SLAM）同时包含定位与建图两个问题，提高SLAM系统的定位精度是构建精确稠密三维点云地图的前提。视觉里程计作为SLAM的关键部件，其主要任务是根据相邻图像的匹配信息估计相机运动，其中对图像进行特征匹配是最关键环节之一，匹配的准确度直接影响视觉里程计的精度 ［ 1 1 - 3 3 1-3 ］ 。常用的特征匹配算法有暴力匹配算法和快速最近邻匹配算法 ［ 4 4 - 5 5 4-5 ］ 。然而，当相机运动过快时，图像模糊、场景动态、光线变化和图像特征本身的局部特性，使特征误匹配广泛存在，并成为制约视觉里程计性能提升的一大瓶颈 ［ 6 6 - 7 7 6-7 ］ 。 为解决图像特征误匹配率高的问题，国内外很多学者提出基于惯性测量单元（Inertial Measurement Unit， IMU）辅助的视觉SLAM系统 ［ 8 8 - 9 9 8-9 ］ 。文献［ 10 10 ］提出了一种仿射尺度不变特征变换（Affine Scale-invariant Feature Transform， ASIFT）特征匹配方法，利用IMU提供的运动信息虚构相机视图并估计本质矩阵，以提高特征匹配的稳健性。文献［ 11 11 ］利用图像的对极约束建立了前后两帧图像像素所在极线的数学关系，并结合IMU预测相机位姿获取特征点所在极线，有效地剔除误匹配的特征点。文献［ 12 12 - 14 14 12-14 ］提出了一种全局搜索和局部搜索交替操作并自动切换的特征匹配算法，利用相机投影模型和IMU预积分结果预测特征点可能出现的区域，并对局部固定区域进行搜索匹配。同样地，文献［ 15 15 ］提出的视觉惯性SLAM系统ORB-SLAM3在特征匹配阶段也引入了惯导约束，利用IMU预测相机位姿，将双目相机左右匹配或三角化得到的地图点投影至图像，预测像素位置。 然而IMU测量值本身存在误差，在惯导辅助图像特征匹配时，通常将全图像的特征匹配转换为以预测位置为原点，某一固定值为半径的局部固定区域特征匹配。为了适配不同精度IMU，普遍做法是设置一个较大的搜索半径值，以避免漏掉特征点，这在IMU精度较高时易造成计算资源浪费，增加图像特征的误匹配率，使SLAM系统的位姿估计精度降低。为此，本文提出了一种自适应的惯导辅助视觉特征匹配方法，以惯性测量单元误差为基础，根据误差传播定律自动调整图像特征匹配搜索半径，不仅能提升特征点匹配的准确度，还能提高双目视觉惯性SLAM的定位精度，并构建更高精度的稠密地图。 2 惯导辅助图像特征匹配 1 为提高视觉惯性里程中图像特征匹配的准确度，ORB-SLAM3利用IMU预积分预测相机位姿，将图像预处理获取的地图点三维坐标投影至当前帧，预测地图点在当前帧图像的像素位置，从而将全图像特征匹配转换为局部区域的特征匹配，具体流程如 图1 图1 所示。 2.1　双目图像预处理 2 双目相机一般由左眼和右眼两个水平放置的相机组成，经过校正的左右相机只在 <math id="M1"><mi>x</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468195&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468194&type= 2.20133328 2.79399991 轴上有位移，所以地图点在左右相机的像素坐标也只在 <math id="M2"><mi>x</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468195&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468194&type= 2.20133328 2.79399991 轴（对应图像的 <math id="M3"><mi>u</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467183&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467180&type= 1.86266661 2.79399991 轴）上有差异。通过计算双目图像对应像素点的视差，可以估计出像素的深度。记地图点 <math id="M4"><mi>P</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467188&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467186&type= 2.62466669 2.79399991 的左侧像素坐标为 <math id="M5"><msub><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi mathvariant="normal">l</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467194&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467192&type= 3.13266683 4.57200003 ，右侧像素坐标为 <math id="M6"><msub><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467217&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467214&type= 3.21733332 4.57200003 ，几何关系如 图2 图2 所示。根据相似三角形原理可得地图点在相机系下的深度值 <math id="M7"><mi>z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467280&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467277&type= 1.77800000 2.79399991 为： <math display="block" id="M8"><mtable><mtr><mtd><mi>z</mi><mo>=</mo><mfrac><mrow><mi>f</mi><mi>b</mi></mrow><mrow><mi>d</mi></mrow></mfrac></mtd></mtr><mtr><mtd><mi>d</mi><mo>=</mo><mo stretchy="false">|</mo><mtext> </mtext><msub><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi mathvariant="normal">l</mi></mrow></msub><mo>-</mo><msub><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow></msub><mtext> </mtext><mo stretchy="false">|</mo></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467233&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467229&type= 20.06599998 14.39333248 ， 其中： <math id="M9"><mi>f</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467241&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467237&type= 2.45533323 3.64066648 为相机的焦距， <math id="M10"><mi>b</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467249&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467245&type= 1.86266661 2.79399991 为双目相机的基线。 将特征点在相机坐标系下的深度值 <math id="M11"><mi>z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467280&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467277&type= 1.77800000 2.79399991 代入针孔相机投影模型，可得地图点在相机系下的坐标 <math id="M12"><msub><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>c</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467287&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467283&type= 3.55599999 3.72533321 为： <math display="block" id="M13"><msub><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>c</mi></mrow></msub><mo>=</mo><mfrac><mrow><mi>z</mi></mrow><mrow><mi>f</mi></mrow></mfrac><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mi>u</mi><mo>-</mo><msub><mrow><mi>c</mi></mrow><mrow><mi>x</mi></mrow></msub></mtd></mtr><mtr><mtd><mi>v</mi><mo>-</mo><msub><mrow><mi>c</mi></mrow><mrow><mi>y</mi></mrow></msub></mtd></mtr><mtr><mtd><mi>f</mi></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467292&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467290&type= 23.96066666 14.39333248 ， 其中： <math id="M14"><mo stretchy="false">(</mo><mi>u</mi><mo>,</mo><mi>v</mi><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467708&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467691&type= 8.72066593 3.55599999 是地图点 <math id="M15"><mi>P</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467303&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467300&type= 2.62466669 2.79399991 在左目相机图像平面坐标系下的像素坐标， <math id="M16"><msub><mrow><mi>c</mi></mrow><mrow><mi>x</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467307&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467305&type= 2.70933342 3.72533321 和 <math id="M17"><msub><mrow><mi>c</mi></mrow><mrow><mi>y</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467312&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467309&type= 2.62466669 3.72533321 是相机内参。 2.2　IMU预积分 2 惯性测量单元是由三轴陀螺仪和三轴加速度计组成的，陀螺仪和加速度计分别以一定的频率采集载体系 <math id="M18"><mo stretchy="false">(</mo><mi>b</mi><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467333&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467315&type= 4.82600021 3.55599999 相对于惯性系 <math id="M19"><mo stretchy="false">(</mo><mi>i</mi><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467337&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467335&type= 4.23333359 3.55599999 的三轴角速度 <math id="M20"><mi mathvariant="bold-italic">ω</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467342&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467340&type= 2.62466669 2.79399991 和加速度 a 。为解决视觉惯性里程计中采样频率不一致和重复积分问题，文献［ 16 16 ］首次提出了欧式空间中的预积分方法；随后，文献［ 17 17 ］在前者的基础上提出了IMU在流形面的预积分方法，得到了与初始状态无关的相对运动增量，从而显著减少了重复计算。 IMU采样频率远高于相机，在两个连续图像帧 <math id="M21"><mi>i</mi><mo>,</mo><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467346&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467344&type= 4.06400013 3.64066648 之间有多帧IMU数据，通过 式（3） 式（3） 对IMU量测值预积分可得到两帧图像的相对运动增量。 <math display="block" id="M22"><mtable><mtr><mtd><mi mathvariant="normal">Δ</mi><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><mo>=</mo><mstyle displaystyle="true"><munderover><mo>∏</mo><mrow><mi>k</mi><mo>=</mo><mn mathvariant="normal">1</mn></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><mi>E</mi><mi>x</mi><mi>p</mi><mo stretchy="false">(</mo><mo stretchy="false">(</mo><msub><mrow><mover><mrow><mi mathvariant="bold-italic">ω</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow></msub><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><mi>k</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">g</mi></mrow></mfenced></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">η</mi></mrow><mrow><mi>k</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">d</mi></mrow></mfenced></mrow></msubsup><mo stretchy="false">)</mo><mo>⋅</mo><mi>Δ</mi><msub><mrow><mi>T</mi></mrow><mrow><mi>i</mi><mi>m</mi><mi>u</mi></mrow></msub><mo stretchy="false">)</mo></mrow></mtd></mtr><mtr><mtd><mi mathvariant="normal">Δ</mi><msub><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><mo>=</mo><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>k</mi><mo>=</mo><mi>i</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><mi>Δ</mi><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msub><mo>⋅</mo><mo stretchy="false">(</mo><msub><mrow><mover><mrow><mi mathvariant="bold-italic">f</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow></msub><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><mi>k</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">a</mi></mrow></mfenced></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">η</mi></mrow><mrow><mi>k</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">a</mi><mi mathvariant="normal">d</mi></mrow></mfenced></mrow></msubsup><mo stretchy="false">)</mo></mrow><mo>⋅</mo><mi mathvariant="normal">Δ</mi><msub><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow></msub></mtd></mtr><mtr><mtd><mi mathvariant="normal">Δ</mi><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><mo>=</mo><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>k</mi><mo>=</mo><mi>i</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><mfenced open="[" close="]" separators="|"><mrow><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><mi mathvariant="normal">Δ</mi><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msub><mo stretchy="false">(</mo><msub><mrow><mover><mrow><mi mathvariant="bold-italic">f</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow></msub><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><mi>k</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">a</mi></mrow></mfenced></mrow></msubsup><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">η</mi></mrow><mrow><mi>k</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">a</mi><mi mathvariant="normal">d</mi></mrow></mfenced></mrow></msubsup><mo stretchy="false">)</mo><mo>⋅</mo><mi mathvariant="normal">Δ</mi><msubsup><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo>+</mo><mi mathvariant="normal">Δ</mi><msub><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msub><mo>⋅</mo><mi mathvariant="normal">Δ</mi><msub><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow></msub></mrow></mfenced></mrow></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467350&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467348&type= 91.01666260 30.64933395 ， 其中： <math id="M23"><mi mathvariant="normal">Δ</mi><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><mo>,</mo><mi mathvariant="normal">Δ</mi><msub><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><mo>,</mo><mi mathvariant="normal">Δ</mi><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467356&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467353&type= 26.50066566 3.89466691 分别为相邻两图像帧 <math id="M24"><mi>i</mi><mo>,</mo><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467361&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467359&type= 4.06400013 3.64066648 经IMU预积分解算得到的相对姿态，速度和位置； <math id="M25"><mi mathvariant="normal">E</mi><mi mathvariant="normal">x</mi><mi mathvariant="normal">p</mi><mo stretchy="false">(</mo><mo>⋅</mo><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467383&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467382&type= 9.22866726 3.97933316 为指数映射，可将旋转向量映射为李群空间的旋转矩阵； <math id="M26"><mi mathvariant="normal">Δ</mi><msub><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467387&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467385&type= 8.29733276 3.72533321 为IMU的采样时间间隔； <math id="M27"><msub><mrow><mover><mrow><mi mathvariant="bold-italic">ω</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467391&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467389&type= 3.64066648 3.72533321 和 <math id="M28"><msub><mrow><mover><mrow><mi mathvariant="bold-italic">f</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467395&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467393&type= 2.96333337 5.33400011 分别为 <math id="M29"><mi>k</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467449&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467447&type= 1.69333339 2.79399991 时刻陀螺仪和加速度计的量测值； <math id="M30"><msubsup><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><mi>k</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">g</mi></mrow></mfenced></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467406&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467403&type= 4.65666676 4.06400013 和 <math id="M31"><msubsup><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><mi>k</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">a</mi></mrow></mfenced></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467411&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467409&type= 4.57200003 3.64066648 分别为 <math id="M32"><mi>k</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467449&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467447&type= 1.69333339 2.79399991 时刻陀螺仪和加速度计的零偏； <math id="M33"><msubsup><mrow><mi mathvariant="bold-italic">η</mi></mrow><mrow><mi>k</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">d</mi></mrow></mfenced></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467438&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467420&type= 5.67266655 4.48733330 和 <math id="M34"><msubsup><mrow><mi mathvariant="bold-italic">η</mi></mrow><mrow><mi>k</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">a</mi><mi mathvariant="normal">d</mi></mrow></mfenced></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467445&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467441&type= 5.58799982 4.06400013 分别为 <math id="M35"><mi>k</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467449&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467447&type= 1.69333339 2.79399991 时刻陀螺仪和加速度计离散的高斯白噪声，是其连续噪声与采样频率开方的乘积，单位分别为 <math id="M36"><mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">d</mi></mrow><mo>/</mo><mrow><mi mathvariant="normal">s</mi></mrow></mrow><mo>,</mo><mrow><mrow><mi mathvariant="normal">m</mi></mrow><mo>/</mo><mrow><msup><mrow><mi mathvariant="normal">s</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup></mrow></mrow></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467455&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467452&type= 17.18733406 3.47133350 。 2.3　预测像素坐标 2 利用惯性测量单元的测量值和预积分原理可以计算出载体在相邻两帧图像之间的旋转矩阵 <math id="M37"><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467461&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467458&type= 6.01133299 3.89466691 （与2.2中 <math id="M38"><mi mathvariant="normal">Δ</mi><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467468&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467464&type= 8.29733276 3.89466691 等价）和平移向量 <math id="M39"><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467473&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467470&type= 4.31799984 3.89466691 （与2.2中 <math id="M40"><mi mathvariant="normal">Δ</mi><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467479&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467476&type= 6.77333355 3.89466691 等价）。根据上一图像帧时刻载体在世界系下的位姿和相邻两图像间相对位姿，可预测当前图像帧时刻载体在世界系下的位姿为： <math display="block" id="M41"><mtable><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><mo>=</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></mtd></mtr><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><mo>=</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><mo>+</mo><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub><mo>+</mo><msub><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub><mi mathvariant="normal">Δ</mi><msub><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">c</mi></mrow></msub><mo>+</mo><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><msub><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><mi>w</mi></mrow></msub><mi mathvariant="normal">Δ</mi><msubsup><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">c</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467501&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467498&type= 63.83866882 13.54666710 ， 其中： <math id="M42"><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub><mo>,</mo><msub><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub><mo>,</mo><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467505&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467503&type= 19.30400085 3.64066648 分别为上一图像帧 <math id="M43"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468002&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467999&type= 1.10066664 2.79399991 时刻载体在世界系 <math id="M44"><mi>w</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467517&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467514&type= 2.96333337 2.79399991 的姿态，速度和位置； <math id="M45"><mi mathvariant="normal">Δ</mi><msub><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">c</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467521&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467519&type= 6.09600019 3.72533321 为相机的采样间隔； <math id="M46"><msub><mrow><mi mathvariant="bold-italic">g</mi></mrow><mrow><mi>w</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467525&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467523&type= 3.80999994 4.57200003 为世界系 <math id="M47"><mi>w</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467647&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467645&type= 2.96333337 2.79399991 下的重力加速度。 将当前图像帧 <math id="M48"><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467867&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467864&type= 1.60866666 3.64066648 时刻载体在世界系 <math id="M49"><mi>w</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467647&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467645&type= 2.96333337 2.79399991 的位姿转换为世界系 <math id="M50"><mi>w</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467647&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467645&type= 2.96333337 2.79399991 相对 <math id="M51"><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467867&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467864&type= 1.60866666 3.64066648 时刻载体系 <math id="M52"><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467565&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467563&type= 2.62466669 3.72533321 的位姿为： <math display="block" id="M53"><mtable><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub><mi>w</mi></mrow></msub><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msubsup></mtd></mtr><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub><mi>w</mi></mrow></msub><mo>=</mo><mo>-</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub><mi>w</mi></mrow></msub><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467572&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467568&type= 23.45266724 9.73666668 . 利用视觉惯性里程计标定的相机系 <math id="M54"><mi>c</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467669&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467667&type= 1.52400005 2.79399991 与载体系 <math id="M55"><mi>b</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467635&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467633&type= 1.86266661 2.79399991 变换矩阵，预测当前图像帧 <math id="M56"><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467867&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467864&type= 1.60866666 3.64066648 时刻相机相对世界系 <math id="M57"><mi>w</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467647&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467645&type= 2.96333337 2.79399991 的位姿为 <math display="block" id="M58"><mtable columnalign="left"><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>c</mi><mi>w</mi></mrow></msub><mo>=</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>c</mi><mi>b</mi></mrow></msub><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub><mi>w</mi></mrow></msub></mtd></mtr><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><mi>c</mi><mi>w</mi></mrow></msub><mo>=</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>c</mi><mi>b</mi></mrow></msub><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub><mi>w</mi></mrow></msub><mo>+</mo><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><mi>c</mi><mi>b</mi></mrow></msub></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467605&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467603&type= 25.99266624 10.49866676 ， 其中： <math id="M59"><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>c</mi><mi>b</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467609&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467607&type= 4.82600021 3.72533321 和 <math id="M60"><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><mi>c</mi><mi>b</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467612&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467611&type= 3.13266683 3.72533321 分别为相机系 <math id="M61"><mi>c</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467669&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467667&type= 1.52400005 2.79399991 与载体系 <math id="M62"><mi>b</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467635&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467633&type= 1.86266661 2.79399991 的旋转矩阵和平移向量， <math id="M63"><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>c</mi><mi>w</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467639&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467637&type= 5.33400011 3.72533321 和 <math id="M64"><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><mi>c</mi><mi>w</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467643&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467641&type= 3.64066648 3.72533321 分别为利用IMU预积分预测出的世界系 <math id="M65"><mi>w</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467647&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467645&type= 2.96333337 2.79399991 相对于 <math id="M66"><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467665&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467664&type= 1.60866666 3.64066648 时刻相机系 <math id="M67"><mi>c</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467669&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467667&type= 1.52400005 2.79399991 的旋转矩阵和平移向量。 当相机模型为针孔相机模型时，利用双目相机模型获取的地图点在当前帧图像上对应像素坐标为： <math display="block" id="M68"><mtable columnalign="left"><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>c</mi></mrow></msub><mo>=</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>c</mi><mi>w</mi></mrow></msub><msub><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>w</mi></mrow></msub><mo>+</mo><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><mi>c</mi><mi>w</mi></mrow></msub></mtd></mtr><mtr><mtd><mi>u</mi><mo>=</mo><msub><mrow><mi>f</mi></mrow><mrow><mi>x</mi></mrow></msub><mfrac><mrow><msub><mrow><mi>X</mi></mrow><mrow><mi>c</mi></mrow></msub></mrow><mrow><msub><mrow><mi>Z</mi></mrow><mrow><mi>c</mi></mrow></msub></mrow></mfrac><mo>+</mo><msub><mrow><mi>c</mi></mrow><mrow><mi>x</mi></mrow></msub></mtd></mtr><mtr><mtd><mi>v</mi><mo>=</mo><msub><mrow><mi>f</mi></mrow><mrow><mi>y</mi></mrow></msub><mfrac><mrow><msub><mrow><mi>Y</mi></mrow><mrow><mi>c</mi></mrow></msub></mrow><mrow><msub><mrow><mi>Z</mi></mrow><mrow><mi>c</mi></mrow></msub></mrow></mfrac><mo>+</mo><msub><mrow><mi>c</mi></mrow><mrow><mi>y</mi></mrow></msub></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467674&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467672&type= 26.58533096 23.28333473 ， 其中： <math id="M69"><msub><mrow><mi>f</mi></mrow><mrow><mi>x</mi></mrow></msub><mo>,</mo><msub><mrow><mi>f</mi></mrow><mrow><mi>y</mi></mrow></msub><mo>,</mo><msub><mrow><mi>c</mi></mrow><mrow><mi>x</mi></mrow></msub><mo>,</mo><msub><mrow><mi>c</mi></mrow><mrow><mi>y</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467678&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467676&type= 16.42533302 4.57200003 为相机的内参数； <math id="M70"><msub><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>w</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467681&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467679&type= 4.31799984 3.72533321 为地图点 <math id="M71"><mi>P</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467685&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467683&type= 2.62466669 2.79399991 在世界系下的三维坐标； <math id="M72"><msub><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>c</mi></mrow></msub><mo>=</mo><msup><mrow><mfenced open="[" close="]" separators="|"><mrow><msub><mrow><mi>X</mi></mrow><mrow><mi>c</mi></mrow></msub><mo>,</mo><msub><mrow><mi>Y</mi></mrow><mrow><mi>c</mi></mrow></msub><mo>,</mo><msub><mrow><mi>Z</mi></mrow><mrow><mi>c</mi></mrow></msub></mrow></mfenced></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467689&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467688&type= 28.10933495 4.74133301 为地图点在当前帧相机系的坐标； <math id="M73"><mo stretchy="false">(</mo><mi>u</mi><mo>,</mo><mi>v</mi><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467708&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467691&type= 8.72066593 3.55599999 为地图点对应像素坐标。 式（7） 式（7） 表明前一帧图像像素点对应地图点在当前帧图像的位置。相机的内参数可以通过相机标定获取，因此只需通过IMU量测值预测相机位姿便可确定地图点在当前帧图像的像素位置。但IMU预积分预测相机位姿存在一定误差，因此在图像特征匹配时没有直接匹配预测特征点，而是在以预测位置为原点，某一阈值为半径的局部区域内搜索特征点并匹配，以防出现无法匹配的情况。为适配不同精度的惯性测量单元，目前普遍的做法是设置一个较大的固定值作为搜索半径，以避免漏掉特征点。但这种惯例在IMU精度较高时易造成计算资源浪费，同时增加了图像特征的误匹配率，使SLAM系统的位姿估计精度降低。 3 自适应惯导辅助特征匹配 1 针对惯导辅助图像特征匹配方法中固定搜索半径过大的问题，本文提出了一种搜索半径自适应算法。根据误差传播定律和惯导辅助图像特征匹配原理计算预测像素位置的中误差，确定地图点最有可能出现的区域半径，将局部固定区域特征匹配转化为局部自适应区域的特征匹配，确定自适应特征匹配搜索半径的流程如 图3 图3 所示。 3.1　预积分误差分析 2 由于预积分量与IMU噪声存在复杂的依赖关系，难以直接利用误差传播定律递推出预积分的信息矩阵，因此可将IMU噪声从预积分量中分离出来，得到预积分测量噪声与IMU噪声关系为： <math display="block" id="M74"><mtable><mtr><mtd><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">ϕ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><mo>=</mo><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>k</mi><mo>=</mo><mi>i</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><mi mathvariant="normal">Δ</mi><msubsup><mrow><mover><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>k</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msubsup><msubsup><mrow><mi mathvariant="bold-italic">J</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow><mrow><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></mrow></msubsup><msubsup><mrow><mi mathvariant="bold-italic">η</mi></mrow><mrow><mi>k</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">d</mi></mrow></mfenced></mrow></msubsup><mi mathvariant="normal">Δ</mi><msub><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow></msub></mrow></mtd></mtr><mtr><mtd><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><mo>=</mo><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>k</mi><mo>=</mo><mi>i</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><mfenced open="[" close="]" separators="|"><mrow><mo>-</mo><mi mathvariant="normal">Δ</mi><msub><mrow><mover><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msub><mo stretchy="false">(</mo><msub><mrow><mover><mrow><mi mathvariant="bold-italic">a</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow></msub><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><mi>i</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">a</mi></mrow></mfenced></mrow></msubsup><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>∧</mo></mrow></msup><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">ϕ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msub><mi mathvariant="normal">Δ</mi><msub><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow></msub><mo>+</mo><mi mathvariant="normal">Δ</mi><msub><mrow><mover><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msub><msubsup><mrow><mi mathvariant="bold-italic">η</mi></mrow><mrow><mi>k</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">a</mi><mi mathvariant="normal">d</mi></mrow></mfenced></mrow></msubsup><mi mathvariant="normal">Δ</mi><msub><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow></msub></mrow></mfenced></mrow></mtd></mtr><mtr><mtd><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><mo>=</mo><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>k</mi><mo>=</mo><mi>i</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></munderover></mstyle><mrow><mfenced open="[" close="]" separators="|"><mrow><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><mi mathvariant="normal">Δ</mi><msub><mrow><mover><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msub><msubsup><mrow><mi mathvariant="bold-italic">η</mi></mrow><mrow><mi>k</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">a</mi><mi mathvariant="normal">d</mi></mrow></mfenced></mrow></msubsup><mi mathvariant="normal">Δ</mi><msubsup><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup><mo>+</mo><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msub><mi mathvariant="normal">Δ</mi><msub><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow></msub><mo>-</mo><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><mi mathvariant="normal">Δ</mi><msub><mrow><mover><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msub><mo stretchy="false">(</mo><msub><mrow><mover><mrow><mi mathvariant="bold-italic">a</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>k</mi></mrow></msub><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><mi>i</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">a</mi></mrow></mfenced></mrow></msubsup><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>∧</mo></mrow></msup><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">ϕ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>k</mi></mrow></msub></mrow></msub><mi mathvariant="normal">Δ</mi><msubsup><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mrow></mfenced></mrow></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467718&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467717&type= 116.58599854 30.64933395 ， 其中： <math id="M75"><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">ϕ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467723&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467721&type= 7.53533268 3.89466691 为 <math id="M76"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467769&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467767&type= 1.10066664 2.79399991 到 <math id="M77"><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467773&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467771&type= 1.60866666 3.64066648 时刻IMU预积分量中姿态误差 <math id="M78"><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467735&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467732&type= 7.70466709 3.89466691 对应的李代数； <math id="M79"><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467754&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467736&type= 7.11199999 3.89466691 为 <math id="M80"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467769&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467767&type= 1.10066664 2.79399991 到 <math id="M81"><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467773&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467771&type= 1.60866666 3.64066648 时刻IMU预积分量的速度误差； <math id="M82"><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467765&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467763&type= 6.94266701 3.89466691 为 <math id="M83"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467769&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467767&type= 1.10066664 2.79399991 到 <math id="M84"><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467773&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467771&type= 1.60866666 3.64066648 时刻IMU预积分量的位置误差； <math id="M85"><msubsup><mrow><mi mathvariant="bold-italic">J</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow><mrow><mfenced separators="|"><mrow><mi>k</mi></mrow></mfenced></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467778&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467775&type= 4.99533367 3.64066648 为 <math id="M86"><mi>k</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467781&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467779&type= 1.69333339 2.79399991 时刻右雅可比矩阵；矩阵上方“~”表示该矩阵含有误差，并非真实值，为观测值或由观测值构成，下同；上标“∧”表示取向量反对称矩阵，下同。 三个预积分状态量误差表达式较为复杂，直接计算会给计算平台带来资源的浪费，经推导与简化可得预积分量误差的递推公式为： <math display="block" id="M87"><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">ϕ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></mtd></mtr><mtr><mtd><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></mtd></mtr><mtr><mtd><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></mtd></mtr></mtable></mrow></mfenced><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mi mathvariant="normal">Δ</mi><msub><mrow><mover><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mo>-</mo><mi mathvariant="normal">Δ</mi><msub><mrow><mover><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub><mo>⋅</mo><mo stretchy="false">(</mo><msub><mrow><mover><mrow><mi mathvariant="bold-italic">f</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><mi>i</mi></mrow><mrow><mo stretchy="false">(</mo><mi>a</mi><mo stretchy="false">)</mo></mrow></msubsup><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>∧</mo></mrow></msup><mi mathvariant="normal">Δ</mi><msub><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow></msub></mtd><mtd><mi mathvariant="bold-italic">I</mi></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mo>-</mo><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><mi mathvariant="normal">Δ</mi><msub><mrow><mover><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub><mo>⋅</mo><mo stretchy="false">(</mo><msub><mrow><mover><mrow><mi mathvariant="bold-italic">f</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><mi>i</mi></mrow><mrow><mo stretchy="false">(</mo><mi>a</mi><mo stretchy="false">)</mo></mrow></msubsup><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>∧</mo></mrow></msup><mi mathvariant="normal">Δ</mi><msubsup><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mtd><mtd><mi mathvariant="normal">Δ</mi><msub><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow></msub><mi mathvariant="bold-italic">I</mi></mtd><mtd><mi mathvariant="bold-italic">I</mi></mtd></mtr></mtable></mrow></mfenced><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">ϕ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub></mtd></mtr><mtr><mtd><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub></mtd></mtr><mtr><mtd><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub></mtd></mtr></mtable></mrow></mfenced><mo>+</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">J</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow><mrow><mfenced separators="|"><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></mfenced></mrow></msubsup></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mi mathvariant="normal">Δ</mi><msub><mrow><mover><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub><mi mathvariant="normal">Δ</mi><msub><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow></msub></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><mi mathvariant="normal">Δ</mi><msub><mrow><mover><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub><mi mathvariant="normal">Δ</mi><msup><mrow><msub><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow></msub></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup></mtd></mtr></mtable></mrow></mfenced><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">η</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">d</mi></mrow></mfenced></mrow></msubsup></mtd></mtr><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">η</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">a</mi><mi mathvariant="normal">d</mi></mrow></mfenced></mrow></msubsup></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467800&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467798&type= 148.67468262 22.09799957 ， 其中： <math id="M88"><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">ϕ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467804&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467802&type= 9.73666668 3.89466691 为 <math id="M89"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468002&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467999&type= 1.10066664 2.79399991 到 <math id="M90"><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468040&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468036&type= 8.29733276 3.64066648 时刻IMU预积分量中姿态误差 <math id="M91"><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467819&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467816&type= 9.90600014 3.89466691 对应的李代数； <math id="M92"><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">v</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467824&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467822&type= 9.22866726 3.89466691 和 <math id="M93"><mi>δ</mi><msub><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467845&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467827&type= 9.14400005 3.89466691 分别为 <math id="M94"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468002&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467999&type= 1.10066664 2.79399991 到 <math id="M95"><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468040&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468036&type= 8.29733276 3.64066648 时刻IMU预积分量中速度误差和位置误差； <math id="M96"><mi mathvariant="normal">Δ</mi><msub><mrow><mover><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467861&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467858&type= 10.49866676 4.57200003 为 <math id="M97"><mi>j</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467867&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467864&type= 1.60866666 3.64066648 到 <math id="M98"><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468040&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468036&type= 8.29733276 3.64066648 时刻载体相对姿态； <math id="M99"><msub><mrow><mover><mrow><mi mathvariant="bold-italic">f</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467879&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467876&type= 6.26533365 5.33400011 为 <math id="M100"><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468040&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468036&type= 8.29733276 3.64066648 时刻加速度计测量值； <math id="M101"><msubsup><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><mi>i</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">a</mi></mrow></mfenced></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467903&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467886&type= 4.57200003 3.64066648 为 <math id="M102"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468002&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467999&type= 1.10066664 2.79399991 时刻加速度计零偏，这里假设相邻两图像帧间偏差不变； <math id="M103"><msubsup><mrow><mi mathvariant="bold-italic">J</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow><mrow><mfenced separators="|"><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></mfenced></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467915&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467912&type= 8.38199997 4.06400013 为 <math id="M104"><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468040&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468036&type= 8.29733276 3.64066648 时刻的右雅克比矩阵； <math id="M105"><msubsup><mrow><mi mathvariant="bold-italic">η</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">d</mi></mrow></mfenced></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467928&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467925&type= 6.34999990 4.48733330 和 <math id="M106"><msubsup><mrow><mi mathvariant="bold-italic">η</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">a</mi><mi mathvariant="normal">d</mi></mrow></mfenced></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467934&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467931&type= 6.34999990 4.06400013 分别为 <math id="M107"><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468040&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468036&type= 8.29733276 3.64066648 时刻陀螺仪和加速度计的离散高斯白噪声； I 为单位矩阵； <math id="M108"><mi mathvariant="normal">Δ</mi><msub><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467947&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467943&type= 8.29733276 3.72533321 为IMU采样间隔。 根据误差传播定律可得，两相邻图像帧之间IMU预积分量的协方差 <math id="M109"><msub><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467967&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467964&type= 6.01133299 3.89466691 为： <math display="block" id="M110"><msub><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><mo>=</mo><mi mathvariant="bold-italic">A</mi><msub><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub><msup><mrow><mi mathvariant="bold-italic">A</mi></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup><mo>+</mo><mi mathvariant="bold-italic">B</mi><msubsup><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">d</mi></mrow></mfenced></mrow></msubsup><msup><mrow><mi mathvariant="bold-italic">B</mi></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467974&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467970&type= 44.61933517 4.48733330 ， 其中： <math display="block" id="M111"><mi mathvariant="bold-italic">A</mi><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mi mathvariant="normal">Δ</mi><msub><mrow><mover><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mo>-</mo><mi mathvariant="normal">Δ</mi><msub><mrow><mover><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub><mo>⋅</mo><mo stretchy="false">(</mo><msub><mrow><mover><mrow><mi mathvariant="bold-italic">f</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><mi>i</mi></mrow><mrow><mfenced separators="|"><mrow><mi>a</mi></mrow></mfenced></mrow></msubsup><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>∧</mo></mrow></msup><mi mathvariant="normal">Δ</mi><msub><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow></msub></mtd><mtd><mi>I</mi></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mo>-</mo><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><mi mathvariant="normal">Δ</mi><msub><mrow><mover><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub><mo>⋅</mo><mo stretchy="false">(</mo><msub><mrow><mover><mrow><mi mathvariant="bold-italic">f</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">b</mi></mrow><mrow><mi>i</mi></mrow><mrow><mfenced separators="|"><mrow><mi>a</mi></mrow></mfenced></mrow></msubsup><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>∧</mo></mrow></msup><mi mathvariant="normal">Δ</mi><msubsup><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mtd><mtd><mi mathvariant="normal">Δ</mi><msub><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow></msub><mi>I</mi></mtd><mtd><mi>I</mi></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467980&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467976&type= 74.16799927 22.09799957 ， <math display="block" id="M112"><mi mathvariant="bold-italic">B</mi><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">J</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow><mrow><mfenced separators="|"><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></mfenced></mrow></msubsup></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mi mathvariant="normal">Δ</mi><msub><mrow><mover><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub><mi mathvariant="normal">Δ</mi><msub><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow></msub></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></mfrac><mi mathvariant="normal">Δ</mi><msub><mrow><mover><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>˜</mi></mrow></mover></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub><mi mathvariant="normal">Δ</mi><msubsup><mrow><mi>T</mi></mrow><mrow><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi><mi mathvariant="normal">u</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mtd></mtr></mtable></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467985&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467983&type= 44.87333298 21.59000015 ， <math display="block" id="M113"><mtable><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow><mrow><mi mathvariant="normal">d</mi></mrow></msubsup><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi><mo>+</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow><mrow><mi mathvariant="normal">d</mi></mrow></msubsup><mo>=</mo><mo>⋯</mo><mo>=</mo><msubsup><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow><mrow><mi mathvariant="normal">d</mi></mrow></msubsup><mo>=</mo></mtd></mtr><mtr><mtd><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msubsup><mrow><mi>η</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">d</mi><mi>x</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><msubsup><mrow><mi>η</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">d</mi><mi>y</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><msubsup><mrow><mi>η</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">d</mi><mi>z</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><msubsup><mrow><mi>η</mi></mrow><mrow><mi mathvariant="normal">a</mi><mi mathvariant="normal">d</mi><mi>x</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><msubsup><mrow><mi>η</mi></mrow><mrow><mi mathvariant="normal">a</mi><mi mathvariant="normal">d</mi><mi>y</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><msubsup><mrow><mi>η</mi></mrow><mrow><mi mathvariant="normal">a</mi><mi mathvariant="normal">d</mi><mi>z</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mtd></mtr></mtable></mrow></mfenced></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467991&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467988&type= 50.71533585 39.45466614 ， 其中： <math id="M114"><msub><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467997&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467993&type= 8.21266651 3.89466691 为 <math id="M115"><mi>i</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468002&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41467999&type= 1.10066664 2.79399991 到 <math id="M116"><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468040&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468036&type= 8.29733276 3.64066648 时刻IMU预积分量的协方差，通常认为初始时刻IMU预积分量无误差，即 <math id="M117"><msub><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468029&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468026&type= 6.01133299 3.64066648 为零矩阵； <math id="M118"><msubsup><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub></mrow><mrow><mi mathvariant="normal">d</mi></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468034&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468032&type= 6.60400009 4.06400013 为 <math id="M119"><mi>j</mi><mo>-</mo><mn mathvariant="normal">1</mn></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468040&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468036&type= 8.29733276 3.64066648 时刻IMU测量值噪声的协方差，一般假设任意时刻IMU协方差都为固定值，且由陀螺仪 <math id="M120"><msup><mrow><mi mathvariant="bold-italic">η</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">d</mi></mrow></mfenced></mrow></msup><mo>=</mo><msup><mrow><mfenced open="[" close="]" separators="|"><mrow><msub><mrow><mi>η</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">d</mi><mi>x</mi></mrow></msub><mo>,</mo><msub><mrow><mi>η</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">d</mi><mi>y</mi></mrow></msub><mo>,</mo><msub><mrow><mi>η</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">d</mi><mi>z</mi></mrow></msub></mrow></mfenced></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468047&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468043&type= 33.95133591 5.58799982 和加速度计标定噪声 <math id="M121"><msup><mrow><mi mathvariant="bold-italic">η</mi></mrow><mrow><mfenced separators="|"><mrow><mi mathvariant="normal">a</mi><mi mathvariant="normal">d</mi></mrow></mfenced></mrow></msup><mo>=</mo><msup><mrow><mfenced open="[" close="]" separators="|"><mrow><msub><mrow><mi>η</mi></mrow><mrow><mi mathvariant="normal">a</mi><mi mathvariant="normal">d</mi><mi>x</mi></mrow></msub><mo>,</mo><msub><mrow><mi>η</mi></mrow><mrow><mi mathvariant="normal">a</mi><mi mathvariant="normal">d</mi><mi>y</mi></mrow></msub><mo>,</mo><msub><mrow><mi>η</mi></mrow><mrow><mi mathvariant="normal">a</mi><mi mathvariant="normal">d</mi><mi>z</mi></mrow></msub></mrow></mfenced></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468052&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468049&type= 33.44333267 5.58799982 构成。 3.2　自适应阈值的确定 2 由IMU预积分误差和误差传播定律可得图像间相对位姿的协方差 <math id="M122"><msub><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468125&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468123&type= 6.01133299 3.89466691 ，利用惯性导航辅助特征匹配原理可将其传递至每个地图点对应的像素，获取预测像素位置的中误差。由于特征匹配过程中的函数与位姿有关，因此误差传递时从李代数的角度对函数进行线性化。 将IMU预积分相对位姿误差传递至当前图像帧时刻载体相对世界系的位姿误差过程中，需要对 式（4） 式（4） 线性化，得到其关于相对姿态 <math id="M123"><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468080&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468077&type= 6.01133299 3.89466691 和位置 <math id="M124"><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468086&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468083&type= 4.31799984 3.89466691 的导数为： <math display="block" id="M125"><mtable><mtr><mtd><mfrac><mrow><mo>∂</mo><mi mathvariant="normal">l</mi><mi mathvariant="normal">n</mi><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>∨</mo></mrow></msup></mrow><mrow><mo>∂</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></mrow></mfrac><mo>=</mo></mtd></mtr><mtr><mtd><munder><mrow><mi mathvariant="normal">l</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi></mrow><mrow><mi>ϕ</mi><mo>→</mo><mn mathvariant="normal">0</mn></mrow></munder><mfrac><mrow><mi mathvariant="normal">l</mi><mi mathvariant="normal">n</mi><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><mi mathvariant="normal">E</mi><mi mathvariant="normal">x</mi><mi mathvariant="normal">p</mi><msup><mrow><mo stretchy="false">(</mo><mi>ϕ</mi><mo stretchy="false">)</mo><mo stretchy="false">)</mo></mrow><mrow><mo>∨</mo></mrow></msup><mo>-</mo><mi mathvariant="normal">l</mi><mi mathvariant="normal">n</mi><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>∨</mo></mrow></msup></mrow><mrow><mi>ϕ</mi></mrow></mfrac><mo>=</mo></mtd></mtr><mtr><mtd><munder><mrow><mi mathvariant="normal">l</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">m</mi></mrow><mrow><mi>ϕ</mi><mo>→</mo><mn mathvariant="normal">0</mn></mrow></munder><mfrac><mrow><mi mathvariant="normal">l</mi><mi mathvariant="normal">n</mi><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>∨</mo></mrow></msup><mo>+</mo><msubsup><mrow><mi mathvariant="bold-italic">J</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msubsup><mi>ϕ</mi><mo>-</mo><mi mathvariant="normal">l</mi><mi mathvariant="normal">n</mi><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>∨</mo></mrow></msup></mrow><mrow><mi>ϕ</mi></mrow></mfrac><mo>=</mo></mtd></mtr><mtr><mtd><msubsup><mrow><mi>J</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msubsup><mo stretchy="false">(</mo><mi mathvariant="normal">l</mi><mi mathvariant="normal">n</mi><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>∨</mo></mrow></msup><mo stretchy="false">)</mo></mtd></mtr><mtr><mtd><mfrac><mrow><mo>∂</mo><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></mrow><mrow><mo>∂</mo><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></mrow></mfrac><mo>=</mo><mfrac><mrow><mo>∂</mo><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><mo stretchy="false">)</mo></mrow><mrow><mo>∂</mo><msub><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></mrow></mfrac><mo>=</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468094&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468091&type= 67.47933960 48.85266495 . 则当前图像帧时刻载体相对世界系位姿的协方差为： <math display="block" id="M126"><mtable><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><mi mathvariant="normal">w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><mo>=</mo><mi mathvariant="bold-italic">C</mi><msub><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><msup><mrow><mi mathvariant="bold-italic">C</mi></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></mtd></mtr><mtr><mtd><mi mathvariant="bold-italic">C</mi><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msubsup><mrow><mi mathvariant="bold-italic">J</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msubsup><mo stretchy="false">(</mo><mi mathvariant="normal">l</mi><mi mathvariant="normal">n</mi><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>∨</mo></mrow></msup><mo stretchy="false">)</mo></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub></mrow></msub></mtd></mtr></mtable></mrow></mfenced></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468100&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468097&type= 42.92599869 16.59466553 ， 其中： <math id="M127"><mi mathvariant="normal">l</mi><mi mathvariant="normal">n</mi><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>∨</mo></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468122&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468120&type= 14.05466747 4.40266657 为 <math id="M128"><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468111&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468108&type= 6.01133299 3.89466691 对应的李代数； <math id="M129"><msubsup><mrow><mi mathvariant="bold-italic">J</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msubsup><mo stretchy="false">(</mo><mi mathvariant="normal">l</mi><mi mathvariant="normal">n</mi><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>∨</mo></mrow></msup><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468117&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468115&type= 23.11400032 5.33400011 为 <math id="M130"><mi mathvariant="normal">l</mi><mi mathvariant="normal">n</mi><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>∨</mo></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468122&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468120&type= 14.05466747 4.40266657 右乘雅克比矩阵的逆； <math id="M131"><msub><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><msub><mrow><mi>b</mi></mrow><mrow><mi>i</mi></mrow></msub><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468125&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468123&type= 6.01133299 3.89466691 为IMU预积分获取相对位姿的协方差；上标“∨”表示由反对称矩阵转为向量，下同。 同理，依据 式（5） 式（5） 和（6）由当前图像帧时刻载体系相对世界系位姿协方差可得相机相对世界系位姿的协方差为： <math display="block" id="M132"><mtable><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><mi>c</mi><mi>w</mi></mrow></msub><mo>=</mo><mi mathvariant="bold-italic">D</mi><msub><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow></msub><msup><mrow><mi mathvariant="bold-italic">D</mi></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></mtd></mtr><mtr><mtd><mi mathvariant="bold-italic">D</mi><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mo>-</mo><msubsup><mrow><mi mathvariant="bold-italic">J</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msubsup><mo stretchy="false">(</mo><mi mathvariant="normal">l</mi><mi mathvariant="normal">n</mi><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>c</mi><mi>w</mi></mrow></msub><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>∨</mo></mrow></msup><mo stretchy="false">)</mo></mtd><mtd><mn mathvariant="normal">0</mn></mtd></mtr><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>c</mi><mi>w</mi></mrow></msub><msubsup><mrow><mi mathvariant="bold-italic">t</mi></mrow><mrow><mi>w</mi><msub><mrow><mi>b</mi></mrow><mrow><mi>j</mi></mrow></msub></mrow><mrow><mo>∧</mo></mrow></msubsup></mtd><mtd><mo>-</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>c</mi><mi>w</mi></mrow></msub></mtd></mtr></mtable></mrow></mfenced></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468143&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468127&type= 49.02199936 16.51000023 . 利用相机的针孔投影模型将预测相机位姿协方差传递到每个地图点对应像素，得到像素的协方差为： <math display="block" id="M133"><mtable><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><msub><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>c</mi></mrow></msub></mrow></msub><mo>=</mo><mi mathvariant="bold-italic">E</mi><msub><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><mi>c</mi><mi>w</mi></mrow></msub><msup><mrow><mi mathvariant="bold-italic">E</mi></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></mtd></mtr><mtr><mtd><mi mathvariant="bold-italic">E</mi><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mo>-</mo><mo stretchy="false">(</mo><msub><mrow><mi mathvariant="bold-italic">R</mi></mrow><mrow><mi>c</mi><mi>w</mi></mrow></msub><msub><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>w</mi></mrow></msub><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>∧</mo></mrow></msup></mtd><mtd><mi>I</mi></mtd></mtr></mtable></mrow></mfenced></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468145&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468144&type= 32.93532944 10.24466610 ， <math display="block" id="M134"><mtable><mtr><mtd><msub><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><mi>p</mi></mrow></msub><mo>=</mo><mi mathvariant="bold-italic">F</mi><msub><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><msub><mrow><mi mathvariant="bold-italic">P</mi></mrow><mrow><mi>c</mi></mrow></msub></mrow></msub><msup><mrow><mi mathvariant="bold-italic">F</mi></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></mtd></mtr><mtr><mtd><mi mathvariant="bold-italic">F</mi><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><mfrac><mrow><msub><mrow><mi>f</mi></mrow><mrow><mi>x</mi></mrow></msub></mrow><mrow><msub><mrow><mi>Z</mi></mrow><mrow><mi>c</mi></mrow></msub></mrow></mfrac></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mo>-</mo><mfrac><mrow><msub><mrow><mi>f</mi></mrow><mrow><mi>x</mi></mrow></msub><msub><mrow><mi>X</mi></mrow><mrow><mi>c</mi></mrow></msub></mrow><mrow><msup><mrow><msub><mrow><mi>Z</mi></mrow><mrow><mi>c</mi></mrow></msub></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup></mrow></mfrac></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mfrac><mrow><msub><mrow><mi>f</mi></mrow><mrow><mi>y</mi></mrow></msub></mrow><mrow><msub><mrow><mi>Z</mi></mrow><mrow><mi>c</mi></mrow></msub></mrow></mfrac></mtd><mtd><mo>-</mo><mfrac><mrow><msub><mrow><mi>f</mi></mrow><mrow><mi>y</mi></mrow></msub><msub><mrow><mi>Y</mi></mrow><mrow><mi>c</mi></mrow></msub></mrow><mrow><msup><mrow><msub><mrow><mi>Z</mi></mrow><mrow><mi>c</mi></mrow></msub></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup></mrow></mfrac></mtd></mtr></mtable></mrow></mfenced></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468147&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468146&type= 36.49133301 25.82333374 . 自适应特征匹配搜索半径与像素协方差的对应关系为： <math display="block" id="M135"><mtable><mtr><mtd><msub><mrow><mi>σ</mi></mrow><mrow><mi>u</mi></mrow></msub><mo>=</mo><msub><mrow><mi>σ</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mroot><mrow><msub><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><msub><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>u</mi></mrow></msub></mrow></msub></mrow><mrow/></mroot></mtd></mtr><mtr><mtd><msub><mrow><mi>σ</mi></mrow><mrow><mi>v</mi></mrow></msub><mo>=</mo><msub><mrow><mi>σ</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub><mroot><mrow><msub><mrow><mi mathvariant="bold-italic">Σ</mi></mrow><mrow><msub><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi>v</mi></mrow></msub></mrow></msub></mrow><mrow/></mroot></mtd></mtr><mtr><mtd><mi>m</mi><mo>=</mo><mn mathvariant="normal">3</mn><mi mathvariant="normal">m</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">x</mi><mo stretchy="false">{</mo><msub><mrow><mi>σ</mi></mrow><mrow><mi>u</mi></mrow></msub><mo>,</mo><msub><mrow><mi>σ</mi></mrow><mrow><mi>v</mi></mrow></msub><mo stretchy="false">}</mo></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468149&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468148&type= 28.02466965 18.11866570 ， 其中： <math id="M136"><msub><mrow><mi>σ</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468151&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468150&type= 2.87866688 3.72533321 为单位权中误差； <math id="M137"><msub><mrow><mi>σ</mi></mrow><mrow><mi>u</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468154&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468153&type= 2.87866688 3.72533321 和 <math id="M138"><msub><mrow><mi>σ</mi></mrow><mrow><mi>v</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468156&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468155&type= 2.87866688 3.72533321 分别为像素坐标 <math id="M139"><mi>p</mi><mo>=</mo><msup><mrow><mo stretchy="false">(</mo><mi>u</mi><mo>,</mo><mi>v</mi><mo stretchy="false">)</mo></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468175&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468157&type= 17.18733406 4.23333359 的中误差； <math id="M140"><mi>m</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468181&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468178&type= 2.87866688 2.79399991 为特征匹配的搜索半径，取像素坐标的极限误差。 4 实验结果与分析 1 为验证所提方法的可行性和有效性，分别使用EuRoC和TUM两个公共数据集中的不同场景序列进行实验分析 ［ 18 18 - 19 19 18-19 ］ 。其中EuRoC数据集由搭载MT9V034摄像机和ADIS16448惯性测量单元的微型飞行器采集，同时使用激光跟踪仪或Vicon动作捕捉系统实时捕捉飞行时位姿，作为飞行器的真实轨迹；TUM数据集由搭载uEye相机和BMI160惯性测量单元的移动小车采集，真实轨迹由OptiTrack动作捕捉系统提供。 本文选择两个公开数据集中MH01、V101、V201、Corridor1、Magistrale1、Room1和Slides1这7个代表性序列，每个序列都给出了距离、加速度计和陀螺仪的噪声，如 表1 表1 所示。 序列 场景 距离/m 陀螺仪噪声 /（rad·s -1 ·Hz <math id="M141"><msup><mrow/><mrow><mo>-</mo><mfrac><mrow><mn>1</mn></mrow><mrow><mn>2</mn></mrow></mfrac></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468187&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468184&type= 3.04800010 4.65666676 ） 加速度计噪声/（m·s -2 ·Hz <math id="M142"><msup><mrow/><mrow><mo>-</mo><mfrac><mrow><mn>1</mn></mrow><mrow><mn>2</mn></mrow></mfrac></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468189&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468188&type= 3.04800010 4.65666676 ） MH01 厂房 80.6 3 0.000 17 3 0.002 V101 房间1 58.6 V201 房间2 36.5 Corridor1 走廊 305 4 0.000 16 4 0.028 Magistrale1 大厅 918 Room1 室内 146 Slides1 滑梯 289 4.1　自适应特征匹配结果分析 2 首先，利用本文的自适应算法在不同图像序列上进行测试，计算特征匹配搜索半径，得到各序列自适应特征匹配搜索半径累积分布函数，如 图4 图4 所示。图中 <math id="M143"><mi>F</mi><mo stretchy="false">(</mo><mi>x</mi><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468193&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468192&type= 8.55133343 3.55599999 表示搜索半径小于等于 <math id="M144"><mi>x</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468195&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468194&type= 2.20133328 2.79399991 的比例。 从图中可以看出，各图像序列依据本文算法获得的特征匹配搜索半径绝大部分小于ORB-SLAM3中的固定值，这是因为ORB-SLAM3设定了一个较大的搜索半径以适配不同规格的设备，避免IMU精度过低时漏掉特征点，导致系统跟踪失败。而且不同图像序列对应的特征匹配搜索半径并不完全与IMU精度相关，这是由于不同图像序列使用的相机模型不同，EuRoC采用的是针孔相机，而TUM采用的是鱼眼相机；另外，每个地图点所处位置也不同，进而说明特征匹配搜索半径应该设定为自适应阈值，综合考虑相机模型、惯性测量单元噪声和每个地图点位置等因素自适应调节，以提高特征匹配的准确度。 然后利用改进前后的特征匹配算法分别计算各图像序列特征匹配所需时间，得到平均每个特征点匹配所用时间。如 图5 图5 所示，改进后算法在EuRoC数据集对应的三个序列中特征匹配时间有所降低，而在TUM数据集对应的序列中有所增加，这是由于自适应特征匹配算法在缩小特征匹配搜索半径的同时，增加了函数线性化及误差传递的计算工作量；TUM数据集采用鱼眼相机，相机投影模型较为复杂，占用了一些计算资源，但增加的时间属于微秒级，在可以接受的范围内。 4.2　局部地图跟踪结果分析 2 IMU初始化后，ORB-SLAM3跟踪线程利用局部地图点跟踪建立图像帧间的多视图几何关系，并通过最小化重投影误差估计载体位姿。本文提出的自适应阈值算法直接作用于局部地图跟踪的特征匹配阶段，因此可通过评估跟踪线程位姿精度间接验证本文特征匹配方法的有效性，同时使用不同数据集中不同实验场景序列验证本文算法的鲁棒性。 以V101序列为例，改进的跟踪线程位姿估计算法与原算法定位轨迹如 图6 图6 所示。选用的数据为该序列在10次评测中均方根误差最接近均值的两组。可以看出，本文算法输出的位姿精度有所提升，这说明本文提出的自适应算法能够降低图像特征的误匹配率，因为基于特征点法视觉里程计精度主要取决于图像特征点匹配的准确度。 将改进前后ORB-SLAM3系统跟踪线程位姿估计结果分别与真实轨迹对比，并计算其绝对位姿误差，对比结果如 图7 图7 所示。可以看出，本文算法整体位姿精度都有所提升，其中系统刚开始的16秒内精度提升尤为明显，这是因为初始时刻IMU预测位姿精度不高，为避免漏掉特征点，原算法设置了一个较大的特征匹配搜索半径。而本文自适应特征匹配方法通过误差传播定律获取了每个地图点对应的搜索半径，使其搜索区域显著减小，因此初始一段时间特征点匹配的准确率会有明显提升，跟踪线程位姿估计也会更接近真值。 表2 表2 是改进前后ORB-SLAM3帧间跟踪位姿在7个不同场景序列中的绝对位姿误差对比结果，这里的数据为各序列10次评测绝对位姿误差均方根的平均值。由 表2 表2 可知，在测试的7个不同场景序列中，改进后算法输出的位姿精度平均提升38.09%，说明自适应算法能够提高特征点匹配的准确度，而且在不同设备和不同场景的序列中精度均有所提升，具有较强的鲁棒性。 注： 精度提升= <math id="M145"><mfrac><mrow><mi>r</mi><mi>m</mi><mi>s</mi><mo stretchy="false">(</mo><mi mathvariant="normal">B</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">f</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">e</mi><mo stretchy="false">)</mo><mo>-</mo><mi>r</mi><mi>m</mi><mi>s</mi><mo stretchy="false">(</mo><mi mathvariant="normal">A</mi><mi mathvariant="normal">f</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">r</mi><mo stretchy="false">)</mo></mrow><mrow><mi>r</mi><mi>m</mi><mi>s</mi><mo stretchy="false">(</mo><mi mathvariant="normal">B</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">f</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">e</mi><mo stretchy="false">)</mo></mrow></mfrac><mo>×</mo><mn mathvariant="normal">100</mn><mi mathvariant="normal">%</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468230&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468229&type= 47.49800110 7.70466709 数据集 序列 Before/m After/m 精度提升 3 EuRoC MH01 0.374 1 0.217 0 41.99% V101 0.525 9 0.327 5 37.73% V201 0.226 6 0.103 5 54.32% 4 TUM Corridor1 0.123 2 0.077 8 36.85% Magistrale1 0.907 2 0.473 9 47.76% Room1 0.111 6 0.079 4 28.85% Slides1 0.403 7 0.326 5 19.12% 4.3　ORB-SLAM3系统跟踪结果分析 2 为验证算法的可用性，本文对比了不同数据集中不同图像序列利用改进前后ORB-SLAM3系统处理输出位姿的绝对位姿误差，如 表3 表3 所示，这里的数据为各序列10次评测绝对位姿误差均方根的平均值。 注： 精度提升= <math id="M146"><mfrac><mrow><mi>r</mi><mi>m</mi><mi>s</mi><mo stretchy="false">(</mo><mi mathvariant="normal">B</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">f</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">e</mi><mo stretchy="false">)</mo><mo>-</mo><mi>r</mi><mi>m</mi><mi>s</mi><mo stretchy="false">(</mo><mi mathvariant="normal">A</mi><mi mathvariant="normal">f</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">r</mi><mo stretchy="false">)</mo></mrow><mrow><mi>r</mi><mi>m</mi><mi>s</mi><mo stretchy="false">(</mo><mi mathvariant="normal">B</mi><mi mathvariant="normal">e</mi><mi mathvariant="normal">f</mi><mi mathvariant="normal">o</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">e</mi><mo stretchy="false">)</mo></mrow></mfrac><mo>×</mo><mn mathvariant="normal">100</mn><mi mathvariant="normal">%</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468230&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=41468229&type= 47.49800110 7.70466709 数据集 序列 Before/m After/m 精度提升 3 EuRoC MH01 0.038 3 0.031 0 19.06% V101 0.037 9 0.037 8 0.26% V201 0.025 8 0.022 7 12.02% 4 TUM Corridor1 0.037 9 0.031 6 16.62% Magistrale1 0.894 5 0.466 2 47.88% Room1 0.012 4 0.011 1 10.48% Slides1 0.368 2 0.337 6 8.31% 由 表3 表3 可知，在选用的图像序列中，依据本文特征匹配方法改进后的ORB-SLAM3系统的绝对轨迹误差均优于原算法，改进后系统整体位姿精度平均提高16.38%。证明改进后的ORB-SLAM3系统能够在不同场景序列中良好运行，具有较强的鲁棒性，同时定位精度也有所提升。这是因为ORB-SLAM3系统后端采用非线性优化法，即使最终输出位姿经历了三次优化，但其仍与给定的初始位姿具有较强的相关性。而这里的初始位姿正是由跟踪线程提供的，因此本文提出的自适应阈值惯性导航辅助视觉SLAM特征匹配方法直接提升了跟踪线程位姿估计的精度，为SLAM系统提供了较为准确的初值，进而使SLAM系统整体定位精度提高。 4.4　稠密地图的构建 2 为验证算法在地图构建中的有效性，本文利用改进后ORB-SLAM3系统估计的位姿和ELAS（Efficient Large Scale Stereo Matching）算法恢复出的深度数据进行三维点云拼接。以V101序列为例，构建的稠密地图，如 图8 图8 所示。从图中可以看出虽然还有些不规整的地方，但该算法能够完整地表述物体轮廓，几乎没有重影，且周围的离散点云也比较少，墙面、地面近乎在一个平面。这说明本文提出的自适应惯导辅助视觉里程计特征匹配方法不仅能够提升视觉惯性SLAM系统的定位精度，在稠密建图方面也有较好的效果，能够为三维地形重建提供新思路。 5 结 论 1 为解决ORB-SLAM3中图像特征匹配失误率高的问题，本文提出了自适应阈值惯导辅助的图像特征匹配算法。以惯性测量单元误差为基础，根据误差传播定律自动调整图像特征匹配搜索半径，从而利用不同精度的惯性测量数据高效辅助视觉SLAM的特征匹配。实验结果表明，该方法可有效缩小特征匹配的搜索半径，为特征点提供区域约束，显著提高图像特征匹配的准确度，使跟踪线程位姿精度提高约38.09%，系统整体位姿精度提高约16.38%，并且可以构建更精确的稠密三维点云地图。 参考文献 邸凯昌 ， 万文辉 ， 赵红颖 ， 等 . 视觉SLAM技术的进展与应用 ［J］. 测绘学报 ， 2018 ， 47 （ 6 ）： 770 - 779 . doi: 10.11947/j.AGCS.2018.20170652 http://dx.doi.org/10.11947/j.AGCS.2018.20170652 视觉SLAM技术的进展与应用 DI K C ， WAN W H ， ZHAO H Y ， et al . Progress and applications of visual SLAM ［J］. Acta Geodaetica et Cartographica Sinica ， 2018 ， 47 （ 6 ）： 770 - 779 . （in Chinese） . doi: 10.11947/j.AGCS.2018.20170652 http://dx.doi.org/10.11947/j.AGCS.2018.20170652 Progress and applications of visual SLAM KROMBACH N ， DROESCHEL D ， HOUBEN S ， et al . Feature-based visual odometry prior for real-time semi-dense stereo SLAM ［J］. Robotics and Autonomous Systems ， 2018 ， 109 ： 38 - 58 . doi: 10.1016/j.robot.2018.08.002 http://dx.doi.org/10.1016/j.robot.2018.08.002 Feature-based visual odometry prior for real-time semi-dense stereo SLAM 成怡 ， 白佳琪 ， 修春波 . RGB-D视觉SLAM算法的改进及应用 ［J］. 光学 精密工程 ， 2017 ， 15 （ 10 ）： 160 - 166 . RGB-D视觉SLAM算法的改进及应用 CHENG Y ， BAI J Q ， XIU C B . Improvement and application of RGB-D vision SLAM algorithm ［J］. Opt. Precision Eng. ， 2017 ， 15 （ 10 ）： 160 - 166 . （in Chinese） Improvement and application of RGB-D vision SLAM algorithm 黄平 ， 曹镇 ， 王欢 . 基于环形特征匹配的双目视觉里程计 ［J］. 光学学报 ， 2021 ， 41 （ 15 ）： 166 - 176 . 基于环形特征匹配的双目视觉里程计 HUANG P ， CAO Z ， WANG H . Stereo visual odometry based on ring feature matching ［J］. Acta Optica Sinica ， 2021 ， 41 （ 15 ）： 166 - 176 . （in Chinese） Stereo visual odometry based on ring feature matching 王才东 ， 刘丰阳 ， 李志航 ， 等 . 基于双目视觉特征点匹配的图像拼接方法研究 ［J］. 激光与光电子学进展 ， 2021 ， 58 （ 12 ）： 357 - 365 . 基于双目视觉特征点匹配的图像拼接方法研究 WANG C D ， LIU F Y ， LI Z H ， et al . Research on image mosaic method based on binocular vision feature point matching ［J］. Laser ＆ Optoelectronics Progress ， 2021 ， 58 （ 12 ）： 357 - 365 . （in Chinese） Research on image mosaic method based on binocular vision feature point matching HONG S ， KIM J . Selective image registration for efficient visual SLAM on planar surface structures in underwater environment ［J］. Autonomous Robots ， 2019 ， 43 （ 7 ）： 1665 - 1679 . doi: 10.1007/s10514-018-09824-1 http://dx.doi.org/10.1007/s10514-018-09824-1 Selective image registration for efficient visual SLAM on planar surface structures in underwater environment 蔡浩原 ， 赵晟霖 ， 崔松叶 ， 等 . 动态磁场校准的九轴惯性融合 ［J］. 光学 精密工程 ， 2020 ， 28 （ 9 ）： 2007 - 2016 . doi: 10.37188/OPE.20202809.2007 http://dx.doi.org/10.37188/OPE.20202809.2007 动态磁场校准的九轴惯性融合 CAI H Y ， ZHAO S L ， CUI S Y ， et al . Nine-axis inertial fusion method based on dynamic magnetic field calibration ［J］. Opt. Precision Eng. ， 2020 ， 28 （ 9 ）： 2007 - 2016 . （in Chinese） . doi: 10.37188/OPE.20202809.2007 http://dx.doi.org/10.37188/OPE.20202809.2007 Nine-axis inertial fusion method based on dynamic magnetic field calibration 潘林豪 ， 田福庆 ， 应文健 ， 等 . 融合双目视觉与惯导信息的高效视觉里程计算法 ［J］. 计算机应用研究 ， 2021 ， 38 （ 6 ）： 1739 - 1743， 1769 . 融合双目视觉与惯导信息的高效视觉里程计算法 PAN L H ， TIAN F Q ， YING W J ， et al . Efficient visual odometry algorithm combining stereo vision and inertial navigation information ［J］. Application Research of Computers ， 2021 ， 38 （ 6 ）： 1739 - 1743， 1769 . （in Chinese） Efficient visual odometry algorithm combining stereo vision and inertial navigation information 吴建清 ， 宋修广 . 同步定位与建图技术发展综述 ［J］. 山东大学学报（工学版） ， 2021 ， 51 （ 5 ）： 16 - 31 . 同步定位与建图技术发展综述 WU J Q ， SONG X G . Review on development of simultaneous localization and mapping technology ［J］. Journal of Shandong University （Engineering Science） ， 2021 ， 51 （ 5 ）： 16 - 31 . （in Chinese） Review on development of simultaneous localization and mapping technology MASIERO A ， VETTORE A . Improved feature matching for mobile devices with IMU ［J］. Sensors （Basel， Switzerland） ， 2016 ， 16 （ 8 ）： E1243 . doi: 10.3390/s16081243 http://dx.doi.org/10.3390/s16081243 Improved feature matching for mobile devices with IMU 吴斌 ， 王旭日 . 惯性导航辅助图像特征匹配方法研究 ［J］. 激光与光电子学进展 ， 2020 ， 57 （ 10 ）： 346 - 354 . doi: 10.3788/lop57.101509 http://dx.doi.org/10.3788/lop57.101509 惯性导航辅助图像特征匹配方法研究 WU B ， WANG X R . Inertial navigation aided image feature matching method ［J］. Laser ＆ Optoelectronics Progress ， 2020 ， 57 （ 10 ）： 346 - 354 . （in Chinese） . doi: 10.3788/lop57.101509 http://dx.doi.org/10.3788/lop57.101509 Inertial navigation aided image feature matching method GUAN Q ， WEI G L ， WANG Y ， et al . A dual-mode automatic switching feature points matching algorithm fusing IMU data ［J］. Measurement ， 2021 ， 185 ： 110043 . doi: 10.1016/j.measurement.2021.110043 http://dx.doi.org/10.1016/j.measurement.2021.110043 A dual-mode automatic switching feature points matching algorithm fusing IMU data GUAN Q ， WEI G L ， WANG L C ， et al . A novel feature points tracking algorithm in terms of IMU-aided information fusion ［J］. IEEE Transactions on Industrial Informatics ， 2021 ， 17 （ 8 ）： 5304 - 5313 . doi: 10.1109/tii.2020.3024079 http://dx.doi.org/10.1109/tii.2020.3024079 A novel feature points tracking algorithm in terms of IMU-aided information fusion 张一 ， 姜挺 ， 江刚武 ， 等 . 适用于高精度同时定位与地图构建的均衡化亚像素ORB特征提取方法 ［J］. 光学 精密工程 ， 2018 ， 26 （ 10 ）： 2575 - 2583 . doi: 10.3788/ope.20182610.2575 http://dx.doi.org/10.3788/ope.20182610.2575 适用于高精度同时定位与地图构建的均衡化亚像素ORB特征提取方法 ZHANG Y ， JIANG T ， JIANG G W ， et al . Uniform distributed subpixel ORB feature extraction method for high-precision SLAM ［J］. Opt. Precision Eng. ， 2018 ， 26 （ 10 ）： 2575 - 2583 . （in Chinese） . doi: 10.3788/ope.20182610.2575 http://dx.doi.org/10.3788/ope.20182610.2575 Uniform distributed subpixel ORB feature extraction method for high-precision SLAM CAMPOS C ， ELVIRA R ， RODRÍGUEZ J J G ， et al . ORB-SLAM3： an accurate open-source library for visual， visual-inertial， and multimap SLAM ［J］. IEEE Transactions on Robotics ， 2021 ， 37 （ 6 ）： 1874 - 1890 . doi: 10.1109/tro.2021.3075644 http://dx.doi.org/10.1109/tro.2021.3075644 ORB-SLAM3： an accurate open-source library for visual， visual-inertial， and multimap SLAM LUPTON T ， SUKKARIEH S . Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions ［J］. IEEE Transactions on Robotics ， 2012 ， 28 （ 1 ）： 61 - 76 . doi: 10.1109/tro.2011.2170332 http://dx.doi.org/10.1109/tro.2011.2170332 Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions FORSTER C ， CARLONE L ， DELLAERT F ， et al . On-Manifold Preintegration Theory for Fast and Accurate Visual-Inertial Navigation ［J］. IEEE Transactions on Robotics ， 33 （ 1 ）： 1 - 21 . doi: 10.1109/tro.2016.2597321 http://dx.doi.org/10.1109/tro.2016.2597321 On-Manifold Preintegration Theory for Fast and Accurate Visual-Inertial Navigation BURRI M ， NIKOLIC J ， GOHL P ， et al . The EuRoC micro aerial vehicle datasets ［J］. The International Journal of Robotics Research ， 2016 ， 35 （ 10 ）： 1157 - 1163 . doi: 10.1177/0278364915620033 http://dx.doi.org/10.1177/0278364915620033 The EuRoC micro aerial vehicle datasets SCHUBERT D ， GOLL T ， DEMMEL N ， et al . The TUM VI benchmark for evaluating visual-inertial odometry ［C］. 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. Madrid ， Spain . IEEE ， 2018 ： 1680 - 1687 . doi: 10.1109/iros.2018.8593419 http://dx.doi.org/10.1109/iros.2018.8593419 The TUM VI benchmark for evaluating visual-inertial odometry 10.37188/OPE.20233105.0621 TP391.4 A 1004-924X（2023）05-0621-10 贾晓雪,赵冬青,张乐添等.基于自适应惯导辅助特征匹配的视觉SLAM算法[J].光学精密工程,2023,31(05):621-630. 贾晓雪,赵冬青,张乐添等.基于自适应惯导辅助特征匹配的视觉SLAM算法[J].光学精密工程,2023,31(05):621-630. JIA Xiaoxue,ZHAO Dongqing,ZHANG Letian,et al.A visual SLAM algorithm based on adaptive inertial navigation assistant feature matching[J].Optics and Precision Engineering,2023,31(05):621-630. JIA Xiaoxue,ZHAO Dongqing,ZHANG Letian,et al.A visual SLAM algorithm based on adaptive inertial navigation assistant feature matching[J].Optics and Precision Engineering,2023,31(05):621-630. 国家自然科学基金项目（No.41774037， No.41904039） 2022-05-27 2022-07-05 2023-03-10 2023-03-22 光学精密工程 5 31