暑假研习系列：线、面SLAM从零入门（二）

二、空间直线的表述

1. 空间直线的参数化

1. 零空间与生成子空间表示

   该种表示方法是一种过参数表示，用8个参数表示4自由度的直线。个人直观的理解为：不重合且以齐次表示的两个点之间的连线。

   表述方式为：$L_{2\times4}:=\begin{bmatrix}X_1^T\\X_2^T\end{bmatrix}=\begin{bmatrix}\widetilde{X_1} & 1\\ \widetilde{X_2} &1\end{bmatrix}=\begin{bmatrix} u_1&v_1&w_1& 1\\u_2&v_2&w_2&1\end{bmatrix}$L2×4​:=[X1T​X2T​​]=[X1​​X2​​​11​]=[u1​u2​​v1​v2​​w1​w2​​11​]。

   另外，关于[12]中生成空间的表述部分没有看懂，我个人以为应该不影响。

2. Plucker(普吕克)矩阵表述

   该种表示方法尽管用16个参数表示4自由度的直线，但是仅有6个非零元素，除去齐次的一个自由度，又因为满足行列式为零，所以自由度为4。[12]疑问：满足行列式为零不属于一个约束条件吗？为什么能够去除自由度？该表述是否可以无约束优化？

   表述方法为：$L:=X_1X_2^T-X_2X_1^T$L:=X1​X2T​−X2​X1T​。L为4x4的齐次反对称阵，其秩为2，并且L与定义它的两点无关。

   • 给定从世界坐标系c到相机w的变换矩阵$T_{cw}= \begin{bmatrix} R_{cw} & t_{cw} \\ 0 & 1 \end{bmatrix}$Tcw​=[Rcw​0​tcw​1​]，通过下述公式将Plucker矩阵进行变换：
     $L_c=T_{cw}L_wT_{cw}^T$Lc​=Tcw​Lw​TcwT​

   • Plucker矩阵与Plucker直线坐标的转换：$L_{4\times 4}=\begin{bmatrix} [d]^{\hat{\quad}} & {l} \\ {-{l}^T} & 0 \end{bmatrix}$L4×4​=[[d]^−lT​l0​]

3. Plucker直线坐标表示

   该种表示方法是一种过参数表示，用6个参数表示4自由度的直线，是Plucker矩阵中六个非零元素的排列$l_{12}:l_{13}:l_{14}:l_{23}:l_{42}:l_{34}$l12​:l13​:l14​:l23​:l42​:l34​。我个人直观上理解，就是LSR中定义的那两个线的方向及线的垂直方向。

   表述方式为：$L:= (\overline {l},d)^T=\begin{bmatrix}\widetilde{X}_2\times \widetilde{X}_1\\\widetilde{X}_2-\widetilde{X}_1\end{bmatrix}$L:=(l,d)T=[X2​×X1​X2​−X1​​]。其中，$\overline{l}$l表示直线的方向向量，占据三个维度，且模长为1，可以用两个线上点相减后归一化得到；$d=\overline{l}\times q$d=l×q，占据三个维度，表示从原点指向线上一点q的向量，与直线本身组成平面的法向量，且$||d||$∣∣d∣∣，为原点到直线的距离。证明详见[10]。注意，由于过参数数化，两者存在约束关系$d^\mathsf{T}\overline{l}=0$dTl=0（正交），并不能直接用在SLAM中的无约束问题优化里面。

   还可以理解为两个平面相交获得直线：假设存在两个平面

   • 引理：在Plucker坐标系，两条直线$L_1,L_2$L1​,L2​相交的充要条件是：$l_1d_2+l_2d_1=0$l1​d2​+l2​d1​=0

   • 给定从世界坐标系c到相机w的变换矩阵$T_{cw}= \begin{bmatrix} R_{cw} & t_{cw} \\ 0 & 1 \end{bmatrix}$Tcw​=[Rcw​0​tcw​1​]，通过下述公式将Plucker坐标系进行变换：
     $L_c = \begin{bmatrix} \overline{l}_c \\ d_c \end{bmatrix} = T_{cw}^{'}L_w=\begin{bmatrix} R_{cw} &[t_{cw}]^{\hat{ \quad} }R_{cw} \\ 0 & R_{cw} \end{bmatrix}\begin{bmatrix} \overline{l}_w \\ d_w \end{bmatrix}$Lc​=[lc​dc​​]=Tcw′​Lw​=[Rcw​0​[tcw​]^Rcw​Rcw​​][lw​dw​​]
     其中，hat表示反对称。

4. 正交表示法

   该方法是一种非过参数表示，具有四个自由度，包括旋转的3个自由度和距离的一个自由度，所以可以在无约束优化中直接使用。然而正交表示并不能像Plucker坐标一样很好地表示坐标变换和重投影,因此除了在后端优化中采用正交表示,其他情况下均采用Plucker坐标。

   表述方式为：$L=[U,W]\in [SO(3),SO(2)]$L=[U,W]∈[SO(3),SO(2)]。在获得直线的Plücker坐标后，将L进行QR分解，得到：
   $U := R(\varphi_x,\varphi_y,\varphi_z)= [\frac{l}{||l||},\frac{d}{||d||},\frac{l\times d}{||l\times d||}]\\ W := \begin{bmatrix}\cos(\phi) & -\sin(\phi)\\ \sin(\phi) & \cos(\phi)\end{bmatrix}= \begin{bmatrix} w_1 &-w_2 \\w_2 & w_1\end{bmatrix} = \frac{1}{\sqrt{(||l||^2+||d||^2)}}\begin{bmatrix} ||l|| &-{||d||} \\ || d|| & ||l||\end{bmatrix}\\$U:=R(φx​,φy​,φz​)=[∣∣l∣∣l​,∣∣d∣∣d​,∣∣l×d∣∣l×d​]W:=[cos(ϕ)sin(ϕ)​−sin(ϕ)cos(ϕ)​]=[w1​w2​​−w2​w1​​]=(∣∣l∣∣2+∣∣d∣∣2)​1​[∣∣l∣∣∣∣d∣∣​−∣∣d∣∣∣∣l∣∣​]
   因此四个自由度分别是$\varphi_x,\varphi_y,\varphi_z,$φx​,φy​,φz​,：分别代表直线坐标轴围绕xyz轴旋转的角度，以及参数$\phi$ϕ：隐含了坐标原点到直线的距离。

   • 正交表示L’到Plücker坐标L之间的变换方式为：
     $L =[w_1u_1^T, w_2u_2^T]^T= \frac{1}{\sqrt{(||l||^2+||d||^2)}}L^{'}$L=[w1​u1T​,w2​u2T​]T=(∣∣l∣∣2+∣∣d∣∣2)​1​L′
     其中$u_i$ui​代表的是U矩阵的第i列。虽然L和L’有一个尺度的差，但是它们表示的是同一条空间直线。【疑问：在坐标转换中为什么$u_3$u3​不参与计算？】

2. 空间直线在Plucker坐标中的初始化

对偶定理：在3D射影空间内，点和平面对偶，直线自对偶。其中，对偶是指可以互换定理中两者的位置，如点和平面对偶，则可以互换任意定理内点和平面的位置。

已知存在不重合的两点$X_1,X_2$X1​,X2​，均以4.1的方式齐次表述。且有原点$\widetilde{C_0}=[u_0,v_0,w_0]$C0​​=[u0​,v0​,w0​]。

利用以上3点可以构建一个平面$\pi = [\pi_u,\pi_v,\pi_w,\pi_q]$π=[πu​,πv​,πw​,πq​]，有：
$\pi=\begin{bmatrix}(\widetilde{X_1}-\widetilde{C_0})\times(\widetilde{X_2}-\widetilde{C_0})\\ -\widetilde{C_0}^T(\widetilde{X_1}\times\widetilde{X_2})\end{bmatrix}$π=[(X1​​−C0​​)×(X2​​−C0​​)−C0​​T(X1​​×X2​​)​]
在两帧视图中，每帧视图都可以在空间恢复一条直线，分别记为$\pi_1,\pi_2$π1​,π2​。

由对偶定理可知，$L:=X_1X_2^T-X_2X_1^T$L:=X1​X2T​−X2​X1T​的对偶表述为$L^*:=\pi_1\pi_2^T-\pi_2\pi_1^T$L∗:=π1​π2T​−π2​π1T​，表示两平面的交线确定的直线，且$L_c^*=T_{cw}^{-T}L_w^*T_{cw}^{-1}$Lc∗​=Tcw−T​Lw∗​Tcw−1​。此时，Plucker矩阵对偶矩阵$L^*$L∗与$L$L对应规则如下：
$l_{12}:l_{13}:l_{14}:l_{23}:l_{42}:l_{34}=l_{34}^*:l_{42}^*:l_{23}^*:l_{14}^*:l_{13}^*:l_{12}^*$l12​:l13​:l14​:l23​:l42​:l34​=l34∗​:l42∗​:l23∗​:l14∗​:l13∗​:l12∗​
之后，可以从Plucker矩阵及对偶矩阵来还原Plucker坐标。

3. 空间直线的观测模型

空间直线的观测模型存在多种定义方式，可以通过3D-3D的方式,也可以通过3D-2D的方式定义空间直线与匹配图像直线的误差,通常采用后一种方法(3D-2D)精度更高。这一结论来源于[13]。

线特征从归一化平面到像素平面的投影内参矩阵K’ 点特征内参矩阵为K。已知存在空间直线L，点C和D是直线L上两点，点c和d是它们在像素平面上的投影。即存在关系：$c=KC, d=KD;L=C\times D, l = c\times d$c=KC,d=KD;L=C×D,l=c×d。那么有:
$l=K^{'}L=\begin{bmatrix}f_y&0&0\\0&f_x&0\\-f_yc_x&-f_xc_y&f_xf_y\end{bmatrix}L$l=K′L=⎣⎡​fy​0−fy​cx​​0fx​−fx​cy​​00fx​fy​​⎦⎤​L
上式表明，直线的线投影只和法向量有关和方向向量无关。

4. 空间直线的误差模型

因为同一条直线在不同图像线段的长度和大小都是不一样的，所以不可以直接从两幅图像的线段中得到坐标及误差[11]。衡量线的投影误差必须从空间中重投影回当前的图像中才能定义误差。

二维平面上的直线坐标表示：$ax+by+z=0$ax+by+z=0，因此可以用$I=(a,b,c)^\mathsf{T}$I=(a,b,c)T表示一条非齐次直线。

利用几何距离定义的非线性误差：定义直线在像素平面下两个端点为$x_1=(u_1,v_1,1); x_2=(u_2,v_2,1)$x1​=(u1​,v1​,1);x2​=(u2​,v2​,1)，有
$e = \begin{bmatrix}\frac{x_1^\mathsf{T}I}{\sqrt{a^2+b^2}}, \frac{x_2^\mathsf{T}I}{\sqrt{a^2+b^2}}\end{bmatrix}^\mathsf{T}$e=[a2+b2​x1T​I​,a2+b2​x2T​I​​]T
计算误差的整体流程大致如下：

• 在世界坐标系下存在空间直线$L_w$Lw​，假设以plucker坐标表示。
• 利用点坐标变换矩阵$T_{cw}$Tcw​推导出线特征变换矩阵$T_{cw}^{’}$Tcw’​，利用变换矩阵将世界坐标系下的直线转化为相机坐标系下的直线。
• 将相机坐标下的空间直线利用线特征的内参矩阵投影到像素平面上，获得直线l
• 计算误差e

在[1]中有一张关于点、线特征观测模型的图表：

三、基于点线误差的雅各比矩阵

该部分省略推导过程，推导过程详见[1]。

四、点线综合的视觉词典

Shuai Yang[14]在Dorian和 Lee的基础上，提出了点线综合的视觉词典，能够提供更加鲁棒的回环检测。

• 描述子：ORB（点）和LBD（线）

• 描述子组成：256位二进制+1位标志位（0：表示点特征，1：表示线特征）

• 相似度判别：TF-IDF加权法则

  TF词频：词汇在待检测图像上存在的频率，出现次数越多，区分度越高。

  IDF逆文档频率：词汇在词典上存在的频率，出现次数越低，区分度越高。

  词汇权重$w_i=TF_i\times IDF_i = \frac{n_i}{n}\times \log(\frac{N}{N_i})$wi​=TFi​×IDFi​=nni​​×log(Ni​N​)

• 词典结构：当一侦图像加入图像数据库时，需要将其提取到的描述子与视觉字典中作为聚类中心的节点，计算汉明距离，并将这些描述子划分到词典中距离它最近的叶子节点。

参考：

[1] 谢晓佳. 基于点线综合特征的双目视觉 SLAM 方法[D]. 浙江大学, 2017.

[2] 博客：https://www.cnblogs.com/Jessica-jie/p/7510931.html，及这位博主的相关笔记

[3] 博客：https://blog.****.net/wuliyanyan/article/details/103233264

[4] Von Gioi R G, Jakubowicz J, Morel J M, et al. LSD: a line segment detector[J]. Image Processing On Line, 2012, 2: 35-55.

[5] Zhang L, Koch R. An efficient and robust line segment matching approach based on LBD descriptor and pairwise geometric consistency[J]. Journal of Visual Communication and Image Representation, 2013, 24(7): 794-805.

[6] Akinlar C, Topal C. EDLines: A real-time line segment detector with a false detection control[J]. Pattern Recognition Letters, 2011, 32(13): 1633-1642.

[7] C. Topal, C. Akinlar, Edge Drawing: A Combined Real-Time Edge and Segment Detector,” Journal of Visual Communication and Image Representation, 23(6), 862-872, 2012.

[8] C. Topal, C. Akinlar, and Y. Genc, Edge Drawing: A Heuristic Approach to Robust Real-Time Edge Detection, Proceedings of the ICPR, pp. 2424-2427, August 2010.

[9] Zhang L, Koch R. An efficient and robust line segment matching approach based on LBD descriptor and pairwise geometric consistency[J]. Journal of Visual Communication and Image Representation, 2013, 24(7): 794-805.

[10] 知乎：https://zhuanlan.zhihu.com/p/149028042

[11] 知乎：https://zhuanlan.zhihu.com/p/65674067

[12] 计算机视觉中的多视图几何（第二版）

[13] Nister D,Naroditsky O, Bergen J. Visual odometry: Computer Vision and Pattern Recognion, 2004. CVPR 2004. Proceedings of the 2004 IEEE Computer Society Conference on, 2004[C].

[14] Yang S. Place Recognition using Multiple Feature Types[J]. Advances in Robotics & Automation,2015,01(s2).