基于逆深度滤波的双目折反射全景相机动态SLAM系统

基于逆深度滤波的双目折反射全景相机动态SLAM系统 Dynamic SLAM of binocular catadioptric panoramic camera based on inverse depth filter first-author 张　裕（1992-），男，河南遂平人，博士研究生， 2015年于长春理工大学获得学士学位，2018年于内蒙古农业大学获得硕士学位，主要从事机器视觉及视觉SLAM方面的研究。E-mail： zhangyu@mails.cust.edu.cn http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777363&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777365&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777375&type= 22.01332855 32.00401306 张　裕 （1992-），男，河南遂平人，博士研究生， 2015年于长春理工大学获得学士学位，2018年于内蒙古农业大学获得硕士学位，主要从事机器视觉及视觉SLAM方面的研究。E-mail： zhangyu@mails.cust.edu.cn zhangyu@mails.cust.edu.cn corresp E-mail： xxp@cust.edu.cn E-mail： xxp@cust.edu.cn 徐熙平（1969-），男，吉林长春人，教授，博士生导师，1993年、1999年于长春光学精密机械学院分别获得学士、硕士学位，2004年于长春理工大学获得博士学位，主要从事光电检测技术与质量控制方面的研究。E-mail： xxp@cust.edu.cn http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777367&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777386&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777384&type= 22.01332855 32.00399780 徐熙平 （1969-），男，吉林长春人，教授，博士生导师，1993年、1999年于长春光学精密机械学院分别获得学士、硕士学位，2004年于长春理工大学获得博士学位，主要从事光电检测技术与质量控制方面的研究。E-mail： xxp@cust.edu.cn xxp@cust.edu.cn 长春理工大学 光电工程学院，吉林 长春 130022 长春理工大学 光电工程学院，吉林 长春 130022 College of Optoelectronic Engineering， Changchun University of Science and Technology， Changchun 130022， China College of Optoelectronic Engineering， Changchun University of Science and Technology， Changchun 130022， China 为了在复杂的动态环境下能够准确地估计折反射全景相机的位姿，提出一种基于逆深度滤波的双目折反射全景相机动态SLAM系统。分析了双目折反射全景相机系统，在球面进行三角化计算出空间点的深度信息。采用一种基于贝叶斯滤波的逆深度滤波器，对动态地图点进行剔除，只使用静态地图点对折反射全景相机进行位姿估计，可以在动态环境下得到精确的相机位姿估计结果。分别在低动态环境和高动态环境下进行了实验，实验结果表明，在高动态环境中，传统的SLAM系统计算的轨迹出现漂移，而基于逆深度滤波的双目折反射全景相机动态SLAM系统始终可以稳定运行。所提方法在两段高动态环境下的绝对轨迹误差的均方根误差分别为0.52 m和0.56 m，精度比传统的全景SLAM系统提升了96.35%。 To achieve accurate pose estimation of a catadioptric panoramic camera in complex dynamic environments， this study proposed a binocular catadioptric panoramic camera simultaneous localization and mapping （SLAM） system for dynamic environments based on an inverse depth filter. The binocular catadioptric panoramic camera system was analyzed， and the depth information of space points was calculated through the process of triangulation on a sphere. An inverse depth filter based on Bayesian filtering was used to eliminate dynamic map points， whereby only static map points were then used to estimate the poses of the camera. Accurate pose estimation results were obtained in dynamic environments. More specifically， experiments were conducted in both low and high dynamic environments， where results showed that when the trajectory calculated by traditional SLAM system drifted， the dynamic SLAM system based on inverse depth filtering was always executed in a stable manner. The root mean square errors of the absolute trajectory error of our method in the two groups of highly dynamic environments are 0.52 m and 0.56 m， respectively. The accuracy is up to 96.35% higher than that of the traditional panoramic SLAM system. 折反射全景相机 同时定位与地图构建 动态环境 逆深度滤波 catadioptric panoramic camera Simultaneous Localization and Mapping（SLAM） dynamic environments inverse depth filter 1 引　言 1 在同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）理论不断突破与发展的基础上，移动机器人等技术的研究水平显著提高。视觉传感器的引入拓宽了SLAM的应用。视觉SLAM通过图像序列来估计相机运动，根据图像特征的提取方法主要分为基于特征点的视觉SLAM方法和直接SLAM法两种 ［ 1 1 - 2 2 1-2 ］ 。双目立体视觉可以很容易在各种场景中提取视差，从而得到场景中的深度信息。双目SLAM既可以在运动时估计深度，亦可在静止时估计，对周围的物体和自身移动的轨迹的感知更加精确。但是现实中不存在理想静态环境，移动对象的存在不可避免 ［ 3 3 ］ 。动态对象会给相机位姿估计带来误差 ［ 4 4 ］ 。因此，如何解决动态环境中动态点带来误差的问题成为视觉SLAM的关键。 全景相机具有大视场的特性，即使面对快速运动的情况，依然可以获得帧之间的对应关系 ［ 5 5 ］ 。近年来，诸多学者使用大视场相机进行SLAM。Matsukih等将鱼眼相机用于DSO ［ 6 6 ］ （Direct Sparse Odometry），证明了大视场相机用于DSO算法效果较好 ［ 7 7 ］ 。为了精确估计相机位置，文章［8-10］采用鲁棒特征和传统的扩展卡尔曼滤波（EKF）-SLAM方法，但这些方法的处理速度太低，并且缺乏有效的反馈定位方法。Lukierski等人提出一种密集3D重建算法，不依赖关键点，能够更好地采集强梯度区域图像的像素，用来估计相机位置。文献［ 11 11 - 12 12 11-12 ］基于全景相机使用稀疏视觉上的方法进行视觉里程计研究，更好地定位相机。 针对动态环境，Li等 ［ 13 13 ］ 提出了实时深度边缘的RGB-D SLAM系统。该系统计算每个关键帧中点是静态点的可能性，使用静态加权方法降低动态物体对相机位姿估计的影响。但是，该系统需要依赖RGB-D相机提供的深度信息，这对于普通相机是不适用的。文献［ 14 14 - 15 15 14-15 ］利用深度学习对动态目标的检测，达到了去除动态特征点，提高位姿估计精度的目的。 上述工作都只能利用相邻两帧之间的信息来判断场景中对象是动态还是静态的。为了利用更多图像帧的信息，更加准确地判断场景中对象的运动状态，本文提出基于逆深度滤波的双目折反射全景相机动态SLAM系统，以去除动态点为目标，利用静态点对相机进行位姿估计。使用双目全景相机通过立体视觉计算的距离信息对视觉里程计进行补偿推断，采用一种基于贝叶斯滤波的逆深度滤波器，对动态地图点进行剔除，在动态环境下得到更加精确的相机位姿估计结果。此方法可以解决传统SLAM系统面对动态环境时计算轨迹出现漂移的现象，并且计算的位姿结果也更加精确。 2 双目折反射全景相机系统 1 2.1　折反射全景相机投影模型 2 折反射全景相机通过顶端反射镜扩大相机的视场，由反射镜和成像模组两部分组成。其结构如 图1 图1 所示。 为了表示世界坐标系下点到像素坐标系下的投影过程，应用球面投影模型对折反射全景相机的投影模型进行分析 ［ 16 16 ］ 。球面投影模型的成像过程如 图2 图2 所示。 首先，将世界坐标系下的空间点 <math id="M1"><msub><mrow><mi>X</mi></mrow><mrow><mi>c</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777404&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777416&type= 3.55599999 3.72533321 以球心为投影中心投影到单位球上于点 <math id="M2"><msub><mrow><mi>X</mi></mrow><mrow><mi>s</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777408&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777406&type= 3.47133350 3.72533321 ，坐标为： <math display="block" id="M3"><msub><mrow><mi>X</mi></mrow><mrow><mi>s</mi></mrow></msub><mo>=</mo><msup><mrow><mfenced open="[" close="]" separators="|"><mrow><msub><mrow><mi>x</mi></mrow><mrow><mi>s</mi></mrow></msub><mo>,</mo><msub><mrow><mi>y</mi></mrow><mrow><mi>s</mi></mrow></msub><mo>,</mo><msub><mrow><mi>z</mi></mrow><mrow><mi>s</mi></mrow></msub></mrow></mfenced></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup><mo>=</mo><mfrac><mrow><msub><mrow><mi>X</mi></mrow><mrow><mi>c</mi></mrow></msub></mrow><mrow><mfenced open="‖" close="‖" separators="|"><mrow><msub><mrow><mi>X</mi></mrow><mrow><mi>c</mi></mrow></msub></mrow></mfenced></mrow></mfrac></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777420&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777409&type= 38.35400009 10.07533360 . 折反射全景相机反射镜的镜面参数由 <math id="M4"><mi>ξ</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777411&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777410&type= 1.60866666 3.64066648 来表示。将投影中心由球心移动到 <math id="M5"><msub><mrow><mi>C</mi></mrow><mrow><mi>p</mi></mrow></msub><mo>=</mo><msup><mrow><mfenced separators="|"><mrow><mn mathvariant="normal">0,0</mn><mo>,</mo><mo>-</mo><mi>ξ</mi></mrow></mfenced></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777432&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777422&type= 27.68600082 5.50333309 ，以 <math id="M6"><msub><mrow><mi>C</mi></mrow><mrow><mi>p</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777434&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777424&type= 3.72533321 3.72533321 为投影中心将单位球上的三维点 <math id="M7"><msub><mrow><mi>X</mi></mrow><mrow><mi>s</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777427&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777437&type= 3.47133350 3.72533321 投影到与 C p 距离为1的二维平面上对应点 <math id="M8"><msub><mrow><mi>X</mi></mrow><mrow><mi>m</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777445&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777440&type= 4.31799984 3.72533321 ，对应关系如下： <math display="block" id="M9"><msub><mrow><mi>X</mi></mrow><mrow><mi>m</mi></mrow></msub><mo>=</mo><mi>D</mi><mfenced separators="|"><mrow><msub><mrow><mi>X</mi></mrow><mrow><mi>s</mi></mrow></msub></mrow></mfenced><mo>=</mo><msup><mrow><mfenced open="[" close="]" separators="|"><mrow><mfrac><mrow><msub><mrow><mi>x</mi></mrow><mrow><mi>s</mi></mrow></msub></mrow><mrow><mi>ξ</mi><mo>+</mo><msub><mrow><mi>z</mi></mrow><mrow><mi>s</mi></mrow></msub></mrow></mfrac><mo>,</mo><mfrac><mrow><msub><mrow><mi>y</mi></mrow><mrow><mi>s</mi></mrow></msub></mrow><mrow><mi>ξ</mi><mo>+</mo><msub><mrow><mi>z</mi></mrow><mrow><mi>s</mi></mrow></msub></mrow></mfrac><mo>,</mo><mn mathvariant="normal">1</mn></mrow></mfenced></mrow><mrow><mi mathvariant="normal">T</mi></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777451&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777447&type= 55.62599945 10.75266743 . 最后，经过广义投影矩阵 K ，完成图像坐标系到像素坐标系之间的变换，关系如下： <math display="block" id="M10"><mi>p</mi><mo>=</mo><mi mathvariant="bold-italic">K</mi><msub><mrow><mi>x</mi></mrow><mrow><mi>m</mi></mrow></msub><mo>=</mo><mfenced open="[" close="]" separators="|"><mrow><mtable><mtr><mtd><msub><mrow><mi>f</mi></mrow><mrow><mi>x</mi></mrow></msub></mtd><mtd><msub><mrow><mi>f</mi></mrow><mrow><mi>x</mi></mrow></msub><mi>s</mi></mtd><mtd><msub><mrow><mi>u</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><msub><mrow><mi>f</mi></mrow><mrow><mi>y</mi></mrow></msub></mtd><mtd><msub><mrow><mi>v</mi></mrow><mrow><mn mathvariant="normal">0</mn></mrow></msub></mtd></mtr><mtr><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">0</mn></mtd><mtd><mn mathvariant="normal">1</mn></mtd></mtr></mtable></mrow></mfenced><msub><mrow><mi>x</mi></mrow><mrow><mi>m</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777460&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777453&type= 42.24866486 15.23999977 . 上述推导公式表示世界坐标系下的点在全景相机下的成像过程，很容易得到像素点到空间点的反投影过程。 2.2　双目全景相机三角测量 2 双目折反射全景相机系统可以利用双目图像计算特征点的深度信息。将深度信息输入逆深度滤波器，对动态地图点进行剔除。这里通过三角化来计算特征点深度，针对折反射全景相机的投影模型，将特征点反投影到球面，在球面上分析三角化过程。 如 图3 图3 所示，空间点 P 同时被双目折反射全景相机观测到， <math id="M11"><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=29777462&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777456&type= 2.62466669 4.57200003 和 <math id="M12"><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=29777480&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777471&type= 2.79399991 4.57200003 分别是左右两个折反射相机上对应的特征点反投影到球面上的向量。其中利用两个几何关系相对固定的相机以不同视角获取被测物体的影像信息，再利用同一个视觉信息在不同相机下的匹配特征不同进行双目相机标定 ［ 17 17 - 18 18 17-18 ］ 。两个相机间的位姿关系通过标定是已知的，根据对极几何的定义， <math id="M13"><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=29777470&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777469&type= 2.62466669 4.57200003 和 <math id="M14"><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=29777480&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777471&type= 2.79399991 4.57200003 满足以下关系： <math display="block" id="M15"><msub><mrow><mi mathvariant="bold-italic">s</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow></msub><msub><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow></msub><mo>=</mo><msub><mrow><mi mathvariant="bold-italic">s</mi></mrow><mrow><mi mathvariant="normal">l</mi></mrow></msub><mi mathvariant="bold-italic">R</mi><msub><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi mathvariant="normal">l</mi></mrow></msub><mo>+</mo><mi mathvariant="bold-italic">t</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777492&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777481&type= 24.46866608 4.57200003 . 在 式（4） 式（4） 的两侧同时乘一个 <math id="M16"><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow><mrow><mo>∧</mo></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777499&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777498&type= 3.21733332 4.82600021 ，得到： <math display="block" id="M17"><msub><mrow><mi mathvariant="bold-italic">s</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow></msub><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow><mrow><mo>∧</mo></mrow></msubsup><msub><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow></msub><mo>=</mo><mn mathvariant="normal">0</mn><mo>=</mo><msub><mrow><mi mathvariant="bold-italic">s</mi></mrow><mrow><mi mathvariant="normal">l</mi></mrow></msub><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow><mrow><mo>∧</mo></mrow></msubsup><mi mathvariant="bold-italic">R</mi><msub><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi mathvariant="normal">l</mi></mrow></msub><mo>+</mo><msubsup><mrow><mi mathvariant="bold-italic">p</mi></mrow><mrow><mi mathvariant="normal">r</mi></mrow><mrow><mo>∧</mo></mrow></msubsup><mi mathvariant="bold-italic">t</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777511&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777508&type= 42.58732986 4.82600021 . 通过方程可以求得 <math id="M18"><msub><mrow><mi>s</mi></mrow><mrow><mi mathvariant="normal">l</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777502&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777514&type= 1.94733346 3.72533321 。于是本文计算了特征点的深度信息，可以确定其在空间中的坐标。将深度信息输入到逆深度滤波器模块可以区分出地图点的类别。 3 逆深度滤波关键算法 1 深度滤波器采用概率的方式对空间点进行深度估计，随着新的图像帧的进入，对深度的后验概率分布进行更新。Forster等 ［ 19 19 ］ 发现在大规模场景下逆深度的统计直方图更接近高斯均匀混合分布，因此，提出了逆深度滤波器的方法，就是使用空间点深度值的倒数代替深度值在深度滤波器中对地图点进行处理。逆深度滤波器将最新时刻求得的逆深度观测值，与上一时刻的逆深度估计值进行融合，直到地图点的逆深度值收敛。使用逆深度滤波器可以将地图点分为有效的和无效的测量值两种类别。 虽然文献［ 20 20 ］中模型最初是用来找到不同基线下最准确的逆深度测量值的，但是该模型也适合检测动态环境下双目折反射全景相机SLAM系统中的动态地图点，因此将逆深度滤波器应用到双目折反射全景相机的动态SLAM系统中，使用逆深度滤波器来区分地图点中的动态点和静态点。 式（6） 式（6） 给出了逆深度滤波器的概率模型： <math display="block" id="M19"><mtable><mtr><mtd><mi>p</mi><mfenced separators="|"><mrow><mi>x</mi><mo stretchy="false">|</mo><mi>Z</mi><mo>,</mo><mi mathvariant="normal">π</mi></mrow></mfenced><mo>=</mo><mi mathvariant="normal">π</mi><mi>N</mi><mfenced separators="|"><mrow><mi>x</mi><mo stretchy="false">|</mo><mi>Z</mi><mo>,</mo><msup><mrow><mi>τ</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup></mrow></mfenced><mo>+</mo></mtd></mtr><mtr><mtd><mfenced separators="|"><mrow><mn mathvariant="normal">1</mn><mo>-</mo><mi mathvariant="normal">π</mi></mrow></mfenced><mi>U</mi><mfenced separators="|"><mrow><mi>x</mi><mo stretchy="false">|</mo><msub><mrow><mi>Z</mi></mrow><mrow><mi mathvariant="normal">m</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi></mrow></msub><mo>,</mo><msub><mrow><mi>Z</mi></mrow><mrow><mi mathvariant="normal">m</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">x</mi></mrow></msub></mrow></mfenced></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777534&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777516&type= 42.07933044 11.09133339 . 针对双目折反射全景相机的动态SLAM系统，本文对 式（6） 式（6） 中各符号进行了重新定义。使用 <math id="M20"><mi mathvariant="normal">π</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777612&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777610&type= 1.94733346 2.79399991 代表计算出的地图点是静态点的概率；对应的 <math id="M21"><mn mathvariant="normal">1</mn><mo>-</mo><mi mathvariant="normal">π</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777541&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777522&type= 8.72066593 2.79399991 代表的是该点是动态点的概率。 <math id="M22"><mi>N</mi><mfenced separators="|"><mrow><mi>x</mi><mo stretchy="false">|</mo><mi>Z</mi><mo>,</mo><msup><mrow><mi>τ</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777527&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777524&type= 16.93333435 4.48733330 是指静态点是服从 <math id="M23"><mi>Z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777652&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777677&type= 2.37066650 2.79399991 为均值， <math id="M24"><msup><mrow><mi>τ</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777563&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777548&type= 2.87866688 3.04800010 为方差的高斯分布，其中 <math id="M25"><mi>x</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777566&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777564&type= 2.03200006 2.79399991 代表由双目全景相机计算得到的逆深度值， <math id="M26"><mi>Z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777652&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777677&type= 2.37066650 2.79399991 代表真实的逆深度值， <math id="M27"><msup><mrow><mi>τ</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777554&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777553&type= 2.87866688 3.04800010 代表一个像素对逆深度的扰动； <math id="M28"><mi>U</mi><mfenced separators="|"><mrow><mi>x</mi><mo stretchy="false">|</mo><msub><mrow><mi>Z</mi></mrow><mrow><mi mathvariant="normal">m</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi></mrow></msub><mo>,</mo><msub><mrow><mi>Z</mi></mrow><mrow><mi mathvariant="normal">m</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">x</mi></mrow></msub></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777574&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777573&type= 23.36800003 4.82600021 是指动态点在 <math id="M29"><mfenced separators="|"><mrow><msub><mrow><mi>Z</mi></mrow><mrow><mi mathvariant="normal">m</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi></mrow></msub><mo>,</mo><msub><mrow><mi>Z</mi></mrow><mrow><mi mathvariant="normal">m</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">x</mi></mrow></msub></mrow></mfenced></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777559&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777575&type= 16.59466553 4.82600021 内服从均匀分布，可以由场景中几何的先验得到 <math id="M30"><msub><mrow><mi>Z</mi></mrow><mrow><mi mathvariant="normal">m</mi><mi mathvariant="normal">i</mi><mi mathvariant="normal">n</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777578&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777561&type= 5.58799982 3.72533321 和 <math id="M31"><msub><mrow><mi>Z</mi></mrow><mrow><mi mathvariant="normal">m</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">x</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777581&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777576&type= 5.92666674 3.72533321 。对于一个地图点，可以随着新的图像帧的进入，不断更新对概率 <math id="M32"><mi mathvariant="normal">π</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777612&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777610&type= 1.94733346 2.79399991 的估计，直到它收敛，以此来区分动态地图点和静态地图点。 如 图4 图4 所示，基于ORB-SLAM2的框架，添加了逆深度滤波线程。使用双目折反射全景相机对空间点的深度进行测量，然后转换成逆深度。将得到的地图点输入逆深度滤波器，随着观测次数的增加，地图点的逆深度值逐渐收敛。对于收敛的地图点，判断它是静态点的概率 <math id="M33"><mi mathvariant="normal">π</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777612&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777610&type= 1.94733346 2.79399991 是否大于阈值，区分出动态地图点和静态地图点。然后剔除动态地图点，只有静态地图点被输入到跟踪模块用于相机的位姿估计，以此达到更加鲁棒和精确的相机位姿计算。 现假设有一组通过双目折反射全景相机测量得到的含有噪声的地图点的逆深度值 <math id="M34"><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>,</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo>,</mo><mo>⋯</mo><msub><mrow><mi>x</mi></mrow><mrow><mi>N</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777606&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777604&type= 17.78000069 4.14866638 ，它们之间是相互独立的，现在要估计真实的逆深度值 <math id="M35"><mi>Z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777652&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777677&type= 2.37066650 2.79399991 和概率 <math id="M36"><mi mathvariant="normal">π</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777612&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777610&type= 1.94733346 2.79399991 ，可以通过求最大后验概率得到。根据贝叶斯公式，其后验具有以下形式： <math display="block" id="M37"><mtable><mtr><mtd><mi>p</mi><mfenced separators="|"><mrow><mi>Z</mi><mo>,</mo><mi>π</mi><mo stretchy="false">|</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>,</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo>,</mo><mo>⋯</mo><msub><mrow><mi>x</mi></mrow><mrow><mi>N</mi></mrow></msub></mrow></mfenced><mo>=</mo></mtd></mtr><mtr><mtd><mfrac><mrow><mi>p</mi><mfenced separators="|"><mrow><mi>Z</mi><mo>,</mo><mi>π</mi></mrow></mfenced><mi>p</mi><mfenced separators="|"><mrow><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>,</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo>,</mo><mo>⋯</mo><msub><mrow><mi>x</mi></mrow><mrow><mi>N</mi></mrow></msub><mo stretchy="false">|</mo><mi>Z</mi><mo>,</mo><mi>π</mi></mrow></mfenced></mrow><mrow><mi>p</mi><mfenced separators="|"><mrow><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>,</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo>,</mo><mo>⋯</mo><msub><mrow><mi>x</mi></mrow><mrow><mi>N</mi></mrow></msub></mrow></mfenced></mrow></mfrac><mo>∝</mo></mtd></mtr><mtr><mtd><mi>p</mi><mfenced separators="|"><mrow><mi>Z</mi><mo>,</mo><mi>π</mi></mrow></mfenced><mi>p</mi><mfenced separators="|"><mrow><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>,</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo>,</mo><mo>⋯</mo><msub><mrow><mi>x</mi></mrow><mrow><mi>N</mi></mrow></msub><mo stretchy="false">|</mo><mi>Z</mi><mo>,</mo><mi>π</mi></mrow></mfenced><mo>.</mo></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777591&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777614&type= 48.25999832 24.63800049 由于深度测量值相互独立，因此 式（7） 式（7） 右边可写成： <math display="block" id="M38"><mtable><mtr><mtd><mi>p</mi><mfenced separators="|"><mrow><mi>Z</mi><mo>,</mo><mi>π</mi></mrow></mfenced><mi>p</mi><mfenced separators="|"><mrow><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">1</mn></mrow></msub><mo>,</mo><msub><mrow><mi>x</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msub><mo>,</mo><mo>⋯</mo><msub><mrow><mi>x</mi></mrow><mrow><mi>N</mi></mrow></msub><mo stretchy="false">|</mo><mi>Z</mi><mo>,</mo><mi>π</mi></mrow></mfenced><mo>∝</mo></mtd></mtr><mtr><mtd><mi>p</mi><mfenced separators="|"><mrow><mi>Z</mi><mo>,</mo><mi>π</mi></mrow></mfenced><munder><mo movablelimits="false">∏</mo><mrow><mi>n</mi></mrow></munder><mrow><mi>p</mi><mfenced separators="|"><mrow><msub><mrow><mi>x</mi></mrow><mrow><mi>n</mi></mrow></msub><mo stretchy="false">|</mo><mi>Z</mi><mo>,</mo><mi>π</mi></mrow></mfenced></mrow><mo>.</mo></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777623&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777617&type= 47.49800110 13.63133430 真实后验概率可以用贝塔-高斯分布来近似 ［ 18 18 ］ ，如下： <math display="block" id="M39"><mtable><mtr><mtd><mi>q</mi><mo stretchy="false">(</mo><mi>Z</mi><mo>,</mo><mi>π</mi><mo stretchy="false">|</mo><msub><mrow><mi>a</mi></mrow><mrow><mi>N</mi></mrow></msub><mo>,</mo><msub><mrow><mi>b</mi></mrow><mrow><mi>N</mi></mrow></msub><mo>,</mo><msub><mrow><mi>μ</mi></mrow><mrow><mi>N</mi></mrow></msub><mo>,</mo><msub><mrow><mi>σ</mi></mrow><mrow><mi>N</mi></mrow></msub><mo stretchy="false">)</mo><mo>=</mo></mtd></mtr><mtr><mtd><mi>B</mi><mi>e</mi><mi>t</mi><mi>a</mi><mfenced separators="|"><mrow><mi>π</mi><mo stretchy="false">|</mo><msub><mrow><mi>a</mi></mrow><mrow><mi>N</mi></mrow></msub><mo>,</mo><msub><mrow><mi>b</mi></mrow><mrow><mi>N</mi></mrow></msub></mrow></mfenced><mi>N</mi><mfenced separators="|"><mrow><mi>Z</mi><mo stretchy="false">|</mo><msub><mrow><mi>μ</mi></mrow><mrow><mi>N</mi></mrow></msub><mo>,</mo><msubsup><mrow><mi>σ</mi></mrow><mrow><mi>N</mi></mrow><mrow/></msubsup></mrow></mfenced><mo>,</mo></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777619&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777625&type= 44.45000076 10.66800022 其中 <math id="M40"><msub><mrow><mi>a</mi></mrow><mrow><mi>N</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777627&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777621&type= 3.55599999 3.72533321 和 <math id="M41"><msub><mrow><mi>b</mi></mrow><mrow><mi>N</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777630&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777629&type= 3.55599999 3.72533321 是Beta分布的参数。为了减少计算量，继续推导公式的迭代形式是必要的。通过推导可以得到迭代形式： <math display="block" id="M42"><mtable><mtr><mtd><mi>q</mi><mo stretchy="false">(</mo><mi>Z</mi><mo>,</mo><mi>π</mi><mo stretchy="false">|</mo><msub><mrow><mi>a</mi></mrow><mrow><mi>N</mi></mrow></msub><mo>,</mo><msub><mrow><mi>b</mi></mrow><mrow><mi>N</mi></mrow></msub><mo>,</mo><msub><mrow><mi>μ</mi></mrow><mrow><mi>N</mi></mrow></msub><mo>,</mo><msub><mrow><mi>σ</mi></mrow><mrow><mi>N</mi></mrow></msub><mo stretchy="false">)</mo><mo>×</mo><mo>∝</mo></mtd></mtr><mtr><mtd><mi>q</mi><mo stretchy="false">(</mo><mi>Z</mi><mo>,</mo><mi>π</mi><mo stretchy="false">|</mo><msub><mrow><mi>a</mi></mrow><mrow><mi>N</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub><mo>,</mo><msub><mrow><mi>b</mi></mrow><mrow><mi>N</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub><mo>,</mo><msub><mrow><mi>μ</mi></mrow><mrow><mi>N</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub><mo>,</mo><msub><mrow><mi>σ</mi></mrow><mrow><mi>N</mi><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msub><mo stretchy="false">)</mo><mi>p</mi><mfenced separators="|"><mrow><msub><mrow><mi>x</mi></mrow><mrow><mi>N</mi></mrow></msub><mo stretchy="false">|</mo><mi>Z</mi><mo>,</mo><mi>π</mi></mrow></mfenced><mo>.</mo></mtd></mtr></mtable></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777633&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777631&type= 65.27799988 10.66800022 通过匹配 <math id="M43"><mi>Z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777652&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777677&type= 2.37066650 2.79399991 和 <math id="M44"><mi>π</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777705&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777693&type= 1.94733346 2.79399991 的第一阶和第二阶矩，用Beta <math id="M45"><mo>×</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777656&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777640&type= 2.53999996 2.79399991 Gaussian分布近似 式（10） 式（10） 的后验，导出了 <math id="M46"><msub><mrow><mi>a</mi></mrow><mrow><mi>N</mi></mrow></msub><mo>,</mo><msub><mrow><mi>b</mi></mrow><mrow><mi>N</mi></mrow></msub><mo>,</mo><msub><mrow><mi>μ</mi></mrow><mrow><mi>N</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777659&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777657&type= 14.64733315 4.57200003 和 <math id="M47"><msubsup><mrow><mi>σ</mi></mrow><mrow><mi>N</mi></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777662&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777661&type= 3.38666677 3.97933316 的更新公式。推导过程参考文献［ 21 21 ］。计算 <math id="M48"><mi>q</mi><mo stretchy="false">(</mo><mi>Z</mi><mo>,</mo><mi>π</mi><mo stretchy="false">|</mo><msub><mrow><mi>a</mi></mrow><mrow><mi>N</mi></mrow></msub><mo>,</mo><msub><mrow><mi>b</mi></mrow><mrow><mi>N</mi></mrow></msub><mo>,</mo><msub><mrow><mi>μ</mi></mrow><mrow><mi>N</mi></mrow></msub><mo>,</mo><msub><mrow><mi>σ</mi></mrow><mrow><mi>N</mi></mrow></msub><mo stretchy="false">)</mo></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777666&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777664&type= 32.17333603 4.82600021 的一阶矩，用来近似估计 <math id="M49"><mi>π</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777705&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777693&type= 1.94733346 2.79399991 和 <math id="M50"><mi>Z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777652&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777677&type= 2.37066650 2.79399991 ： <math display="block" id="M51"><mi>Z</mi><mo>=</mo><msub><mrow><mi>μ</mi></mrow><mrow><mi>N</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777673&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777645&type= 10.83733368 4.57200003 ， <math display="block" id="M52"><mi>π</mi><mo>=</mo><mfrac><mrow><msub><mrow><mi>a</mi></mrow><mrow><mi>N</mi></mrow></msub></mrow><mrow><msub><mrow><mi>a</mi></mrow><mrow><mi>N</mi></mrow></msub><mo>+</mo><msub><mrow><mi>b</mi></mrow><mrow><mi>N</mi></mrow></msub></mrow></mfrac></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777676&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777648&type= 19.47333336 8.89000034 . 因此，每当有新的一帧图片传入系统，本文使用 式（11） 式（11） 对逆深度 <math id="M53"><mi>Z</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777652&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777677&type= 2.37066650 2.79399991 进行更新，使用 式（12） 式（12） 对该地图点是静态点的概率 <math id="M54"><mi>π</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777687&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777700&type= 1.94733346 2.79399991 进行更新。对 <math id="M55"><mi>π</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777705&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777693&type= 1.94733346 2.79399991 设定一个阈值，若地图点收敛后 <math id="M56"><mi>π</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777687&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777700&type= 1.94733346 2.79399991 大于该阈值，则认为该地图点为静态点；否则剔除该地图点，不参与折反射全景相机的位姿解算。 图5 图5 是逆深度滤波的具体实现示意图。首先，通过双目折反射全景相机初始化出一组地图点，随着新的双目图像输入系统，生成新地图点的观测并对已存在的地图点生成重复观测，不断对地图点的深度和概率 <math id="M57"><mi>π</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777705&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777693&type= 1.94733346 2.79399991 更新。地图点被分为已收敛的地图点和未收敛的地图点，未收敛的地图点继续更新直到收敛。已经收敛的地图点是静态点的概率 <math id="M58"><mi>π</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777705&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777693&type= 1.94733346 2.79399991 大于等于阈值时被判断是静态点，参与全景相机的位姿估计；概率 <math id="M59"><mi>π</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777705&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777693&type= 1.94733346 2.79399991 小于阈值的是动态点，被剔除。 4 实验与分析 1 为了验证基于逆深度滤波的双目折反射全景相机动态SLAM系统的性能，在公开数据集 ［ 22 22 ］ 上进行实验。该数据集由装载2个折反射全景相机的汽车在城市街道中采集，共包含12 607帧图像。图像中有大量的动态物体和静态物体，非常适合对动态环境下折反射全景相机的SLAM系统性能进行测试。 在数据集中选取多段包含动态对象的图像序列，并分为低动态序列和高动态序列。选取了一段低动态和两段高动态的图像序列进行实验，对本文提出的SLAM算法进行验证，对SLAM系统分别评估了绝对轨迹误差（Absolute trajectory Error， ATE）和相对轨迹误差（Relative Pose Error， RPE）两个指标。ATE是估计轨迹与ground truth之间的误差，它可以直接反映出位姿估计算法的精度和全局一致性，通常用于评估整个SLAM系统的性能。RPE是评价系统漂移的指标，它是固定时间段内两个位姿变化量的差。考虑ATE和RPE在平移上的误差，其均方根误差（Root Mean Square Error，RMSE）的计算如下： <math display="block" id="M60"><msub><mrow><mi>E</mi></mrow><mrow><mi mathvariant="normal">a</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">s</mi></mrow></msub><mo>=</mo><mroot><mrow><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mi>N</mi></mrow></mfrac><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>i</mi><mo>=</mo><mn mathvariant="normal">1</mn></mrow><mrow><mi>N</mi></mrow></munderover></mstyle><mrow><msubsup><mrow><mfenced open="‖" close="‖" separators="|"><mrow><mi mathvariant="normal">t</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">s</mi><mo stretchy="false">(</mo><msubsup><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">t</mi><mo>,</mo><mi>i</mi></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msubsup><msub><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="normal">e</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">i</mi><mo>,</mo><mi>i</mi></mrow></msub><mo stretchy="false">)</mo></mrow></mfenced></mrow><mrow><mn mathvariant="normal">2</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mrow></mrow><mrow/></mroot></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777708&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777706&type= 53.67866135 9.90600014 ， <math display="block" id="M61"><msub><mrow><mi>E</mi></mrow><mrow><mi mathvariant="normal">r</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">s</mi></mrow></msub><mo>=</mo><mroot><mrow><mfrac><mrow><mn mathvariant="normal">1</mn></mrow><mrow><mi>N</mi><mo>-</mo><mi mathvariant="normal">Δ</mi><mi>t</mi></mrow></mfrac><msubsup><mrow><mstyle displaystyle="true"><munderover><mo largeop="true">∑</mo><mrow><mi>i</mi><mo>=</mo><mn mathvariant="normal">1</mn></mrow><mrow><mi>N</mi><mo>-</mo><mi mathvariant="normal">Δ</mi><mi>t</mi></mrow></munderover></mstyle><mrow><mfenced open="‖" close="‖" separators="|"><mrow><mi mathvariant="normal">t</mi><mi mathvariant="normal">r</mi><mi mathvariant="normal">a</mi><mi mathvariant="normal">n</mi><mi mathvariant="normal">s</mi><mfenced separators="|"><mrow><mo stretchy="false">(</mo><msubsup><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">t</mi><mo>,</mo><mi>i</mi></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msubsup><msub><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">t</mi><mo>,</mo><mi>i</mi><mo>+</mo><mi mathvariant="normal">Δ</mi><mi>t</mi></mrow></msub><msup><mrow><mo stretchy="false">)</mo></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msup><mo stretchy="false">(</mo><msubsup><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="normal">e</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">i</mi><mo>,</mo><mi>i</mi></mrow><mrow><mo>-</mo><mn mathvariant="normal">1</mn></mrow></msubsup><msub><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="normal">e</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">i</mi><mo>,</mo><mi>i</mi><mo>+</mo><mi mathvariant="normal">Δ</mi><mi>t</mi></mrow></msub><mo stretchy="false">)</mo></mrow></mfenced></mrow></mfenced></mrow></mrow><mrow><mn mathvariant="normal">2</mn></mrow><mrow><mn mathvariant="normal">2</mn></mrow></msubsup></mrow><mrow/></mroot></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777712&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777710&type= 95.67333221 11.09133339 ， 其中： <math id="M62"><mi>N</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777730&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777722&type= 2.87866688 2.87866688 表示轨迹中包含的位姿个数， <math id="M63"><mi>i</mi><mo>=</mo><mn mathvariant="normal">1,2</mn><mo>,</mo><mo>⋯</mo><mo>,</mo><mi>N</mi></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777734&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777732&type= 22.01333237 3.21733332 ； <math id="M64"><msub><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="normal">g</mi><mi mathvariant="normal">t</mi><mo>,</mo><mi>i</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777724&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777736&type= 5.92666674 3.72533321 和 <math id="M65"><msub><mrow><mi mathvariant="bold-italic">T</mi></mrow><mrow><mi mathvariant="normal">e</mi><mi mathvariant="normal">s</mi><mi mathvariant="normal">t</mi><mi mathvariant="normal">i</mi><mo>,</mo><mi>i</mi></mrow></msub></math> http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777727&type= http://html.publish.founderss.cn/rc-pub/api/common/picture?pictureId=29777725&type= 7.11199999 3.72533321 分别表示真实轨迹和SLAM系统估计出的轨迹；trans是取括号内变量的平移部分。 本文使用RMSE、平均误差、中值误差和标准差（Standard Deviation， SD）评价ATE指标。将使用本文方法计算出的轨迹分别与单目全景视觉里程计方法及真值进行对比。其中，单目全视觉里程计（Visual Odometry， VO）是基于ORB-SLAM2改进的适合折反射全景相机的算法，把计算出的RMSE、平均误差、中值误差和SD进行统计，结果如 表1 表1 所示。使用相对位移评价RPE指标， 图6 图6 是本文计算出的轨迹与真值之间的误差，其中6（a）是低动态序列，6（b）和6（c）为高动态序列。分析 图6 图6 可知，本文计算出的轨迹与真值的误差一直在较小的范围内变化，没有因为动态对象而出现轨迹的突变和漂移，证明了本文算法的鲁棒性。 2 Sequence 4 Catadioptric Pano VO 4 Ours RMSE Median Mean Std RMSE Median Mean Std Low dynamic sequence（a） 0.35 0.33 0.31 0.12 0.29 0.28 0.25 0.09 High dynamic sequence（b） 12.63 9.69 10.67 4.11 0.52 0.51 0.47 0.16 High dynamic sequence（c） 15.35 12.14 12.13 5.29 0.56 0.52 0.53 0.19 m 图7 图7 中是不同方法得到的轨迹与真值的对比。本文计算出的轨迹与真值更加接近，而基于ORB-SLAM2改进的折反射全景VO ［ 23 23 ］ 在低动态环境下与本文计算的轨迹最接近，因为在低动态环境下RANSAC算法将动态点作为外点，受到动态点的影响小。而在另外两个序列中存在高动态目标，此时RANSAC算法失效，本文的系统可以有效去除动态地图点并准确估计出相机位姿。 分析 表1 表1 中的数据，本文的系统在3个序列中的表现都比折反射全景VO要好，对比ATE 的RMSE，在3个序列上本文系统比折反射全景VO分别提高了17.14%，95.88%，96.35%。在面对复杂的动态环境时，本文系统能够有效地去除动态地图点，位姿估计精度相比传统方法有大幅的提升。 5 结　论 1 为了在复杂的动态环境下能够准确地估计折反射全景相机的位姿，本文提出了基于逆深度滤波的双目折反射全景相机动态SLAM系统，基于ORB-SLAM2的框架设计了双目折反射全景相机的SLAM系统，并在跟踪模块和局部建图模块中间添加了基于贝叶斯滤波的逆深度滤波器模块，对折反射全景相机求出的地图点进行状态估计，剔除动态地图点，只使用收敛的静态地图点对折反射全景相机进行位姿估计，在动态环境下得到了更加精确的相机位姿估计结果。该系统在3个序列中的绝对轨迹误差 的RMSE，比折反射全景VO分别提高了17.14%，95.88%和96.35%。在面对动态环境时，当传统的SLAM系统计算的轨迹出现漂移，基于逆深度滤波的双目折反射全景相机动态SLAM系统始终运行稳定，具有更强的鲁棒性。 参考文献 TAKETOMI T ， UCHIYAMA H ， IKEDA S . Visual SLAM algorithms： a survey from 2010 to 2016 ［J］. IPSJ Transactions on Computer Vision and Applications ， 2017 ， 9 （ 1 ）： 1 - 11 . doi: 10.1186/s41074-017-0027-2 http://dx.doi.org/10.1186/s41074-017-0027-2 Visual SLAM algorithms： a survey from 2010 to 2016 FUENTES-PACHECO J ， RUIZ-ASCENCIO J ， RENDÓN-MANCHA J M . Visual simultaneous localization and mapping： a survey ［J］. Artificial Intelligence Review ， 2015 ， 43 （ 1 ）： 55 - 81 . doi: 10.1007/s10462-012-9365-8 http://dx.doi.org/10.1007/s10462-012-9365-8 Visual simultaneous localization and mapping： a survey SUN Y X ， LIU M ， MENG M Q H . Improving RGB-D SLAM in dynamic environments： a motion removal approach ［J］. Robotics and Autonomous Systems ， 2017 ， 89 ： 110 - 122 . doi: 10.1016/j.robot.2016.11.012 http://dx.doi.org/10.1016/j.robot.2016.11.012 Improving RGB-D SLAM in dynamic environments： a motion removal approach CADENA C ， CARLONE L ， CARRILLO H ， et al . Past， present， and future of simultaneous localization and mapping： toward the robust-perception age ［J］. IEEE Transactions on Robotics ， 2016 ， 32 （ 6 ）： 1309 - 1332 . doi: 10.1109/TRO.2016.2624754 http://dx.doi.org/10.1109/TRO.2016.2624754 Past， present， and future of simultaneous localization and mapping： toward the robust-perception age WANG S B ， YUE J G ， DONG Y C ， et al . Real-time omnidirectional visual SLAM with semi-dense mapping ［C］. 2018 IEEE Intelligent Vehicles Symposium . 2630，2018 ， Changshu， China . IEEE ， 2018 ： 695 - 700 . doi: 10.1109/ivs.2018.8500460 http://dx.doi.org/10.1109/ivs.2018.8500460 Real-time omnidirectional visual SLAM with semi-dense mapping ENGEL J ， KOLTUN V ， CREMERS D . Direct sparse odometry ［J］. IEEE Transactions on Pattern Analysis and Machine Intelligence ， 2018 ， 40 （ 3 ）： 611 - 625 . doi: 10.1109/tpami.2017.2658577 http://dx.doi.org/10.1109/tpami.2017.2658577 Direct sparse odometry MATSUKI H ， STUMBERG LVON ， USENKO V ， et al . Omnidirectional DSO： direct sparse odometry with fisheye cameras ［J］. IEEE Robotics and Automation Letters ， 2018 ， 3 （ 4 ）： 3693 - 3700 . doi: 10.1109/lra.2018.2855443 http://dx.doi.org/10.1109/lra.2018.2855443 Omnidirectional DSO： direct sparse odometry with fisheye cameras VALIENTE D ， GIL A ， FERNÁNDEZ L ， et al . A comparison of EKF and SGD applied to a view-based SLAM approach with omnidirectional images ［J］. Robotics and Autonomous Systems ， 2014 ， 62 （ 2 ）： 108 - 119 . doi: 10.1016/j.robot.2013.11.009 http://dx.doi.org/10.1016/j.robot.2013.11.009 A comparison of EKF and SGD applied to a view-based SLAM approach with omnidirectional images VALIENTE D ， GHAFFARI JADIDI M ， VALLS MIRÓ J ， et al . Information-based view initialization in visual SLAM with a single omnidirectional camera ［J］. Robotics and Autonomous Systems ， 2015 ， 72 ： 93 - 104 . doi: 10.1016/j.robot.2015.05.005 http://dx.doi.org/10.1016/j.robot.2015.05.005 Information-based view initialization in visual SLAM with a single omnidirectional camera CHENG J Y ， WANG C Q ， MENG M Q H . Robust visual localization in dynamic environments based on sparse motion removal ［J］. IEEE Transactions on Automation Science and Engineering ， 2020 ， 17 （ 2 ）： 658 - 669 . doi: 10.1109/tase.2019.2940543 http://dx.doi.org/10.1109/tase.2019.2940543 Robust visual localization in dynamic environments based on sparse motion removal CARUSO D ， ENGEL J ， CREMERS D . Large-scale direct SLAM for omnidirectional cameras ［C］. 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. 282，2015 ， Hamburg， Germany. IEEE ， 2015 ： 141 - 148 . doi: 10.1109/iros.2015.7353366 http://dx.doi.org/10.1109/iros.2015.7353366 Large-scale direct SLAM for omnidirectional cameras MUR-ARTAL R ， TARDOS J . Probabilistic semi-dense mapping from highly accurate feature-based monocular SLAM ［C］. Robotics： Science and Systems XI. Robotics ： Science and Systems Foundation ， 2015 . doi: 10.1109/tro.2015.2463671 http://dx.doi.org/10.1109/tro.2015.2463671 Probabilistic semi-dense mapping from highly accurate feature-based monocular SLAM LI S L ， LEE D . RGB-D SLAM in dynamic environments using static point weighting ［J］. IEEE Robotics and Automation Letters ， 2017 ， 2 （ 4 ）： 2263 - 2270 . doi: 10.1109/lra.2017.2724759 http://dx.doi.org/10.1109/lra.2017.2724759 RGB-D SLAM in dynamic environments using static point weighting XIAO L H ， WANG J G ， QIU X S ， et al . Dynamic-SLAM： semantic monocular visual localization and mapping based on deep learning in dynamic environment ［J］. Robotics and Autonomous Systems ， 2019 ， 117 ： 1 - 16 . doi: 10.1016/j.robot.2019.03.012 http://dx.doi.org/10.1016/j.robot.2019.03.012 Dynamic-SLAM： semantic monocular visual localization and mapping based on deep learning in dynamic environment BRASCH N ， BOZIC A ， LALLEMAND J ， et al . Semantic monocular SLAM for highly dynamic environments ［C］. 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）. 15，2018 ， Madrid， Spain. IEEE ， 2018 ： 393 - 400 . doi: 10.1109/iros.2018.8593828 http://dx.doi.org/10.1109/iros.2018.8593828 Semantic monocular SLAM for highly dynamic environments MEI C ， RIVES P . Single view point omnidirectional camera calibration from planar grids ［C］. Proceedings 2007 IEEE International Conference on Robotics and Automation . 1014，2007 ， Rome， Italy . IEEE ， 2007 ： 3945 - 3950 . doi: 10.1109/robot.2007.364084 http://dx.doi.org/10.1109/robot.2007.364084 Single view point omnidirectional camera calibration from planar grids 李勤文 ， 倪首军 ， 王志乾 ， 等 . 非公共视场双目相机位姿标定 ［J］. 光学 精密工程 ， 2020 ， 28 （ 12 ）： 2737 - 2744 . doi: 10.37188/OPE.20202812.2737 http://dx.doi.org/10.37188/OPE.20202812.2737 非公共视场双目相机位姿标定 LI Q W ， NI SH J ， WANG ZH Q ， et al . Calibration of binocular camera with non-common field of view ［J］. Opt. Precision Eng. ， 2020 ， 28 （ 12 ）： 2737 - 2744 . （in Chinese） . doi: 10.37188/OPE.20202812.2737 http://dx.doi.org/10.37188/OPE.20202812.2737 Calibration of binocular camera with non-common field of view 束安 ， 裴浩东 ， 周姗姗 ， 等 . 非合作航天器的立体视觉位姿测量 ［J］. 光学 精密工程 ， 2021 ， 29 （ 3 ）： 493 - 502 . doi: 10.37188/OPE.20212903.0493 http://dx.doi.org/10.37188/OPE.20212903.0493 非合作航天器的立体视觉位姿测量 SHU A ， PEI H D ， ZHOU SH SH ， et al . Stereo measurement of position and attitude of non-cooperative spacecraft ［J］. Opt. Precision Eng. ， 2021 ， 29 （ 3 ）： 493 - 502 . （in Chinese） . doi: 10.37188/OPE.20212903.0493 http://dx.doi.org/10.37188/OPE.20212903.0493 Stereo measurement of position and attitude of non-cooperative spacecraft FORSTER C ， PIZZOLI M ， SCARAMUZZA D . SVO： fast semi-direct monocular visual odometry ［C］. 2014 IEEE International Conference on Robotics and Automation. May 31 - June 7 ， 2014 ， Hong Kong， China. IEEE ， 2014： 15 - 22 . doi: 10.1109/icra.2014.6906584 http://dx.doi.org/10.1109/icra.2014.6906584 SVO： fast semi-direct monocular visual odometry VOGIATZIS G ， HERNÁNDEZ C . Video-based， real-time multi-view stereo ［J］. Image and Vision Computing ， 2011 ， 29 （ 7 ）： 434 - 441 . doi: 10.1016/j.imavis.2011.01.006 http://dx.doi.org/10.1016/j.imavis.2011.01.006 Video-based， real-time multi-view stereo MARIOTTINI G L ， PRATTICHIZZO D . Image-based visual servoing with central catadioptric cameras ［J］. The International Journal of Robotics Research ， 2008 ， 27 （ 1 ）： 41 - 56 . doi: 10.1177/0278364907084320 http://dx.doi.org/10.1177/0278364907084320 Image-based visual servoing with central catadioptric cameras SCHÖNBEIN M ， GEIGER A . Omnidirectional 3D reconstruction in augmented Manhattan worlds ［C］. 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems . 1418，2014 ， Chicago ， IL ， USA . IEEE ， 2014 ： 716 - 723 . doi: 10.1109/iros.2014.6942637 http://dx.doi.org/10.1109/iros.2014.6942637 Omnidirectional 3D reconstruction in augmented Manhattan worlds 张裕 ， 徐熙平 ， 张宁 ， 等 . 基于折反射全景相机的视觉里程计研究 ［J］. 光子学报 ， 2021 ， 50 （ 4 ）： 190 - 198 . doi: 10.3788/gzxb20215004.0415002 http://dx.doi.org/10.3788/gzxb20215004.0415002 基于折反射全景相机的视觉里程计研究 ZHANG Y ， XU X P ， ZHANG N ， et al . Research on visual odometry based on catadioptric panoramic camera ［J］. Acta Photonica Sinica ， 2021 ， 50 （ 4 ）： 190 - 198 . （in Chinese） . doi: 10.3788/gzxb20215004.0415002 http://dx.doi.org/10.3788/gzxb20215004.0415002 Research on visual odometry based on catadioptric panoramic camera 10.37188/OPE.20223011.1282 TP242.2 A 1004-924X（2022）11-1282-08 张裕,张越,张宁等.基于逆深度滤波的双目折反射全景相机动态SLAM系统[J].光学精密工程,2022,30(11):1282-1289. 张裕,张越,张宁等.基于逆深度滤波的双目折反射全景相机动态SLAM系统[J].光学精密工程,2022,30(11):1282-1289. ZHANG Yu,ZHANG Yue,ZHANG Ning,et al.Dynamic SLAM of binocular catadioptric panoramic camera based on inverse depth filter[J].Optics and Precision Engineering,2022,30(11):1282-1289. ZHANG Yu,ZHANG Yue,ZHANG Ning,et al.Dynamic SLAM of binocular catadioptric panoramic camera based on inverse depth filter[J].Optics and Precision Engineering,2022,30(11):1282-1289. 国家自然科学基金青年科学基金资助项目（No.61803045）；吉林省科技发展计划项目（No.20200201010JC） 2021-09-29 2021-12-06 2022-06-10 2022-06-21 光学精密工程 11 30