VSLAM::[手寫VIO_課堂筆記]第三講(上)_基於優化的IMU與視覺信息融合(最小二乘問題)

1. 第三講(上)_基於優化的IMU與視覺信息融合(上)

第三講(上):
(1)最小二乘問題的求解推導
(2)其中有LM算法的相關推導以及魯棒核函數的推導

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-DVaCKDuP-1581175171790)(imgs/2020-02-08-19-52-55.png)]

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-dX8rieuQ-1581175171791)(imgs/2020-02-08-19-54-46.png)]

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-FLUop90t-1581175171791)(imgs/2020-02-08-19-59-10.png)]

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-YRJrFsBU-1581175171792)(imgs/2020-02-08-20-00-24.png)]

1.1. 最小二乘與非線性優化

1.1.1. 最小二乘

在這裏插入圖片描述

在這裏插入圖片描述

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-YHU1bruK-1581175171793)(imgs/2020-02-08-20-07-32.png)]

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-5pK2ovtE-1581175171794)(imgs/2020-02-08-20-10-23.png)]

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-ycYWtcw9-1581175171794)(imgs/2020-02-08-20-18-16.png)]

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-3zoCGIx3-1581175171795)(imgs/2020-02-08-20-21-03.png)]

1.1.2. 非線性最小二乘

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-yMgrvFWd-1581175171795)(imgs/2020-02-08-20-23-01.png)]

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-OJeawyVX-1581175171796)(imgs/2020-02-08-20-28-03.png)]

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-CjoHNJp6-1581175171796)(imgs/2020-02-08-20-33-24.png)]

1.1.2.1. LM算法增量ΔX\Delta X表達式推導

求解如下:
(JTJ+μI)ΔX=JTf(VΛVT+μI)ΔX=JTf(V(Λ+μI)VT)ΔX=JTf(Λ+μI)VTΔX=VTJTf(Λ+μI)VTΔX=VTF(x)TVTΔX=(Λ+μI)1VTF(x)T \begin{aligned} &(\boldsymbol{J}^T\boldsymbol{J}+\mu\boldsymbol{I})\boldsymbol{\Delta X}=-\boldsymbol{J}^T\boldsymbol{f} \\ &\Longrightarrow (\boldsymbol{V \Lambda V^T}+\mu \boldsymbol{I})\boldsymbol{\Delta X}=-\boldsymbol{J}^T\boldsymbol{f} \\ &\Longrightarrow(\boldsymbol{V} (\boldsymbol{\Lambda}+\mu \boldsymbol{I})\boldsymbol{V}^T) \boldsymbol{\Delta X}=-\boldsymbol{J}^T\boldsymbol{f} \\ &\Longrightarrow(\boldsymbol{\Lambda}+\mu \boldsymbol{I})\boldsymbol{V}^T\boldsymbol{\Delta X}=-\boldsymbol{V}^T\boldsymbol{J}^T\boldsymbol{f} \\ &\Longrightarrow(\boldsymbol{\Lambda}+\mu \boldsymbol{I})\boldsymbol{V}^T\boldsymbol{\Delta X}=-\boldsymbol{V}^T F^{'}(x)^T \\ &\Longrightarrow\boldsymbol{V}^T\boldsymbol{\Delta X}=-(\boldsymbol{\Lambda}+\mu \boldsymbol{I})^{-1}\boldsymbol{V}^T F^{'}(x)^T \\ \end{aligned}

VTΔX=[1λ1+μ0001λ2+μ001λj+μ]VTF(x)T \begin{aligned} \boldsymbol{V}^T\boldsymbol{\Delta X}=- \begin{bmatrix} \frac{1}{\lambda_1+\mu}&0&0& \cdots \\ 0 & \frac{1}{\lambda_2+\mu} & 0 &\cdots \\ 0 & \cdots & & \frac{1}{\lambda_j+\mu} \end{bmatrix} \boldsymbol{V}^T F^{'}(x)^T \end{aligned}

ΔX=[v1vj][1λ1+μ0001λ2+μ001λj+μ][v1vj]F(x)T \begin{aligned} \boldsymbol{\Delta X}=- \begin{bmatrix} \boldsymbol{v_1} & \cdots &\boldsymbol{v_j} \end{bmatrix} \begin{bmatrix} \frac{1}{\lambda_1+\mu}&0&0& \cdots \\ 0 & \frac{1}{\lambda_2+\mu} & 0 &\cdots \\ 0 & \cdots & & \frac{1}{\lambda_j+\mu} \end{bmatrix} \begin{bmatrix} \boldsymbol{v_1} \\ \vdots \\ \boldsymbol{v_j} \end{bmatrix} F^{'}(x)^T \end{aligned}

ΔX=j=1nvjTFTλj+μvj \begin{aligned} \therefore \boldsymbol{\Delta X}= -\sum_{j=1}^{n}\frac{\boldsymbol{v}_j^T {\boldsymbol{F'}}^T}{\lambda_j+\mu} \boldsymbol{v}_j \end{aligned}


[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-53rVW8Hm-1581175171797)(imgs/2020-02-08-20-49-03.png)]

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-4SahEgEc-1581175171797)(imgs/2020-02-08-20-52-54.png)]

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-3hVqI6Bb-1581175171797)(imgs/2020-02-08-20-56-19.png)]

從上圖可以看到,由於μ\mu的值在抖動,使得ΔX\Delta X的步長在來回震盪變化,使得loss值也隨着抖動,有一部分的計算是多餘的.

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-078WyV7m-1581175171797)(imgs/2020-02-08-20-56-29.png)]

1.1.2.2. 魯棒核函數相關推導

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-B2QXTkke-1581175171798)(imgs/2020-02-08-20-57-54.png)]

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-QQbqdQhE-1581175171798)(imgs/2020-02-08-21-09-40.png)]

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-ibOzpdtt-1581175171798)(imgs/2020-02-08-20-59-45.png)]

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-D0dLitrs-1581175171799)(imgs/2020-02-08-20-59-57.png)]

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-LYdb0TcJ-1581175171799)(imgs/2020-02-08-21-00-20.png)]

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-MbesSX6U-1581175171800)(imgs/2020-02-08-21-18-55.png)]

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片保存下來直接上傳(img-KgxbmfdM-1581175171800)(imgs/2020-02-08-21-20-32.png)]

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章