精华内容
下载资源
问答
  • Reference Plucker Coordinates for Lines in the Space. ...https://zhuanlan.zhihu.com/p/65674067 关于线特征很好的解读.   直线的自由度 3D空间中的直线有4个自由度,通常认为一条线由两个点或者面

    Reference

    1. Plucker Coordinates for Lines in the Space.
    2. Structure-From-Motion Using Lines: Representation, Triangulation and Bundle Adjustment. 本文深度参考这篇论文
    3. 多视图几何.
    4. https://zhuanlan.zhihu.com/p/65674067 关于线特征很好的解读.

     


    直线的自由度

    3D空间中的直线有4个自由度,通常认为一条线由两个点或者面组成,所以自由度有2x3=6个,但是由于直线绕着自身的方向轴旋转和移动的话,直线还是这个直线,所以真实自由度为6-2=4个。

     


    Plucker坐标系

    Plucker坐标系是一种比较重要的表示方法,后面很多的表示方法也能和该方法进行互换

    Plucker坐标系使用两个向量表示, L : = ( l ^ , m ) L:=(\hat{l},m) L:=(l^,m),其中:

    • l ^ \hat{l} l^表示直线的方向向量,模长为1;
    • m = l × p m=l\times p m=l×p, p p p为直线上的一点,可以看到这部分也表示线与原点组成平面的法线;
    • ∥ m ∥ ∥ l ∥ \frac{\|m\|}{\|l\|} lm表示原点到直线的距离;

    所以,这个直线的表示形式基本上是方向向量和法向量的表示方法,如下图:

    在这里插入图片描述

    另一点, m m m表示了原点到直线的距离,公式推导如下:
    p ⊥ = p − ( l ^ ⋅ p ) l ^ = ( l ^ ⋅ l ^ ) p − ( l ^ ⋅ p ) l ^ = l ^ × ( p × l ^ ) = l ^ × m (1) \begin{aligned} \boldsymbol{p}_{\perp} &=\boldsymbol{p}-(\hat{\boldsymbol{l}} \cdot \boldsymbol{p}) \hat{\boldsymbol{l}} \\ &=(\hat{\boldsymbol{l}} \cdot \hat{\boldsymbol{l}}) \boldsymbol{p}-(\hat{\boldsymbol{l}} \cdot \boldsymbol{p}) \hat{l} \\ &=\hat{\boldsymbol{l}} \times(\boldsymbol{p} \times \hat{\boldsymbol{l}}) \\ &=\hat{\boldsymbol{l}} \times \boldsymbol{m} \end{aligned} \tag{1} p=p(l^p)l^=(l^l^)p(l^p)l^=l^×(p×l^)=l^×m(1)
    l ^ ⋅ p \hat{l}\cdot p l^p表示把 p p p向量映射到直线方向上的长度,也就是上图看到的 p ⊥ − p p_{\perp}-p pp的长度;所以 p ⊥ \boldsymbol{p}_{\perp} p的模长就是远点到直线的最短距离,因为 l ^ \hat{l} l^ m m m垂直,因此:
    ∣ ∣ p ⊥ ∣ ∣ = ∣ ∣ l ^ ∣ ∣ ⋅ ∣ ∣ m ∣ ∣ s i n ( 90 ) = ∣ ∣ m ∣ ∣ (2) ||p_{\perp}||=||\hat{l}|| \cdot ||m||sin(90)=||m|| \tag{2} p=l^msin(90)=m(2)

    两种特殊的情况

    自然而然的想到,如果直线过原点怎么表示?如果直线是无穷远点怎么表示?

    过原点的直线

    因为直线过原点,那么就把p设为原点,则 m = 0 → × l = 0 → m=\overrightarrow{0}\times l=\overrightarrow{0} m=0 ×l=0 ,跟它的实际意义也相符;

    过无穷远点的直线

    直线过无穷点时,那么就把p设为无穷远点,有 p = t p ‾ p=t\overline{p} p=tp,则坐标可以写作:
    L : = ( l , t p ‾ × l ) : = ( l t , p ‾ × l ) = ( 0 , m ) (3) L:=(l, t\overline{p}\times l):=(\frac{l}{t}, \overline{p}\times l)=(0, m) \tag{3} L:=(l,tp×l):=(tl,p×l)=(0,m)(3)

     

    在空间中的表示方法

    使用两个点进行表示

    给定两个3D空间中的点,有: M T = ( M ‾ , m ) T , N T = ( N ‾ , n ) T M^T=(\overline{M}, m)^T, N^T=(\overline{N}, n)^T MT=(M,m)T,NT=(N,n)T,其中 m , n m,n m,n一方面使得直线变为其次坐标的表示,另一方面表示该点的逆深度,所以根据上面的Plucker坐标的表示,有:
    L = ( l ‾ m ) = ( M ‾ m − N ‾ n M ‾ × N ‾ ) = ( n M ‾ − m N ‾ M ‾ × N ‾ ) = ( a b ) (4) L=\left(\begin{array}{c}\overline{l} \\ m\end{array}\right)=\left(\begin{array}{c} \frac{\overline{M}}{m}-\frac{\overline{N}}{n} \\ \overline{M}\times \overline{N} \end{array} \right) = \left(\begin{array}{c} n\overline{M}-m\overline{N} \\ \overline{M}\times \overline{N} \end{array} \right) = \left(\begin{array}{c} a \\ b \end{array} \right) \tag{4} L=(lm)=(mMnNM×N)=(nMmNM×N)=(ab)(4)
    这里简单分析一下自由度,由上可知,Plucker的表示下一条直线有6个参数表示,是5维齐次坐标系下的齐次表示,同时,因为Plucker坐标中的 a , b a, b a,b分别表示线的方向以及线和原点组成的法向量,因此有约束 a T b = 0 a^Tb=0 aTb=0的约束,所以最终Plucker表示下的直线就有4个自由度,和空间直线的自由度一致。

    使用两个平面的交表示

    这部分属于空间中点和线的对偶形式,给定3D空间中的两个平面,有: M T = ( M ‾ , m ) T , N T = ( N ‾ , n ) T M^T=(\overline{M}, m)^T, N^T=(\overline{N}, n)^T MT=(M,m)T,NT=(N,n)T,这里 m , n m, n m,n就不表示逆深度了,而是表示截距,同样按照Plucker坐标的表示,有:
    L = ( l ‾ m ) = ( M ‾ × N ‾ M ‾ m − N ‾ n ) = ( M ‾ × N ‾ n M ‾ − m N ‾ ) = ( a b ) (5) L=\left(\begin{array}{c}\overline{l} \\ m \end{array}\right)=\left(\begin{array}{c} \overline{M}\times \overline{N} \\ \frac{\overline{M}}{m}-\frac{\overline{N}}{n} \end{array} \right) = \left(\begin{array}{c} \overline{M}\times \overline{N} \\ n\overline{M}-m\overline{N} \end{array} \right) = \left(\begin{array}{c} a \\ b \end{array} \right) \tag{5} L=(lm)=(M×NmMnN)=(M×NnMmN)=(ab)(5)

    简单的说一下上个公式中的b是怎么得来的,假设点 X X X在直线上,则有:
    { M ‾ X + m = 0 N ‾ X + n = 0 → ( n M ‾ − m N ‾ ) X = 0 \begin{aligned} \begin{cases} \overline{M}X+m=0 \\ \overline{N}X+n=0 \end{cases} \rightarrow (n\overline{M}-m\overline{N})X=0 \end{aligned} {MX+m=0NX+n=0(nMmN)X=0
    所以可以看到 b = ( n M ‾ − m N ‾ ) b=(n\overline{M}-m\overline{N}) b=(nMmN)垂直于直线上一点,同时 b b b也垂直于直线的方向向量 ( M ‾ × N ‾ ) (\overline{M}\times \overline{N}) (M×N),因此 b b b就是直线与原点构成平面的法向量。

     


    投影模型

    对于投影矩阵 P = [ P ‾ , p ] 3 × 4 P=[\overline{P}, p]_{3\times4} P=[P,p]3×4,那么经过投影之后的点而言,有:
    l i m a g e = m i m a g e × n i m a g e = ( P M w ) × ( P N w ) = ( P ‾ M ‾ + p m ) × ( P ‾ N ‾ + p n ) = P ‾ M ‾ × P ‾ N ‾ + p m × P ‾ N ‾ + P ‾ M ‾ × p n + p m × p n = d e t ( P ‾ ) P ‾ − T ( M ‾ × N ‾ ) + [ p ] × P ‾ ( m N ‾ − n M ‾ ) = [ d e t ( P ‾ ) P ‾ − T , [ p ] × P ‾ ] [ M ‾ × N ‾ m N ‾ − n M ‾ ] = P ~ 3 × 6 L 6 × 1 (6) \begin{aligned} l_{image}&=m_{image}\times n_{image} \\ &=(PM^w) \times (PN^w) \\ &=(\overline{P}\overline{M}+pm) \times (\overline{P}\overline{N}+pn) \\ &=\overline{P}\overline{M} \times \overline{P}\overline{N}+pm \times \overline{P}\overline{N}+\overline{P}\overline{M} \times pn+ pm \times pn \\ &=det(\overline{P})\overline{P}^{-T}(\overline{M} \times \overline{N})+[p]_{\times}\overline{P}(m\overline{N}-n\overline{M}) \\ &=[det(\overline{P})\overline{P}^{-T}, [p]_{\times}\overline{P}] \begin{bmatrix} \overline{M} \times \overline{N} \\ m\overline{N}-n\overline{M} \end{bmatrix} \\ &=\tilde{P}_{3\times 6}\mathbf{L}_{6\times1} \end{aligned} \tag{6} limage=mimage×nimage=(PMw)×(PNw)=(PM+pm)×(PN+pn)=PM×PN+pm×PN+PM×pn+pm×pn=det(P)PT(M×N)+[p]×P(mNnM)=[det(P)PT,[p]×P][M×NmNnM]=P~3×6L6×1(6)
    对于由两个面相交而得到的直线表示来说,把直线表示替换掉为面表示就可以了(对偶赛高)。

    根据上面的推导很容易扩展出世界坐标系到相机坐标系的转移矩阵有:
    [ n c b c ] = [ R c w [ t c w ] × R c w 0 R c w ] [ n w b w ] = [ R w c T [ − R w c T t w c ] × R w c T 0 R w c T ] [ n w b w ] = [ R w c T R w c T [ t w c ] × 0 R w c T ] [ n w b w ] (7) \left[\begin{array}{c} \mathbf{n}_{c} \\ \mathbf{b}_{c} \end{array}\right]= \left[\begin{array}{cc} \mathrm{R}_{cw} & {\left[\mathbf{t}_{cw}\right]_{\times} \mathrm{R}_{cw}} \\ \mathbf{0} & \mathrm{R}_{cw} \end{array}\right]\left[\begin{array}{c} \mathbf{n}_{w} \\ \mathbf{b}_{w} \end{array}\right]= \left[\begin{array}{cc} \mathrm{R}_{wc}^{T} & {\left[-\mathrm{R}_{wc}^{T}\mathbf{t}_{wc}\right]_{\times} \mathrm{R}_{wc}^T} \\ \mathbf{0} & \mathrm{R}_{wc}^T \end{array}\right]\left[\begin{array}{c} \mathbf{n}_{w} \\ \mathbf{b}_{w} \end{array}\right]= \left[\begin{array}{cc} \mathrm{R}_{wc}^{T} & {\mathrm{R}_{wc}^{T}\left[\mathbf{t}_{wc}\right]_{\times} } \\ \mathbf{0} & \mathrm{R}_{wc}^T \end{array}\right]\left[\begin{array}{c} \mathbf{n}_{w} \\ \mathbf{b}_{w} \end{array}\right] \tag{7} [ncbc]=[Rcw0[tcw]×RcwRcw][nwbw]=[RwcT0[RwcTtwc]×RwcTRwcT][nwbw]=[RwcT0RwcT[twc]×RwcT][nwbw](7)
    简单说一下上述公式:第一行由公式(6)得到,把其中的 P P P R , t R,t R,t替换就可以了;第二行也很好理解,就是简单的把直线的方向进行了旋转。后面之所以把整个公式变换为camera->world主要是因为这是位姿的表示方法。其中的一步推导用到了一个性质,该性质在李群中比较常见,即:当 M ∈ S O ( 3 ) M\in\mathbf{SO(3)} MSO(3)时,有 [ M u ] × = M [ u ] × M T [Mu]_{\times}=M[u]_{\times}M^T [Mu]×=M[u]×MT

     


    三角化

    这里主要涉及两个方法:第一个方法是解耦的方法,即先获取表达式的初值,再对初值进行正交补偿;第二个方法是耦合的来解,即使用迭代的方法得到一个即满足优化函数,又能满足正交要求的解。下面简单的说明一下这两个方法。

    获取直线表示的初值

    这部分比较简单,优化的目标函数就是投影误差。假设直线 L \mathbf{L} L被相机 P i , i = 1... n P_i,i=1...n Pi,i=1...n观测到,那么对于每一次的观测,可以定义误差:
    E = ( x i ⊤ P ~ i L ) 2 + ( y i ⊤ P ~ i L ) 2 (8) E=\left(\mathbf{x}^{i \top} \widetilde{\mathbf{P}}^{i} \mathbf{L}\right)^{2}+\left(\mathbf{y}^{i^{\top}} \widetilde{\mathbf{P}}^{i} \mathbf{L}\right)^{2} \tag{8} E=(xiP iL)2+(yiP iL)2(8)
    可以看到基本上就是在第 i 帧上的图像上的两个对应点到投影直线的距离。所以所有的观测都考虑之后有:
    B ( L , M ) = ∑ i = 1 n ( ( x i ⊤ P ~ i L ( l 1 ) 2 + ( l 2 ) 2 ) 2 + ( y i ⊤ P ~ i L ( l 1 ) 2 + ( l 2 ) 2 ) 2 ) = ∥ A ( 2 n × 6 ) L ∥ 2  with  A = ( ⋯ x i ⊤ P ~ i y i ⊤ P ~ i … ) (9) \begin{aligned} \mathcal{B}(\mathbf{L}, \mathcal{M}) &=\sum_{i=1}^{n}\left(\left(\frac{\mathbf{x}^{i \top} \widetilde{\mathbf{P}}^{i} \mathbf{L}}{\sqrt{(l_1)^2+(l_2)^2}}\right)^{2}+\left(\frac{\mathbf{y}^{i^{\top}} \widetilde{\mathbf{P}}^{i} \mathbf{L}}{\sqrt{(l_1)^2+(l_2)^2}}\right)^{2}\right) \\ &=\left\|A_{(2 n \times 6)} \mathbf{L}\right\|^{2} \quad \text { with } \quad \mathrm{A}=\left(\begin{array}{c} \cdots \\ \mathbf{x}^{i^{\top}} \widetilde{\mathbf{P}}^{i} \\ \mathbf{y}^{i^{\top}} \widetilde{\mathbf{P}}^{i} \\ \ldots \end{array}\right) \end{aligned} \tag{9} B(L,M)=i=1n((l1)2+(l2)2 xiP iL)2+((l1)2+(l2)2 yiP iL)2=A(2n×6)L2 with A=xiP iyiP i(9)
    其中:

    • l 3 × 1 = P ~ i L l_{3\times 1}=\widetilde{\mathbf{P}}^{i} \mathbf{L} l3×1=P iL,那么 l 1 = l ( 1 ) , l 2 = l ( 2 ) l_1=l(1), l_2=l(2) l1=l(1),l2=l(2)

    该方程有很多的解法,这里就不赘述了。需要注意的是这个部分只是求解了方程,并没有考虑Plucker约束,即 a T b = 0 \mathbf{a}^T\mathbf{b}=0 aTb=0

     

    对Plucker约束进行补偿

    这部分可以着重参考论文【2】,基本思路是把这个问题当做纯数学问题进行求解,获取与初值最接近且正交的向量,但是这个解就不一定满足公式(9)的最优解了。

     

    类线性算法(Quasi-linear algorithm)

    这个算法主要是使用迭代的思路使得解既能满足公式(9)的优化函数,又能满足Plucker正交关系的解,主要的思路如下:

    1. 先按照公式(9)求得一个初始 L 0 L_0 L0

    2. 构建一个迭代公式:
      C k ( L k + 1 ) = L k T [ 0 I 3 × 3 I 3 × 3 0 ] ⏟ G L k + 1 = b k T a k + 1 + a k T b k + 1 = 0 C_k(L_{k+1})=L_k^T\underbrace{\begin{bmatrix}0 & I_{3\times3} \\ I_{3\times3} & 0\end{bmatrix}}_{G}L_{k+1}=b_k^Ta_{k+1}+a^T_kb_{k+1}=0 Ck(Lk+1)=LkTG [0I3×3I3×30]Lk+1=bkTak+1+akTbk+1=0
      可以看到这个思路主要想使得迭代前后两次的方向向量和法向量是正交的

    3. 为了求解上述公式,显然可以得到最优的 L k + 1 L_{k+1} Lk+1是在 L k T G L_{k}^TG LkTG的零空间中,所以对 L k T G L_{k}^TG LkTG进行了SVD分解:
      L k T G = U 1 × 1 T Σ 1 × 6 V 6 × 6 T = U 1 × 1 T Σ 1 × 6 ( v 6 × 1 , V ‾ 6 × 5 ) T L_{k}^TG=U_{1\times1}^T\Sigma_{1\times6}V_{6\times6}^T=U_{1\times1}^T\Sigma_{1\times6}(v_{6\times1}, \overline{V}_{6\times5})^T LkTG=U1×1TΣ1×6V6×6T=U1×1TΣ1×6(v6×1,V6×5)T
      上述公式中, v 6 × 1 v_{6\times 1} v6×1是唯一一个特解,而 V 6 × 5 V_{6 \times 5} V6×5就是 L k + 1 L_{k+1} Lk+1所在的零空间,所以 L k + 1 L_{k+1} Lk+1可以被 V 6 × 5 V_{6\times 5} V6×5的五个列向量组成的空间基底线性表示;

    4. 那么现在的问题变为如何求解这个线性表示了,或者说变为求解 γ 5 × 1 \gamma_{5\times 1} γ5×1这个向量。其实也简单,经过第三步其实已经得到了 V 6 × 5 V_{6\times 5} V6×5,那么设 L k + 1 = V ‾ 6 × 5 γ 5 × 1 L_{k+1}=\overline{V}_{6\times 5}\gamma_{5\times 1} Lk+1=V6×5γ5×1,代入公式(9)后得到:
      B ( L k + 1 , M ) = ∥ A 2 n × 6 V ‾ 6 × 5 γ 5 × 1 ∥ 2 = 0 \mathcal{B}(\mathbf{L_{k+1}}, \mathcal{M})=\|A_{2n\times 6}\overline{V}_{6\times 5}\gamma_{5\times 1}\|^2=0 B(Lk+1,M)=A2n×6V6×5γ5×12=0
      这个时候就比较明了了,构建优化目标函数为:
      m i n r ∥ A V ‾ γ ∥ 2 s . t . ∥ γ ∥ 2 = 1 \begin{aligned} \mathrm{min}_{r} &\|A\overline{V}\gamma\|^2 \\ s.t. &\|\gamma\|^2=1 \end{aligned} minrs.t.AVγ2γ2=1
      这里其实没有特别想明白为什么约束条件是 ∥ γ ∥ 2 = 1 \|\gamma\|^2=1 γ2=1而不是 ∑ γ i = 1 \sum\gamma_i=1 γi=1,私以为可能是为了方便运算,因为上式直接用拉格朗日乘子法解的话,用 ∥ γ ∥ 2 = 1 \|\gamma\|^2=1 γ2=1的约束显然比 ∑ γ i = 1 \sum\gamma_i=1 γi=1要容易的多;

    5. 经过这个之后,还有一个步骤是要更新公式(9)中的A,因为每个直线与观测线之间的误差都与直线表示 L k + 1 {L_{k+1}} Lk+1有关,因此当获得了新的直线表示之后记得更新这个地方;

    6. 重复2、 3、 4、 5步骤直到得到的 L k + 1 L_{k+1} Lk+1收敛;

     


    线特征的正交表示(Orthonormal Representation)

    正交表示主要的贡献点在于可以使得整个表示的未知数与自由度是对应的,也就是可以用最小表示进行线特征的表示,这样的好处对于优化问题是不言而喻的,就和李群和李代数的关系一样。下面就简单的介绍一下这个表示方法。

    正交表示的推导

    假设直线的表示为: L = [ n T , d T ] T \mathcal{L}=[n^T, d^T]^T L=[nT,dT]T,写作矩阵形式为 C = [ a , b ] 3 × 2 \mathbf{C}=\left[\mathbf{a}, \mathbf{b}\right]_{3\times2} C=[a,b]3×2,其中:

    • n \mathbf{n} n表示直线的法向量, d \mathbf{d} d表示直线的方向向量;
    • a = n , b = d \mathbf{a}=\mathbf{n}, \mathbf{b}=\mathbf{d} a=n,b=d

    将矩阵 C \mathbf{C} C进行QR分解有:
    C ∼ ( a ∥ a ∥ b ∥ b ∥ a × b ∥ a × b ∥ ) ⏟ S O ( 3 ) ( ∥ a ∥ 0 0 ∥ b ∥ 0 0 ) ⏟ ( ∥ a ∥ ∥ b ∥ ) ⊤ ∈ P 1 (10) C \sim \underbrace{\left(\frac{a}{\|a\|} \frac{b}{\|\mathbf{b}\|} \frac{a \times b}{\|\mathbf{a} \times \mathbf{b}\|}\right)}_{S O(3)} \underbrace{\left(\begin{matrix}\|\mathbf{a}\| & 0 \\ 0 & \|\mathbf{b}\| \\ 0 & 0\end{matrix}\right)}_{(\|\mathbf{a}\|\|\mathbf{b}\|)^{\top} \in \mathbb{P}^{1}} \tag{10} CSO(3) (aabba×ba×b)(ab)P1 a000b0(10)
    整个分解之后由两部分组成,一部分是旋转矩阵,另一部分是一个一维的齐次坐标的表示。

    简单的说一下这个部分:

    1. 第二部分的右上角的元素为0是必然的,因为 a ⊥ b \mathbf{a}\perp\mathbf{b} ab

    2. 因为第二个部分本质上只有两个变量,都除以 ∥ b ∥ \|\mathbf{b}\| b之后,其实可以得到两个元素为 ( d = ∥ a ∥ / ∥ b ∥ , 1 ) (d=\|\mathbf{a}\|/\|\mathbf{b}\|, 1) (d=a/b,1),其中第一个元素根据公式(1)可以知道这部分就是直线到原点的距离。

    所以看到,经过QR分解之后的整个直线表示的状态量刚好是4个(第一部分有3个,第二部分有1个),因此说正交表示可以最小表示直线。

    到这里其实还并没有完成正交表示,因为作者认为第二部分并不容易进行优化和更新,所以做了一个变化,把这部分变作了一个 S O ( 2 ) \mathbf{SO(2)} SO(2)的表示,毕竟相比于这个 3 × 2 3\times 2 3×2的矩阵来说, S O ( 2 ) \mathbf{SO(2)} SO(2)的更新更加稳定且容易,最重要的是并没有造成信息的缺失,有如下的过程:
    W = [ 1 1 + d 2 d 1 + d 2 − d 1 + d 2 1 1 + d 2 ] = [ ∥ a ∥ ∥ a ∥ 2 + ∥ b ∥ 2 ∥ b ∥ ∥ a ∥ 2 + ∥ b ∥ 2 − ∥ b ∥ ∥ a ∥ 2 + ∥ b ∥ 2 ∥ a ∥ ∥ a ∥ 2 + ∥ b ∥ 2 ] = [ c o s ( ϕ ) − s i n ( ϕ ) s i n ( ϕ ) c o s ( ϕ ) ] = [ w 1 − w 2 w 2 w 1 ] ∈ S O ( 2 ) (11) W=\begin{bmatrix} \frac{1}{\sqrt{1+d^2}} & \frac{d}{\sqrt{1+d^2}} \\ \frac{-d}{\sqrt{1+d^2}} & \frac{1}{\sqrt{1+d^2}} \end{bmatrix} = \begin{bmatrix} \frac{\|a\|}{\sqrt{\|a\|^2+\|b\|^2}} & \frac{\|b\|}{\sqrt{\|a\|^2+\|b\|^2}} \\ -\frac{\|b\|}{\sqrt{\|a\|^2+\|b\|^2}} & \frac{\|a\|}{\sqrt{\|a\|^2+\|b\|^2}} \end{bmatrix} = \begin{bmatrix} cos(\phi) & -sin(\phi) \\ sin(\phi) & cos(\phi)\end{bmatrix} = \begin{bmatrix}w1 & -w2 \\ w2 & w1\end{bmatrix} \in \mathbf{SO(2)} \tag{11} W=[1+d2 11+d2 d1+d2 d1+d2 1]=a2+b2 aa2+b2 ba2+b2 ba2+b2 a=[cos(ϕ)sin(ϕ)sin(ϕ)cos(ϕ)]=[w1w2w2w1]SO(2)(11)
    在公式(10)中,比较容易看到,当对 θ \theta θ进行更新了之后,可以通过 c o s ( ϕ ) / s i n ( ϕ ) cos(\phi)/sin(\phi) cos(ϕ)/sin(ϕ)的公式来得到直线到原点的距离。

    综上所述,直线的正交表示可以写作如下形式:
    C ∼ ( U , W ) ∈ S O ( 3 ) × S O ( 2 ) C \sim (U, W) \in \mathbf{SO(3)} \times \mathbf{SO(2)} C(U,W)SO(3)×SO(2)
    这部分用下图表示十分合适:

    在这里插入图片描述

     

    正交表示与Plucker坐标系的转换

    正交表示可以最小的表示直线,但是该表示方法的投影方程并没有线性的表示,即该方法并不能像Plucker坐标系表示方法一样,通过公式(7)得到一个线性的映射关系,但是我们又特别的看重该表示方法的最小表示的性质,好在两者之间的互换关系还是十分简单的,如下:
    L 6 × 1 ⏟ P l u c k e r 表 示 = ∥ a ∥ 2 + ∥ b ∥ 2 [ w 1 u 1 w 2 u 2 ] ⏟ 正 交 表 示 (12) \underbrace{\mathcal{L}_{6\times1}}_{Plucker表示}=\sqrt{\|a\|^2+\|b\|^2} \underbrace{\begin{bmatrix} w1\mathbf{u_1} \\ w2\mathbf{u_2} \end{bmatrix}}_{正交表示} \tag{12} Plucker L6×1=a2+b2 [w1u1w2u2](12)
    这里面差了一个归一化参数,这部分其实可以不用管,因为可以在算 ( a , b ) (\mathbf{a}, \mathbf{b}) (a,b)的时候稍微进行一下归一化或者其他的方式限制这个值。

     


    优化问题

    这部分其实跟点的过程一样,也主要是观测方程误差函数,上面谈到的公式(6)和公式(8)其实已经简单的涉及到了这部分,下面的内容主要是从头到尾的串一遍了。

    这部分用下面的一张图表示:

    在这里插入图片描述

    其中:

    • L W L^W LW表示在世界坐标系的表示, L C L^{C} LC表示在相机坐标系下的表示;
    • L n L^n Ln表示归一化平面上的线, L I L^I LI表示在图像坐标系下的线;
    • 这里故意把归一化平面与图像平面分开画,两个平面之间其实差了一个内参矩阵,不过这个矩阵与常见的K不同,这里需要稍微进行推导。

    所以这里进行从世界坐标系到图像坐标系的整个投影可以表示为:
    l i a m g e = K l n o r m a l = K P ~ 3 × 6 w L 6 × 1 = K m ′ (13) l^{iamge} = \mathcal{K} \mathbf{l^{normal}}=\mathcal{K}\tilde{P}_{3\times 6}\mathbf{^wL}_{6\times1}=\mathcal{K}\mathbf{m}^{\prime} \tag{13} liamge=Klnormal=KP~3×6wL6×1=Km(13)
    其中:

    • K \mathcal{K} K表示直线的内参矩阵;
    • P ~ 3 × 6 w L 6 × 1 \tilde{P}_{3\times 6}\mathbf{^wL}_{6\times1} P~3×6wL6×1参考公式(6);
    • 这里的 m ′ \mathbf{m}^{\prime} m表示在相机的归一化坐标系下的法向量;

    简单的说一下 K \mathcal{K} K的推导:

    1. 在归一化平面上,假设直线的点为: n C , n D ^nC, {^n}D nC,nD
    2. 在图像平面上,经过内参映射之后的点为: i c , i d {^i}c, {^i}d ic,id,其中两者的关系为 i c = K n C , i d = K n D {^i}c=K^nC, {^i}d=K{^n}D ic=KnC,id=KnD,其中K就是熟悉的相机内参矩阵;

    对于归一化平面和相机平面,两者其实都是2维的射影平面,所以在归一化平面和归一化平面上的直线的表示为:
    i c × i d = ( K n C ) × ( K n D ) = [ f x X C + c x f y Y C + c y 1 ] × [ f x X D + c x f y Y D + c y 1 ] = [ 0 − 1 ( f y Y C + c y ) 1 0 − ( f x X C + c x ) − ( f y Y C + c y ) ( f x X C + c x ) 0 ] [ f x X D + c x f y Y D + c y 1 ] = [ f y ( Y C − Y D ) f x ( X C − X D ) f x f y ( X C Y D − Y C X D ) + f x c y ( X D − X C ) + f y c x ( Y D − Y C ) ] = [ f y 0 0 0 f x 0 − f y c x − f x c y f x f y ] [ Y D − Y C X D − X C X C Y D − Y C X D ] = [ f y 0 0 0 f x 0 − f y c x − f x c y f x f y ] [ X C Y C 1 ] × [ X D Y D 1 ] = K ( n C × n D ) = K m ′ (14) \begin{aligned} {^i}c \times {^i}d &= (K{^n}C) \times (K{^n}D) \\ &=\begin{bmatrix}fxX_C+cx \\ fyY_C+cy \\ 1\end{bmatrix}_{\times}\begin{bmatrix}fxX_D+cx \\ fyY_D+cy \\ 1\end{bmatrix} \\ &=\begin{bmatrix}0 & -1 & (fyY_C+cy) \\ 1 & 0 & -(fxX_C+cx) \\ -(fyY_C+cy) & (fxX_C+cx) & 0 \end{bmatrix}\begin{bmatrix}fxX_D+cx \\ fyY_D+cy \\ 1\end{bmatrix} \\ &=\begin{bmatrix}fy(Y_C-Y_D) \\ fx(X_C-X_D) \\ fxfy(X_CY_D-Y_CX_D)+fxcy(X_D-X_C)+fycx(Y_D-Y_C) \end{bmatrix} \\ &=\begin{bmatrix}fy & 0 & 0 \\ 0 & fx & 0 \\ -fycx & -fxcy & fxfy \end{bmatrix}\begin{bmatrix}Y_D-Y_C \\ X_D-X_C \\ X_CY_D-Y_CX_D \end{bmatrix} \\ &=\begin{bmatrix}fy & 0 & 0 \\ 0 & fx & 0 \\ -fycx & -fxcy & fxfy \end{bmatrix} \begin{bmatrix}X_C \\ Y_C \\ 1 \end{bmatrix}_{\times} \begin{bmatrix}X_D \\ Y_D \\ 1 \end{bmatrix} \\ &=\mathcal{K}({^n}C \times {^n}D) \\ &=\mathcal{K}\mathbf{m}^{\prime} \end{aligned}\tag{14} ic×id=(KnC)×(KnD)=fxXC+cxfyYC+cy1×fxXD+cxfyYD+cy1=01(fyYC+cy)10(fxXC+cx)(fyYC+cy)(fxXC+cx)0fxXD+cxfyYD+cy1=fy(YCYD)fx(XCXD)fxfy(XCYDYCXD)+fxcy(XDXC)+fycx(YDYC)=fy0fycx0fxfxcy00fxfyYDYCXDXCXCYDYCXD=fy0fycx0fxfxcy00fxfyXCYC1×XDYD1=K(nC×nD)=Km(14)
    上述的推导其实很容易看出,因为两个归一化平面的点的叉乘就是在相机坐标系下的Plucker表示中的法向量,因此可以看到在归一化坐标系到图像坐标系的转换中,仅仅涉及到了直线表示中的法向量部分。

     

    误差函数

    用下面这张图表示误差量,如下:

    在这里插入图片描述

    图中的 I L I_L IL表示直线 L \mathcal{L} L在图像平面的投影,所以定义误差项为(就是简单的两个点到直线的距离):
    r L = [ r 1 r 2 ] = [ c T I l 1 2 + l 2 2 d T I l 1 2 + l 2 2 ] (15) \mathbf{r_L}=\begin{bmatrix}\mathbf{r_1} \\ \mathbf{r_2} \end{bmatrix} = \begin{bmatrix}\frac{c^TI}{\sqrt{l_1^2+l_2^2}} \\ \frac{d^TI}{\sqrt{l_1^2+l_2^2}} \end{bmatrix} \tag{15} rL=[r1r2]=l12+l22 cTIl12+l22 dTI(15)
     

    求解Jacobian

    跟对3D点的优化问题一样,就是从误差不停的递推到位姿以及直线表示上,用到最最最基本的求导的链式法则:

    通用的公式如下:
    ∂ r L ∂ X = ∂ r L ∂ L I ∂ L I ∂ L n ∂ L n ∂ L c { ∂ L c ∂ θ  X= θ ∂ L c ∂ t  X=t ∂ L c ∂ L w ∂ L w ∂ ( θ , ϕ )  X= L w (16) \frac{\partial \mathbf{r_L}}{\partial X}=\frac{\partial \mathbf{r_L}}{\partial L^{I}} \frac{\partial L^{I}}{\partial L^{n}} \frac{\partial L^{n}}{\partial L^{c}} \begin{aligned}\begin{cases} \frac{\partial L^{c}}{\partial \theta} &\text{ X=}\theta \\\frac{\partial L^{c}}{\partial t} &\text{ X=t} \\\frac{\partial L^{c}}{\partial L^{w}}\frac{\partial L^{w}}{\partial{(\theta,\phi)}} &\text{ X=}L^{w}\end{cases} \end{aligned}\tag{16} XrL=LIrLLnLILcLnθLctLcLwLc(θ,ϕ)Lw X=θ X=t X=Lw(16)
    先对前面最通用的部分进行求解:

    第一部分:
    ∂ r L ∂ L I = [ ∂ r 1 ∂ l 1 ∂ r 1 ∂ l 2 ∂ r 1 ∂ l 3 ∂ r 2 ∂ l 1 ∂ r 2 ∂ l 2 ∂ r 2 ∂ l 3 ] = [ − l 1 c T L I ( l 1 2 + l 2 2 ) 3 2 + u c ( l 1 2 + l 2 2 ) 1 2 − l 2 c T L I ( l 1 2 + l 2 2 ) 3 2 + v c ( l 1 2 + l 2 2 ) 1 2 1 ( l 1 2 + l 2 2 ) 1 2 − l 1 d T L I ( l 1 2 + l 2 2 ) 3 2 + u d ( l 1 2 + l 2 2 ) 1 2 − l 2 d T L I ( l 1 2 + l 2 2 ) 3 2 + v d ( l 1 2 + l 2 2 ) 1 2 1 ( l 1 2 + l 2 2 ) 1 2 ] 2 × 3 (17) \begin{aligned}\frac{\partial \mathbf{r_L}}{\partial L^{I}} &= \begin{bmatrix}\frac{\partial{\mathbf{r1}}}{\partial{l_1}} & \frac{\partial{\mathbf{r1}}}{\partial{l_2}} & \frac{\partial{\mathbf{r1}}}{\partial{l_3}} \\ \frac{\partial{\mathbf{r2}}}{\partial{l_1}} & \frac{\partial{\mathbf{r2}}}{\partial{l_2}} & \frac{\partial{\mathbf{r2}}}{\partial{l_3}}\end{bmatrix} \\&=\begin{bmatrix}\frac{-l_1 c^TL^{I}}{(l_1^2+l_2^2)^{\frac{3}{2}}}+\frac{u_c}{(l_1^2+l_2^2)^{\frac{1}{2}}} & \frac{-l_2 c^TL^{I}}{(l_1^2+l_2^2)^{\frac{3}{2}}}+\frac{v_c}{(l_1^2+l_2^2)^{\frac{1}{2}}} & \frac{1}{(l_1^2+l_2^2)^{\frac{1}{2}}} \\ \frac{-l_1 d^TL^{I}}{(l_1^2+l_2^2)^{\frac{3}{2}}}+\frac{u_d}{(l_1^2+l_2^2)^{\frac{1}{2}}} & \frac{-l_2 d^TL^{I}}{(l_1^2+l_2^2)^{\frac{3}{2}}}+\frac{v_d}{(l_1^2+l_2^2)^{\frac{1}{2}}} & \frac{1}{(l_1^2+l_2^2)^{\frac{1}{2}}} \end{bmatrix}_{2\times 3}\end{aligned} \tag{17} LIrL=[l1r1l1r2l2r1l2r2l3r1l3r2]=(l12+l22)23l1cTLI+(l12+l22)21uc(l12+l22)23l1dTLI+(l12+l22)21ud(l12+l22)23l2cTLI+(l12+l22)21vc(l12+l22)23l2dTLI+(l12+l22)21vd(l12+l22)211(l12+l22)2112×3(17)
    其中:

    • l 1 , l 2 , l 3 l1, l2, l3 l1,l2,l3表示图像坐标系下直线的三个参数;
    • u c , v c u_c, v_c uc,vc表示点c的xy坐标值, u d , v d u_d, v_d ud,vd同理;

    第二部分:

    根据公式(13)可知:
    ∂ L I ∂ L n = K 3 × 3 (18) \frac{\partial L^{I}}{\partial L^{n}}=\mathcal{K}_{3\times3} \tag{18} LnLI=K3×3(18)
    第三部分:

    由公式(6)和(13)可知,直线的Plucker表示在归一化平面上只用了其中的法向量部分,因此若有 c L = [ c n , c d ] T \mathcal{{^c}L}=\left[\mathbf{{^c}n}, \mathbf{{^c}d}\right]^T cL=[cn,cd]T,那么 n L = c n \mathcal{{^n}L}=\mathbf{{^c}n} nL=cn,所以求导有:
    ∂ L n ∂ L c = [ I 3 × 3 0 3 × 3 ] 3 × 6 (19) \frac{\partial L^{n}}{\partial L^{c}}=\begin{bmatrix}\mathbf{I}_{3\times3} & 0_{3\times3}\end{bmatrix}_{3\times6} \tag{19} LcLn=[I3×303×3]3×6(19)
    第四部分就分这几种情况进行讨论:

    对于位姿的姿态部分

    根据公式(7)有:
    ∂ c L ∂ θ = [ ∂ n c ∂ θ ∂ d c ∂ θ ] = [ ∂ ( R w c T ( n w + [ t w c ] × b w ) ) ∂ θ ∂ R w c T b w ∂ θ ] = [ [ R w c T ( n w + [ t w c ] × b w ) ] × [ R w c T b w ] × ] 6 × 3 (20) \frac{\partial {^c}L}{\partial \theta} = \begin{bmatrix}\frac{\partial n_c}{\partial \theta} \\ \frac{\partial d_c}{\partial \theta}\end{bmatrix} = \begin{bmatrix}\frac{\partial{(R_{wc}^T(n_w+[t_{wc}]_{\times}b_w))}}{\partial \theta} \\ \frac{\partial{R_{wc}^Tb_w}}{\partial \theta}\end{bmatrix}=\begin{bmatrix} [R_{wc}^T(n_w+[t_{wc}]_{\times}b_w)]_{\times} \\ [R_{wc}^Tb_w]_{\times} \end{bmatrix}_{6\times3} \tag{20} θcL=[θncθdc]=[θ(RwcT(nw+[twc]×bw))θRwcTbw]=[[RwcT(nw+[twc]×bw)]×[RwcTbw]×]6×3(20)

    上述的推导使用了李群的右扰动模型,即 ( R w c E x p ( θ ) ) T = E x p ( − θ ) R w c T (R_{wc}Exp(\theta))^T=Exp(-\theta)R_{wc}^T (RwcExp(θ))T=Exp(θ)RwcT

    对于位姿的位移部分

    同样根据公式(7)有:
    ∂ c L ∂ t = [ ∂ n c ∂ t ∂ d c ∂ t ] = [ ∂ ( R w c T ( n w + [ t w c ] × b w ) ) ∂ t ∂ R w c T b w ∂ t ] = [ − R w c T [ b w ] × 0 ] 6 × 3 (21) \frac{\partial {^c}L}{\partial t} = \begin{bmatrix}\frac{\partial n_c}{\partial t} \\ \frac{\partial d_c}{\partial t}\end{bmatrix} = \begin{bmatrix}\frac{\partial{(R_{wc}^T(n_w+[t_{wc}]_{\times}b_w))}}{\partial t} \\ \frac{\partial{R_{wc}^Tb_w}}{\partial t}\end{bmatrix}=\begin{bmatrix} -R_{wc}^T[b_{w}]_{\times} \\ \mathbf{0} \end{bmatrix}_{6\times3} \tag{21} tcL=[tnctdc]=[t(RwcT(nw+[twc]×bw))tRwcTbw]=[RwcT[bw]×0]6×3(21)

    对于世界坐标系下直线表示部分

    这部分按照公式(16)的步骤,依旧分两个部分:

    1. ∂ L c ∂ L w \frac{\partial L^{c}}{\partial L^{w}} LwLc部分:
      ∂ L c ∂ L w = [ ∂ n C ∂ n W ∂ n C ∂ b W ∂ d C ∂ n W ∂ d C ∂ b W ] = [ R w c T R w c T [ t w c ] × 0 R w c T ] (22) \frac{\partial L^{c}}{\partial L^{w}}=\begin{bmatrix}\frac{\partial{\mathbf{n^C}}}{\partial{\mathbf{n^W}}} & \frac{\partial{\mathbf{n^C}}}{\partial{\mathbf{b^W}}} \\ \frac{\partial{\mathbf{d^C}}}{\partial{\mathbf{n^W}}} & \frac{\partial{\mathbf{d^C}}}{\partial{\mathbf{b^W}}}\end{bmatrix} =\left[\begin{array}{cc}\mathrm{R}_{wc}^{T} & {\mathrm{R}_{wc}^{T}\left[\mathbf{t}_{wc}\right]_{\times} } \\\mathbf{0} & \mathrm{R}_{wc}^T\end{array}\right] \tag{22} LwLc=[nWnCnWdCbWnCbWdC]=[RwcT0RwcT[twc]×RwcT](22)

    2. ∂ L w ∂ ( θ , ϕ ) \frac{\partial L^{w}}{\partial(\theta, \phi)} (θ,ϕ)Lw部分,这部分其实还可以继续分,如下:
      ∂ L w ∂ ( θ , ϕ ) = [ ∂ L w ∂ θ , ∂ L w ∂ ϕ ] = [ ∂ L w ∂ U ∂ U ∂ θ , ∂ L w ∂ W ∂ W ∂ ϕ ] \frac{\partial L^{w}}{\partial(\theta, \phi)}=\left[\frac{\partial L^{w}}{\partial \theta}, \frac{\partial L^{w}}{\partial \phi}\right]=\left[\frac{\partial L^{w}}{\partial{U}}\frac{\partial{U}}{\partial \theta}, \frac{\partial L^{w}}{\partial{W}}\frac{\partial{W}}{\partial \phi}\right] (θ,ϕ)Lw=[θLw,ϕLw]=[ULwθU,WLwϕW]

      第一部分

      ∂ L w ∂ U ∂ U ∂ θ = ∂ [ w 1 u 1 w 2 u 2 ] ∂ [ u 1 , u 2 , u 3 ] ∂ [ u 1 , u 2 , u 3 ] ∂ θ = [ w 1 0 0 0 w 2 0 ] 6 × 9 [ 0 − u 3 u 2 u 3 0 − u 1 − u 2 u 1 0 ] 9 × 3 = [ 0 − w 1 u 3 w 1 u 2 − w 2 u 3 0 − w 2 u 1 ] 6 × 3 (23) \begin{aligned}\frac{\partial L^{w}}{\partial{U}}\frac{\partial{U}}{\partial \theta}&=\frac{\partial{\begin{bmatrix} w1\mathbf{u_1} \\ w2\mathbf{u_2} \end{bmatrix}}}{\partial{[\mathbf{u_1},\mathbf{u_2}, \mathbf{u_3}]}}\frac{\partial{[\mathbf{u_1},\mathbf{u_2}, \mathbf{u_3}]}}{\partial{\theta}} \\&=\begin{bmatrix}w1 & 0 & 0 \\ 0 & w2 & 0 \end{bmatrix}_{6\times9}\begin{bmatrix}0 & -\mathbf{u3} & \mathbf{u2} \\ \mathbf{u3} & 0 & -\mathbf{u1} \\ -\mathbf{u2} & \mathbf{u1} & 0 \end{bmatrix}_{9\times3} \\&= \begin{bmatrix} 0 & -w1\mathbf{u3} & w1\mathbf{u2} \\ -w2\mathbf{u3} & 0 & -w2\mathbf{u1} \end{bmatrix}_{6\times3}\end{aligned} \tag{23} ULwθU=[u1,u2,u3][w1u1w2u2]θ[u1,u2,u3]=[w100w200]6×90u3u2u30u1u2u109×3=[0w2u3w1u30w1u2w2u1]6×3(23)

      第二部分

      ∂ L w ∂ W ∂ W ∂ ϕ = ∂ [ w 1 u 1 w 2 u 2 ] ∂ [ w 1 , w 2 ] T ∂ [ w 1 , w 2 ] T ∂ ϕ = [ u 1 0 0 u 2 ] 6 × 2 [ − w 2 w 1 ] 2 × 1 = [ − w 2 u 1 w 1 u 2 ] 6 × 1 (24) \begin{aligned}\frac{\partial L^{w}}{\partial{W}}\frac{\partial{W}}{\partial \phi}&=\frac{\partial{\begin{bmatrix} w1\mathbf{u_1} \\ w2\mathbf{u_2} \end{bmatrix}}}{\partial{[w1, w2]^T}}\frac{\partial{[w1, w2]^T}}{\partial{\phi}} \\&=\begin{bmatrix}\mathbf{u1} & 0 \\ 0 & \mathbf{u2} \end{bmatrix}_{6\times2}\begin{bmatrix} -w2 \\ w1 \end{bmatrix}_{2\times1} \\&= \begin{bmatrix} -w2\mathbf{u1} \\ w1\mathbf{u2}\end{bmatrix}_{6\times1}\end{aligned} \tag{24} WLwϕW=[w1,w2]T[w1u1w2u2]ϕ[w1,w2]T=[u100u2]6×2[w2w1]2×1=[w2u1w1u2]6×1(24)

      其中 w 1 = c o s ( ϕ ) , w 2 = s i n ( ϕ ) w1=cos(\phi), w2=sin(\phi) w1=cos(ϕ),w2=sin(ϕ)

    3. 两个部分合起来为:
      ∂ L w ∂ ( θ , ϕ ) = [ R w c T R w c T [ t w c ] × 0 R w c T ] 6 × 6 [ 0 − w 1 u 3 w 1 u 2 − w 2 u 1 − w 2 u 3 0 − w 2 u 1 w 1 u 2 ] 6 × 4 (25) \frac{\partial L^{w}}{\partial(\theta, \phi)}=\left[\begin{array}{cc}\mathrm{R}_{wc}^{T} & {\mathrm{R}_{wc}^{T}\left[\mathbf{t}_{wc}\right]_{\times} } \\\mathbf{0} & \mathrm{R}_{wc}^T\end{array}\right]_{6\times6}\begin{bmatrix} 0 & -w1\mathbf{u3} & w1\mathbf{u2} & -w2\mathbf{u1} \\ -w2\mathbf{u3} & 0 & -w2\mathbf{u1} & w1\mathbf{u2} \end{bmatrix}_{6\times4} \tag{25} (θ,ϕ)Lw=[RwcT0RwcT[twc]×RwcT]6×6[0w2u3w1u30w1u2w2u1w2u1w1u2]6×4(25)

     


    总结

    线特征这部分确实比点特征要复杂很多,表示方法就很不同(这部分也是笔者花了很久才接受的一部分),但是好在整个推导过程都可以借助点的转移方程逐步进行推导。

    展开全文
  • SLAM前端线特征匹配

    2021-06-23 16:21:01
    此次线特征匹配的代码来源于: https://github.com/drozdvadym/opencv_line_descriptor. 执行代码前,需要注意,...执行线特征匹配过程如下: 首先从上面的网址下载好zip文件,然后解压,在加压后的文件里输入: mkdir

    此次线特征匹配的代码来源于: https://github.com/drozdvadym/opencv_line_descriptor.
    执行代码前,需要注意,opencv版本必须是2.4.9的,经验证opencv3在编译的过程中不仅会报错,而且无法生成可执行文件。尽管要求opencv2.4.9,但仍然还是会报错,错误如下:
    在这里插入图片描述
    目前还没有解决该错误,不过没有影响,还是会在build下面生成可执行文件的。
    执行线特征匹配过程如下:
    首先从上面的网址下载好zip文件,然后解压,在加压后的文件里输入:

    mkdir build
    cd build
    cmake ..
    make -j4
    

    然后进入build文件夹下,输入:

    ./example_line_descriptor_matching
    

    生成结果:
    在这里插入图片描述

    展开全文
  • || 学好数学和Code,走遍天下饭都有 || 三份关于线特征SLAM系统 的 开源代码…… ORB-SLAM2 :https://github.com/raulmur/ORB_SLAM2 Stereo PL-SLAM :https://githu...

    ||                            学好数学和Code,走遍天下饭都有                                        ||

     

    三份关于线特征的SLAM系统 的 开源代码…… 

     

    ---------另----------

    进阶开源VIO系统

    展开全文
  • 针对基于线特征的单目SLAM(同时定位与地图构建)中的数据关联问题,提出了一种基于线段端点patch确认的迭代数据关联算法。算法依据近似共线和端点近似重合两个指标来获取线特征的最近邻关联对,使用基于线段端点...
  • slslam----基于线特征的双目SLAM系统(关键知识点整理)1.注2. line 提取与跟踪2.1 简要2.2 主要代码2.2.1 line map 结构2.2.2 添加 line 进 line map3. 当前帧位姿估计3.1 简要3.2 主要代码 (BA)3.2.1 设置优化...

    1.注

    slslam 的论文 《Building a 3-D Line-Based Map Using Stereo SLAM》
    项目代码部分开源,也是我很喜欢的一个双目线特征SLAM,但是代码不完整,并且代码的可读性较差,后续有时间我将会对这个系统进行重写,1)完善所有的代码工程 2)让整个框架可读性增高。 目前先对这个框架配合代码进行记录

    2. line 提取与跟踪

    2.1 简要

    简要:提取使用lsd算法,跟踪采用lbd匹配,匹配包含前后帧左目线的匹配,以及右目线特征与左目线特征匹配。(为了后期方便计算,我们将提取到线特征端点坐标,转到归一化坐标系,代码中不是这样的,这里是为了方便介绍)。

    经过匹配跟踪后,我们保留的线特征有两个特点:

    1. 每个线特征在左右目是成对出现的,并且被观测到的帧超过一帧,这样一个线路标,那么它在某一帧的左右目的观测下,有两条线,四个点,8个观测值
    2. 由于线特征是基于匹配进行跟踪的,这样导致观测到此线的帧都是连续的

    2.2 主要代码

    2.2.1 line map 结构

    线路标结构中主要存储: 线特征的普吕克坐标六维度向量表示(其实不是完整的普吕克坐标),此坐标相对于它的初始帧位姿

    map<int, landmark_t *> lm_map ; // line map
    
    typedef struct
    {
        Vector6d line;
        Vector2d tt;
        Vector3d pvn;
        bool twice_observed;
        bool ba_updated;
        bool currently_visible;
        int init_kfid;
        vector<obs_t> obs_vec;
    } landmark_t;
    
    

    2.2.2 添加 line 进 line map

    // 根据 线在左右目的观测以及 基线,对线进行三角化,然后添加到地图
    void SLAM::add_lms()
    {
    
        int kfid = kfs.size() > 0 ? kfs.rbegin()->first : -1;
        // obit: 为 <线特征id,8维向量>
    
        for ( map<int, Vector8d> ::iterator obit = curr_obs.begin(); obit != curr_obs.end();
             ++obit)
        {
            // 查找观测对应的 线路标
            map<int, landmark_t *>::iterator lmit = lms.find(obit->first);
            
            // 若没有找到,说明这个线路标为 新的路标,则创建线路标
            if (lmit == lms.end())
            {
                landmark_t *lm = new landmark_t;
                lm->line = initialize_lm(obit->second, baseline); // baseline = 0.12  ; 双目 line 三角化
                lm->twice_observed = false;
                lm->ba_updated = false;
                lm->currently_visible = false;
                lm->tt = Vector2d(0, 0);
                lm->obs_vec.push_back(obs_t(kfid + 1, obit->second)); // 记录此观测:<关键帧的id ,对应的 对应的线特征 8维向量>
                lm->init_kfid = kfid + 1; // start_frame 帧
                Vector3d dv = lm->line.tail(3);  // d向量,直线的方向向量
                lm->pvn = gc_vec3_normalize(dv); // d向量,直线的方向向量 归一化
                lms.insert(make_pair(obit->first, lm));
            }
            else // 若非新线路标,则更新观测容器
            {
                lmit->second->obs_vec.push_back(obs_t(kfid + 1, obit->second));
            }
        }
    }
    

    线特征提取和跟踪完成之后,就可以把信息打包好,发给后续的位姿估计了。

    3. 当前帧位姿估计

    3.1 简要

    提取到了当前帧的线特征,与上一帧的线特征进行比较,找到共同观测到的线特征,用这些线特征来计算当前帧的位姿,具体步骤为:

    1. 找到当前帧与上一帧共同观测到的线特征集合,假设有 n 对
    2. 在n对特征中随机选择一组,通过线的数学计算(线路标与相机平面之间的关系)计算出一个大概的相机位姿,然后对其他线特征进行重投影误差reprojection_error计算,得到内点,记录内点数量,这样迭代 n次, 返回内点最多对应的相机位姿。 (也就是RANSAC 算法
    3. 得到上一步骤的内点线特征以及初始的相机位姿,进行一次 BA ;位姿用六位向量表示,固定上一帧的位姿与内点线特征对应的线路标(4维向量正交表示),仅优化当前帧位姿
    4. 优化得到的位姿是相对于上一帧的位姿

    3.2 主要代码 (BA)

    主要介绍这儿的BA 估计当前帧相对于上一帧的位姿

    3.2.1 设置优化需要的参数索引

    注意

    1. 这儿当前帧位姿设置为前面 RANSAC 得到的初始位姿,然后再转为六维向量
    2. 上一帧的位姿设置为单位矩阵,计算出来的位姿就是相对于上一帧的位姿
    vec_camera_param.push_back(gc_Rt_to_wt(T));        // 0  将待优化的位姿,转为 六维度的向量 装入相机参数
    vec_camera_param.push_back(gc_Rt_to_wt(pose_t())); // 1   第二帧位姿 直接 设置为 单位矩阵,因为是固定第二帧位姿,仅优化 第一帧位姿  (第二帧为上一帧!!!)
    
        for (size_t i = 0; i < inliers.size(); i++)
        {
            // 这个观测涉及的一个相机位姿与线路标: 相机位姿不固定,线路标固定
    
            vec_camera_index.push_back(0);
            vec_line_index.push_back(fidx);
            vec_fixed_index.push_back(0);
            vec_fixed_index.push_back(1);
            vec_observations.push_back(obs1[inliers[i]]);
    
            // (这是上一帧)这个观测涉及的一个相机位姿与线路标: 相机位姿固定,线路标固定
            vec_camera_index.push_back(1);
            vec_line_index.push_back(fidx);
            vec_fixed_index.push_back(1);
            vec_fixed_index.push_back(1);
            vec_observations.push_back(obs0[inliers[i]]);
    
            vec_line_param.push_back(gc_av_to_orth(lines[inliers[i]])); // 线路标转为正交表示
            fidx++;
        }
    

    3.2.2 build ceres

    使用自动求导,LineReprojectionError 中残差为4维向量,对应4个观测点到各自的投影直线的距离值。观测值4个点,8个数,为归一化平面上的点

        void LBAProblem::build(Problem *problem)
        {
            const int _line_block_size = line_block_size();
            const int _camera_block_size = camera_block_size();
            double *lines = mutable_lines();
            double *cameras = mutable_cameras();
    
            const double *_observations = observations();
    
            for (int i = 0; i < num_observations(); ++i)
            {
                CostFunction *cost_function;
    
                cost_function = new AutoDiffCostFunction<LineReprojectionError, 4, 6, 4>(new LineReprojectionError(_observations[8 * i + 0],
                                                                                                                   _observations[8 * i + 1],
                                                                                                                   _observations[8 * i + 2],
                                                                                                                   _observations[8 * i + 3],
                                                                                                                   _observations[8 * i + 4],
                                                                                                                   _observations[8 * i + 5],
                                                                                                                   _observations[8 * i + 6],
                                                                                                                   _observations[8 * i + 7]));
    
                // If enabled use Huber's loss function.
                // double focal_length = 320.0;
                double focal_length = 406.05;
                LossFunction *loss_function = robustify ? new HuberLoss(1.0 / focal_length) : NULL;
                //    LossFunction* loss_function = robustify ? new CauchyLoss(1.0) : NULL;
    
                double *camera = cameras + _camera_block_size * camera_index()[i];
                double *line = lines + _line_block_size * line_index()[i];
    
                problem->AddResidualBlock(cost_function, loss_function, camera, line);
    
                if (fixed_index()[2 * i])
                    problem->SetParameterBlockConstant(camera);
                if (fixed_index()[2 * i + 1])
                    problem->SetParameterBlockConstant(line);
            }
        }
    

    4. 滑动窗口优化

    4.1 简要

    数据处理步骤:

    1. 找到满足窗口大小的帧,得到每一帧包含的线路标,以及它在这个窗口下被观测的次数,大于两次观测的线路表添加进 map<int,int > lm_set 记录 ,第一个值对应 线路标在 line map 中的线id值,第二个值为被观测的次数。把所有窗口下的关键帧位姿添加到优化参数vec_camera_param中。
    2. 遍历这个 lm_set ,找到每个线路标,然后遍历这个线路标的观测帧,若这个观测帧不在窗口内,那么固定它的位姿。把线路标通过它的start_frame 位姿转到世界坐标系下,然后转为正交表示加入vec_line_param,观测添加进 vec_observations 中。
    3. 将所有参数 vec_observations ,vec_camera_param,vec_line_param,转为double 数组形式,进行ceres求解。同时还要标记,哪些线路标与帧是需要固定的。
    4. 优化完成之后,再把相机位姿更新,6维向量转为Rt ;线路标从世界坐标系下转到它的 start_frame 存储。

    4.2 主要代码 (Local BA)

    局部的滑动窗口 BA,构造如论文中图所示。 代码结构有点乱,其实若用g2o可能会更容易表示些。
    在这里插入图片描述

        // 1.遍历 ba_kfs
        // 统计每一帧中线特征帧被观测次数
        // 每一帧加入 vec_camera_param ,kfid_map 记录
        //
    
        //  it : (kf全局id ,滑动窗口内或外的序号)
        for (mii::iterator it = ba_kfs.begin(); it != ba_kfs.end(); ++it)
        {
            // if ( it->second >= (int) ba_window_size )
            if (it->second >= (int)FLAGS_ba_window_size)
                continue;
    
            keyframe_t *kf = kfs[it->first];
    
            for (set<int>::iterator lmit = kf->member_lms.begin(); lmit != kf->member_lms.end(); ++lmit)
            {
    
                mii::iterator lit = lm_set.find(*lmit);
    
                if (lit == lm_set.end())
                    lm_set.insert(make_pair(*lmit, 1));
                else
                    lit->second++;
            }
    
            vec_kfs.push_back(kf);
            vec_camera_param.push_back(gc_Rt_to_wt(kf->T));
            kfid_map.insert(make_pair(it->first, kfidx++)); // ( kf的全局id,index )
        }
    
        int lmidx = 0;
        mii lmid_map;
        vector<landmark_t *> vec_lms;
    
        // 遍历ba_kfs中统计的 线 lm_set
        for (mii::iterator lit = lm_set.begin(); lit != lm_set.end(); ++lit)
        {
            if (lit->second < 2) // 过滤少于两次的观测
                continue;
            // lmit :(线路标的全局id,线路标指针对象)
            lm_map::iterator lmit = lms.find(lit->first); // 在总的线特征集合 lms 查看
            if (lmit == lms.end())                        // 首先要被找到
                continue;
            lmit->second->twice_observed = true;
            lmit->second->ba_updated = true;
    
            // 对找到的 线landmark 遍历其所有的观测
            for (size_t j = 0; j < lmit->second->obs_vec.size(); ++j)
            {
    
                // lmit :(线路标的全局id,线路标指针对象)
    
                obs_t &obt = lmit->second->obs_vec[j];
    
                if (ba_kfs.find(obt.id) == ba_kfs.end()) // 此观测所对应的 帧不是在 ba_kfs 中的,则过滤
                    continue;
    
                // iit : (kf的全局id,index)
                mii::iterator iit = kfid_map.find(obt.id); // 查找 :帧与其对应的新序号
                if (iit == kfid_map.end())                 // 若此帧是未包含在 ba_kfs 中的,那么添加
                {
                    vec_fixed_index.push_back(1); // 说明此帧,不属于滑动窗口内 !!! 需要固定
                    vec_camera_index.push_back(kfidx);
    
                    keyframe_t *kf = kfs[obt.id];
    
                    vec_kfs.push_back(kf);
                    vec_camera_param.push_back(gc_Rt_to_wt(kf->T)); // 加入到 相机优化参数数组中
                    kfid_map.insert(make_pair(obt.id, kfidx++));
                }
                else // 此帧已经在 ba_kfs 中(滑动窗口中)
                {
                    // if ( iit->second < (int) ba_window_size )
                    if (iit->second < (int)FLAGS_ba_window_size) // 少于  FLAGS_ba_window_size,则需要优化
                        vec_fixed_index.push_back(0);
                    else // 超过窗口大小,也设置为 固定
                        vec_fixed_index.push_back(1);
                    vec_camera_index.push_back(iit->second); // 相机数量 index
                }
                // lmit :(线路标的全局id,线路标指针对象)
    
                //  lmid_map 为map : (线路标的全局id,线的新index)
    
                iit = lmid_map.find(lmit->first);
                if (iit == lmid_map.end())
                {
                    vec_line_index.push_back(lmidx);
                    //  lmid_map 为map : (线路标的全局id,线的新index)
                    lmid_map.insert(make_pair(lmit->first, lmidx++));
                }
                else // 已经出现过,向向量中添加 线的index
                {
                    vec_line_index.push_back(iit->second);
                }
                // 将在 滑动窗口内的 关键帧所对应的 线路标的观测添加进入 观测向量
                vec_observations.push_back(obt.obs);
            }
            // 将线(普吕克坐标)转到它第一次被观测的相机的坐标系下
            Vector6d line = gc_line_from_pose(lmit->second->line, kfs[lmit->second->init_kfid]->T);
            // 将 普吕克坐标转为 正交表示
            vec_line_param.push_back(gc_av_to_orth(line));
            vec_lms.push_back(lmit->second); // 把线路标加入 vec_lms 中
        }
    
    

    5. 回环检测与位姿图优化

    1. 回环检测 基于 LBD 与 DBOW
    2. 位姿图优化通过添加帧与帧之间的边来构建pose graph

    由于之前开源的代码也不完整,可读性一般,结构乱,等后面有时间了 我重写完这个工程,再说明吧。

    展开全文
  • 基于点和线特征的实时单目SLAM

    千次阅读 热门讨论 2017-11-30 09:42:01
    PL-SLAM: Real-Time Monocular Visual ...但是,有许多环境,尽管纹理少,仍然可以可靠地估计基于线的几何图元,例如在城市中或者室内场景,或者所谓的“曼哈顿世界”,其中结构化边缘是主要的。本文中,我们提出了...
  • [SLAM]2D激光线特征提取

    千次阅读 2016-07-16 11:03:00
    感觉对噪声数据真的没办法了,窝成一团的点,提取的线十分破碎而且乱...  1 function [ lineSegCoord ] = extractLineSegment( model,normals,intervalPts,normalDelta,dThreshold) 2 %...
  • SLAM中基于线特征的初始化方法线特征的初始化直线段检测算法---LSD:a Line Segment Detector描述子---LBD:Line detection and description计算旋转矩阵三焦点张量计算t 线特征的初始化 ORB_SLAM2的东西差不多讲完...
  • 使用点和线特征的激光雷达-单目视觉里程计 首发在泡泡机器人slam公众号 标题:Lidar-Monocular Visual Odometry using Point and Line Features 作者:Shi-Sheng Huang, Ze-Yu Ma, Tai-Jiang Mu, Hongbo Fu, and Shi...
  • 一、线特征及描述子 0. 点、线特征优缺点对比 线特征:优点 在于具有天然的光照及视角不变性,同时更高级的特征也使追踪的鲁棒性和准确性有所提高。特别是在特定的人造场景(室内,走廊)等场景,能够克服无纹理...
  • 作为svo系列的改进版本,pl-svo增加了线特征。一定程度上的增加了svo的稳定性。不过,据我自己评测结果来看,速度的确是一如既往的快,但是稳定性和准确性吗,就和svo一样一言难尽了(和svo有所提升,具体见论文)。...
  • 有两个关于点线特征综合的视觉slam代码,还有10篇左右的参考文献
  • 这个工具的作用就是构造一个三维的路标,再根据一系列的pose将这些路标投影到一系列照片上,也就是视频。可以用来进行VO-SLAM的仿真,也很容易拓展为VIO-SLAM的仿真工具。github地址:[LISS]
  • 大家好,对于VSLAM研究人员当然要对最新的VSLAM的研究做下总结了,这次为大家介绍PL-SLAM。 上图就是PL-SLAM的框架,是不是感觉和ORBSLAM很像呢?比较典型打特征法+直接法的结合的关键帧的Vslam 跟ORBSLAM有三个...
  • SLAM

    2020-10-20 13:13:21
    SLAM 2020/10/20 卡尔曼滤波: 两大问题: 计算量大:需要计算系统的协方差矩阵 不确定性:受外界因素影响 粒子滤波器法: 将SLAM问题分解为机器人的定位和路标集合的估计两个子问题来进行求解 步骤 ...
  • PL-SLAM: 基于点和线条的实时单目slam

    万次阅读 热门讨论 2017-05-31 13:16:30
    大家好,对于VSLAM研究人员当然要对最新的VSLAM的研究做下总结了,这次为大家介绍PL-SLAM。 上图就是PL-SLAM的框架,是不是感觉和ORBSLAM很像呢?比较典型打特征法+直接法的结合的关键帧的Vslam 跟ORBSLAM有三个...