-
摘要: 针对利用平面特征计算RGB-D相机位姿时的求解退化问题, 提出平面和直线融合的RGB-D视觉里程计(Plane-line-based RGB-D visual odometry, PLVO). 首先, 提出基于平面−直线混合关联图(Plane-line hybrid association graph, PLHAG)的多特征关联方法, 充分考虑平面和平面、平面和直线之间的几何关系, 对平面和直线两类几何特征进行一体化关联. 然后, 提出基于平面和直线主辅相济、自适应融合的RGB-D相机位姿估计方法. 具体来说, 鉴于平面特征通常比直线特征具有更好的准确性和稳定性, 通过自适应加权的方法, 确保平面特征在位姿计算中的主导作用, 而对平面特征无法约束的位姿自由度(Degree of freedom, DoF), 使用直线特征进行补充, 得到相机的6自由度位姿估计结果, 从而实现两类特征的融合, 解决了单纯使用平面特征求解位姿时的退化问题. 最后, 通过公开数据集上的定量实验以及真实室内环境下的机器人实验, 验证了所提出方法的有效性.
-
关键词:
- RGB-D视觉里程计 /
- 平面−直线融合 /
- 机器人定位 /
- 自适应融合 /
- 多特征联合关联
Abstract: A plane-line-based RGB-D visual odometry (PLVO) is proposed to solve the degenerate problem in the pose estimation of an RGB-D camera using plane features. First, the plane-line hybrid association graph (PLHAG) is proposed to associate two types of geometric features. Planes and lines are associated in an integrated framework, which fully exploits the geometric relationships between two planes and between a plane and a line, respectively. Then, the pose of an RGB-D camera is estimated based on the adaptive fusion of planes and lines. Generally speaking, the plane features are more accurately and stably extracted than the line features. As a result, in our method, the planes dominate the calculation of the camera pose through an adaptive weighting algorithm. As for the degrees of freedom (DoFs) of the pose that cannot be constrained by planes, the line features are supplementarily used to obtain the full 6 DoF pose estimation of the camera. Thus, the fusion of two types of features is achieved and the degenerate problem using only plane features is solved. Various experiments on public benchmarks as well as in real-world environments demonstrate the efficiency of the proposed method. -
视觉里程计(Visual odometry, VO)在移动机器人定位和导航等任务中发挥着越来越重要的作用[1-4]. 近年来, 高层几何特征, 如平面、直线、边缘等, 在视觉里程计以及同步定位与建图(Simultaneous localization and mapping, SLAM)中得到了越来越多的关注[5-7]. 室内环境是移动机器人的重要作业环境之一, 由于室内场景属于典型的人造环境, 其中存在大量平面、直线等几何结构, 能够为机器人定位与建图提供重要的高层环境特征[8-10]. 与目前最常用的点特征相比[11], 高层几何特征对环境纹理缺失以及光照变化有更强的鲁棒性[12-13]. 另外, 几何特征是对原始传感数据的高层参数化表示, 能够在尽可能减小数据存储量的前提下保留环境的结构信息, 为机器人的自主导航[14]及场景理解[15]提供导航地图和语义描述.
在众多高层几何特征(平面、直线、线段、边缘、轮廓等)中, 平面特征具有鲁棒性强、易稳定提取和表示简便等优势, 因而在VO和SLAM领域得到了较好的应用[16-18]. 然而, 在使用平面参数计算视觉里程计时, 常常面临因约束不足而导致的相机位姿求解退化问题[5, 19-20]. Taguchi等[19]同时使用点特征和平面特征, 给出了可以完全约束相机位姿的特征组合方式, 但并没有对只使用平面特征时的退化情况进行具体的分析. Cupec等[20]在基于平面和线段特征的场景识别中, 假设环境包含至少两个非平行平面, 此时平面特征对相机位姿估计提供至少5自由度(Five degree of freedom, 5 DoF)约束, 使用扩展卡尔曼滤波(Extended Kalman filter, EKF)对该5 DoF位姿进行估计, 之后再利用平面与线段的投票机制对剩余的一个自由度进行计算. 然而, 由于RGB-D相机视野范围以及观测距离的限制, 会出现难以满足环境包含至少两个非平行平面的假设, 如只能观测到平行的桌面和地面的情况. Sun等[5]根据平面特征在三维空间的分布情况, 检测出用平面计算位姿时的退化情况, 并提出基于STING (Statistical information grid)的扫描匹配方法, 对无法被平面约束的位姿自由度进行计算.
除了平面特征外, 直线特征也广泛应用于移动机器人室内环境VO和SLAM[21-24]. 与平面特征相比, 直线特征在室内场景中的数量更多, 也更不容易出现退化情况(只要存在两条非平行直线即可完全约束位姿的计算). 同时, 相比于点特征, 直线特征对低纹理环境具有更强的鲁棒性[22-23]. 然而, 通常情况下直线都分布于物体的边缘区域, 导致其深度值与RGB值存在很大的观测噪声[25], 因此, 直线参数估计的准确性和稳定性不及平面特征.
鉴于上述问题以及平面和直线特征在机器人位姿估计方面的互补性, 本文提出基于平面和直线融合的RGB-D视觉里程计(Plane-line-based RGB-D visual odometry, PLVO). 首先, 针对多几何特征的关联问题, 本文对Sun等[5]提出的平面关联图(Plane association graph, PAG)进行扩展, 提出平面−直线混合关联图(Plane-line hybrid association graph, PLHAG), 在PAG的基础上, 加入对平面与直线之间几何关系的考虑, 实现对两类不同几何特征的统一关联. 其次, 提出基于平面和直线融合的RGB-D相机位姿估计方法, 在用平面计算位姿出现退化情况时, 引入直线特征作为平面特征的补充, 通过二者的自适应融合, 完成6 DoF位姿的计算. 值得说明的是, 由于平面特征参数的准确性和稳定性均优于直线特征, 故本文根据平面特征对位姿的约束情况来控制直线特征对位姿求解的贡献, 使得直线特征只对平面特征无法约束的自由度进行约束, 因此, 即使在用平面特征求解时出现退化的情况, 也尽可能保证平面特征占主导作用, 充分利用平面特征良好的准确性和稳定性, 提升位姿估计的效果.
1. PLVO系统框架
PLVO系统框架如图1所示. 首先, 平面特征提取模块[5]从RGB-D相机采集的深度图像中提取平面特征, 而直线特征提取模块[26]从RGB图像中检测二维直线, 并利用深度信息将其投影到相机坐标系得到三维直线特征. 其次, 将平面和直线特征输入到平面与直线特征匹配模块中, 其中包含两个子模块: PLHAG构建和PLHAG匹配. 在PLHAG构建子模块中, 对相邻两帧提取的特征分别构建PLHAG, 并将其传入PLHAG匹配子模块完成特征的关联. 然后, 将具有关联关系的平面和直线特征输入RGB-D相机位姿估计模块, 优先基于平面特征对相机位姿进行计算, 当出现退化情况时, 用直线特征对平面无法约束的位姿自由度进行计算, 最终得到6 DoF相机位姿的估计结果.
2. 特征提取与关联
2.1 平面与直线特征提取
使用基于STING的平面提取算法[5]对深度图像中的平面特征进行提取, 并将提取出的平面的参数表示为$\pi = {[{{\boldsymbol{n}}^{\rm{T}}},d]^{\rm{T}}}$, 其中, ${\boldsymbol{n}} \in {S^2}$表示平面的单位法向量(${S^2}$表示三维欧氏空间 ${{\bf{R}}^3}$中的单位球面), $d \in {\bf{R}}$表示相机坐标系原点到平面的垂直距离. 平面$\pi $上的点对应的图像RGB值的均值和协方差分别表示为${{\boldsymbol{r}}_\pi }$和${{\boldsymbol{S}}_\pi }$.
使用LSD (Line segment detector)算法[26]检测RGB图像中的直线, 并将其投影到三维相机坐标系下, 得到的三维空间直线参数表示为${\mathcal{L}} = [{{\boldsymbol{u}}^{\rm{T}}}, {{\boldsymbol{v}}^{\rm{T}}}]^{\rm{T}}$, 其中, ${\boldsymbol{v}} \in {S^2}$表示直线的单位方向向量, 而${\boldsymbol{u}} \in {{\bf{R}}^3}$的方向为直线与相机坐标系原点构成平面的法向量方向, ${\boldsymbol{u}}$的长度为原点到直线的垂直距离.
2.2 平面−直线混合关联图(PLHAG)构建
文献[5]提出了基于PAG的平面特征关联方法. PAG是一种全连接图结构, 其节点表示平面特征, 节点之间的边则代表平面之间的几何关系. 相邻帧之间的平面特征匹配是通过比较每帧构建的PAG之间的节点相似度完成的. 本文对PAG进行扩展, 针对多几何特征的关联问题, 提出基于PLHAG的平面和直线混合关联方法. 在PLHAG中, 不仅考虑平面之间的几何关系, 另外加入了直线与平面之间的几何关系, 同时建立平面与平面、直线与直线之间的关联关系.
对于某一帧中提取出的平面集合$\{ {\pi _i}\} ,i \in \{ 1, \cdots ,{N_\pi }\}$与直线集合$\{ {{\mathcal{L}}_j}\} ,$$j \in \{ 1, \cdots ,{N_{\mathcal{L}}}\} $, 构建的PLHAG表示为 $G = (V,E)$. $V$表示节点集合, 其中包含两类节点类型, 即平面节点${v_{\pi ,i}} = {\pi _i}$与直线节点${v_{{\mathcal{L}},j}} = {{\mathcal{L}}_j}$. 相应地, 边的集合$E$也包含两种类型, 即平面节点之间的边(Plane-plane edge, PPE)${e_{\pi ,ik}}$, 以及连接直线节点与平面节点的边(Line-plane edge, LPE)${e_{{\mathcal{L}}\pi ,jk}}$. 与PAG不同的是, PLHAG不是全连接图结构. 考虑到两类几何特征之间的差异, 直线节点与直线节点之间不进行连接, 具体原因将在第2.3节中详细说明.
平面节点之间的边${e_{\pi ,ik}}$定义为
$$\begin{split} {e_{\pi ,ik}} = ({\omega _{\pi ,ik}},{\alpha _{\pi ,ik}},{d_{\pi ,ik}}),\qquad\qquad\qquad \\ \qquad\qquad\qquad i,k \in \{ 1, \cdots, {N_\pi }\} ,\;i \ne k \end{split} $$ (1) $${\omega _{\pi ,ik}} = \left\{ {\begin{array}{*{20}{l}} {{\rm{parallel}},}&{{\text{若}} \; {\alpha _{\pi ,ik}} < {\delta _{{\rm{rad}}}}} \\ {{\rm{non}}\text{-}{\rm{parallel}},}&{{{\text{否则}}}{\rm{}}} \end{array}} \right.$$ (2) $${\alpha _{\pi ,ik}} = \arccos \left( {{\boldsymbol{n}}_i^{\rm{T}}{{\boldsymbol{n}}_k}} \right)\quad\qquad\qquad\qquad\qquad$$ (3) $${d_{\pi ,ik}} = \left\{ {\begin{array}{*{20}{l}} {\left| {{d_i} - {d_k}} \right|,}&{{\text{若}}\; {\omega _{\pi ,ik}} = {\rm{parallel}}} \\ {0,}&{{{\text{否则}}}{\rm{}}} \end{array}} \right.$$ (4) 类似地, 直线节点与平面节点之间的边${e_{{\mathcal{L}}\pi ,jk}}$定义为
$$\begin{split} &{e_{{\mathcal{L}}\pi ,jk}} = ({\omega _{{\mathcal{L}}\pi ,jk}},{\alpha _{{\mathcal{L}}\pi ,jk}},{d_{{\mathcal{L}}\pi ,jk}}), \quad\qquad\quad\\ &\quad\qquad\quad\;\;\; j \in \{ 1, \cdots ,{N_{\mathcal{L}}}\} ,\;k \in \{ 1, \cdots, {N_\pi }\} \end{split} $$ (5) $${\omega _{{\mathcal{L}}\pi ,jk}} = \left\{ {\begin{array}{*{20}{l}} {{\rm{parallel}},} &{若 \; \left| {\dfrac{\pi }{2} - {\alpha _{{\mathcal{L}}\pi ,jk}}} \right| < {\delta _{{\rm{rad}}}}} \\ {{\rm{non \text{-} parallel}},} &{{\text{否则}}} \end{array}} \right.$$ (6) $${\alpha _{{\mathcal{L}}\pi ,jk}} = \arccos \left( {{\boldsymbol{v}}_j^{\rm{T}}{{\boldsymbol{n}}_k}} \right)\qquad\qquad\qquad\qquad $$ (7) $${d_{{\mathcal{L}}\pi ,jk}} = \left\{ {\begin{array}{*{20}{l}} {\left| {{\boldsymbol{n}}_k^{\rm{T}}{{[{{\boldsymbol{v}}_j}]}_ \times }{{\boldsymbol{u}}_j} + {d_k}} \right|,} &{{\text{若}} \; {\omega _{{\mathcal{L}}\pi ,jk}} = {\rm{parallel}}} \\ {0,} &{{\text{否则}}} \end{array}} \right.$$ (8) 其中, 枚举量${\omega _{\pi ,ik}}$和${\omega _{{\mathcal{L}}\pi ,jk}}$分别表示平面与平面以及直线与平面之间的几何关系类型, 即平行关系(parallel)与非平行关系(non-parallel), 如式(2)和式(6)所示, 在本文实验中阈值${\delta _{{\rm{rad}}}}$取值为10°. 由式(3)和式(7)可知, ${\alpha _{\pi ,ik}}$为${{\boldsymbol{n}}_i}$与${{\boldsymbol{n}}_k}$之间的夹角, ${\alpha _{{\mathcal{L}}\pi ,jk}}$为${{\boldsymbol{v}}_j}$与${{\boldsymbol{n}}_k}$之间的夹角. 根据式(4)和式(8)可知, 当两个平面(或直线和平面)之间为平行关系时, ${d_{\pi ,ik}}$(${d_{{\mathcal{L}}\pi ,jk}}$)的值为两个平行平面(或平行直线与平面)之间的垂直距离. 至此, 完成PLHAG的构建, 下面将分别使用相邻两帧特征构建的PLHAG完成平面特征与直线特征的匹配.
2.3 基于PLHAG的特征匹配
对于从相邻两帧提取的特征集合$\{ {\pi _{ci}},{{\mathcal{L}}_{cj}}\} ,i \in \{ 1, \cdots ,{N_{\pi c}}\} ,\;j \in \{ 1, \cdots ,{N_{{\mathcal{L}}c}}\}\;$ 和 $\{ {\pi _{rm}},{{\mathcal{L}}_{rp}}\} ,\;m\, \in \{ 1, \cdots ,{N_{\pi r}}\} ,p \in \{ 1, \cdots ,{N_{{\mathcal{L}}r{}}}\}$, 其中, 下标$c$与$r$分别代表当前帧与参考帧, 使用第2.2节提出的方法分别构建PLHAG${G_c} = ({V_c},{E_c})$ 和 ${G_r} = ({V_r},{E_r})$. 在本节中, 通过计算${G_c}$中节点与${G_r}$中节点之间的相似度, 建立两帧特征之间的匹配关系.
首先, 在计算节点相似度之前, 分别定义两个PPE之间和两个LPE之间的关系, 如式(9)和式(10)所示, 其中阈值${\delta _{{\rm{dist}}}}$在实验中取值为0.06 m.
$$\left\{ {\begin{array}{*{20}{l}} {{e_{\pi c,ik}} = {e_{\pi r,ml}},}&{{{\text{若}}}\;{\omega _{\pi c,ik}} = {\omega _{\pi r,ml}}}\\ {}&{{{\text{且}}}\;{\kern 1pt} \left| {{\alpha _{\pi c,ik}} - {\alpha _{\pi r,ml}}} \right| < {\delta _{{\rm{rad}}}}}\\ {}&{{{\text{且}}} \;\left| {{d_{\pi c,ik}} - {d_{\pi r,ml}}} \right| < {\delta _{{\rm{dist}}}}}\\ {{e_{\pi c,ik}} \ne {e_{\pi r,ml}},}&{{{\text{否则}}}{\rm{}}} \end{array}} \right.$$ (9) $$\left\{ {\begin{array}{*{20}{l}} {{e_{{{\cal L}}\pi c,jk}} = {e_{{{\cal L}}\pi r,pl}},}&{{{\text{若}}}\;{\omega _{{{\cal L}}\pi c,jk}} = {\omega _{{{\cal L}}\pi r,pl}}}\\ {}&{{{\text{且}}}\;\left| {{\alpha _{{{\cal L}}\pi c,jk}} - {\alpha _{{{\cal L}}\pi r,pl}}} \right| < {\delta _{{\rm{rad}}}}}\\ {}&{{{\text{且}}}\;\left| {{d_{{{\cal L}}\pi c,jk}} - {d_{{{\cal L}}\pi r,pl}}} \right| < {\delta _{{\rm{dist}}}}}\\ {{e_{{{\cal L}}\pi c,jk}} \ne {e_{{{\cal L}}\pi r,pl}},}&{{\text{否则}}{\rm{}}} \end{array}} \right.$$ (10) 满足${e_{\pi c,ik}} = {e_{\pi r,ml}}$关系的两个PPE (满足${e_{{\mathcal{L}}\pi c,jk}} = {e_{{\mathcal{L}}\pi r,pl}}$关系的两个LPE)称为相似边, 相似边代表其所连接的两个平面节点(直线节点与平面节点)之间相似的几何关系, 用于后文中对节点几何相似度的定义.
接着, 对平面和直线节点分别定义节点之间的相似度. 两个平面节点${v_{\pi c,i}} \in {V_c}$和${v_{\pi r,m}} \in {V_r}$之间的相似度${s_\pi }\left( {{v_{\pi c,i}},{v_{\pi r,m}}} \right)$由两部分组成, 即颜色相似度${s_{\pi ,{\rm{col}}}}\left( {{v_{\pi c,i}},{v_{\pi r,m}}} \right)$ 与几何相似度${s_{\pi ,{\rm{geo}}}} ( {v_{\pi c,i}}, {v_{\pi r,m}} )$.
$$\begin{split} {s_\pi }\left( {{v_{\pi c,i}},{v_{\pi r,m}}} \right) =\;& {s_{\pi ,{\rm{col}}}}\left( {{v_{\pi c,i}},{v_{\pi r,m}}} \right)+ \\ &{s_{\pi ,{\rm{geo}}}}\left( {{v_{\pi c,i}},{v_{\pi r,m}}} \right) \end{split} $$ (11) 颜色相似度${s_{\pi ,{\rm{col}}}}\left( {{v_{\pi c,i}},{v_{\pi r,m}}} \right) \in \left[ {0,1} \right]$ 为
$${s_{\pi ,{\rm{col}}}}\left( {{v_{\pi c,i}},{v_{\pi r,m}}} \right) = \frac{1}{{1 + {{\tilde s}_{\pi ,{\rm{col}}}}\left( {{v_{\pi c,i}},{v_{\pi r,m}}} \right)}}$$ (12) 其中, ${\tilde s_{\pi ,{\rm{col}}}}\left( {{v_{\pi c,i}},{v_{\pi r,m}}} \right) \in \left[ {0,\infty } \right)$定义为两个平面上的点对应图像像素点的RGB值分布的巴氏距离(即颜色相异度), 即
$$\begin{split} {{\tilde s}_{\pi ,{\rm{col}}}} &\left( {{v_{\pi c,i}},{v_{\pi r,m}}} \right) = \frac{1}{8}{\left( {{{\boldsymbol{r}}_{\pi c,i}} - {{\boldsymbol{r}}_{\pi r,m}}} \right)^{\rm{T}}}{\boldsymbol{S}}_\pi ^{ - 1}\;\times \\ &\left( {{{\boldsymbol{r}}_{\pi c,i}} - {{\boldsymbol{r}}_{\pi r,m}}} \right) + \frac{1}{2}\ln \left( {\frac{{\left| {{{\boldsymbol{S}}_\pi }} \right|}}{{\sqrt {\left| {{{\boldsymbol{S}}_{\pi c,i}}} \right| \cdot \left| {{{\boldsymbol{S}}_{\pi r,m}}} \right|} }}} \right) \end{split} $$ (13) 其中, ${{\boldsymbol{S}}_\pi } = \left( {{{\boldsymbol{S}}_{\pi c,i}} + {{\boldsymbol{S}}_{\pi r,m}}} \right) /2$. 因为具有关联关系的平面特征与其所在扫描帧中其他关联特征之间应具有相同的几何关系. 所以, 通过以相似边连接的平面节点的颜色相似度计算节点之间的几何相似度${s_{\pi ,{\rm{geo}}}}\left( {{v_{\pi c,i}},{v_{\pi r,m}}} \right)$, 即
$$\begin{split} & {s_{\pi ,{\rm{geo}}}}\left( {{v_{\pi c,i}},{v_{\pi r,m}}} \right) = \\& \qquad \frac{1}{{\left| {{I_{im}}} \right|}}\sum\limits_{(k,l) \in {I_{im}}} {{s_{\pi ,{\rm{col}}}}\left( {{v_{\pi c,k}},{v_{\pi r,l}}} \right)} \end{split}$$ (14) 其中, ${I_{im}}$为索引对的集合, 索引到${V_c}$和${V_r}$中分别与${v_{\pi c,i}}$和${v_{\pi r,m}}$以相似边(式(9))连接的平面节点.
$$\begin{split} {I_{im}} =\;& \Big\{ {(k,l)|{e_{\pi c,ik}} = {e_{\pi r,ml}},k \in \left\{ {1, \cdots ,{N_{\pi c}}} \right\},} \\ & {k \ne i,\;l \in \left\{ {1, \cdots ,{N_{\pi r}}} \right\},l \ne m} \Big\} \\[-10pt] \end{split} $$ (15) 而$\left| {{I_{im}}} \right|$则代表集合${I_{im}}$中元素的个数.
与平面特征不同, 直线特征多数分布在几何物体的边缘以及图像中RGB值变化剧烈的区域, 故直线上的点对应像素的颜色信息受观测噪声影响较大, 不适合用于对直线特征的描述. 因此, 本文不计算两个直线节点之间的颜色相似度, 只计算其几何相似度, 因而在第2.2节构建的PLHAG中, 直线节点和直线节点之间也不进行连接.
对于两个直线节点${v_{{\mathcal{L}}c,j}} \in {V_c}$和${v_{{\mathcal{L}}r,p}}$$ \in {V_r}$, 二者之间的相似度${s_{\mathcal{L}}}\left( {{v_{{\mathcal{L}}c,j}},{v_{{\mathcal{L}}r,p}}} \right)$ 定义为
$${s_{\mathcal{L}}}\left( {{v_{{\mathcal{L}}c,j}},{v_{{\mathcal{L}}r,p}}} \right) = {s_{{\mathcal{L}},{\rm{geo}}}}\left( {{v_{{\mathcal{L}}c,j}},{v_{{\mathcal{L}}r,p}}} \right)$$ (16) 其中, ${s_{{\mathcal{L}},{\rm{geo}}}}\left( {{v_{{\mathcal{L}}c,j}},{v_{{\mathcal{L}}r,p}}} \right)$表示直线节点的几何相似度, 由以相似边连接的平面节点的颜色相似度计算:
$${s_{{\mathcal{L}},{\rm{geo}}}}( {{v_{{\mathcal{L}}c,j}},{v_{{\mathcal{L}}r,p}}} ) = \frac{1}{{\left| {{I_{jp}}} \right|}} \sum\limits_{(k,l) \in {I_{jp}}} {{s_{\pi ,{\rm{col}}}}( {{v_{\pi c,k}},{v_{\pi r,l}}} )} $$ (17) $$\begin{split} {I_{jp}} =& \Big\{ {(k,l)|{e_{{\mathcal{L}}\pi c,jk}} = {e_{{\mathcal{L}}\pi r,pl}},} \qquad\qquad\qquad \\& {k \in \left\{ {1, \cdots ,{N_{\pi c}}} \right\},\;l \in \left\{ {1, \cdots ,{N_{\pi r}}} \right\}} \Big\} \end{split} $$ (18) 其中, $\left| {{I_{jp}}} \right|$为集合${I_{jp}}$中元素的个数.
在使用PLHAG进行特征关联时, 根据式(11)和式(16)分别对来自当前帧与参考帧的每一对平面节点之间以及直线节点之间的节点相似度进行计算, 通过比较节点相似度的大小来寻找最佳匹配关系, 具体算法流程如算法1所示.
算法 1. 基于PLHAG的特征关联算法
输入. 两帧PLHAG ${G_c}$和${G_r}$.
输出. 平面特征匹配对集合$\Pi $与直线特征匹配对集合${L}$.
1: 初始化: $\Pi = \emptyset $, ${{L }}=\emptyset $
2: for $i = 1 \to {N_{\pi c}}$ do
3: ${m_i} \;=\; \arg \mathop {\max }\nolimits_m {s_\pi }( {{v_{\pi c,i}},\;{v_{\pi r,m}}} )$, 其中, $m$ 满足 $m \in\{ 1, \cdots ,{N_{\pi r}} \}$以及${I_{im}} \ne \emptyset $
4: if ${s_\pi }\left( {{v_{\pi c,i}},{v_{\pi r,{m_i}}}} \right) > {\delta _s}$ then
5: $\Pi = \Pi \cup \{ {v_{\pi c,i}},{v_{\pi r,{m_i}}}\} $
6: end if
7: end for
8: for $j = 1 \to {N_{{\mathcal{L}}c}}$ do
9: ${p_j} =\; \arg \mathop {\max }\nolimits_p {s_{\mathcal{L}}}\left( {{v_{{\mathcal{L}}c,j}},\;{v_{{\mathcal{L}}r,p}}} \right)$, 其中, $p$满足 $p \in \{ 1, \cdots , {N_{\mathcal{L}r}} \}$以及${I_{jp}} \ne \emptyset $
10: if ${s_\mathcal{L}}( {{v_{\mathcal{L}c,j}},{v_{\mathcal{L}r,{p_j}}}} ) > {\delta _s}$ then
11: ${{L}} = {{L}} \cup \{ {v_{\mathcal{L}c,j}},{v_{\mathcal{L}r,{p_j}}}\}$
12: end if
13: end for
需要说明的是, 算法第4行和第10行中阈值${\delta _s}$的作用是对不正确的匹配关系进行排除, 如对于当前帧中新观测到的特征, 参考帧中不存在任何特征与其匹配, 这种情况就需要通过对节点相似度数值的限制进行排除. 由于关联特征与非关联特征所对应节点的相似度数值的差异比较大, 因此, 阈值${\delta _s}$的选择对算法结果的影响非常小. 本文实验中${\delta _s}$取值为0.99. 在图2中, 针对场景中某种具体的特征分布情况, 给出了PLHAG结构的示意图, 其中, 图2(b)中实线和虚线分别标识了平面和直线节点. 对于图2(a)中平面, 假设有
$$\begin{split} {s_{\pi ,{\rm{col}}}}&({v_{\pi c,1}},{v_{\pi r,1}}) = {s_{\pi ,{\rm{col}}}}({v_{\pi c,2}},{v_{\pi r,2}}) =\\ & {s_{\pi ,{\rm{col}}}}({v_{\pi c,3}},{v_{\pi r,3}}) = \eta \qquad\; \end{split} $$ (19) $${s_{\pi ,{\rm{col}}}}({v_{\pi c,1}},{v_{\pi r,2}}) = {s_{\pi ,{\rm{col}}}}({v_{\pi c,1}},{v_{\pi r,3}}) = \varepsilon $$ (20) 其中, $\eta $接近1, $\varepsilon $充分小. 以节点${v_{\mathcal{L}c,1}}$和${v_{\mathcal{L}r,1}}$为例, 从图2(a)中特征之间位置关系可以看出
$$\left\{\begin{split} &{e_{\mathcal{L}\pi c,11}} = {e_{\mathcal{L}\pi r,11}}\;\;\\ &{e_{\mathcal{L}\pi c,12}} = {e_{\mathcal{L}\pi r,12}}\;\;\\ &{e_{\mathcal{L}\pi c,13}} = {e_{\mathcal{L}\pi r,13}}\end{split}\right.$$ (21) 则 ${v_{\mathcal{L}c,1}}$和 ${v_{\mathcal{L}r,1}}$ 所对应的索引集合为${I_{11}} = $$\{ (1,1), (2,2),(3,3) \}$, 根据式(15)可知
$${s_\mathcal{L}}({v_{\mathcal{L}c,1}},{v_{\mathcal{L}r,1}}) = \eta $$ (22) 同理可知
$${s_\mathcal{L}}({v_{\mathcal{L}c,1}},{v_{\mathcal{L}r,2}}) = {s_\mathcal{L}}({v_{\mathcal{L}c,1}},{v_{\mathcal{L}r,3}}) = \frac{1}{3}\left( {\eta {\rm{ + }}2\varepsilon } \right)$$ (23) 根据算法1可知, $\left\{ {{\mathcal{L}_{c1}},{\mathcal{L}_{r1}}} \right\}$为一对匹配的直线特征. 基于PLHAG的多特征关联方法利用图结构充分考虑特征之间的几何关系, 提高了特征关联的鲁棒性. 与PAG相比, PLHAG对原单一节点类型进行拓展, 将平面和直线特征纳入统一框架, 同时考虑了不同特征之间的约束关系, 提升了算法的鲁棒性和适用性.
3. RGB-D相机位姿估计
对于RGB-D相机, 平面特征比直线特征具有更强的稳定性和鲁棒性, 因为平面特征多提取于规则几何物体表面, 如地面、墙面、桌面等, 而直线特征多分布于物体边缘, 对应的点云观测噪声显著高于来自规则物体表面的点云观测噪声[25, 27]. 为此, 本文主要使用平面特征计算相机位姿. 然而, 由于RGB-D相机的观测视野较小, 且观测距离有限, 导致能够提取出的平面特征数量不够充足, 因此, 经常出现平面特征无法完全约束6 DoF相机位姿计算的情况, 产生位姿求解的退化问题. 针对此问题, 本文提出基于平面和直线自适应融合的RGB-D相机6 DoF位姿估计方法, 在平面特征无法完全提供全部约束时, 使用直线特征作为平面特征的补充, 完成6 DoF位姿的计算.
使用第2.2节中的方法进行特征关联, 得到具有关联关系的平面特征集合$\Pi = \{ {\pi _{ci}},{\pi _{ri}}\} , i \in \{ 1, \cdots ,{N_\pi }\}$以及直线特征集合 ${{L}} = \{ {{\mathcal{L}}_{cj}},{{\mathcal{L}}_{rj}}\} , j \in \{ 1, \cdots ,{N_{\mathcal{L}}}\}$. 首先, 计算RGB-D相机的三维空间旋转变换${\boldsymbol{R}} \in SO(3)$和平移变换${\boldsymbol{t}} \in {{\bf{R}}^3}$, 使得平面特征的匹配误差最小, 即目标函数(24)和(25)达到最小.
$${J_{\pi R}}({\boldsymbol{R}}) = \sum\limits_{i = 1}^{{N_\pi }} {{{\left\| {{{\boldsymbol{n}}_{ci}} - {\boldsymbol{R}}{{\boldsymbol{n}}_{ri}}} \right\|}^2}} \qquad$$ (24) $${J_{\pi t}}({\boldsymbol{t}}) = \sum\limits_{i = 1}^{{N_\pi }} {{{\left( {{d_{ci}} - \left( {{d_{ri}} - {\boldsymbol{n}}_{ci}^{\rm{T}}{\boldsymbol{t}}} \right)} \right)}^2}} $$ (25) 为了检测是否出现退化解, 构造矩阵${\boldsymbol{H}} = \sum\nolimits_{i = 1}^{{N_\pi }} {{{\boldsymbol{n}}_{ri}}{\boldsymbol{n}}_{ci}^{\rm{T}}}$, 并计算其奇异值分解 (Singular value decomposition, SVD).
$${\boldsymbol{H}} = {{\boldsymbol{Q}}_r}{\boldsymbol{\Lambda Q}}_c^{\rm{T}} = {\lambda _1}{{\boldsymbol{q}}_{r1}}{\boldsymbol{q}}_{c1}^{\rm{T}} + {\lambda _2}{{\boldsymbol{q}}_{r2}}{\boldsymbol{q}}_{c2}^{\rm{T}} + {\lambda _3}{{\boldsymbol{q}}_{r3}}{\boldsymbol{q}}_{c3}^{\rm{T}}$$ (26) 其中, ${{\boldsymbol{Q}}_r} = [{{\boldsymbol{q}}_{r1}},{{\boldsymbol{q}}_{r2}},{{\boldsymbol{q}}_{r3}}]$和 ${{\boldsymbol{Q}}_c} = [{{\boldsymbol{q}}_{c1}},{{\boldsymbol{q}}_{c2}},{{\boldsymbol{q}}_{c3}}]$为正交单位矩阵, 而${\boldsymbol{\Lambda }} = {\rm{diag}}\{ {\lambda _1},{\lambda _2},{\lambda _3}\} ,$${\lambda _1} \geq {\lambda _2} \geq {\lambda _3}$为对角矩阵. 根据矩阵${\boldsymbol{H}}$的奇异值, 可以判断平面特征是否能够完全约束6 DoF位姿的计算, 具体分为3种情况, 即6 DoF约束情况、5 DoF约束情况和3 DoF约束情况, 其中后两种为退化情况. 下面分别对这3种情况给出详细的分析和具体的位姿计算方法.
3.1 6 DoF约束情况
如果${\boldsymbol{H}}$为非奇异矩阵, 即${\lambda _1} \geq {\lambda _2} \geq {\lambda _3} > 0$, 则使式(24)和式(25)达到最小值的解${\boldsymbol{\hat R}}$和${\boldsymbol{\hat t}}$分别由式(27)和式(28)得到.
$${\boldsymbol{\hat R}} = {{\boldsymbol{Q}}_c}{\boldsymbol{Q}}_r^{\rm{T}}\qquad\qquad\;\;$$ (27) $${\boldsymbol{\hat t}} = {\left( {{\boldsymbol{A}}_\pi ^{\rm{T}}{{\boldsymbol{A}}_\pi }} \right)^{ - 1}}{\boldsymbol{A}}_\pi ^{\rm{T}}{{\boldsymbol{d}}_\pi }$$ (28) 其中
$${{\boldsymbol{A}}_\pi } = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol{n}}_{c1}^{\rm{T}}} \\ \vdots \\ {{\boldsymbol{n}}_{c{N_\pi }}^{\rm{T}}} \end{array}} \right],\;\; {{\boldsymbol{d}}_\pi } = \left[ {\begin{array}{*{20}{c}} {{d_{r1}} - {d_{c1}}} \\ \vdots \\ {{d_{r{N_\pi }}} - {d_{c{N_\pi }}}} \end{array}} \right]$$ (29) 3.2 5 DoF约束情况
如果${\boldsymbol{H}}$为奇异矩阵, 且${\lambda _1} \geq {\lambda _2} > {\lambda _3} = 0$, 此时平面特征的法向量$\{ {{\boldsymbol{n}}_{ci}}\} $($\{ {{\boldsymbol{n}}_{ri}}\} $), $i \in \{ 1, \cdots , {N_\pi }\}$在空间中是共面关系, 且垂直于向量${{\boldsymbol{q}}_{c3}}$(${{\boldsymbol{q}}_{r3}}$). 在这种情况下, 旋转变换${\boldsymbol{\hat R}}$为
$${\boldsymbol{\hat R}} = \left\{ {\begin{array}{*{20}{l}} {{{\boldsymbol{Q}}_c}{\boldsymbol{Q}}_r^{\rm{T}}},&{ \; \det ({{\boldsymbol{Q}}_c}{\boldsymbol{Q}}_r^{\rm{T}}) = 1} \\ {{\boldsymbol{Q}}{'_c}{\boldsymbol{Q}}_r^{\rm{T}}},&{ \; \det ({{\boldsymbol{Q}}_c}{\boldsymbol{Q}}_r^{\rm{T}}) = - 1} \end{array}} \right.$$ (30) 其中, ${\boldsymbol{Q}}{'_c} = [{{\boldsymbol{q}}_{c1}},{{\boldsymbol{q}}_{c2}}, - {{\boldsymbol{q}}_{c3}}]$. 而此时平移变换无法直接由式(28)计算得到, 因为$\det ({\boldsymbol{A}}_\pi ^{\rm{T}}{{\boldsymbol{A}}_\pi }) = 0$. 根据式(29)以及平面法向量$\{ {{\boldsymbol{n}}_{ci}}\} ,\;i \in \{ 1, \cdots ,{N_\pi }\} $的空间分布情况可知, 矩阵${{\boldsymbol{A}}_\pi }$的零空间(null space)可以表示为
$${\rm{null(}}{{\boldsymbol{A}}_\pi }{\rm{) = }}\eta {{\boldsymbol{q}}_{c3}},\;\;\eta \in {\bf{R}}{\rm{}}$$ (31) 由此可知, 沿着向量${{\boldsymbol{q}}_{c3}}$方向的平移变换无法被关联的平面特征约束. 因此, 在计算平移变换的误差函数中加入直线特征的匹配误差, 即
$${J_t}({\boldsymbol{t}}) = {J_{\pi t}}({\boldsymbol{t}}) + {J_{{\mathcal{L}}t}}({\boldsymbol{t}})$$ (32) 其中
$${J_{{\mathcal{L}}t}}({\boldsymbol{t}}) = \sum\limits_{j = 1}^{{N_{\mathcal{L}}}} {{w_j}{{\left\| {{{\boldsymbol{u}}_{cj}} - \left( {{\boldsymbol{\hat R}}{{\boldsymbol{u}}_{rj}} + {{[{{\boldsymbol{v}}_{cj}}]}_ \times }{\boldsymbol{t}}} \right)} \right\|}^2}} $$ (33) $${w_j} = \left\| {{{\boldsymbol{v}}_{cj}} \times {{\boldsymbol{q}}_{c3}}} \right\|\quad\qquad\qquad\qquad\qquad\qquad$$ (34) 此时, 使得式(32)最小化的平移最优解为
$${\boldsymbol{\hat t}} = {\left( {{{\boldsymbol{A}}^{\rm{T}}}{\boldsymbol{WA}}} \right)^{ - 1}}{{\boldsymbol{A}}^{\rm{T}}}{\boldsymbol{Wd}}$$ (35) 其中
$${\boldsymbol{A}} = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{A}}_\pi }} \\ {{{\boldsymbol{A}}_{\mathcal{L}}}} \end{array}} \right],\;\; {\boldsymbol{d}} = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{d}}_\pi }} \\ {{{\boldsymbol{d}}_{\mathcal{L}}}} \end{array}} \right]$$ (36) $${{\boldsymbol{A}}_{\mathcal{L}}} = \left[ {\begin{array}{*{20}{c}} {{{[{{\boldsymbol{v}}_{c1}}]}_ \times }} \\ \vdots \\ {{{[{{\boldsymbol{v}}_{c{N_{\mathcal{L}}}}}]}_ \times }} \end{array}} \right], \;\;{{\boldsymbol{d}}_{\mathcal{L}}} = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol{\hat R}}{{\boldsymbol{u}}_{r1}} - {{\boldsymbol{u}}_{c1}}} \\ \vdots \\ {{\boldsymbol{\hat R}}{{\boldsymbol{u}}_{r{N_{\mathcal{L}}}}} - {{\boldsymbol{u}}_{c{N_{\mathcal{L}}}}}} \end{array}} \right]$$ (37) $${\boldsymbol{W}} = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol{I}}_{{N_\pi }}}}&{}&{}&{} \\ {}&{{w_1}{{\boldsymbol{I}}_3}}&{}&{} \\ {}&{}& \ddots &{} \\ {}&{}&{}&{{w_{{N_{\mathcal{L}}}}}{{\boldsymbol{I}}_3}} \end{array}} \right]$$ (38) 其中, ${{\boldsymbol{I}}_n}$为$n$维单位矩阵.
由式(32)和式(33)可知, 直线特征$\{ {{\mathcal{L}}_{cj}},{{\mathcal{L}}_{rj}}\} $对误差函数(32)的贡献量由权值${w_j}$进行控制. 由式(34)可见, 对于${\boldsymbol{t}} \notin {\rm{null}}({{\boldsymbol{A}}_\pi })$, 即在平面特征能提供较强约束的平移方向上, 直线特征的权值较小, 则其对平移变换求解的贡献也较小; 而对于${\boldsymbol{t}} \in {\rm{null}}({{\boldsymbol{A}}_\pi })$, 即在平面特征无法提供约束的平移方向上, 直线特征的权值较大, 则其对平移变换求解的贡献也较大; 当直线方向与${{\boldsymbol{q}}_{c3}}$垂直时, ${w_j} = 1$, 此时直线对平移变换求解的贡献达到最大. 图3给出了一些典型室内场景中平面与直线特征的空间分布, 并通过灰度映射的方式反映出每条直线特征的权值大小. 从图中可以看出, 对${{\boldsymbol{q}}_{c3}}$方向的平移约束最强的直线, 也就是垂直于该方向的直线, 其权值最大(图中黑色直线); 而对于平行于该方向的直线, 其权值最小(图中浅灰色直线).
总体来说, 在平面特征对平移求解约束较强的方向上, 尽量使用准确性和稳定性更强的平面参数进行求解, 而当产生退化时, 再使用直线特征对平面特征无法约束的位姿方向的求解进行补充, 实现了两类特征的自适应融合. 这样既确保了算法的准确性, 同时也有效地解决了使用平面特征求解位姿时出现的退化问题.
3.3 3 DoF约束情况
如果${\boldsymbol{H}}$为奇异矩阵, 且${\lambda _1} > {\lambda _2} = {\lambda _3} = 0$, 此时平面特征的法向量$\{ {{\boldsymbol{n}}_{ci}}\} $($\{ {{\boldsymbol{n}}_{ri}}\} $), $i \in \{ 1, \cdots , {N_\pi }\}$在空间中是共线关系, 且都沿着向量${{\boldsymbol{q}}_{c1}}$(${{\boldsymbol{q}}_{r1}}$)的方向分布. 在这种情况下, 以${{\boldsymbol{q}}_{c1}}$为旋转轴的旋转变换无法被平面特征所约束, 即在最小化式(24)时存在退化情况.
假设存在旋转变换${{\boldsymbol{R}}_1}$使得两帧平面特征的法向量对齐, 即${{\boldsymbol{n}}_{ci}} = {{\boldsymbol{R}}_1}{{\boldsymbol{n}}_{ri}}$, $\forall i \in \{ 1, \cdots ,{N_\pi }\} $, 而${{\boldsymbol{R}}_1}$与相机真正的旋转变换${\boldsymbol{R}}$之间还相差一个1 DoF旋转, 即以${{\boldsymbol{q}}_{c1}}$为旋转轴的旋转变换${{\boldsymbol{R}}_2}$. 在本文方法中, ${{\boldsymbol{R}}_2}$是通过直线特征计算的, 下文分析直线特征的姿态对计算${{\boldsymbol{R}}_2}$准确性的影响, 并基于此给出求解相机旋转变换时, 平面与直线特征的自适应融合方法.
考虑任意一对具有匹配关系的直线特征${{\boldsymbol{v}}_{cj}}$和${{\boldsymbol{v}}_{rj}}$, 假设${{\boldsymbol{v}}_{rj}}$经过${{\boldsymbol{R}}_1}$变换后为${\boldsymbol{v}}{'_{rj}}$, 则由${{\boldsymbol{v}}_{cj}}$和${\boldsymbol{v}}{'_{rj}}$可以计算${{\boldsymbol{R}}_2}$, 即
$${{\boldsymbol{R}}_2} = \cos \theta {{\boldsymbol{I}}_3} + \left( {1 - \cos \theta } \right){{\boldsymbol{q}}_{c1}}{\boldsymbol{q}}_{c1}^{\rm{T}} + \sin \theta {[{{\boldsymbol{q}}_{c1}}]_ \times }$$ (39) 其中
$$\theta = \arccos \left( {1 - \frac{{1 - {\boldsymbol{v}}_{cj}^{\rm{T}}{\boldsymbol{v}}{'_{rj}}}}{{1 - \left( {{\boldsymbol{q}}_{c1}^{\rm{T}}{{\boldsymbol{v}}_{cj}}} \right)\left( {{\boldsymbol{q}}_{c1}^{\rm{T}}{\boldsymbol{v}}{'_{rj}}} \right)}}} \right)$$ (40) 令$\phi = \langle {{{\boldsymbol{q}}_{c1}},{{\boldsymbol{v}}_{cj}}} \rangle = \langle {{{\boldsymbol{q}}_{c1}},{\boldsymbol{v}}{'_{rj}}} \rangle \in [ {0,\pi } ]$, $\beta = \langle {{{\boldsymbol{v}}_{cj}},{\boldsymbol{v}}{'_{rj}}} \rangle \in [ {0,\min \{ {2\phi ,2(\pi - \phi )} \}} ]$($\langle { \cdot , \cdot } \rangle $表示向量之间的夹角), 如图4所示, 则式(40)可以化简为
$$\theta = \arccos \left( {1 - \frac{{1 - \cos \beta }}{{{{\sin }^2}\phi }}} \right)$$ (41) 计算旋转角$\theta $对$\beta $的偏导数, 可得
$$\frac{{\partial \theta }}{{\partial \beta }} = \frac{{\sin \beta }}{{\sqrt {\left( {1 - \cos \beta } \right)\left( {\cos \beta - \cos 2\phi } \right)} }}$$ (42) 
图 4 角度$\theta $, $\beta $与$\phi $示意图Fig. 4 Illustration of angles $\theta $, $\beta $ and $\phi $图5给出了在$\phi $取不同值的时候, 偏导数$\frac{{\partial \theta }}{{\partial \beta }}$的曲线. 从图5和式(41)均可以看出, 当$\phi = 0.5\pi $, 即${{\boldsymbol{v}}_{cj}}({\boldsymbol{v}}{'_{rj}})$与旋转轴${{\boldsymbol{q}}_{c1}}$垂直时, $\theta $与$\beta $呈线性关系; 而$\phi $接近0或$\pi $, 即${{\boldsymbol{v}}_{cj}}({\boldsymbol{v}}{'_{rj}})$与${{\boldsymbol{q}}_{c1}}$之间接近共线时, $\beta $的微小扰动会使得$\theta $剧烈变化, 大幅降低旋转角度估计的准确性和稳定性. 基于此, 在直线特征匹配误差中, 通过自适应加权方法调整每个直线特征对求解旋转变换的贡献, 即
图 5 $\phi $取不同值时偏导数$\frac{{\partial \theta }}{{\partial \beta }}$随$\beta $变化曲线Fig. 5 Shape of the function $\frac{{\partial \theta }}{{\partial \beta }}$ w.r.t. $\beta $ as the value of $\phi $ changes$${J_R}({\boldsymbol{R}}) = {J_{\pi R}}({\boldsymbol{R}}) + {J_{{\mathcal{L}}R}}({\boldsymbol{R}})\qquad$$ (43) $${J_{{\mathcal{L}}R}}({\boldsymbol{R}}) = \sum\limits_{j = 1}^{{N_{\mathcal{L}}}} {{w_{Rj}}{{\left\| {{{\boldsymbol{v}}_{cj}} - {\boldsymbol{R}}{{\boldsymbol{v}}_{rj}}} \right\|}^2}} $$ (44) 其中, 权值${w_{Rj}}$的计算方法为
$${w_{Rj}} = \left\| {{{\boldsymbol{v}}_{cj}} \times {{\boldsymbol{q}}_{c1}}} \right\|$$ (45) 类似地, 构造矩阵${{\boldsymbol{H}}_{\mathcal{L}}} = {\boldsymbol{H}} + \sum\nolimits_{j = 1}^{{N_{\mathcal{L}}}} {{w_{Rj}}{{\boldsymbol{v}}_{rj}}{\boldsymbol{v}}_{cj}^{\rm{T}}} $并计算其SVD, 即
$$\begin{split} {{\boldsymbol{H}}_{\mathcal{L}}} =\;& {{\boldsymbol{Q}}_{{\mathcal{L}}r}}{{\boldsymbol{\Lambda }}_{\mathcal{L}}}{\boldsymbol{Q}}_{{\mathcal{L}}c}^{\rm{T}} = {\lambda _{{\mathcal{L}}1}}{{\boldsymbol{q}}_{{\mathcal{L}}r1}}{\boldsymbol{q}}_{{\mathcal{L}}c1}^{\rm{T}}\;+ \\ & {\lambda _{{\mathcal{L}}2}}{{\boldsymbol{q}}_{{\mathcal{L}}r2}}{\boldsymbol{q}}_{{\mathcal{L}}c2}^{\rm{T}} + {\lambda _{{\mathcal{L}}3}}{{\boldsymbol{q}}_{{\mathcal{L}}r3}}{\boldsymbol{q}}_{{\mathcal{L}}c3}^{\rm{T}} \end{split} $$ (46) 其中, ${{\boldsymbol{Q}}_{{\mathcal{L}}r}} = [{{\boldsymbol{q}}_{{\mathcal{L}}r1}},{{\boldsymbol{q}}_{{\mathcal{L}}r2}},{{\boldsymbol{q}}_{{\mathcal{L}}r3}}]$ 和 ${{\boldsymbol{Q}}_{{\mathcal{L}}c}} = [{{\boldsymbol{q}}_{{\mathcal{L}}c1}}, {{\boldsymbol{q}}_{{\mathcal{L}}c2}}, {{\boldsymbol{q}}_{{\mathcal{L}}c3}}]$为正交单位矩阵, 而 ${{\boldsymbol{\Lambda }}_{\mathcal{L}}} \;=\; {\rm{diag}}\{ {\lambda _{{\mathcal{L}}1}},{\lambda _{{\mathcal{L}}2}}, {\lambda _{{\mathcal{L}}3}}\} ,$${\lambda _{{\mathcal{L}}1}} \geq {\lambda _{{\mathcal{L}}2}} \geq {\lambda _{{\mathcal{L}}3}}$为对角矩阵. 最小化式(43)的最优解${\boldsymbol{\hat R}}$可由式(47)进行计算, 即
$${\hat{\boldsymbol R}} = {{\boldsymbol{Q}}_{{{\cal L}}c}}{\boldsymbol{Q}}_{{{\cal L}}r{}}^{\rm{T}}$$ (47) 对于平移变换的求解, 也无法直接由式(28)得到, 而此时矩阵${{\boldsymbol{A}}_\pi }$的零空间为
$${\rm{null(}}{{\boldsymbol{A}}_\pi }) = {\eta _2}{{\boldsymbol{q}}_{c2}} + {\eta _3}{{\boldsymbol{q}}_{c3}},\;\;{\eta _2},{\eta _3} \in {\bf{R}}$$ (48) 这种情况下, 与5 DoF约束情况类似, 误差函数中加入直线特征的匹配误差, 构造为式(32), 而平移变换的求解方法也如式(35)所示. 与5 DoF约束情况不同的是, 权值${w_j}$的计算方法由式(34)调整为式(49), 即
$${w_j} = \frac{1}{2}\left( {\left\| {{{\boldsymbol{v}}_{cj}} \times {{\boldsymbol{q}}_{c2}}} \right\| + \left\| {{{\boldsymbol{v}}_{cj}} \times {{\boldsymbol{q}}_{c3}}} \right\|} \right)$$ (49) 图6给出了3 DoF约束情况的两个场景下平面与直线特征的空间分布, 并通过灰度映射的方式反映出每条直线特征的旋转权值${w_{Rj}}$以及平移权值${w_j}$的大小. 从图6(b)可以看出, 垂直于${{\boldsymbol{q}}_{c1}}$方向(即平行于平面)的直线(图中黑色直线)的旋转权值${w_{Rj}}$较大, 即这些直线对于求解旋转变换的贡献较大; 而平行于${{\boldsymbol{q}}_{c1}}$方向的直线(图中浅灰色直线)的权值${w_{Rj}}$较小, 即对求解旋转变换的贡献较小. 而从图6(c)可以看出, 垂直于平面方向的直线(图中黑色直线)的平移权值${w_j}$最大, 因为这样的直线对于平面特征无法约束的平移自由度, 即平行于平面方向的平移的约束是最大的.
4. 实验与分析
本文实验主要分为3个部分: 1) 基于PLHAG的平面−直线特征关联算法验证(第4.1节); 2)不同VO算法在数据集上的对比实验(第4.2节); 3)移动机器人真实环境VO实验(第4.3节). 实验所用处理器为Intel i7 CPU (1.8 GHz), 运行内存8 GB. 第4.1节和第4.2节实验中所用数据集为TUM RGB-D公开数据集[28]; 第4.3节真实环境实验中所用移动机器人平台为Pioneer 3-DX, 传感器为Microsoft Kinect 2.0.
4.1 特征关联实验
为了验证本文提出的基于PLHAG的平面−直线混合关联算法的有效性, 对TUM数据集中5个图像序列中的平面与直线特征进行人工标注, 并将本文算法与Proença等[25]提出的视觉里程计(简称为Prob-RGBD-VO)中使用的平面与直线的匹配方法进行比较. 本文使用准确率(Precision rate)和召回率(Recall rate)作为特征关联实验的评测指标. 其中, 准确率定义为被关联算法成功关联并且被人工标注为关联特征的特征数量除以被关联算法成功关联的特征数量. 召回率定义为被关联算法成功关联并且被人工标注为关联特征的特征数量除以人工标注为关联特征的特征数量. 在实验中, 对图像序列中每一帧特征关联结果的准确率和召回率进行计算, 并将每个序列上的统计结果以箱线图的形式进行展示, 如图7所示, 以此验证特征关联算法的性能.
在对比算法Prob-RGBD-VO中, 对平面和直线特征分别进行关联. 对于平面特征, 先利用平面在图像上的重叠区域和几何关系(法向量夹角与截距差)筛选出可能具有匹配关系的候选, 接着, 对于两个平面$\pi $和$\pi '$, 定义平面到平面的距离为$\| d{\boldsymbol{n}} - d'{\boldsymbol{n}}' \|$, 然后在匹配候选中选出平面到平面距离最小的匹配对. 对于直线特征, 首先对图像中提取出的直线特征计算描述子LBD (Line band descriptor)[29], 并使用$k$近邻搜索建立匹配关系, 然后保留满足图像2D直线几何约束(直线方向和截距相似)的匹配对[25].
实验分别使用两种特征关联方法进行相邻帧之间的平面与直线关联, 并对其准确率与召回率进行统计. 基于PLHAG的关联方法和Prob-RGBD-VO中所使用的关联方法的平均准确率分别为91.6%和88.9%, 平均召回率分别为95.0%和91.7%. 由于Prob-RGBD-VO中关联方法将两类特征分别进行关联, 且并未考虑同一扫描帧的特征之间的几何关系, 而在PLHAG中, 充分考虑了平面与平面、平面与直线之间的几何关系, 进而提高了方法的准确性和鲁棒性.
另外, 为了验证算法的时间性能, 本文对基于PLHAG以及PAG的特征关联算法的对数关联时间分别进行统计, 并绘制统计结果的箱线图, 如图8(a)所示. 需要说明的是, 基于PLHAG的特征关联算法的平均关联时间是0.33$ \mathrm{m}\mathrm{s} $, 完全满足算法实时性的要求. 图8(b)给出的节点数量统计直方图显示了PLHAG与PAG中节点数量的差异. 可以看到, 与基于PAG的算法相比, PLHAG由于直线特征的加入使得图结构规模变大, 节点数量增加, 而在进行特征关联时需要对图结构进行搜索, 因此, 特征关联过程的运行时间有所增加. 尽管如此, 从图8(a)可以看出, 增加的时间小于1$ \mathrm{m}\mathrm{s} $, 对VO系统实时性产生的影响可以忽略不计.
图 8 基于PLHAG与PAG特征关联方法时间性能对比Fig. 8 Comparison of real-time performance for PLHAG and PAG based feature association, respectively4.2 公开数据集VO对比实验
在本节实验中, 在TUM数据集5个场景中运行本文提出的PLVO算法, 并将其结果与4种VO算法plane-seg-VO[30], Prob-RGBD-VO[25], Canny-VO[31]以及STING-VO[5]进行对比. plane-seg-VO[30]是基于平面与线段特征的RGB-D视觉里程计, 在计算相机位姿时, 先利用平面法向量和线段方向向量计算旋转变换, 接着通过对齐平面上的点以及线段端点确定平移变换, 最后利用迭代最近点 (Iterative closest point, ICP)算法对点云进行精确匹配. Prob-RGBD-VO[25]是基于点、直线和平面的RGB-D视觉里程计, 通过3种特征的联合优化实现对位姿的估计. Canny-VO[31]通过对RGB图像中提取的Canny边缘进行扫描匹配, 实现视觉里程计的计算.
实验采用的对比性能指标为相对位姿误差(Relative pose error, RPE)和绝对轨迹误差(Absolute trajectory error, ATE), 其评测工具分别为TUM数据集网站提供的python评测脚本evaluate_ate.py和evaluate_rpe.py. 本文分别对各个算法统计RPE和ATE的均方根误差(Root mean square error, RMSE), 结果如表1和表2所示. 为了尽量避免调参不当对4种对比算法性能的影响, plane-seg-VO[30], Prob-RGBD-VO[25]与Canny-VO[31]的结果以及STING-VO[5]在fr2/desk和fr3/cabinet上的结果均取自于各自论文公开的实验结果. 从表1和表2中可以看出, 本文提出的PLVO在绝大多数场景中都能获得最优的准确性, 原因在于PLVO利用平面和直线进行位姿估计时, 充分利用了两种特征的互补性, 即平面特征的准确性和稳定性, 以及直线特征克服退化的能力. 并且, 在PLVO利用直线特征对平面特征无法约束的位姿自由度进行补充的同时, 尽可能地保留平面特征的主导作用, 充分发挥平面特征参数提取的准确性和稳定性优势, 实现了两类特征的自适应融合. 图9展示了其中三个图像序列的ATE与RPE结果评测图, 从中可以看出, 使用PLVO计算得到的传感器轨迹与运动捕捉系统获得的真实轨迹有着极高的重合度, RPE曲线也稳定在较小的范围内, 充分证明了本文算法的准确性和鲁棒性.
表 1 不同VO算法相对位姿均方根误差对比Table 1 Comparison of RMSE of RPE for different VO methodsVO 算法 plane-seg-VO Prob-RGBD-VO Canny-VO STING-VO PLVO fr1/desk — 0.023 m/1.70° 0.031 m/1.92° 0.025 m/1.90° 0.021 m/1.37° fr2/desk — — 0.008 m/0.45° 0.048 m/1.75° 0.008 m/0.42° fr2/xyz 0.005 m/0.36° — 0.004 m/0.31° 0.004 m/0.34° 0.004 m/0.30° fr2/360_hemisphere — 0.069 m/1.10° 0.108 m/1.09° 0.092 m/1.47° 0.066 m/0.99° fr3/cabinet 0.034 m/2.04° 0.039 m/1.80° 0.036 m/1.63° 0.011 m/1.02° 0.029 m/1.24° fr3/str_ntex — 0.019 m/0.70° 0.027 m/0.59° 0.014 m/0.83° 0.012 m/0.49° fr3/str_tex — — 0.013 m/0.48° 0.021 m/0.59° 0.013 m/0.45° fr3/office — — 0.010 m/0.50° 0.009 m/0.50° 0.007 m/0.47° 表 2 不同VO算法绝对轨迹均方根误差对比(m)Table 2 Comparison of RMSE of ATE for different VO methods (m)VO 算法 Prob-RGBD-VO Canny-VO STING-VO PLVO fr1/desk 0.040 0.044 0.041 0.038 fr2/desk — 0.037 0.098 0.044 fr2/xyz — 0.008 0.010 0.008 fr2/360_hemisphere 0.203 0.079 0.122 0.105 fr3/cabinet 0.200 0.057 0.070 0.052 fr3/str_ntex 0.054 0.031 0.040 0.030 fr3/str_tex — 0.013 0.028 0.013 fr3/office — 0.085 0.089 0.081 为了更加充分地验证多特征融合在VO系统中的作用, 分别计算基于平面特征的VO (P-VO)、基于直线特征的VO (L-VO)以及基于平面−直线(无自适应加权, 即式(33)中${w_j} = 1$, 式(44)中${w_{Rj}} = 1$)的VO, 统计RPE指标的RMSE如表3所示. 从表3中可以明显看出多特征融合以及自适应加权算法对系统准确性的提升作用. 需要说明的是, 在P-VO中, 会频繁出现位姿求解退化情况, 表4统计了每个图像序列上的求解退化出现次数所占的比例. 为了便于对VO定位精度进行更加客观的比较, 在表3所示的P-VO的实验结果中, 没有统计出现求解退化时的位姿估计结果. 而PLVO中没有出现过退化情况, 由此可以看出多特征融合不仅提高了算法的准确性, 而且避免了使用单特征的位姿求解算法可能出现的退化问题.
表 3 相对位姿均方根误差消融实验结果Table 3 Results of ablation experiment in term of the RMSE of RPEVO 算法 PLVO PLVO (无加权) L-VO P-VO* fr1/desk 0.021 m/1.37° 0.041 m/1.52° 0.039 m/1.56° 0.042 m/1.95° fr2/desk 0.008 m/0.42° 0.011 m/0.42° 0.018 m/0.52° 0.016 m/0.55° fr2/xyz 0.004 m/0.30° 0.005 m/0.34° 0.007 m/0.37° 0.004 m/0.27° fr2/360_hemisphere 0.066 m/0.99° 0.096 m/1.20° 0.162 m/1.22° 0.118 m/1.42° fr3/cabinet 0.029 m/1.24° 0.054 m/1.44° 0.097 m/1.70° 0.029 m/1.71° fr3/str_ntex 0.012 m/0.49° 0.013 m/0.55° 0.015 m/0.48° 0.013 m/0.53° fr3/str_tex 0.013 m/0.45° 0.015 m/0.49° 0.016 m/0.47° 0.023 m/0.75° fr3/office 0.007 m/0.47° 0.012 m/0.57° 0.016 m/0.59° 0.014 m/0.62° * 在P-VO实验中, 出现位姿求解退化情况的位姿估计没有参与RPE的计算. 表 4 P-VO中位姿求解退化情况所占比例Table 4 Ratio of the degenerate cases in P-VOfr1/desk fr2/desk fr2/xyz fr2/360_hemisphere fr3/cabinet fr3/str_ntex fr3/str_tex fr3/office Ratio (%) 73.3 60.3 46.3 91.9 83.4 37.9 40.4 17.3 此外, 本文分别对PLVO、P-VO以及L-VO算法的运行时间进行统计, 其箱线图如图10所示. 相比于P-VO与L-VO算法, PLVO算法分别增加了直线提取和平面提取的运行时间, 因此实时性略逊于P-VO与L-VO. 尽管如此, PLVO的帧率仍可达到6 ~ 10 Hz, 满足系统实时性的要求.
图 10 各个图像序列上PLVO、P-VO以及L-VO每帧运行时间统计箱线图Fig. 10 Boxplot of statistics of the runtime per frame for PLVO, P-VO and L-VO4.3 室内环境移动机器人VO实验
为了验证本文算法的有效性, 使用PLVO进行真实环境的移动机器人视觉里程计实验. 将Microsoft Kinect 2.0传感器固定在Pioneer 3-DX机器人上离地面大约0.34 m的位置. 在实验中, 用手柄控制机器人通过一个实验室, 环境规模为8.68 m$ \times $7.86 m. 用PLVO算法进行机器人的定位与增量式建图, 所构建的三维点云地图如图11所示. 图11(a)为俯视全景图, 图11(b)和图11(c)给出了在全景图中标注的I, II, III三处对应的RGB图像和局部放大的点云地图. 从图11中可以看出, PLVO在真实室内环境中能够较准确地完成移动机器人的位姿估计.
图 11 实验室场景下移动机器人定位与增量式建图结果Fig. 11 Real-world experiment in a laboratory using a mobile robot5. 结束语
本文提出了一种基于平面和直线自适应融合的视觉里程计算法. 通过对相邻两帧的平面和直线特征统一构建PLHAG, 充分考虑了平面与平面、平面与直线之间的几何关系, 实现了两类特征的混合关联, 提高了多特征关联结果的准确率和召回率. 而在估计RGB-D相机位姿时, 充分发挥了平面特征准确、稳定的优点及其在机器人位姿估计中的主导作用. 针对平面特征在位姿估计中存在的退化问题, 引入直线特征对平面无法约束的自由度进行约束, 并设计能够充分体现约束强度的权重, 实现了两类特征的主辅相济与自适应融合. 多角度的实验验证充分证明了本文提出算法的准确性和有效性.
-
图 4 角度$\theta $, $\beta $与$\phi $示意图
Fig. 4 Illustration of angles $\theta $, $\beta $ and $\phi $
图 5 $\phi $取不同值时偏导数$\frac{{\partial \theta }}{{\partial \beta }}$随$\beta $变化曲线
Fig. 5 Shape of the function $\frac{{\partial \theta }}{{\partial \beta }}$ w.r.t. $\beta $ as the value of $\phi $ changes
图 8 基于PLHAG与PAG特征关联方法时间性能对比
Fig. 8 Comparison of real-time performance for PLHAG and PAG based feature association, respectively
图 10 各个图像序列上PLVO、P-VO以及L-VO每帧运行时间统计箱线图
Fig. 10 Boxplot of statistics of the runtime per frame for PLVO, P-VO and L-VO
图 11 实验室场景下移动机器人定位与增量式建图结果
Fig. 11 Real-world experiment in a laboratory using a mobile robot
表 1 不同VO算法相对位姿均方根误差对比
Table 1 Comparison of RMSE of RPE for different VO methods
VO 算法 plane-seg-VO Prob-RGBD-VO Canny-VO STING-VO PLVO fr1/desk — 0.023 m/1.70° 0.031 m/1.92° 0.025 m/1.90° 0.021 m/1.37° fr2/desk — — 0.008 m/0.45° 0.048 m/1.75° 0.008 m/0.42° fr2/xyz 0.005 m/0.36° — 0.004 m/0.31° 0.004 m/0.34° 0.004 m/0.30° fr2/360_hemisphere — 0.069 m/1.10° 0.108 m/1.09° 0.092 m/1.47° 0.066 m/0.99° fr3/cabinet 0.034 m/2.04° 0.039 m/1.80° 0.036 m/1.63° 0.011 m/1.02° 0.029 m/1.24° fr3/str_ntex — 0.019 m/0.70° 0.027 m/0.59° 0.014 m/0.83° 0.012 m/0.49° fr3/str_tex — — 0.013 m/0.48° 0.021 m/0.59° 0.013 m/0.45° fr3/office — — 0.010 m/0.50° 0.009 m/0.50° 0.007 m/0.47° 
下载: 导出CSV
表 2 不同VO算法绝对轨迹均方根误差对比(m)
Table 2 Comparison of RMSE of ATE for different VO methods (m)
VO 算法 Prob-RGBD-VO Canny-VO STING-VO PLVO fr1/desk 0.040 0.044 0.041 0.038 fr2/desk — 0.037 0.098 0.044 fr2/xyz — 0.008 0.010 0.008 fr2/360_hemisphere 0.203 0.079 0.122 0.105 fr3/cabinet 0.200 0.057 0.070 0.052 fr3/str_ntex 0.054 0.031 0.040 0.030 fr3/str_tex — 0.013 0.028 0.013 fr3/office — 0.085 0.089 0.081
下载: 导出CSV
表 3 相对位姿均方根误差消融实验结果
Table 3 Results of ablation experiment in term of the RMSE of RPE
VO 算法 PLVO PLVO (无加权) L-VO P-VO* fr1/desk 0.021 m/1.37° 0.041 m/1.52° 0.039 m/1.56° 0.042 m/1.95° fr2/desk 0.008 m/0.42° 0.011 m/0.42° 0.018 m/0.52° 0.016 m/0.55° fr2/xyz 0.004 m/0.30° 0.005 m/0.34° 0.007 m/0.37° 0.004 m/0.27° fr2/360_hemisphere 0.066 m/0.99° 0.096 m/1.20° 0.162 m/1.22° 0.118 m/1.42° fr3/cabinet 0.029 m/1.24° 0.054 m/1.44° 0.097 m/1.70° 0.029 m/1.71° fr3/str_ntex 0.012 m/0.49° 0.013 m/0.55° 0.015 m/0.48° 0.013 m/0.53° fr3/str_tex 0.013 m/0.45° 0.015 m/0.49° 0.016 m/0.47° 0.023 m/0.75° fr3/office 0.007 m/0.47° 0.012 m/0.57° 0.016 m/0.59° 0.014 m/0.62° * 在P-VO实验中, 出现位姿求解退化情况的位姿估计没有参与RPE的计算.
下载: 导出CSV
表 4 P-VO中位姿求解退化情况所占比例
Table 4 Ratio of the degenerate cases in P-VO
fr1/desk fr2/desk fr2/xyz fr2/360_hemisphere fr3/cabinet fr3/str_ntex fr3/str_tex fr3/office Ratio (%) 73.3 60.3 46.3 91.9 83.4 37.9 40.4 17.3
下载: 导出CSV
-
[1] 丁文东, 徐德, 刘希龙, 张大朋, 陈天. 移动机器人视觉里程计综述. 自动化学报, 2018, 44(3): 385-400Ding Wen-Dong, Xu De, Liu Xi-Long, Zhang Da-Peng, Chen Tian. Review on visual odometry for mobile robots. Acta Automatica Sinica, 2018, 44(3): 385-400 [2] 王楠, 马书根, 李斌, 王明辉, 赵明扬. 震后建筑内部层次化SLAM的地图模型转换方法. 自动化学报, 2015, 41(10): 1723-1733Wang Nan, Ma Shu-Gen, Li Bin, Wang Ming-Hui, Zhao Ming-Yang. A model transformation of map representation for hierarchical SLAM that can be used for after-earthquake buildings. Acta Automatica Sinica, 2015, 41(10): 1723-1733 [3] 杨晶东, 杨敬辉, 洪炳熔. 一种有效的移动机器人里程计误差建模方法. 自动化学报, 2009, 35(2): 168-173 doi: 10.3724/SP.J.1004.2009.00168Yang Jing-Dong, Yang Jing-Hui, Hong Bing-Rong. An efficient approach to odometric error modeling for mobile robots. Acta Automatica Sinica, 2009, 35(2): 168-173 doi: 10.3724/SP.J.1004.2009.00168 [4] 季秀才, 郑志强, 张辉. SLAM问题中机器人定位误差分析与控制. 自动化学报, 2008, 34(3): 323-330Ji Xiu-Cai, Zheng Zhi-Qiang, Zhang Hui. Analysis and control of robot position error in SLAM. Acta Automatica Sinica, 2008, 34(3): 323-330 [5] Sun Q X, Yuan J, Zhang X B, Sun F C. RGB-D SLAM in indoor environments with STING-based plane feature extraction. IEEE/ASME Transactions on Mechatronics, 2018, 23(3): 1071-1082 doi: 10.1109/TMECH.2017.2773576 [6] 韩锐, 李文锋. 一种基于线特征的SLAM算法研究. 自动化学报, 2006, 32(1): 43-46Han Rui, Li Wen-Feng. Line-feature-based SLAM algorithm. Acta Automatica Sinica, 2006, 32(1): 43-46 [7] 俞毓锋, 赵卉菁, 崔锦实, 査红彬. 基于道路结构特征的智能车单目视觉定位. 自动化学报, 2017, 43(5): 725-734Yu Yu-Feng, Zhao Hui-Jing, Cui Jin-Shi, Zha Hong-Bin. Road structural feature based monocular visual localization for intelligent vehicle. Acta Automatica Sinica, 2017, 43(5): 725-734 [8] Lee T, Kim C, Cho D D. A monocular vision sensor-based efficient SLAM method for indoor service robots. IEEE Transactions on Industrial Electronics, 2019, 66(1): 318-328 doi: 10.1109/TIE.2018.2826471 [9] Li H, Xing Y Z, Zhao J, Bazin J, Liu Z, Liu Y. Leveraging structural regularity of Atlanta world for monocular SLAM. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). Montreal, QC, Canada: IEEE Press, 2019. 2412−2418 [10] Li H, Yao J, Bazin J, Lu X, Xing Y, Liu K. A monocular SLAM system leveraging structural regularity in Manhattan world. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). Brisbane, QLD, Australia: IEEE, 2018. 2518−2525 [11] 张峻宁, 苏群星, 刘鹏远, 朱庆, 张凯. 一种自适应特征地图匹配的改进VSLAM算法. 自动化学报, 2019, 45(3): 553-565Zhang Jun-Ning, Su Qun-Xing, Liu Peng-Yuan, Zhu Qing, Zhang Kai. An improved VSLAM algorithm based on adaptive feature map. Acta Automatica Sinica, 2019, 45(3): 553-565 [12] Yang S, Song Y, Kaess M, Scherer S. Pop-up SLAM: Semantic monocular plane SLAM for low-texture environments. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Daejeon, South Korea: IEEE, 2016. 1222−1229 [13] Lu Y, Song D. Visual navigation using heterogeneous landmarks and unsupervised geometric constraints. IEEE Transactions on Robotics, 2015, 31(3): 736-749 doi: 10.1109/TRO.2015.2424032 [14] 李磊, 陈细军, 曹志强, 候增广, 谭民. 一处室内轮式自主移动机器人的导航控制研究. 自动化学报, 2003, 29(6): 893-899Li Lei, Chen Xi-Jun Cao Zhi-Qiang, Hou Zeng-Guang, Tan Min. Research on the navigation control of an indoor wheeled autonomous mobile robot. Acta Automatica Sinica, 2003, 29(6): 893-899 [15] 庄严, 陈东, 王伟, 韩建达, 王越超. 移动机器人基于视觉室外自然场景理解的研究与进展. 自动化学报, 2010, 36(1): 1-11 doi: 10.3724/SP.J.1004.2010.00001Zhuang Yan, Chen Dong, Wang Wei, Han Jian-Da, Wang Yue-Chao. Status and development of natural scene understanding for vision-based outdoor mobile robot. Acta Automatica Sinica, 2010, 36(1): 1-11 doi: 10.3724/SP.J.1004.2010.00001 [16] Nardi F, Corte B D, Grisetti G. Unified representation and registration of heterogeneous sets of geometric primitives. IEEE Robotics and Automation Letters, 2019, 4(2): 625-632 doi: 10.1109/LRA.2019.2891989 [17] Yang Y, Huang G. Observability analysis of aided INS with heterogeneous features of points, lines, and planes. IEEE Transactions on Robotics, 2019, 35(6): 1399-1418 doi: 10.1109/TRO.2019.2927835 [18] Zhang H, Ye C. Plane-aided visual-inertial odometry for 6-DOF pose estimation of a robotic navigation aid. IEEE Access, 2020, 8: 90042-90051 doi: 10.1109/ACCESS.2020.2994299 [19] Taguchi Y, Jian Y D, Ramalingam S, Feng C. Point-plane SLAM for hand-held 3D sensors. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). Karlsruhe, Germany: IEEE, 2013. 5182−5189 [20] Cupec R, Nyarko E K, Filko D, Kitanov A. Place recognition based on matching of planar surfaces and line segments. International Journal of Robotics Research, 2015, 34(4-5): 674-704 doi: 10.1177/0278364914548708 [21] Zuo X, Xie X, Liu Y, Huang G. Robust visual SLAM with point and line features. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Vancouver, Canada: IEEE, 2017. 1775−1782 [22] Gomez-Ojeda R, Moreno F, Zuiga-Nol D, Scaramuzza D, Gonzalez-Jimenez J. PL-SLAM: A stereo SLAM system through the combination of points and line segments. IEEE Transactions on Robotics, 2019, 35(3): 734-746 doi: 10.1109/TRO.2019.2899783 [23] Fang B, Zhan Z. A visual SLAM method based on point-line fusion in weak-matching scene. International Journal of Advanced Robotic Systems, 2020, 17(2): 1-11 [24] Elqursh A, Elgammal A. Line-based relative pose estimation. In: Proceedings of the Conference on Computer Vision and Pattern Recognition (CVPR). Providence, RI, USA: IEEE, 2011. 3049−3056 [25] Proença P F, Gao Y. Probabilistic RGB-D odometry based on points, lines and planes under depth uncertainty. Robotics and Autonomous Systems, 2018, 104: 25-39 doi: 10.1016/j.robot.2018.02.018 [26] Grompone von Gioi R, Jakubowicz J, Morel J, Randall G. LSD: A fast line segment detector with a false detection control. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2010, 32(4): 722-732 doi: 10.1109/TPAMI.2008.300 [27] Dryanovski I, Valenti R G, Xiao J Z. Fast visual odometry and mapping from RGB-D data. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). Karlsruhe, Germany: IEEE, 2013. 2305−2310 [28] Sturm J, Engelhard N, Endres F, Burgard W, Cremers D. A benchmark for the evaluation of RGB-D SLAM systems. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Vilamoura, Portugal: IEEE, 2012. 573−580 [29] Zhang L, Koch R. An efficient and robust line segment matching approach based on LBD descriptor and pairwise geometric consistency. Journal of Visual Communication and Image Representation, 2013, 24(7): 794-805 doi: 10.1016/j.jvcir.2013.05.006 [30] 董星亮, 苑晶, 黄枢子, 杨少坤, 张雪波, 孙凤池, 黄亚楼. 室内环境下基于平面与线段特征的RGB-D视觉里程计. 机器人, 2018, 40(6): 921-932Dong Xing-Liang, Yuan Jing, Huang Shu-Zi, Yang Shao-Kun, Zhang Xue-Bo, Sun Feng-Chi, Huang Ya-Lou. RGB-D visual odometry based on features of planes and line segments in indoor environments. Robot, 2018, 40(6): 921-932 [31] Zhou Y, Li H, Kneip L. Canny-VO: Visual odometry with RGB-D cameras based on geometric 3-D-2-D edge alignment. IEEE Transactions on Robotics, 2019, 35(1): 184-199 doi: 10.1109/TRO.2018.2875382 期刊类型引用(4)
1. 葛泉波，李凯，张兴国. 基于多关键点检测加权融合的无人机相对位姿估计算法. 自动化学报. 2024(07): 1402-1416 . 
本站查看2. 王西旗，毕京学，杨尚帅. 面向室内弱纹理场景多特征融合RGB-D SLAM方法. 导航定位与授时. 2024(05): 53-65 . 百度学术3. 闫莉萍，李程伟，徐柏凯，夏元清，肖波. 一种基于结构特征的单目视觉惯性里程计. 系统工程与电子技术. 2023(10): 3207-3217 . 百度学术4. 金犇，贝绍轶，尹明锋，李凌岩. 一种基于点面特征的低漂移视觉里程计. 重庆理工大学学报(自然科学). 2022(06): 88-95 . 百度学术其他类型引用(10)
-
下载:
计量
- 文章访问数: 2696
- HTML全文浏览量: 909
- PDF下载量: 260
- 被引次数: 14
