SLAM中线特征的参数化表示方法/重投影/初始化方法
文章目录
- 1. 预备知识
- 齐次坐标
- 空间直线的优化
- 2. 空间直线的表示方法
- 2.1 空间直线的自由度
- 2.2 普吕克坐标 (Plucker Coordinates Representation)
- 直线的唯一确定原理
- 普吕克矩阵
- 基于两端点的普吕克坐标表示方法
- 基于两平面的普吕克坐标表示方法
- 投影模型和Camera-World转换
- 2.3 其他的直线部分表示法
- 最近点和方向
- 两投影线
- Denavit–Hartenberg 参数
- 2.4 正交表示 (Orthonormal Representation)
- 正交表示推导
- 3. 结构信息约束下的线特征表示 —— 2-DoF表示
- 3.1 StructSLAM : Visual SLAM with Building Structure
- 结构线参数化表示方法
- 结构线初始化方法
- 结构线重投影方法
- 3.2 StructVIO : Visual-inertial Odometry with Structural Regularity of Man-made Environments
- 结构线参数化表示方法
- 结构线重投影方法
- 结构线初始化方法
- 3.3 Leveraging Structural Information to Improve Point Line Visual-Inertial Odometry
- 非结构线参数化表示方法
- 非结构线初始化方法
- 非结构线重投影方法
- 结构线参数化表示方法
- 结构线初始化方法
- 结构线重投影方法
由于最近在做结构信息辅助的SLAM，在线特征这块很多概念诸如参数化表示方法(包括普通线和结构线) 、初始化和优化方法等，相比点特征的处理更为复杂，因此这篇文章算是线特征学习的一个阶段性总结。
深度参考了以下资料：
- 什么是齐次坐标?
- Structure-From-Motion Using Lines: Representation, Triangulation and Bundle Adjustment
- SLAM线特征学习（1）——基本的线特征表示与优化推导
- SLAM线特征学习（2）——线特征初始化和最小表示
1. 预备知识
由于涉及到计算机多视图几何中的一些知识，为方便理解在此把一些重要概念列出。
齐次坐标
欧式空间下的笛卡尔坐标可以很好的用来描述二维/三维的空间几何并解决相关数学运算问题，但在计算机视觉领域下的投影空间中存在着许多无法解释和处理的问题，如：两条平行线在投影空间中的相交问题（这也是结构线SLAM中对Vanish Point 灭点/消影点的提取和利用的前提）
图1. 现实世界中相互平行的铁轨，竟然在投影空间中可以相交于无限远处的地平线
在欧氏空间中，我们无法描述这样一个存在于无穷远的点，无穷的坐标并不是我们想要在参数化和优化过程中想要看到的。因此，由 August Ferdinand Möbius提出的齐次坐标，就是用来解决这样一类问题的，它使图形和几何学的计算在投影空间中成为可能。
齐次坐标的本质，就是用 N + 1 个数来表示 N 维坐标，所额外增加的这个参数在表示不同几何体的情况下会具备不同的几何意义。
对于欧氏空间中坐标表示为 (X,Y,Z)(X, Y, Z)(X,Y,Z) 的空间点(显然有3个自由度），额外引入一个变量 www，我们就可以构建该空间点的齐次坐标 (x,y,z,w)(x, y, z, w)(x,y,z,w)，其中X=x/w,Y=y/w,Z=z/wX = x/w, Y = y/w, Z= z/wX=x/w,Y=y/w,Z=z/w，在这里 www 的几何意义就是 逆深度 ，也是SLAM中点特征参数化的常见概念。这里之所以叫“齐次坐标”，就是因为对齐次坐标乘上任意一个非零系数，在欧氏空间中都对应着同一个点。
在本文后续将以 MT=(M‾,m)T\mathbf{M}^T=(\mathbf{\overline{M}}, m)^TMT=(M,m)T 的齐次坐标形式来表示一个空间点。
～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～
对投影空间中两平行线相交于一点的证明
在二维欧氏空间下的两条空间线方程联立即可得到交点，如下：
{Ax+By+C=0Ax+By+D=0\begin{cases} Ax+By+C=0\\ Ax+By+D=0 \end{cases} {Ax+By+C=0Ax+By+D=0
显然当 C≠DC\neq DC=D的情况下 ，欧氏空间下方程组无解。若用齐次坐标重写投影空间下的方程组如下：
很明显现在 CwCwCw 可以等于 DwDwDw 了，也就是当 w=0w=0w=0 （逆深度为0也就是无穷远）的时候有解 (x,y,0)(x, y, 0)(x,y,0)，这个解在投影几何上也称作为Vanish Point（灭点/消失点/消影点）。
～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～
齐次坐标在计算机图形学中是非常有用的基本概念，通过增加一个额外的维度W后，可以用来对几何体进行缩放,旋转,平移,透视投影的矩阵变换.
现在从空间点过渡到空间平面。由于3D射影空间下点和面的对偶性，我们同样用类似的齐次坐标形式来表示射影空间中的一个平面 (x,y,z,w)(x, y, z, w)(x,y,z,w)（仍然是3个自由度：平面上的二维位移+绕法向量的旋转），其中 n=(x,y,z)T\textbf{n} = (x, y, z)^Tn=(x,y,z)T 对应欧氏空间中的该平面法向量，对平面上任一点 P=(X,Y,Z)T\textbf{P}=(X, Y, Z)^TP=(X,Y,Z)T 有 nTP+w=0\textbf{n}^T\textbf{P}+ w =0nTP+w=0（也就是空间平面方程 Ax+By+Cz+D=0），其中 w/∣∣n∣∣w/||\textbf{n}||w/∣∣n∣∣ 为原点到平面的距离。
在本文后续也会以 PT=(P‾,p)T\mathbf{P}^T=(\mathbf{\overline{P}}, p)^TPT=(P,p)T 的齐次坐标形式来表示一个空间平面。
空间直线的优化
在SLAM问题的BA优化问题中，给定第 lll 条空间直线在第 iii 帧相机归一化平面下的观测为线段起点 slmi=(us,vs,1)T\mathbf{s}^{m_i}_l=(u_s,v_s,1)^Tslmi=(us,vs,1)T 和线段终点 elmi=(ue,ve,1)T\mathbf{e}^{m_i}_l=(u_e,v_e,1)^Telmi=(ue,ve,1)T ，根据当前直线参数得到的直线与相机原点构成平面的法向量为 nc\mathbf{n}_cnc，则重投影直线方程即为 llmi=(nc(0),nc(1),nc(2))T\mathbf{l}^{m_i}_l=(\mathbf{n}_c(0),\mathbf{n}_c(1),\mathbf{n}_c(2))^Tllmi=(nc(0),nc(1),nc(2))T 。
空间线的重投影误差定义为：
rLlci=1∣∣zLlmi∣∣(d(slmi,llmi)d(elmi,llmi))\mathbf{r}^{c_i}_{L_l}=\frac{1}{||\mathbf{z}^{m_i}_{L_l}||}\begin{pmatrix} d(\mathbf{s}^{m_i}_l,\mathbf{l}^{m_i}_l) \\ d(\mathbf{e}^{m_i}_l,\mathbf{l}^{m_i}_l)\end{pmatrix} rLlci=∣∣zLlmi∣∣1(d(slmi,llmi)d(elmi,llmi))其中 d(s,l)d(\mathbf{s},\mathbf{l})d(s,l) 为端点 sss 到投影直线 lll 的距离，如下：
d(s,l)=sTll12+l22d(\mathbf{s},\mathbf{l})=\frac{\mathbf{s}^T\mathbf{l}}{\sqrt{l^2_1+l^2_2}} d(s,l)=l12+l22sTl
误差量示意图如下：
2. 空间直线的表示方法
我们将描述多种投影空间中表示3D直线的方法及其特性。其中一些表示法在某些场景下是“片面的”，只能表示所有3D线的一个子集。例如，在尺度重建中要求重建的直线不能位于无穷远处。
研究空间直线的表示方法的意义在于选择一种合适的表示法来解决三角化和BA优化的问题。
- 对于三角化，最重要的准则是：重投影的直线是3D空间直线的线性函数。
- 对于BA优化，由于本质上是一个非线性过程，为直线的参数化提供了更灵活的选择空间。
- 参数化的质量评估，应取决于自由度和约束的数量。
2.1 空间直线的自由度
一条直线的空间自由度为 4，它既可以看作是两个空间点的连接(join)，也可以看作是两个空间平面的相交(intersection)，可以从不同角度去理解这个空间自由度为4:
- 直线自身角度：绕直线自身旋转和沿直线方向移动，直线不变，因此自由度为 6 - 2 = 4。
- 平面相交角度：两个空间平面的自由度为 3 x 2 = 6，两个平面以直线为轴旋转，直线不变，因此自由度为 (3-1) x 2 = 4。
- 两点连接角度：将直线看作是穿过两个正交平面的交点所连接起来的几何体，则两个交点在各自平面上只有2个自由度，因此直线自由度为 2 x 2 = 4，如图
2.2 普吕克坐标 (Plucker Coordinates Representation)
普吕克坐标是一种比较简单的对一条直线进行唯一确定的坐标表示方法，也是一种可以表示所有直线的完备表示方法，许多其他直线的表示方法也可以和普吕克坐标进行互换。
普吕克坐标最直观的理解就是用直线上两点的向量差（线的方向向量）和两点的向量叉乘（极平面的法向量） 这六个参数来表示一条直线，设普吕克坐标为 LT=(aT,bT)T\mathbf{L}^T=(\mathbf{a}^T, \mathbf{b}^T)^TLT=(aT,bT)T，其中 a\mathbf{a}a 和 b\mathbf{b}b 可分别对应下图中的法向量 m\mathbf{m}m 或 方向向量 d\mathbf{d}d，在不同论文中的表述不是非常统一，本文中令 a\mathbf{a}a 为法向量并用 n\mathbf{n}n（normal）表示，b\mathbf{b}b 为直线方向向量并用 v\mathbf{v}v（vector）来表示，即 LT=(nT,vT)T\mathbf{L}^T=(\mathbf{n}^T, \mathbf{v}^T)^TLT=(nT,vT)T。
直线的唯一确定原理
对一条空间直线的唯一确定表示来说，比较直观的三个要素是直线的方向 v\mathbf{v}v ，直线和极点构成平面的法向量 n\mathbf{n}n 和直线到原点的距离 ddd，其中：
- 用方向向量 v\mathbf{v}v 和 距离 ddd 来表示，如下图1，只能确定一个圆（为了方便直观理解，取一条沿着 ZZZ 轴方向的直线并用黄色标注）
- 用法向量 n\mathbf{n}n 和距离 ddd 来表示，如下图2，同样只能确定一个圆（注意与 v\mathbf{v}v & ddd 确定的圆区分）
- 用法向量 v\mathbf{v}v & n\mathbf{n}n & ddd 一起来表示，如下图3，可以唯一确定一条直线（虽然存在两个交点，但可以通过 v\mathbf{v}v 和 n\mathbf{n}n 的方向来区分，详见后续普吕克坐标的计算）
若用直线上两个不同的点 M\mathbf{M}M 和 N\mathbf{N}N 来计算直线的方向向量和法向量，即 v=N−M\mathbf{v}=\mathbf{N} - \mathbf{M}v=N−M 和 n=M×N\mathbf{n}=\mathbf{M} \times \mathbf{N}n=M×N，有 ∣∣n∣∣=∣∣M∣∣∣∣N∣∣sinθ||\mathbf{n}||=||\mathbf{M}||||\mathbf{N}||\sin\theta∣∣n∣∣=∣∣M∣∣∣∣N∣∣sinθ，即 nnn 的长度为两端点和原点构成的三角形的面积的2倍，vvv 的长度又为该三角形底边长，因此 d=∣∣n∣∣∣∣v∣∣d=\frac{||\mathbf{n}||}{||\mathbf{v}||}d=∣∣v∣∣∣∣n∣∣，即 ddd 的几何约束已经蕴含在了计算得到的方向向量 v\mathbf{v}v 和法向量 n\mathbf{n}n 中，由此实现了一条空间直线的唯一确定。
普吕克矩阵
普吕克矩阵广泛应用于直线的普吕克坐标的初始化中，定义为：
L∗=([v]×n−nT0)=X1X2T−X2X1T∈R4×4\mathbf{L}^*=\begin{pmatrix}[\mathbf{v}]_\times & \mathbf{n} \\ -\mathbf{n}^T &0\end{pmatrix} = \mathbf{X}_1\mathbf{X}_2^T -\mathbf{X}_2\mathbf{X}_1^T\in \mathbb{R}^{4\times4} L∗=([v]×−nTn0)=X1X2T−X2X1T∈R4×4其中，由于点和面的对偶性质，X1X_1X1 和 X2X_2X2 可以是线上两个端点 M、NM、NM、N 的齐次坐标，也可以是两视图平面 π1、π2\pi_1、\pi_2π1、π2 的齐次坐标，显然可以从普吕克矩阵中直接获得普吕克坐标。
普吕克矩阵具备特殊性质，它是一个对角线上元素皆为0的反对称矩阵，且行列式为0。若取同一空间直线上任意两对空间点，对应的普吕克坐标则只相差一个系数，即L1∗=αL2∗\mathbf{L}^*_1=\alpha\mathbf{L}^*_2L1∗=αL2∗。因为反对称矩阵中有6个非零元素，但因为齐次坐标的关系仅有5个是有意义的，且由于存在行列式为0的正交约束，因此可反映出直线的自由度为4。
基于两端点的普吕克坐标表示方法
给定2个3D点 MT=(M‾,m)T\mathbf{M}^T=(\overline{\mathbf{M}}, m)^TMT=(M,m)T 和 NT=(N‾,n)T\mathbf{N}^T=(\overline{\mathbf{N}}, n)^TNT=(N,n)T ，6维向量的普吕克坐标 LT=(nT,vT)T\mathbf{L}^T=(\mathbf{n}^T, \mathbf{v}^T)^TLT=(nT,vT)T 表示如下：
{n=M‾×N‾v=(N‾n−M‾m)⋅mn=mN‾−nM‾\begin{cases} \mathbf{n} = \overline{\mathbf{M}} \times \overline{\mathbf{N}} \\ \mathbf{v} = (\frac{\overline{\mathbf{N}}}{n} - \frac{\overline{\mathbf{M}}}{m}) \cdot mn =m\overline{\mathbf{N}}-n\overline{\mathbf{M}} \end{cases} {n=M×Nv=(nN−mM)⋅mn=mN−nM因此普吕克为6参数表示的5维齐次坐标系下的齐次表示，存在约束 nTv=0\mathbf{n}^T\mathbf{v}=0nTv=0，因此普吕克坐标表示的一条空间直线的自由度为5-1=4，符合直线自由度。
基于两平面的普吕克坐标表示方法
给定2个空间平面 PT=(P‾,p)T\mathbf{P}^T=(\overline{\mathbf{P}}, p)^TPT=(P,p)T 和 QT=(Q‾,q)T\mathbf{Q}^T=(\overline{\mathbf{Q}}, q)^TQT=(Q,q)T ，普吕克坐标如下：
{n=(Q‾q−P‾p)⋅pq=pQ‾−qP‾v=P‾×Q‾\begin{cases} \mathbf{n} = (\frac{\overline{\mathbf{Q}}}{q} - \frac{\overline{\mathbf{P}}}{p}) \cdot pq =p\overline{\mathbf{Q}}-q\overline{\mathbf{P}} \\ \mathbf{v} = \overline{\mathbf{P}} \times \overline{\mathbf{Q}} \\ \end{cases} {n=(qQ−pP)⋅pq=pQ−qPv=P×Q对于法向量 n\mathbf{n}n 的获取，假设两平面相交的直线上的一点 XXX，有{P‾X+p=0Q‾X+q=0→Q‾q⋅X=P‾p⋅X→(pQ‾−qP‾)⋅X=0\begin{matrix} \begin{cases} \mathbf{\overline{P}X}+p=0\\ \mathbf{\overline{Q}X}+q=0 \end{cases} & \to \frac{\overline{\mathbf{Q}}}{q}\cdot X = \frac{\overline{\mathbf{P}}}{p}\cdot X & \to (p\overline{\mathbf{Q}}-q\overline{\mathbf{P}})\cdot X = 0 \end{matrix} {PX+p=0QX+q=0→qQ⋅X=pP⋅X→(pQ−qP)⋅X=0可以看到两平面表示与两点表示的普吕克坐标表示方法呈现对偶形式。因此，两视图的直线三角化具备以下闭式解，给定 P1^11 和 P 2^22 两个投影矩阵，l1^11 和 l2^22 两条图像线，对应的3D空间线的普吕克坐标就可以表示为两个恢复的平面 πi∼\pi^i \simπi∼ PiT{^i}^TiTli^ii 的相交。
投影模型和Camera-World转换
给定一个标准 3×43\times 43×4 的透视投影矩阵 P=(P‾,p)P=(\overline{P},\mathbf{p})P=(P,p)，则可以在2D图像线和对应的3D空间线的普吕克坐标间建立关系式如下：limage=mimage×nimage=(PM)×(PN)=(P‾M‾+mp)×(P‾N‾+np)=(P‾M‾)×(P‾N‾)+mp×(P‾N‾)−np×(P‾M‾)+mp×np=det(P‾)P‾−T(M‾×N‾)+[p]×P‾(mN‾−nM‾)=[det(P‾)P‾−T,[p]×P‾][(M‾×N‾)(mN‾−nM‾]=P~L\begin{aligned} l_{image}&=m_{image}\times n_{image} \\ &= (P\mathbf{M})\times (P\mathbf{N}) \\ &=(\overline{P}~ \overline{\mathbf{M}}+m\mathbf{p})\times (\overline{P}~\overline{\mathbf{N}}+n\mathbf{p}) \\ &=(\overline{P}~\overline{\mathbf{M}})\times(\overline{P}~\overline{\mathbf{N}})+m\mathbf{p}\times(\overline{P}~\overline{\mathbf{N}})-n\mathbf{p}\times(\overline{P}~\overline{\mathbf{M}})~+m\mathbf{p}\times n\mathbf{p} \\ &=det(\overline{P})\overline{P}^{-T}(\overline{\mathbf{M}}\times \overline{\mathbf{N}})+[\mathbf{p}]_{\times}\overline{P}(m\overline{\mathbf{N}}-n\overline{{\mathbf{M}}}) \\ &=[det(\overline{P})\overline{P}^{-T},[\mathbf{p}]_{\times}\overline{P}]\begin{bmatrix} (\overline{\mathbf{M}}\times \overline{\mathbf{N}})\\ (m\overline{\mathbf{N}}-n\overline{{\mathbf{M}}}\end{bmatrix}\\ &=\tilde{P}\mathbf{L} \end{aligned} limage=mimage×nimage=(PM)×(PN)=(P M+mp)×(P N+np)=(P M)×(P N)+mp×(P N)−np×(P M) +mp×np=det(P)P−T(M×N)+[p]×P(mN−nM)=[det(P)P−T,[p]×P][(M×N)(mN−nM]=P~L
因此，普吕克坐标表示法中重投影图像线和3D空间线之间是线性转换关系。
limagel_{image}limage 为相机坐标系下的直线法向量，L\mathbf{L}L 为该直线在世界坐标系下的普吕克坐标。
根据上述推导可进一步的拓展出世界坐标系和相机坐标系之间的普吕克坐标的转换关系：[ncvc]=[Rcw[tcw]×Rcw0Rcw][nwvw]=[RwcTRwcT[tcw]×0RwcT][nwvw]\begin{bmatrix} \mathbf{n}_c \\ \mathbf{v}_c \end{bmatrix}=\begin{bmatrix} {\rm{\mathbf{R}}}_{cw} &[{\rm{\mathbf{t}}}_{cw}]_{\times}{\rm{\mathbf{R}}}_{cw} \\ \mathbf{0} & {\rm{\mathbf{R}}}_{cw} \end{bmatrix} \begin{bmatrix} \mathbf{n}_w \\ \mathbf{v}_w \end{bmatrix}=\begin{bmatrix} {\mathbf{R}}_{wc}^T & {\mathbf{R}}_{wc}^T[{\mathbf{t}}_{cw}]_{\times} \\ \mathbf{0} & \mathbf{R}^T_{wc}\end{bmatrix}\begin{bmatrix} \mathbf{n}_w \\ \mathbf{v}_w \end{bmatrix} [ncvc]=[Rcw0[tcw]×RcwRcw][nwvw]=[RwcT0RwcT[tcw]×RwcT][nwvw]其中法向量的转换，将 PPP 用 R,t\mathbf{R} ,\mathbf{t}R,t 替换即可，方向向量的转换直接用 R\mathbf{R}R 进行旋转。
2.3 其他的直线部分表示法
源于论文 Structure-From-Motion Using Lines: Representation, Triangulation and Bundle Adjustment 中提到的3种比较古旧的直线非完备表示法（不能表示所有直线），这里只做简单介绍。其实从最近的线SLAM工作也可以看出来，最常用的基本还是普吕克坐标表示（用于初始化 三角化）和正交参数表示（用于BA优化）。
最近点和方向
一条3D直线也可以用 1. 该直线离原点最近的点 QT=(Q‾T,1)T\mathbf{Q}^T=(\mathbf{\overline{Q}}^T,1)^TQT=(QT,1)T 和 2. 该直线的方向 Q∞T=(Q‾∞T,0)T\mathbf{Q}^T_{\infty}=(\mathbf{\overline{Q}}_\infty^T,0)^TQ∞T=(Q∞T,0)T （逆深度为0表示的即为方向向量）来表示，共计6个参数。显然该直线表示法不能表示那些位于无穷远处的直线（无最近点）。
- 与普吕克坐标的转换关系
- 重投影公式
显然为双线性函数。
两投影线
一条3D直线也可以用两次投影来表示，因为两视图的直线恢复具备唯一解。注意这里的“两投影”，指的是3D直线在对应正交平面的投影线。
已知一条3D直线可以用两个平面的相交来表示，指定这两个平面中， π1\pi_1π1 平行于 xxx 轴，π2\pi_2π2 平行于 yyy 轴，则可以用 π1\pi_1π1 和 π2\pi_2π2 的相交去表示所有非平行于 XYXYXY 平面（或者说垂直于 zzz 轴）的空间3D直线。
- 其中 π2\pi_2π2 与 y=0y=0y=0 平面的相交也就是该空间直线在 y=0y=0y=0 平面的投影线，用 x=az+bx=az+bx=az+b 表示。
- 同样 π1\pi_1π1 与 x=0x=0x=0 平面的相交也就是该空间直线在 x=0x=0x=0 平面的投影线，用 y=cz+dy=cz+dy=cz+d 表示。
如图所示
其中直线过 XYXYXY 平面的交点为 (b,d,0)(b,d,0)(b,d,0) ，方向向量为 (a,c,1)(a,c,1)(a,c,1)，结合两条投影线和 zzz，可将直线上任一点表示为 Q=(az+bcz+dz1)Q=\begin{pmatrix} az+b\\ cz+d \\z \\1 \end{pmatrix}Q=⎝⎜⎜⎛az+bcz+dz1⎠⎟⎟⎞，该表示法为4参数表达，但显而易见的存在奇点(singularities) : 无法表示平行于 z=0z=0z=0 平面的直线。即使选取不同的参考轴和参考平面，始终无法完备表示所有直线。
- 与普吕克坐标的转换关系
任意选取线上两点即可，如 z=0z=0z=0 和 z=1z=1z=1，即有 L=(d−bbc−daac1)L=\begin{pmatrix} d \\ -b \\ bc- da \\ a \\c \\1\end{pmatrix}L=⎝⎜⎜⎜⎜⎜⎜⎛d−bbc−daac1⎠⎟⎟⎟⎟⎟⎟⎞。 - 重投影公式
同样为双线性函数。
Denavit–Hartenberg 参数
也就是机械工程中常用的D-H参数表示，核心思想是设置世界坐标系为参考坐标系，将其 zzz 轴视作固定的一个机械关节，将空间直线视为另一个可动机械关节并对齐所在坐标系的 zzz 轴，并令所在坐标系的 xxx 轴为参考坐标系和直线所在坐标系的 zzz 轴的公共法线，由此就将原本 6 自由度的位姿变换缩减了 2 个自由度（沿轴平移和绕轴旋转），示意图如下所示：
其中包括两个角度、两个距离，这里我们将图中连接两个关节的s型连接称作连杆：
- α\alphaα : 连杆转角，直线所在坐标系的 zzz 轴与世界坐标系的 zzz 轴之间绕直线所在坐标系 xxx 轴的转角。
- θ\thetaθ：关节转角，直线所在坐标系的 xxx 轴与世界坐标系的 xxx 轴之间绕直线所在坐标系 zzz 轴的转角。
- ddd：连杆偏移，直线所在坐标系的 xxx 轴与世界坐标系的 xxx 轴之间沿直线所在坐标系 zzz 轴的位置偏移。
- aaa：连杆长度，直线所在坐标系的 zzz 轴与世界坐标系的 zzz 轴之间沿公共法线（直线所在坐标系 xxx 轴）的位置偏移。
这种表示法当然也存在着限制：a) 由于涉及到了两个距离参数，因此不能表示在无穷远处的直线；b) 同时，也不能表示垂直于参考坐标系的 zzz 轴的直线。
- 与普吕克坐标的转换关系
将该4参数表示的坐标系变换，作用到世界坐标系下 zzz 轴的普吕克坐标 LzT=(000001)TL_z^T=(0~0~ 0~ 0 ~0 ~1)^TLzT=(0 0 0 0 0 1)T 上。 - 重投影公式
其中包含乘积和三角函数，因此也是非线性的，不再赘述。
2.4 正交表示 (Orthonormal Representation)
普吕克坐标虽然能完备的表示所有空间直线，且重投影函数为线性，但本身的6个参数却不是线特征的最小表示（4参数），包含了内在约束 nTv=0n^Tv=0nTv=0。对3D空间直线的过参数化表示可能会带来以下问题：
- 多余的参数会增加每次迭代的计算代价
- 内在约束会带来优化过程中的数值不稳定性。
在这种情况下，正交表示可以说为非线性BA优化而生。用与空间直线自由度匹配的最小4个参数来表示，给优化过程带来了便利。因此，在现在众多基于贺博的PL-VIO框架的经典工作中，正交表示法成为了优化过程中残差计算的普遍选择。
正交表示推导
假设一条3D直线的普吕克坐标为 LT=(nT,vT)T\mathbf{L}^T=(\mathbf{n}^T,\mathbf{v}^T)^TLT=(nT,vT)T，写作矩阵形式为 C3×2=(n,v)\mathbf{C}_{3\times2}=(\mathbf{n},\mathbf{v})C3×2=(n,v)，将矩阵 C\mathbf{C}C 进行QR分解后重新表示为 C3×2=(U3×3,Σ3×2)\mathbf{C}_{3\times2}=(\mathbf{U}_{3\times3},\mathbf{\Sigma}_{3\times2})C3×2=(U3×3,Σ3×2)，即：
C=(n∣∣n∣∣v∣∣v∣∣n×v∣∣n×v∣∣)⏟SO(3)(∣∣n∣∣00∣∣v∣∣00)⏟(∣∣n∣∣,∣∣v∣∣)T∈P1\begin{matrix}\mathbf{C}=\underbrace{\begin{pmatrix}\frac{\mathbf{n}}{||\mathbf{n}||} ~ \frac{\mathbf{v}}{||\mathbf{v}||} ~ \frac{\mathbf{n}\times \mathbf{v}}{||\mathbf{n}\times\mathbf{v}||} \end{pmatrix}}\\ ~~~~~~~~\scriptsize{SO(3)}\end{matrix} \begin{matrix}\underbrace{\begin{pmatrix}||\mathbf{n}|| & 0 \\ 0 & ||\mathbf{v}|| \\ 0 & 0\end{pmatrix}}\\\scriptsize{(||\mathbf{n}||,||\mathbf{v}||)^T\in \mathbb{P}^1}\end{matrix} C=(∣∣n∣∣n ∣∣v∣∣v ∣∣n×v∣∣n×v) SO(3)⎝⎛∣∣n∣∣000∣∣v∣∣0⎠⎞(∣∣n∣∣,∣∣v∣∣)T∈P1其中，
- U3×3\mathbf{U}_{3\times3}U3×3 为第一部分的正交旋转矩阵，自由度为3。
- Σ3×2\mathbf{\Sigma}_{3\times2}Σ3×2 为第二部分的上三角矩阵，由两个标量组成，其中 (∣∣n∣∣,∣∣v∣∣)(||\mathbf{n}||,||\mathbf{v}||)(∣∣n∣∣,∣∣v∣∣) 为一维齐次坐标表示，自由度为1。
- Σ3×2\mathbf{\Sigma}_{3\times2}Σ3×2 中第一行第二列元素（右上角）必定为0，因为普吕克坐标存在约束 nTv=0\mathbf{n}^T\mathbf{v}=0nTv=0。
- Σ3×2\mathbf{\Sigma}_{3\times2}Σ3×2 中同时除以 ∣∣v∣∣||v||∣∣v∣∣ 可得 d=∣∣n∣∣∣∣v∣∣d=\frac{||\mathbf{n}||}{||\mathbf{v}||}d=∣∣v∣∣∣∣n∣∣，因此其中包含了直线到原点的距离信息。
为了方便进行优化和更新，将 Σ\mathbf{\Sigma}Σ 进一步以 (2×2)(2\times2)(2×2) 的 SO(2)SO(2)SO(2)来表示如下：W=11+d2[d−11d]=1∣∣n∣∣+∣∣v∣∣[∣∣n∣∣−∣∣v∣∣∣∣v∣∣∣∣n∣∣]=[cos(ϕ)−sin(ϕ)sin(ϕ)cos(ϕ)]=[w1−w2w2w1]∈SO(2)\mathbf{W}=\frac{1}{\sqrt{1+d^2}}\begin{bmatrix} d & -1\\1 & d\end{bmatrix}=\frac{1}{\sqrt{||\mathbf{n}||+||\mathbf{v}||}}\begin{bmatrix} ||\mathbf{n}|| & -||\mathbf{v}||\\||\mathbf{v}|| & ||\mathbf{n}||\end{bmatrix}=\begin{bmatrix} \cos(\phi) & -\sin(\phi)\\\sin(\phi) & \cos(\phi)\end{bmatrix}=\begin{bmatrix}w1 & -w2 \\ w2 & w1\end{bmatrix} \in SO(2) W=1+d21[d1−1d]=∣∣n∣∣+∣∣v∣∣1[∣∣n∣∣∣∣v∣∣−∣∣v∣∣∣∣n∣∣]=[cos(ϕ)sin(ϕ)−sin(ϕ)cos(ϕ)]=[w1w2−w2w1]∈SO(2)
综上所述，直线的正交表示可以写作：
C=(U,W)∈SO(3)×SO(2)\mathbf{C}=(\mathbf{U},\mathbf{W})\in SO(3)\times SO(2)C=(U,W)∈SO(3)×SO(2)
- 与普吕克坐标的转换关系
正交表示可以最小的表示直线，但是该表示方法的投影方程并没有线性的表示，即该方法并不能像Plucker坐标系表示方法一样，通过公式[ncvc]=[Rcw[tcw]×Rcw0Rcw][nwvw]=[RwcTRwcT[tcw]×0RwcT][nwvw]\begin{bmatrix} \mathbf{n}_c \\ \mathbf{v}_c \end{bmatrix}=\begin{bmatrix} {\rm{\mathbf{R}}}_{cw} &[{\rm{\mathbf{t}}}_{cw}]_{\times}{\rm{\mathbf{R}}}_{cw} \\ \mathbf{0} & {\rm{\mathbf{R}}}_{cw} \end{bmatrix} \begin{bmatrix} \mathbf{n}_w \\ \mathbf{v}_w \end{bmatrix}=\begin{bmatrix} {\mathbf{R}}_{wc}^T & {\mathbf{R}}_{wc}^T[{\mathbf{t}}_{cw}]_{\times} \\ \mathbf{0} & \mathbf{R}^T_{wc}\end{bmatrix}\begin{bmatrix} \mathbf{n}_w \\ \mathbf{v}_w \end{bmatrix} [ncvc]=[Rcw0[tcw]×RcwRcw][nwvw]=[RwcT0RwcT[tcw]×RwcT][nwvw]得到一个线性的映射关系，但是我们又特别的看重该表示方法的最小表示的性质，好在两者之间的互换关系还是十分简单的，如下
（引用于 SLAM线特征学习（1）——基本的线特征表示与优化推导）
在SLAM中，世界坐标系和相机坐标系的线特征转换是非常频繁且重要的。因此在很多经典框架中，一般在直线的初始化和线特征容器中使用普吕克坐标表示，在进行非线性BA优化的时候，求取观测残差最终对正交表示的4个参数 (θ,ϕ)(\bm{\theta},\phi)(θ,ϕ) 的Jacobian矩阵并进行参数的优化更新：U←U⋅R(Δθ)\mathbf{U}\leftarrow \mathbf{U} \cdot \rm{R}(\Delta\bm{\theta})U←U⋅R(Δθ) ，W←W⋅R(Δϕ)\mathbf{W}\leftarrow \mathbf{W} \cdot \rm{R}(\Delta\phi)W←W⋅R(Δϕ)，最后对相应3D空间直线的普吕克坐标进行更新。
详细的线段观测的重投影误差对相机姿态、相机平移和直线参数的Jacobian 推导仍然可见 SLAM线特征学习（1）——基本的线特征表示与优化推导，这里不再赘述。
3. 结构信息约束下的线特征表示 —— 2-DoF表示
在实际应用中，人工环境由于存在大量规则的、结构化的线条，线特征不是无意义的分布在空间中，而是呈现一定的规律：如沿着固定的主方向 (dominant direction，常规的方正建筑环境下为3个互相垂直的方向），相互垂直或平行等，因此在对应的Manhattan世界假设或者Atlanda世界假设下，3D空间直线可以存在更简练的表示方法，这就是一脉相承的 2-DoF 线表示法。
3.1 StructSLAM : Visual SLAM with Building Structure
邹丹平老师组的经典工作，提出了一种新颖的在Manhattan世界假设下的结构线表示方法。
结构线参数化表示方法
原文翻译
- (ca,cb)T(c_a,~c_b)^T(ca, cb)T 为相机光心 OOO 沿着主方向 η\etaη 在参数化平面上的投影点 O′O'O′的坐标。
- θ\thetaθ 为参数平面交点 AAA 到 O′O'O′的 射线在参数平面上的角度。
- hhh 为参数平面交点 AAA 到 O′O'O′的逆深度。
虽然对结构线采用了4个参数的表示方法，但只要确定了对应的参数平面，本质上就只取决于点 AAA 的位置，即只具备其对应的参数平面上的2个自由度。世界坐标系下的线特征的空间表示很明显不应受相机位姿的影响，因此这种表示方法只能建立在相机位姿和正确识别对应参数平面的先验信息上。
结构线初始化方法
原文翻译
- 第一步，计算相机中心沿着该条直线所属的主方向在参数平面的投影，即 (ca,cb)(c_a, c_b)(ca,cb)。
- 第二步，获取相机中心投影到直线在参数平面交点的方向，即 (cosθ,sinθ)(\cos\theta,\sin\theta)(cosθ,sinθ)。
- 结构线的状态初始化为 l=(ca,cb,θ,h)T\mathbf{l}=(c_a, c_b, \theta, h)^Tl=(ca,cb,θ,h)T，对于 hhh 先设置一个初始值，等待后续优化更新。
详细的初始化过程为：
- 假设线段中点的 3×13\times13×1 的齐次坐标为 m~\tilde{m}m~，则世界坐标系下的3D坐标计算如下：m=RwcK−1m~+pw\mathbf{m}=\mathbf{R}^{wc}\mathbf{K}^{-1}\tilde{\mathbf{m}}+\mathbf{p}^wm=RwcK−1m~+pw则该直线在世界坐标系下的普吕克矩阵为：L∗=mηT−ηmT\mathbf{L}^*=\mathbf{m}\eta^T-\eta\mathbf{m}^TL∗=mηT−ηmT其中 η∈R3×1\eta\in\mathbb{R}^{3\times1}η∈R3×1 为对应的主方向。
( 基于点的普吕克矩阵计算，只不过其中一个点的齐次坐标逆深度为0，即本质为一条射线 )- 计算该结构线与对应参数平面 π\piπ的交点 AAA 的坐标如下：l~w=L∗π\tilde{\mathbf{l}}^w=\mathbf{L}^*\pil~w=L∗π参数平面的2D坐标计算为：lp=Plw\mathbf{l}^p=\mathbf{P}\mathbf{l}^wlp=Plw其中 lw\mathbf{l}^wlw 为齐次坐标形式的 l~w\tilde{\mathbf{l}}^wl~w 对应的3D坐标, P∈R2×3\mathbf{P}\in\mathbb{R}^{2\times3}P∈R2×3 为从世界坐标系到2D参数平面的投影转换矩阵。
同样的方法获取相机中心沿着主方向在参数平面的投影 O′O^\primeO′。
o~w=(cηT−ηcT)π\tilde{\mathbf{o}}^w= (\mathbf{c}\eta^T-\eta\mathbf{c}^T)\pio~w=(cηT−ηcT)π则2D投影坐标为：op=Pow\mathbf{o}^p=\mathbf{P}\mathbf{o}^wop=Pow- 初始化结构线为：l=[ca,cb,θ,h]T=[op(1),op(2),arctan(lp(2)−op(2)lp(1)−op(1)),h0]T\mathbf{l}=[c_a,c_b,\theta,h]^T=[\mathbf{o}^p(1),\mathbf{o}^p(2),\arctan(\frac{\mathbf{l}^p(2)-\mathbf{o}^p(2)}{\mathbf{l}^p(1)-\mathbf{o}^p(1)}),h_0]^Tl=[ca,cb,θ,h]T=[op(1),op(2),arctan(lp(1)−op(1)lp(2)−op(2)),h0]T，其中 h0h_0h0 为预设的逆深度初始值。
结构线重投影方法
原文翻译
根据参数平面的投影关系，得到线段中点在世界坐标系下的坐标为lw=PT([ca,cb]T+[cosθ,sinθT]/h)\mathbf{l}^w=\mathbf{P}^T([c_a,c_b]^T+[\cos\theta,\sin\theta^T]/h)lw=PT([ca,cb]T+[cosθ,sinθT]/h)两边同时乘上 hhh 以应对无限远的情况，有
lwh=PT([ca,cb]Th+[cosθ,sinθT])\mathbf{l}^wh=\mathbf{P}^T([c_a,c_b]^Th+[\cos\theta,\sin\theta^T])lwh=PT([ca,cb]Th+[cosθ,sinθT])其中P∈R2×3\mathbf{P}\in\mathbb{R}^{2\times3}P∈R2×3，减去相机中心的世界坐标获得相机坐标系的空间齐次坐标lc=Rcwlwh−Rcwpwh\mathbf{l}^c=\mathbf{R}^{cw}\mathbf{l}^wh-\mathbf{R}^{cw}\mathbf{p}^whlc=Rcwlwh−Rcwpwh和像素坐标li=Klc\mathbf{l}^i=\mathbf{K}\mathbf{l}^cli=Klc 则结构线的投影方程(法向量）即为l‾=v×li\overline{\mathbf{l}}=\mathbf{v}\times\mathbf{l}^il=v×li其中 vvv 为灭点的齐次坐标。
3.2 StructVIO : Visual-inertial Odometry with Structural Regularity of Man-made Environments
邹丹平老师组在结构线SLAM的进一步工作，这次不仅仅是Manhattan Wold假设了，而是更进一步的Atlanta World假设（有着不同水平方向 ϕi∈[0,π/2]\phi_i\in[0,\pi/2]ϕi∈[0,π/2]的Manhattan世界的集合），更为普适通用，如下：
在结构线的一系列处理上和StructSLAM相比也有区别，详见后文。
结构线参数化表示方法
注意红色标注部分与StructSLAM的一致性。
原文翻译(1)
- 每条结构线都会被锚定在第一次被观测到的图像帧下的局部坐标系上，称作起始帧 {S}\{S\}{S}，该局部坐标系的方向与该结构线所属的Manhattan世界的方向 ϕi\phi_iϕi一致，原点即为该图像帧的相机位置 WpS^Wp_SWpS，且 WpS^Wp_SWpS 将加入状态向量进行更新维护。这里将设置一个虚拟的Manhattan世界 ϕ0\phi_0ϕ0，与世界坐标系 WWW 的方向一致，则 ϕi\phi_iϕi 即为相对于世界坐标系的水平方向偏差。
- 对每条结构线都可以找到一个从参数空间 {L}\{L\}{L}到起始帧{S}\{S\}{S}的旋转矩阵 LSR^S_LRLSR，其中，结构线会与{L}\{L\}{L}的 ZZZ 轴对齐，如下图所示：
在参数空间中，每条结构线都可以简单的用其与XYXYXY平面的交点（显然仍为2自由度）来表示，即为 Llp=(a,b,0)T^Ll_p=(a,b,0)^TLlp=(a,b,0)T。这里我们将其转化为逆深度表示法，即为 Llp=(θ,ρ,0)T^Ll_p=(\theta,\rho,0)^TLlp=(θ,ρ,0)T，其中 ρ=1/a2+b2\rho=1/\sqrt{a^2+b^2}ρ=1/a2+b2，θ=arctan(b,a)\theta=\arctan(b,a)θ=arctan(b,a)。逆深度表示法的优点已经多次阐述：1. 可以描述无穷远的特征。2.减少特征初始化的非线性。- 因此，该结构线在起始帧的坐标可以计算如下LSRLlp=aLSR(:,1)+bLSR(:,2)+0⋅LSR(:,2)=cosθρLSR(:,1)+sinθρLSR(:,2)\begin{aligned}^S_LR{^Ll_p}&=a^S_LR(:,1)+b^S_LR(:,2)+0\cdot^S_LR(:,2)\\&=\frac{\cos\theta}{\rho}{^S_LR(:,1)+\frac{\sin\theta}{\rho}}{^S_LR(:,2)}\end{aligned}LSRLlp=aLSR(:,1)+bLSR(:,2)+0⋅LSR(:,2)=ρcosθLSR(:,1)+ρsinθLSR(:,2)无论该结构线对齐到当前局部坐标系的任意一轴，都可以用下列三个矩阵来表示 LSR^S_LRLSR
这里作一下解释，这里参数平面的选取本质上和StructSLAM是一样的，只不过统一使结构线对齐到参数空间 L{L}L 的ZZZ 轴，同时对参数空间到局部Manhattan坐标系的转换 LSR^S_LRLSR 做了具体的直观展示，从而将三个空间 {L}\{L\}{L}、{S}\{S\}{S}、{C}\{C\}{C}联系在一起（其中起始帧 SSS 所在的局部Manhattan坐标系，其实和世界坐标系只差一个方向 ϕi\phi_iϕi 的区别），论文里的这张图就个人感觉来说不是那么直观，我重新画的示意图如下：
没有把相机坐标系画进去是避免产生误解，且更便于直观理解{L}\{L\}{L} 到 {S}\{S\}{S}的旋转关系，建议这里直接把 {S}\{S\}{S} 理解成真实世界的坐标指向，则结构线可能属于 XYZXYZXYZ 三个主方向中的其中一个，将对应的参数平面贴上去即可。
原文翻译(2)
- 从起始坐标系 {S}\{S\}{S} 到 世界坐标系的旋转矩阵为 SWR(ϕi)^W_SR(\phi_i)SWR(ϕi)，只与绕重力方向的水平偏角 ϕi\phi_iϕi有关，因此表示如下
对于竖直方向的结构线，统一令其起始帧坐标系与世界坐标系的方向一致，也就是 SWR==SWR(ϕ0)=I3×3^W_SR=={^W_SR(\phi_0)}=I_{3\times3}SWR==SWR(ϕ0)=I3×3。
结构线重投影方法
原文翻译(1)
为了获取结构线在图像上的投影，需要同时将交点 Llp^LlpLlp 和 参数平面的 zzz 轴方向（其实就是获取灭点）投影到图像平面上。其中Llp^LlpLlp 在世界坐标系下的坐标计算如下：Wlp=SWR(ϕi)LSRLlp+WpS^Wl_p={^W_SR(\phi_i){^S_LR{^Ll_p+{^Wp_S}}}}Wlp=SWR(ϕi)LSRLlp+WpS进一步转换到相机坐标系下 Clp=WCRWlp+CpW^Cl_p={^C_WR{^Wl_p}}+{^Cp_W}Clp=WCRWlp+CpW将Llp^LlpLlp 用逆深度表示法替换，则可得到Clp∼WCRSWR(ϕi)LSR⋅r+(WCRWpS+CpW)⋅ρ^Cl_p\sim{^C_WR}{^W_SR(\phi_i)}{^S_LR\cdot r+({^C_WR}{^Wp_S}+{^Cp_W})\cdot\rho}Clp∼WCRSWR(ϕi)LSR⋅r+(WCRWpS+CpW)⋅ρ其中 r=[cosθ,sinθ,0]Tr=[\cos\theta,\sin\theta,0]^Tr=[cosθ,sinθ,0]T，参数空间的 ZZZ 轴方向投影出的灭点的齐次坐标也可计算如下Cv∼WCRSWR(ϕi)LSR(:,3){^Cv\sim{^C_WR{^W_SR(\phi_i){^S_LR(:,3)}}}}Cv∼WCRSWR(ϕi)LSR(:,3)考虑内参矩阵，则可以得到图像中的直线方程（法向量）如下：iml=(K−T)(Clp×Cv){^{im}l}=(K^{-T})({^Cl_p}\times{^Cv})iml=(K−T)(Clp×Cv)
这里其实可以看到与StructSLAM对结构线参数化表示的本质一致性，都是用角度 θ\thetaθ 和 逆深度 ρ\rhoρ 对结构线与参数平面交点的重新表示，将灭点和参数平面交点重投影回图像平面，叉乘获取直线方程。
结构线初始化方法
原文翻译
初始化一条新的结构线l=(θ,ρ)l=(\theta,\rho)l=(θ,ρ)的关键在于找到角度参数 θ\thetaθ，逆深度 ρ\rhoρ 可以先取预设的一个值。
- 给结构线建立一个起始坐标系。竖直线都令其起始坐标系与世界坐标系对齐；对于水平方向的结构线，令其起始坐标系为偏角为 ϕi\phi_iϕi 的局部manhattan坐标系。
- 角度参数 θ\thetaθ 取决于在XYXYXY平面上的相机中心到结构线的方向。这个方向可以近似为从相机中心到线段中点的射线方向。让 mmm (齐次坐标形式）为观测线段 sss 的中点，投影到参数平面：Lm=SLRWSR(ϕi)CWRK−1m^Lm={^L_SR}{^S_WR(\phi_i)}{^W_CR}K^{-1}mLm=SLRWSR(ϕi)CWRK−1m角度 θ\thetaθ 计算为θ0=arctan(my,mx)\theta_0=\arctan(m_y, m_x)θ0=arctan(my,mx)，其中 Lm=(mx,my,mz)T^Lm=(m_x,m_y,m_z)^TLm=(mx,my,mz)T，将这个过程概括为:l0=(θ0ρ0)=(Π−1(s,ϕi,CWR)ρ0)l_0=\begin{pmatrix}\theta_0 \\ \rho_0\end{pmatrix}=\begin{pmatrix}\Pi^{-1}(s,\phi_i,{^W_CR})\\\rho_0 \end{pmatrix}l0=(θ0ρ0)=(Π−1(s,ϕi,CWR)ρ0)
与StructSLAM相比，每一条结构线都被锚定在了第一次观测的相机帧上，并且和该帧相机的位姿建立了联系，参数平面、起始坐标系、相机坐标系的原点都为起始帧的相机位置，相互之间只需要进行旋转变换即可，不需要将相机中心再向参数平面投影了，进一步简化和规范化了参数化表示的流程。
当然本质基本不变： 在结构信息和相机位姿的约束下，每条结构直线可以只用2个自由度的参数平面来进行表示。
3.3 Leveraging Structural Information to Improve Point Line Visual-Inertial Odometry
对结构线和非结构性都能用两个参数进行表示，思路和StructSLAM、StructVIO相似，作者有一个开源的4参数和2参数线优化的仿真代码，在速度精度上确实有提升，代码解析可参考上一篇博客 PLS-VIO代码解读，
非结构线参数化表示方法
一条空间直线可以表示为普吕克坐标： L=[cnT,cvT]T\mathcal{L}=[^c\mathbf{n}^T, {^c\mathbf{v}}^T]^TL=[cnT,cvT]T，因直线位于相机与直线构成的平面 π\piπ 上，因此基于相同的思路将平面 π\piπ 视为参数平面，建立一个局部坐标系 {P}\{P\}{P}：
- 坐标系原点设置为3D空间线段的端点 s′s^\primes′
- yyy轴方向与相机中心到端点 sss 的射线对齐，zzz轴方向与 π\piπ 的正交向量 cn^c\mathbf{n}cn 平行，xxx轴方向与 yyy 轴 和 zzz 轴垂直。
从相机中心 OOO 到端点 s′s^\primes′ 的距离为 ddd，用逆深度表示为 ρ=1/d\rho=1/dρ=1/d，并且令 θ\thetaθ 为局部坐标系下的直线方向 cv′^c\mathbf{v}^\primecv′ 与 xxx 轴之间的偏角，其中 cv′=RCPcv^c\mathbf{v}^\prime=\mathbf{R}^P_C{^c\mathbf{v}}cv′=RCPcv。因此，在局部参数坐标系{P}\{P\}{P}下就将一条非结构线也用两个参数进行了表示，在线特征的优化过程中可以减少线段参数数量，是一种更紧凑的表达方法。
非结构线初始化方法
原文翻译
在归一化相机平面的线段观测可以用两个端点坐标表示： sc1=[us,vs,1]T\mathbf{s}^{c_1}=[u_s,v_s,1]^Tsc1=[us,vs,1]T 和 ec1=[ue,ve,1]T\mathbf{e}^{c_1}=[u_e,v_e,1]^Tec1=[ue,ve,1]T，三点确定一个平面，加上相机中心 OOO 即可确定平面 π=[πx,πy,πz,πw]\bm{\pi}=[\pi_x,\pi_y,\pi_z,\pi_w]π=[πx,πy,πz,πw]，给定相机坐标系 c1c_1c1 下的两个平面 π1\bm{\pi}_1π1 和 π2\bm{\pi}_2π2，则可获取普吕克矩阵 cL∗^c\mathcal{L}^*cL∗如下:
从cL∗^c\mathcal{L}^*cL∗中可获取相应的c1c_1c1下的普吕克坐标 cL^c\mathcal{L}cL，然后将 cv^c\mathbf{v}cv 从相机坐标系旋转到局部坐标系{P}\{P\}{P}下：cv′=RCPcv^c\mathbf{v}^\prime=R^P_C{^c\mathbf{v}}cv′=RCPcv其中 RCPR^P_CRCP 的列显然由局部坐标系{P}\{P\}{P}的 x,y,zx,y,zx,y,z轴（也就是这三个轴方向在c1c_1c1下的坐标）构成。因此我们可以初始化参数 θ\thetaθ 为 xxx 轴与cv′^c\mathbf{v}^\primecv′之间的角度，其中 ρ\rhoρ 可以预设为一个默认值 ρ0=0.2\rho_0=0.2ρ0=0.2。
非结构线重投影方法
原文翻译
局部参数坐标系下的直线方向向量（注意与结构线区分）计算如下：dv=[cosθ,sinθ,0]T^d\mathbf{v}=[\cos\theta,\sin\theta,0]^Tdv=[cosθ,sinθ,0]T，因此我们可以将 dv^d\mathbf{v}dv 从参数坐标系转换到相机坐标系来获取 cv^c\mathbf{v}cv：cv=RPCdv^c\mathbf{v}=\mathbf{R}^C_P{^d\mathbf{v}}cv=RPCdv其中一个端点 s\mathbf{s}s在相机坐标系下的坐标计算为：s=y∣∣y∣∣⋅d\mathbf{s}=\frac{\mathbf{y}}{\mathbf{||y||}}\cdot ds=∣∣y∣∣y⋅d其中 y\mathbf{y}y 为局部坐标系的 yyy 轴。
为了获得在归一化相机坐标系下的直线投影，需要将起始相机坐标系下的端点 s\mathbf{s}s 和 方向向量 cv^c\mathbf{v}cv 转换到目标坐标系下：
其中 (RCiW,PCiW)(\mathbf{R}^W_{C_i},\mathbf{P}^W_{C_i})(RCiW,PCiW) 为第一次观测到该直线的相机关键帧。则直线投影方程可以直接获得如下:llmi=[s′]×cv′\mathbf{l}^{m_i}_l=[\mathbf{s}^\prime]_{\times}{^c\mathbf{v}^\prime}llmi=[s′]×cv′
结构线参数化表示方法
与StructVIO完全一致，用 lpl=[θ,ρ,0]T\mathbf{l}^l_p=[\theta,\rho,0]^Tlpl=[θ,ρ,0]T 来表示。
结构线初始化方法
原文翻译
结构线的初始化同样需要首先计算普吕克坐标cL=[cnT,cvT]T^c\mathcal{L}=[^c\mathbf{n}^T,^c\mathbf{v}^T]^TcL=[cnT,cvT]T，然后我们通过line triming方法(结合相机位姿)来获得该直线在世界坐标系下的端点坐标 wL=[swT,ewT]T^w\mathcal{L}=[\mathbf{s}^{wT},\mathbf{e}^{wT}]^TwL=[swT,ewT]T。为了获取直线wL^w\mathcal{L}wL与 XYXYXY 平面在世界坐标系下的交点，将齐次坐标表示的参数平面 lp^l\mathbf{p}lp 转移到世界坐标系下：
结合 wL^w\mathcal{L}wL 和 wp^w\mathbf{p}wp即可获得在世界坐标系下的交点 wlp^w\mathbf{l}_pwlp，反过来重新转换到参数空间后即可得到参数空间下的交点llp^l\mathbf{l}_pllp：
根据参数空间的交点坐标 [llpx,llpx,0][^ll{px},^ll{px},0][llpx,llpx,0] 即可实现对 θ\thetaθ 和 ρ\rhoρ的初始化。
这里对结构线初始化的操作和StructSLAM是一样的，都是在世界系下求出直线和参数平面的交点坐标，再转换回参数空间，而StructVIO则是直接投影空间直线的中点回到参数平面上，本质上是一样的。
结构线重投影方法
与StructVIO一致，不再赘述。
SLAM中线特征的参数化表示方法/重投影/初始化方法相关推荐
- SLAM专题（10）- 最小化重投影误差与Bundle Adjustment （BA）
在SFM(structure from motion)的计算中BA(Bundle Adjustment)作为最后一步优化具有很重要的作用,在近几年兴起的基于图的SLAM(simultaneous lo ...
- MySql 数据库 - 重置数据库、重置初始密码方法，数据库初始化方法，长时间不用忘记密码暴力解决方法
很长时间不用了,突然想用 mysql 了,这个时候发现密码忘记了. 确保你的数据库里没有重要的数据了,我们可以把数据库重置一下. 数据库重置方法如下: 步骤一: 删掉 data 文件夹,这个文件夹的具 ...
- 【光线追踪】光线追踪重投影方法(Ray Tracing Reprojection)
光线追踪重投影方法 重投影这项技术一般用于时间性帧复用技术上,例如TAA(Temporal Anti-Aliasing)反走样或者抗锯齿技术.光线追踪使用时间性帧复用技术一直觉得是个难题,比光栅渲染的 ...
- JVM学习笔记之-运行时数据区概述及线程概述，程序计数器（PC寄存器），虚拟机栈(栈,局部变量表,操作数栈,动态连接,方法调用,方法返回地址等),本地方法接口,本地方法栈
运行时数据区概述及线程概述 内存是非常重要的系统资源,是硬盘和CPU的中间仓库及桥梁,承载着操作系统和应用程序的实时运行.JVM内存布局规定了Java在运行过程中内存申请.分配.管理的策略,保证了JV ...
- OpenLayers中文文档2栅格重投影
目录 栅格重投影 使用方法 Usage 实例 Examples 自定义 Custom projection 视图投影的改变 瓦片网格和 外延 它是如何工作的 动态三角测量 高级Advanced 三角测 ...
- 深度学习的Xavier初始化方法
在tensorflow中,有一个初始化函数:tf.contrib.layers.variance_scaling_initializer.Tensorflow 官网的介绍为: variance_sca ...
- OC高效率52之提供“全能初始化”方法
2019独角兽企业重金招聘Python工程师标准>>> #import <Foundation/Foundation.h>@interface EOCRectangle ...
- java 项目启动初始化_Spring项目启动时执行初始化方法
一.applicationContext.xml配置bean init-method="initKeyWord"> classpath:sensitive-word.xml ...
- 视差图（Disparity）三维重投影得到特征点的三维空间坐标的2种方法
文章目录 前提 方法1-perspectiveTransform() 方法2-reprojectimageto3d() PS 参考 前提 1.使用BM或者SGBM算法计算得到了双目图像的合适左右视差图 ...
最新文章
- SQL语句将某字段查询出以逗号隔开
- Mediawiki随笔
- 腾讯在信息流内容理解技术上的解决方案
- php怎么删除表数据,php怎样删除数据表中的数据_后端开发
- 清华团队研发，首款国产电力电子仿真软件来啦~已捐赠哈工大、海工大、清华使用！...
- Mirantis OpenStack fuel web 安装 使用
- 微软企业应用开发三大方向：跨平台、开放/开源与DevOps
- FLUSH TABLES WITH READ LOCK 和 LOCK TABLES比较
- 支付宝辟谣交易 5 万受监控；App Store 宕机；谷歌抛弃 AI | 极客头条
- JavaScript中JSON的处理心得
- 苹果/安桌点歌系统代码
- IIS6/IIS7无法打开IIS管理器的解决方法。
- 临沂最美乡村医生彭玉梅：救人遇车祸身亡
- duration java_Java Duration类| 带示例的multipliedBy（）方法
- 三周爆赚千万 电竞选手在无聊猿游戏赢麻了
- MySQL 8.0如何配置my.cnf
- 可视化工具不知道怎么选？深度评测5大Python数据可视化工具
- Python虚拟环境之pyenv
- 基于多目标算法的冷热电联供型综合能源系统运行优化 多目标粒子群 冷热电联供 综合能源系统 运行优化
- [2011-3-20]开篇点题