DSO前端跟蹤
嗨，各位讀者朋友們好！今天我們接着講前端跟蹤。由於第三講中的初始化部分包含trackFrame估計初始兩幀運動，與本講內容重疊，因此筆者將其初始化中的跟蹤合並到當前講中，具體的區別稍后我們會在介紹跟蹤流程的過程中敘述。
OK，我們還是一如既往地先上個流程圖，如圖1所示。筆者始終認為只有把流程圖畫清晰了，才能說明自己對整個流程足夠了解。在寫博客的時候，流程圖有助於梳理思路，便於讀者們更加清楚地知道整個運行邏輯。
圖1. DSO前端跟蹤流程
特征點法SLAM中可以通過首先進行參考幀和當前幀的特征點匹配找到匹配對，構建數據關聯關系，從而估計相對運動。而基於直接法的DSO前端跟蹤，卻沒有這么方便了，因為直接法中的點都是局部梯度較大的點，除此之外並無其他顯著特征，因為無法做重投影誤差。而直接法的策略是將參考幀中所有點（提取到的梯度點）投影到當前幀中，構建光度誤差函數，聯合優化。那么這里，有個問題就需要特別注意了，直接法無法構建數據關聯關系，那么初始的相對運動就無法確定，所以直接法需要首先設置運動模型，才能繼續后面的步驟。
接下來，我們按照圖1來大致梳理一下DSO前端跟蹤的思路：
1. 設置運動模型，如前所述，需要根據先驗值給出運動模型。
在DSO初始化階段，這個先驗值被設置為旋轉單位陣、位移零向量，表示兩幀間完全重合的意思。
在DSO常規的跟蹤階段，這個先驗值通常是由前面幾幀確定的，比如前一幀和前前幀之間的相對運動作為當前幀的運動模型，稱為單倍速運動模型。DSO還添加了兩倍速、0.5倍速模型，此外還有在此基礎上的一些較小的旋轉模型等。
2. 遍歷所有運動模型，對於每個運動模型都執行后續的直接法估計。只有前面存在滿足要求的運動模型時，后續的模型才不會被繼續驗證，跳出循環。若是所有運動模型都不滿足要求，則說明跟蹤丟失。
在DSO初始化階段，循環迭代會設置跳出條件，除了滿足收斂或者迭代次數達到最大值這兩種情況，初始化階段還設置了一個fails次數，若次數過多，則當前初始化失敗，重新開始。
在DSO常規的跟蹤階段，循環迭代設置的跳出條件是滿足收斂條件或者迭代次數達到最大值。若最終閾值不滿足條件，則驗證下一個運動模型。
3. 直接法運動估計，采用的是有點類似於光流的方法，在這里需要用到圖像金字塔，目的是為了節省計算時間。
從圖像金字塔最頂層開始估計相對運動，並逐漸向下傳播。值得注意的是，我們在上一講中提到了makeNN()這個函數，他的目的是創建每一層每個點的10鄰域信息，並且關聯其上一層的某個最近鄰點。顯然，在基於圖像金字塔的跟蹤過程，上一層的深度值可以傳播到下一層作為初值，這便是第三講的內容最重要的意義。
4. 高斯牛頓優化，這部分內容是整個跟蹤過程最核心的，也是最難的。
已知：初始的運動模型、參考幀圖像中的梯度點、參考幀和當前幀構建的光度誤差函數。
問題：通過優化運動模型，使得光度誤差函數的代價值最小。
求解策略：利用泰勒展開近似非線性函數，通過高斯牛頓法構建增量方程：$Hx = b$。后續便是基於schur消元法求解增量值，基於判斷條件對運動模型進行更新。
在上面的四個主要流程中，最核心的便是第4個——高斯牛頓優化。我們會在這一講着重介紹一下DSO中在直接法運動估計中，這個高斯牛頓法是怎樣操作的。
直接法運動估計
在前面我們提到過，直接法不同於特征點法，可以通過特征匹配來構建數據關聯關系。那么直接法通常的做法呢，是將兩個參考圖像中提取到的梯度點全部投影到當前圖像中，通過調整位姿和光度仿射模型使得兩幀圖像間的光度誤差值最小。
為了更好地描述問題，我們會采用數學符號來完成這一講的內容。在此之前呢，我們要先定義一下符號。（昨天吃飯的時候才剛跟范帝楷聊過這個事情）
假設我們當前有參考幀 $I_{r}$ 和當前幀 $I_{c}$，參考幀中的光度仿射參數為 $(a_{r}, b_{r})$， 同理，當前幀的為 $(a_{c}, b_{c})$。相機內參數矩陣為 $K$，由參考幀 $I_{r}$ 的坐標系到當前幀 $I_{c}$ 的坐標系相對變換的李代數為 $\xi_{cr}$，並且我們有：
\begin{equation}
exp(\xi^{\wedge}_{cr}) = \begin{bmatrix}R_{cr} & t_{cr} \\ 0^{T} & 1\end{bmatrix}
\end{equation}
OK，那么我們接下來開始描述光度誤差函數是怎么定義的。
對於單一的一個點，我們先不考慮DSO中描述的pattern，只單純的考慮對於一個像素，這個光度誤差函數怎么描述。
\begin{equation}
r = w((I_{c}[p_{c}] - b_{c}) - \frac{t_{c} e^{a_{c}}}{t_{r}e^{a_{r}}}(I_{r}[p_{r}] - b_{r})) = w(I_{c}[p_{c}] - a_{cr}I_{r}[p_{r}] - b_{cr})
\end{equation}
其中，點 $p_{r} = [u_{r}, v_{r}, 1]^{T} $ 為參考幀 $I_{r}$ 中梯度較大的點，$p_{c} = [u_{c}, v_{c}, 1]^{T}$ 表示點 $p_{r}$ 在當前幀 $I_{c} $ 中的反投影坐標。具體計算方法：
\begin{equation} \begin{aligned}
p_{c} &= KX_{c}^{'} \\ &= Kd_{c}X_{c} \\ &= Kd_{c}(R_{cr}X_{r} + t_{cr}) \\ &= Kd_{c}(d_{r}^{-1}R_{cr}K^{-1}p_{r} + t_{cr}) \\ &= Kd_{c}exp(\xi^{\wedge}_{cr})(d_{r}^{-1}K^{-1}p_{r})
\end{aligned} \end{equation}
在公式 $(3)$ 中，我們定義了一些新的符號：
1. 點 $p_{r}$ 在參考幀相機坐標系中的點表示為 $X_{r} = d_{r}^{-1}K^{-1}p_{r}$；
2. 點 $p_{r}$ 轉換到當前幀相機坐標系中的點表示為 $X_{c} = R_{cr}X_{r} + t_{cr} = exp(\xi^{\wedge}_{cr})X_{r}$；
3. 點 $p_{r}$ 在當前幀相機坐標系中的歸一化坐標表示為 $X_{c}^{'} = d_{c}X_{c}$；
4. 點 $p_{r}$ 在當前幀相機平面上的像素點坐標表示為 $p_{c} = KX_{c}^{'}$。
值得注意的是，公式 $(3)$ 中我們忽略了部分齊次坐標的轉換，這些轉換盡管影響不大，但是還是希望讀者們能夠知道這里哪些地方的維度發生了變化。
OK，接下來我們開始討論對於上述定義的光度誤差函數，我們會用什么方式來優化？
我們假設待優化的變量為 $x = [\xi_{cr}, a_{cr}, b_{cr}, d]$。其中 $d$ 為單一一個點的逆深度值，對於單個點的能量函數，我們有：
\begin{equation}
E(x) = r(x)^{T}r(x)
\end{equation}
實際上，殘差的權重已經是經過Huber核函數決策得到的了。DSO論文中提到一個關於梯度的權重，這個權重是用在滑窗優化中的，在跟蹤階段並不涉及。因此我們不做討論。
對於公式 $(4)$ 中的能量函數 $E(x)$，我們采用高斯-牛頓法進行迭代優化，每次添加一個增量 $\Delta x$，通過泰勒展開可以得到：
\begin{equation} \begin{aligned}
E(x + \Delta x) &= r(x + \Delta x)^{T} r(x + \Delta x) \\ &\approx (r(x) + J\Delta x)^{T}(r(x) + J\Delta x) \\ &= r(x)^{T}r(x) + 2\Delta x^{T} J^{T}r(x) + \Delta x^{T}J^{T}J\Delta x
\end{aligned} \end{equation}
其中，$J$ 表示殘差 $r$ 對變量 $x$ 的雅克比矩陣 $\frac{\partial r}{\partial x}$。
通過對增量 $\Delta x$ 進行求導，並令導數結果為 $0$，我們可以得到：
\begin{equation}
\frac{\partial E}{\partial \Delta x} = 2J^{T}r(x) + 2J^{T}J\Delta x = 0 \Rightarrow J^{T}J\Delta x = -J^{T} r(x) \Rightarrow H\Delta x = -b
\end{equation}
在DSO中通常包含兩種待優化變量，第一種是跟機器人自身本體相關的變量：位姿+光度仿射參數；第二種是跟環境相關的變量：環境的點雲信息（即觀測點）。但是在前端的跟蹤中，我們並不考慮優化點雲的深度值，因此我們可以采用schur消元法來簡化上述的優化問題。我們用下標 $x$ 來表示位姿+光度仿射參數，用下標 $d$ 表示點的逆深度信息，那么我們可以將增量方程進行分塊：
\begin{equation}
H = \begin{bmatrix}H_{xx} & H_{xd} \\ H_{dx} & H_{dd}\end{bmatrix}
\end{equation}
那么增量方程采用schur消元結果如下：
\begin{equation} \begin{aligned}
\begin{bmatrix}I & -H_{xd}H_{dd}^{-1} \\ 0 & I\end{bmatrix} \begin{bmatrix}H_{xx} & H_{xd} \\ H_{dx} & H_{dd}\end{bmatrix} \begin{bmatrix}\Delta x_{x} \\ \Delta x_{d}\end{bmatrix} = -\begin{bmatrix}I & -H_{xd}H_{dd}^{-1} \\ 0 & I\end{bmatrix} \begin{bmatrix}b_{x} \\ b_{d}\end{bmatrix} \\ \Rightarrow \begin{bmatrix}(H_{xx} - H_{xd}H_{dd}^{-1}H_{dx})\Delta x_{x} \\ H_{dx}\Delta x_{x} + H_{dd}\Delta x_{d}\end{bmatrix} = -\begin{bmatrix}b_{x} - H_{xd}H_{dd}^{-1}b_{d} \\ b_{d}\end{bmatrix}
\end{aligned} \end{equation}
我們最終需要求解的增量方程就是下面這個：
\begin{equation} \begin{aligned}
(H_{xx} - H_{xd}H_{dd}^{-1}H_{dx})\Delta x_{x} = b_{x} - H_{xd}H_{dd}^{-1}b_{d}
\end{aligned} \end{equation}
實際上，我們知道 $H_{xd} = H_{dx}^{T}$，而 $H_{xd}$ 可由 $J_{x}^{T}J_{d}$ 計算得到，$J_{x}$ 表示殘差對位姿+光度仿射參數的雅克比，$J_{d}$ 表示殘差對點的逆深度的雅克比。
知道了我們的終極目標以后，接下來就是把這個目標分解成一個個小問題。顯然，我們需要求 $J_{x}$ 和 $J_{d}$，下面我們就開始推導了。
直接法雅克比推導
首先是推導 $J_{x}$，這里我們參考了塗金戈同學的博客直接法光度誤差導數推導，將雅克比的計算過程重新梳理了一番。
光度仿射參數 $(a, b)$
由公式 $(2)$ 可知：
\begin{equation}
\frac{\partial r}{\partial a_{cr}} = -w I_{r}[p_{r}]
\end{equation}
\begin{equation}
\frac{\partial r}{\partial b_{cr}} = -w
\end{equation}
其實在金戈同學的博客中，他認為DSO的代碼寫錯了，因為DSO中的代碼對應的公式為：$\frac{\partial r}{\partial a_{cr}} = -wa_{cr}I_{r}[p_{r}]$。在這個問題上，我和金戈同學的觀點有點不太一樣。特別是，金戈同學有過疑惑，這個地方到底是 $r$ 對 $a_{cr}$ 的雅克比，還是 $r$ 對 $a_{c}$ 的雅克比？
實際上筆者認為這個地方應該問題不大，我們簡單看看：
1. 如果是 $r$ 對 $a_{cr}$ 的雅克比，那么結果應該就是 $\frac{\partial r}{\partial a_{cr}} = -w I_{r}[p_{r}]$
2. 如果是 $r$ 對 $a_{c}$ 的雅克比，那么結果應該就是 $\frac{\partial r}{\partial a_{cr}} = -wa_{cr}(I_{r}[p_{r}] - b_{r})$
我們仔細分析一下哈，如果說是第一種情況，那么代碼中就不需要多一個 $a_{cr}$，顯然這個是多余的。如果是第二種情況，這里有個特例需要注意一下，我們在初始化階段，$b_{r}$ 是為 $0$ 的，因此可以退化為成DSO初始化代碼中的形式，即 $\frac{\partial r}{\partial a_{c}} = -wa_{cr}(I_{r}[p_{r}] - b_{r}) = -wa_{cr}I_{r}[p_{r}]$，並且 $\frac{\partial r}{\partial b_{c}} = \frac{\partial (-w(b_{c} + a_{cr}b_{r}))}{\partial b_{c}} = -w$。
值得注意的是，在后續 tracking階段，DSO采用的都是對 $a_{c}$ 的雅克比，即第二種情況。如果從代碼的一致性來說，筆者也比較傾向第二種，初始化階段這里也是第二種情況。歡迎有不同觀點的讀者朋友一起交流哈。
位姿 $\xi_{cr}$
回顧一下公式 $(3)$，筆者之所以將其寫得那么細致，目的是為了在這里求雅克比的時候，可以非常自然而然做鏈式法則。
\begin{equation} \begin{aligned}
\frac{\partial r}{\partial \xi_{cr}} &= \frac{\partial r}{\partial I_{c}} \frac{\partial I_{c}}{\partial p_{c}} \frac{\partial p_{c}}{\partial X_{c}^{'}} \frac{\partial X_{c}^{'}}{\partial \xi_{cr}} \\ &= w \begin{bmatrix}g_{x} & g_{y} & 0\end{bmatrix} \begin{bmatrix}f_{x} & 0 & 0 \\ 0 & f_{y} & 0 \\ 0 & 0 & 0\end{bmatrix} \frac{\partial d_{c}X_{c}}{\partial \xi_{cr}}
\end{aligned} \end{equation}
值得注意的是，$g_{x}, g_{y}$ 表示點 $p_{c}$ 在當前幀圖像 $I_{c}$ 中的梯度值。那么接下來，我們就剩最后一項要求了。
假設 $X_{c} = \begin{bmatrix}x_{c} & y_{c} & z_{c} \end{bmatrix}^{T}$，那么$d_{c} = \frac{1}{z_{c}}$，所以兩個變量都跟 $\xi_{cr}$ 有關，因此求雅克比的時候需要我們需要展開：
\begin{equation}
\frac{\partial d_{c}X_{c}}{\partial \xi_{cr}} = \frac{\partial d_{c}}{\partial \xi_{cr}}X_{c} + \frac{\partial X_{c}}{\partial \xi_{cr}}d_{c}
\end{equation}
在視覺SLAM十四講中，李代數那一講中介紹了基於左擾動模型時， $Tp$ 對李代數 $\xi$ 求導的結果是 $(Tp)^{\odot}$，那么在這里我們就直接寫結果了：
\begin{equation}
\frac{\partial X_{c}}{\partial \xi_{cr}} = \begin{bmatrix}1 & 0 & 0 & 0 & z_{c} & -y_{c} \\ 0 & 1 & 0 & -z_{c} & 0 & x_{c} \\ 0 & 0 & 1 & y_{c} & -x_{c} & 0 \end{bmatrix}
\end{equation}
我們知道 $d_{c} = \frac{1}{z_{c}}$，那么由鏈式法則可知：$\frac{\partial d_{c}}{\partial \xi_{rc}} = \frac{\partial d_{c}}{\partial z_{c}} \frac{\partial z_{c}}{\partial \xi_{cr}}$。綜上，我們可以知道 $\frac{\partial d_{c}X_{c}}{\partial \xi_{cr}}$ 的結果如下：
\begin{equation} \begin{aligned}
\frac{\partial d_{c}X_{c}}{\partial \xi_{cr}} &= \frac{\partial d_{c}}{\partial \xi_{cr}}X_{c} + \frac{\partial X_{c}}{\partial \xi_{cr}}d_{c} \\ &= -\frac{1}{z_{c}^{2}} \begin{bmatrix}0 & 0 & x_{c}& x_{c}y_{c} & -x_{c}^{2} & 0 \\ 0 & 0 & y_{c} & y_{c}^{2} & -x_{c}y_{c} & 0 \\ 0 & 0 & z_{c} & y_{c}z_{c} & -x_{c}z_{c} & 0\end{bmatrix} + \frac{1}{z_{c}}\begin{bmatrix}1 & 0 & 0 & 0 & z_{c} & -y_{c} \\ 0 & 1 & 0 & -z_{c} & 0 & x_{c} \\ 0 & 0 & 1 & y_{c}& -x_{c} & 0\end{bmatrix} \\ &= \begin{bmatrix}\frac{1}{z_{c}} & 0 & -\frac{x_{c}}{z_{c}^{2}} & -\frac{x_{c}y_{c}}{z_{c}^{2}} & 1 + \frac{x_{c}^{2}}{z_{c}^{2}} & -\frac{y_{c}}{z_{c}} \\ 0 & \frac{1}{z_{c}} & -\frac{y_{c}}{z_{c}^{2}} & -1 - \frac{y_{c}^{2}}{z_{c}^{2}} & \frac{x_{c}y_{c}}{z_{c}^{2}} & \frac{x_{c}}{z_{c}} \\ 0 & 0 & 0 & 0 & 0 & 0\end{bmatrix}
\end{aligned} \end{equation}
由公式 $(3)$ 我們知道， $X_{c}^{'}$ 是 $X_{c}$ 的歸一化坐標，我們假設 $X_{c}^{'} = \begin{bmatrix}u_{c}^{'} \\ v_{c}^{'} \\ 1\end{bmatrix}$，那么公式 $(15)$ 可簡化為：
\begin{equation} \begin{aligned}
\frac{\partial d_{c}X_{c}}{\partial \xi_{cr}} &= \begin{bmatrix}d_{c} & 0 & -d_{c}u_{c}^{'} & -u_{c}^{'}v_{c}^{'} & 1 + u_{c}^{'2} & -v_{c}^{'} \\ 0 & d_{c} & -d_{c}v_{c}^{'} & -1-v_{c}^{'2} & u_{c}^{'}v_{c}^{'} & u_{c}^{'} \\ 0 & 0 & 0 & 0 & 0 & 0\end{bmatrix}
\end{aligned} \end{equation}
聯立公式 $(10-12)(16)$，我們可以得到最終的雅克比 $J_{x}$：
\begin{equation} \begin{aligned}
J_{x} &= \begin{bmatrix}\frac{\partial r}{\partial \xi_{cr}} & \frac{\partial r}{\partial a_{cr}} & \frac{\partial r}{\partial b_{cr}}\end{bmatrix} \\ &= \begin{bmatrix}wg_{x}f_{x}d_{c} \\ wg_{y}f_{y}d_{c} \\ -d_{c}(wg_{x}f_{x}u_{c}^{'} + wg_{y}f_{y}v_{c}^{'}) \\ -wg_{x}f_{x}u_{c}^{'}v_{c}^{'}-wg_{y}f_{y}(1+v_{c}^{'2}) \\ wg_{x}f_{x}(1+u_{c}^{'2}) + wg_{y}f_{y}(u_{c}^{'}v_{c}^{'}) \\ -wg_{x}f_{x}v_{c}^{'} + wg_{y}f_{y}u_{c}^{'2} \\ -wa_{cr}I_{r}[p_{r}] \\ -w \end{bmatrix}^{T}
\end{aligned} \end{equation}
OK，至此，我們就把跟機器人本體狀態相關的變量雅克比求解完成。接下來再把這個逆深度值的雅克比算一算，我們這一講的內容就完整了。
逆深度 $d_{r}$
同樣，我們參考公式 $(3)$，我們有 $p_{c} = Kd_{c}(d_{r}^{-1}R_{cr}K^{-1}p_{r} + t_{cr})$，我們可以參考塗金戈同學的博客將 $R_{cr}K^{-1}p_{r}$ 看成一個整體，定義為矩陣 $A$，實際上根據矩陣相乘的維度變化我們可以知道，最終 $A$ 是三維的點。那么我們可以將公式 $(3)$ 寫成如下形式：
\begin{equation} \begin{aligned}
p_{c} &= Kd_{c}(d_{r}^{-1}R_{cr}K^{-1}p_{r} + t_{cr}) \\ &= Kd_{c}(d_{r}^{-1}A + t_{cr}) \\ &= Kd_{c}\begin{bmatrix}d_{r}^{-1}a_{1} + t_{x} \\ d_{r}^{-1}a_{2} + t_{y} \\ d_{r}^{-1}a_{3} + t_{z}\end{bmatrix} \\ &= KX_{c}^{'}
\end{aligned} \end{equation}
殊途同歸，殘差對點的逆深度值雅克比可以采用鏈式法則求解：
\begin{equation} \begin{aligned}
\frac{\partial r}{\partial d_{r}} &= \frac{\partial r}{\partial I_{c}} \frac{\partial I_{c}}{\partial p_{c}} \frac{\partial p_{c}}{\partial X_{c}^{'}} \frac{\partial X_{c}^{'}}{\partial d_{r}} \\ &= w \begin{bmatrix}g_{x} & g_{y} & 0\end{bmatrix} \begin{bmatrix}f_{x} & 0 & 0 \\ 0 & f_{y} & 0 \\ 0 & 0 & 0\end{bmatrix} \frac{\partial X_{c}^{'}}{\partial d_{r}}
\end{aligned} \end{equation}
OK，我們的最終目標就是求解 $\frac{\partial X_{c}^{'}}{\partial d_{r}}$ 這個東西了。由公式 $(18)$得到：
\begin{equation} \begin{aligned}
X_{c}^{'} &= d_{c}\begin{bmatrix}d_{r}^{-1}a_{1} + t_{cr}^{x} \\ d_{r}^{-1}a_{2} + t_{cr}^{y} \\ d_{r}^{-1}a_{3} + t_{cr}^{z}\end{bmatrix} = \begin{bmatrix}u_{c}^{'} \\ v_{c}^{'} \\ 1\end{bmatrix}
\end{aligned} \end{equation}
顯然，我們可以得到 $d_{c} = (d_{r}^{-1}a_{3} + t_{cr}^{z})^{-1}$，那么 $u_{c}^{'}，v_{c}^{'}$ 可以記為：
\begin{equation} \begin{aligned}
\begin{bmatrix}u_{c}^{'} \\ v_{c}^{'}\end{bmatrix} = \begin{bmatrix}\frac{d_{r}^{-1}a_{1} + t_{cr}^{x}}{d_{r}^{-1}a_{3} + t_{cr}^{z}} \\ \frac{d_{r}^{-1}a_{2} + t_{cr}^{y}}{d_{r}^{-1}a_{3} + t_{cr}^{z}}\end{bmatrix}
\end{aligned} \end{equation}
接下來就是單變量的求導，筆者就不展開了，直接寫結果：
\begin{equation} \begin{aligned}
\frac{\partial X_{c}^{'}}{\partial d_{r}} = \begin{bmatrix}d_{r}^{-1}d_{c}(t_{cr}^{x} - u_{c}^{'}t_{cr}^{z}) \\ d_{r}^{-1}d_{c}(t_{cr}^{y} - v_{c}^{'}t_{cr}^{z}) \\ 0\end{bmatrix}
\end{aligned} \end{equation}
那么根據鏈式法則，我們最終的雅克比矩陣應該是：
\begin{equation} \begin{aligned}
\frac{\partial r}{\partial d_{r}} &= \frac{\partial r}{\partial I_{c}} \frac{\partial I_{c}}{\partial p_{c}} \frac{\partial p_{c}}{\partial X_{c}^{'}} \frac{\partial X_{c}^{'}}{\partial d_{r}} \\ &= w \begin{bmatrix}g_{x} & g_{y} & 0\end{bmatrix} \begin{bmatrix}f_{x} & 0 & 0 \\ 0 & f_{y} & 0 \\ 0 & 0 & 0\end{bmatrix} \begin{bmatrix}d_{r}^{-1}d_{c}(t_{cr}^{x} - u_{c}^{'}t_{cr}^{z}) \\ d_{r}^{-1}d_{c}(t_{cr}^{y} - v_{c}^{'}t_{cr}^{z}) \\ 0\end{bmatrix} \\ &= wd_{r}^{-1}d_{c}(g_{x}f_{x}(t_{cr}^{x} - u_{c}^{'}t_{cr}^{z}) + g_{y}f_{y}(t_{cr}^{y} - v_{c}^{'}t_{cr}^{z}))
\end{aligned} \end{equation}
至此，兩部分的雅克比我們都已經求出來了。那么上述的問題我們也算講明白了。
總結
終於到了總結的階段了，這一講的篇幅相當大，筆者也寫了很久，特別是公式特別多，寫得相當不容易。
這一講，我們主要講了DSO的直接法運動估計，特別是高斯牛頓法優化。我們詳細介紹了Schur消元法精簡優化模型的方案，由於在跟蹤階段只需要估計相對運動，而不需要估計點的深度值，因此我們優化問題可以變得非常高效。
為了計算增量方程，我們推導了位姿和點逆深度的雅克比矩陣，這里我們通過公式 $(3)$ 將整體的雅克比求解過程進行分階段求解，通過鏈式法則將求解位姿和點逆深度的雅克比進行同構，只在最后一項有所區別，這也是一種比較有用的數學思想，通過將有共性的問題進行同構，使得解決方案更加簡潔。
筆者參考了塗金戈同學的博客，也借鑒了部分推導流程，非常歡迎同學們去閱讀金戈同學的博客園。
參考文獻
[1] 直接法光度誤差導數推導
[2] 視覺SLAM十四講
[3] Jinge
版權說明
如需轉載，請聯系本人郵箱peichu.ye at mail2.gdut.edu.cn。未經允許，不可轉載。