SSL的个人Blog - DSO

DSO后端建图（滑窗优化）

2025-02-20T00:00:00+08:00

当前端跟踪完成后，前端线程会判断是否需要添加关键帧操作，并向后端发送跟踪完成的当前帧，如果不需要添加关键帧，那么可以根据当前帧的位姿信息对滑窗中的未成熟点进行优化更新；当然如果需要添加关键帧的话也需要根据位姿信息对滑窗中的未成熟点进行优化更新，除此之外还需要根据当前滑窗中的状态激活相当一部分的未成熟点ImmaturePoint，然后根据滑窗中的成熟点PointHessian、关键帧、先验信息以及由系统之前边缘化marg的fill-in信息构建最小二乘的图优化问题，当然这里会涉及到一些优化理论知识，比如：

DSO使用First Estimate Jacobian FEJ，解决由于不同优化初值导致的系统不一致性问题。
DSO使用 SVD分解 求正规方程或者使用向零空间投影的方式求解最小二乘问题，来防止解在零空间中漂移的问题。
DSO使用矩阵伴随的性质，构建$\frac{\partial \delta \xi_{ji}}{\partial \delta \xi_j}$和$\frac{\partial \delta \xi_{ji}}{\partial \delta \xi_i}$以此来解决相对增量到绝对增量的转换问题（这是由于残差定义仅包含相对位姿导致的问题）。

最后，DSO定义了一些高效的帧管理和点管理策略，使用某些条件筛选需要被边缘化掉的帧和点，以此构建滑动窗口的边缘化信息HM和bM。

1. 滑窗优化里面到底在做什么？

首先，当前端线程完成对Frame的位姿估计后，需要后端线程根据估计的Frame位姿进行滑动窗口中未成熟点ImmaturePoint的逆深度更新，具体更新逆深度的手段，我会在第2小节中进行说明。如果前端线程判断滑动窗口中不需要添加关键帧，那么后端线程结束；否则，后端线程进入滑动窗口优化阶段。为了保证滑窗中激活点PointHessian 的密度问题，需要对滑动窗口中作为备选力量的未成熟点ImmaturePoint 进行筛选激活，而筛选激活的策略，我会在第3小节中进行说明。当滑动窗口中激活点达到数量要求后，构建残差（根据DSO中的优化模型文章描述的残差公式构建），然后在考虑残差作用、先验作用、marg作用后构建并迭代求解优化的正规方程H和b，滑窗正规方程的构建过程以及构建求解过程中可能出现的问题，我会在第4小节中进行详细描述；最后为了保证求解的实时性，需要严格控制滑动窗口中的规模，DSO使用边缘化的策略来实现，其中会涉及边缘化判断和实施边缘化操作这两个过程来构建边缘化的$H_M$和$b_M$，以供后续滑窗优化使用，我会在第5小节中对边缘化相关内容进行详细描述。整个后端的运行逻辑如下图所示：

2 普通帧和关键帧都要做的未成熟点优化

DSO中的未成熟点优化过程分两个步骤进行，首先需要通过未成熟点的逆深度范围确定的极线线段上搜索一个能量最小的像素点；接下来根据线搜过程以及残差公式构建优化模型，因为残差和待优化量都是标量，因此优化过程可以非常快，整个未成熟点优化过程如下图所示：

2.1 极线搜索

DSO中的极线搜索与视觉SLAM十四讲p309中讲解极线搜索内容基本一致，只不过是块匹配策略不一致而已，在SLAM十四讲中列举了SAD、SSD和NCC块匹配评价方法，而DSO使用的是其pattern根据残差构建能量值来作为极线搜索的块匹配评价方法，DSO使用构建残差能量值的方式也会为基于极线搜索优化过程提供方便。

DSO极线搜索中的小问题

pattern 在线搜过程如何保证？在SLAM十四讲中，提到了块匹配技术，也就是说以投影点和被投影点为中心，与周围的某些固定点之间构建两个图像块，以此来进行极线搜索过程中的匹配区域。但是DSO有固定的pattern，但是根据投影公式分析可知$p_j=KRK^{-1}p_i + Ktd_{pi}$，在搜索过程中，$d_{pi}$是未知的，所以投影点$p_i$和被投影点$p_j$没有明确的pattern对应。DSO使用了$KRK^{-1}$矩阵的左上$2\times2$矩阵和pattern相对量的乘积来近似代表pattern与$p_j$点的相对位置。
线搜起止点如何确定？DSO使用了未成熟点的idepth_min 和 idepth_max 对应的点作为搜索的起止点，当然idepth_min被初始化为0，idepth_max被初始化为NAN，如果idepth_max为NAN,则通过图像的(width + height) * super_param来确定一个搜索长度。

除此之外，DSO还讨论了极线搜索和梯度的关系，我在网络上找到了相关的解释，根据下面的图像进行分析，图中红色实线代表的是根据当前帧位姿得到的极线，而红色虚线代表真实位姿得到的极线，蓝色的线代表的是像素值等值线（垂直于像素梯度），这里假设虚线上浅蓝色点为真实的匹配点，而实线上浅蓝色点是在误差极线上搜索到的匹配点。

左图是极线和梯度平行时，可能会导致的像素误差。可以发现，总体的像素误差主要由极线误差导致。
右图是极线和梯度之间存在角度差时，可能会导致的像素误差，可以发现，整体像素误差除了有一部分来自极线误差外，还有一部分来自梯度的误差。

DSO源码中使用了一个经验公式，来计算像素误差的大小，经验公式如下：

$$ e_p = 0.2 + 0.2 \times \frac{a + b}{a}\\ a = (lx \times dx + ly \times dy)^2\\ b = (lx \times dx - ly \times dy)^2 $$

其中：

$lx$为极线在$x$方向上的分量
$ly$为极线在$y$方向上的分量
$dx$为梯度在$x$方向上的向量
$dy$为梯度在$y$方向上的向量

经过推导不难发现，$e_p=0.2 + \frac{0.2}{\cos^2<l,d>}$，其中$cos<l,d>$为极线与梯度之间的夹角的余弦值，也就是说极线和梯度之间的夹角越大就会导致像素误差越大，并且最小的像素误差为0.4px。这与我在上面的定性分析结果一致。

极线搜索与梯度关系，定性分析存在的问题

位姿误差是如何影响极线的呢？在图像上看来，误差极线和真实极线之间是平行的。通过视觉SLAM十四讲中的推导可以知道，极线搜索的线段参数可以使用$Fp_1$来表示，其中F为基础矩阵，$p_1$为host帧上的像素点。而$F$矩阵可以使用$F=K^{-T}t^{\wedge}RK^{-1}$表示，我从这个公式上很难判断出位姿误差会导致平行的极线误差。
以真实匹配点为起点的图像等值线与误差极线的交点是否能保证与误差极线上搜索的点一致？我认为在极线误差较小的情况下，这个是可以成立的，因为真实匹配点能近似代表误差最小点处，而图像等值线在较小的像素范围内可以看做近似成立，因此误差极线上的点也能近似代表误差最小点处。

2.2 基于极线搜索优化

线搜结束后，可以得到一个粗略的最优值（因为线搜会有步长，以至于找不到全局最优值），因此需要优化过程来逼近极线上的最优值对应的$p_j$点。线搜过程和残差可以通过下面的公式进行表示：

$$ r_k=I_j[p_j]- a_{ji} I_i[p_i] - b_{ji}\\ p_j=l*\delta + p_{j0} $$

其中：

$p_j$为极线搜索的目标点；
$l$为极线方向；
$p_{j0}$为极线搜索结束后确定的$p_j$点；
$\delta$为极线上最优点的$p_j$到当前$p_{j0}$之间的步长（待优化量）；

那么不难推导，残差相对步长$\delta$的雅可比矩阵如下：

$$ \frac{\partial r_k}{\partial \delta}=g^T l $$

其中：

$g$为$p_j$点的梯度值；
$l$为极线方向；

使用GN或者LM方法优化完成后，考虑最优化后的线搜点$p_j=[u,v]^T$，由具体的投影公式推导可以得到逆深度点：

$$ p_r = KR_{ji}K^{-1}[p_i^T, 1]^T\\ {d_{pi}}_u=\frac{p_r[2]\times u - p_r[0]}{Kt_{ji}[0]-Kt_{ji}[2]\times u}\\ {d_{pi}}_v=\frac{p_r[2]\times v - p_r[1]}{Kt_{ji}[1]-Kt_{ji}[2]\times v} $$

除此之外，DSO还需要考虑由于极线误差和像素梯度造成的像素误差errorPixel造成逆深度不确定性的影响，得到逆深度点的min和max如下：

$$ {{d_{pi}}_u}_{min}=\frac{p_r[2]\times (u - l_x\times e_p) - p_r[0]}{Kt_{ji}[0]-Kt_{ji}[2]\times (u - l_x \times e_p)}\\ {{d_{pi}}_v}_{min}=\frac{p_r[2]\times (v - l_y\times e_p) - p_r[1]}{Kt_{ji}[1]-Kt_{ji}[2]\times (v - l_y\times e_p)}\\ {{d_{pi}}_u}_{max}=\frac{p_r[2]\times (u + l_x\times e_p) - p_r[0]}{Kt_{ji}[0]-Kt_{ji}[2]\times (u + l_x \times e_p)}\\ {{d_{pi}}_v}_{max}=\frac{p_r[2]\times (v + l_y\times e_p) - p_r[1]}{Kt_{ji}[1]-Kt_{ji}[2]\times (v + l_y\times e_p)} $$

其中：

$u$为$p_j$的x坐标值；
$v$为$p_j$的y坐标值；
$e_p$为由经验公式求得的像素误差；
$l_x$为单位极线方向的x值；
$l_y$为单位极线方向的y值；
最后，从两个min值和两个max值中选择一个最小值和一个最大值来更新未激活点的idepth_min和idepth_max即可。

3 未成熟点如何激活？

未成熟点激活部分主要涵盖三方面的内容：构建距离地图，激活条件判断、激活点逆深度值优化。以最新关键帧kf的金字塔第一层作为滑窗中现存激活点的投影位置。当某一个激活点投影到kf的第一层后，更新相对像素距离到距离地图上。我想下面的图里描述的应该非常清楚（#代表的是滑窗中的激活点投影位置），值得注意的是，针对某个投影点进行距离地图更新时，需要考虑其他投影点的位置，即某个像素位置A距离投影点p1的位置为5，而计算出距离另一个投影点为4时，就需要更新距离地图上的A位置为4，而不是5。

激活条件的判断主要考虑了下面四种情况：

未成熟点在最新的极线搜索中，它的线搜距离在8个像素以内，代表逆深度值达到一个收敛的范围；
未成熟点在最新的极线搜索中，要求搜索点的质量quality大于3，质量被定义为次优线搜能量值 / 最优线搜能量值；
要求未成熟点的逆深度平均值（idepth_min + idepth_max）大于0；
要求未成熟点可以投影到最新关键帧上，并且要满足距离地图的约束（DSO源码里面做了动态阈值调整，应该是根据工程实践调整出来的超参数）；

对于那些被判断为可以激活的点，DSO将其分别投影到滑动窗口中的关键帧上，构建正规方程，并进行优化迭代求解，其残差构建和雅可比矩阵的求解我在DSO中的优化模型文章中做了详细的推导，这里直接写结论：

$$ r_k = I_j[p_j] - a_{ji}I_i[p_i] - b_{ji}\\ \begin{align*} \frac{\partial{r_k}}{\partial{d_{pi}}}&= \frac{1}{P_Z'} \begin{bmatrix} d_x&d_y \end{bmatrix} \begin{bmatrix} f_x & 0 \\ 0 & f_y \end{bmatrix} \begin{bmatrix} 1 & 0 & -\frac{P_X'}{P_Z'} \\ 0 & 1 & -\frac{P_Y'}{P_Z'} \\ \end{bmatrix} \begin{bmatrix} t_{ji}^X\\t_{ji}^Y\\t_{ji}^Z \end{bmatrix}\\ &= \frac{1}{P_Z'}[d_xf_x(t_X^{ji}-\frac{P_X'}{P_Z'}t^Z_{ji})+d_yf_y(t_Y^{ji}-\frac{P_Y'}{P_Z'}t^Z_{ji})] \end{align*}\\ H_{dd}=\sum_{j\in \Omega}^{j!=i}{\sum_{p_i \in \mathcal{N(p)}}{\frac{\partial{r_k}}{\partial{d_{pi}}}^T\frac{\partial{r_k}}{\partial{d_{pi}}}}}\\ b_d=\sum_{j\in \Omega}^{j\neq i}{\sum_{p_i \in \mathcal{N(p)}}{\frac{\partial{r_k}}{\partial{d_{pi}}}^Tr_k}} $$

其中：

$\Omega$为滑动窗口中的关键帧索引集合；
$i$为$p_i$点对应的host帧索引；
$j$为$p_i$点投影的target帧索引；
$\mathcal{N(p)}$代表的是以$p$为中心的pattern。

从残差对$p_i$点逆深度的雅可比矩阵来看，其优化过程应该相当迅速，因为正规方程中涉及到的内容全是标量。

4 如何根据关键帧和路标点构建优化的正规方程？

这里我们假设地图点激活后，滑动窗口中有3个关键帧和5个地图点，上图表示了帧与帧之间的可视关系，以及帧与激活点之间的可视关系。其中红色线代表某个点的host帧，黑色线代表着某个帧可以看到某个点。

根据DSO中的优化模型文章中的残差模型推导，后端的优化能量函数，残差公式和残差的雅可比矩阵如下：

$$ E_{fpC}=\sum_{i\in \Omega, j \in \Omega}^{j \neq i} \sum_{p \in C(I_i)}\sum_{p_i \in \mathcal{N(p)}} {||r_k||_{\gamma}}\\ r_k=I_j[p_j]-\frac{t_je^{a_i}}{t_ie^{a_j}}I_i[p_i]-(b_j-\frac{t_je^{a_i}}{t_ie^{a_j}}b_i)\\ \frac{\partial{r_k}}{\partial{d_{pi}}}= \frac{1}{P_Z'}[d_xf_x(t_X^{ji}-\frac{P_X'}{P_Z'}t^Z_{ji})+d_yf_y(t_Y^{ji}-\frac{P_Y'}{P_Z'}t^Z_{ji})]\\ \frac{\partial{r_k}}{\partial{\xi_{ji}}}= \begin{bmatrix} d_xf_x&d_yf_y\end{bmatrix} \begin{bmatrix} \frac{d_{pi}}{P_{Z}'} & 0 & -\frac{d_{pi}}{P_{Z}'}\frac{P_{X}'}{P_{Z}'} & -\frac{P_{X}'P_{Y}'}{P_{Z}'^2} & 1+\frac{P_{X}^{2}}{P_{Z}^{2}} & -\frac{P_{Y}'}{P_{Z}'} \\ 0 & \frac{d_{pi}}{P_{Z}'} & -\frac{d_{pi}}{P_{Z}'}\frac{P_{Y}'}{P_{Z}'} & -1-\frac{P_{Y}^{2}}{P_{Z}^{2}} & \frac{P_{X}'P_{Y}'}{P_{Z}^{2}} & \frac{P_{X}'}{P_{Z}'} \end{bmatrix}\\ \frac{\partial r_k}{\partial a_{ji}}=-(I_i[p_i]-b_i)\\ \frac{\partial r_k}{\partial b_{ji}}=-1 $$

其中：

$E_{fpC}$为所有残差组成的能量部分，每个残差都由三个顶点组成，分别是$ij$帧之间的相对参数、点$p$的逆深度$d_{pi}$和相机内参$C$，因为DSO的相机内参是由算法计算出来的，因此需要进行在线标定内参，我在DSO中的去畸变操作中详细描述了DSO相机内参的计算过程；
后端滑窗中使用的$a_{ji}=\frac{t_je^a_{i}}{t_ie^{a_j}}$，与初始化过程的$a_{ji}$不同，初始化过程中是为了保证$e^{a_{ji}}>0$，而后端部分不需要这个保证，因为$a_{ji}$的计算只是做一个雅可比中转的作用，并且使用这种中转方式可以简化整个雅可比的推导过程（相比使用$e^{a_ji}$作为中间量）；
$\frac{\partial r_k}{\partial d_{pi}}$和$\frac{\partial r_k}{\partial \delta \xi_{ji}}$部分直接使用的我在DSO中的优化模型文章中推导的结论；
残差对仿射参数的求导相对简单，都是通过$a_{ji}=\frac{t_je^{a_{i}}}{t_ie^{a_{j}}}$和$b_{ji}=b_j - a_{ji} b_i$作为中间变量进行推导即可，整个推导过程比较简单，这里就不展开说明了；

由于DSO使用算法的方式计算出了一个虚拟的相机内参，因此DSO对相机内参做了在线标定，在滑窗优化过程中，将相机内参作为优化变量，求解残差对相机内参的雅可比矩阵，这个推导过程比较繁琐，涉及正向投影和反向投影两个部分，下面对残差相对内参的雅可比矩阵的详细推导：

反向投影过程，代表$p_i$点向$i$帧的归一化坐标系上进行反向投影：

$$ X_i^n=\frac{1}{f_x}u_i-\frac{c_x}{f_x}\\ Y_i^n=\frac{1}{f_y}v_i-\frac{c_y}{f_y}\\ \frac{\partial P_i^n}{\partial C}=\begin{bmatrix} -\frac{u_i}{f_x^2} & 0 & -\frac{1}{f_x} & 0 \\ 0 & -\frac{v_i}{f_y^2} & 0 & -\frac{1}{f_y} \\ 0 & 0 & 0 & 0 \end{bmatrix} $$

正向投影过程，代表由$i$帧归一化坐标系$P_i^{n}$向$j$帧像素坐标系的投影过程，包含一个坐标变换和投影过程：

$$ P_j' = R_{ji}P_i^{n}+t_{ji}d_{p_i}\\ u_j = f_x \frac{X_j'}{Z_j'} + c_x\\ v_j = f_y \frac{Y_j'}{Z_j'} + c_y\\ \frac{\partial p_j}{\partial C} = \begin{bmatrix} \frac{X_j'}{Z_j'} & 0 & 1 & 0 \\ 0 & \frac{Y_j'}{Z_j'} & 0 & 1 \end{bmatrix}\\ \frac{\partial p_j}{\partial P_i^n}=\frac{\partial p_j}{\partial P_j'} \times \frac{\partial P_j'}{\partial P_i^n}=\begin{bmatrix} \frac{f_x}{Z_j'} & 0 & -\frac{f_xX_j'}{Z_j'^2}\\ 0 & \frac{f_y}{Z_j'} & -\frac{f_yY_j'}{Z_j'^2} \end{bmatrix}\times R_{ji} $$

结合正向和反向投影过程，可得残差对相机内参$C$的雅可比矩阵：

$$ \begin{align*} \frac{\partial r_k[p_j(C,P_i^n(C))]}{\partial C} &= \frac{\partial r_k}{\partial p_j}\times (\frac{\partial p_j}{\partial C} + \frac{\partial p_j}{\partial P_i^n}\times \frac{\partial P_i^n}{\partial C})\\ &=\begin{bmatrix}d_x & d_y\end{bmatrix} ( \begin{bmatrix} \frac{X_j'}{Z_j'} & 0 & 1 & 0 \\ 0 & \frac{Y_j'}{Z_j'} & 0 & 1 \end{bmatrix}+ \begin{bmatrix} \frac{f_x}{Z_j'} & 0 & -\frac{f_xX_j'}{Z_j'^2}\\ 0 & \frac{f_y}{Z_j'} & -\frac{f_yY_j'}{Z_j'^2} \end{bmatrix}\times R_{ji}\times \begin{bmatrix} -\frac{u_i}{f_x^2} & 0 & -\frac{1}{f_x} & 0 \\ 0 & -\frac{v_i}{f_y^2} & 0 & -\frac{1}{f_y} \\ 0 & 0 & 0 & 0 \end{bmatrix} ) \end{align*} $$

根据DSO的残差模型，可以绘制图优化的优化图如下图所示，其中红色线连接三个顶点，分别为两帧之间的相对量顶点和激活点逆深度顶点和相机内参顶点。

根据上图所示的优化图，可以绘制由这些残差构建的Hessian矩阵如下图所示，但是这里会出现一些问题，如果去解这一个由两帧相对量和激活点构建的Hessian矩阵的话，得到的结果应该都是相对量增量，这样解出来的结果只是求解出了滑动窗口帧之间的相对量Tth、a_th和b_th，而不是相对于世界坐标系的绝对量。

讨论仅求帧相对量的可行性

我认为，仅求帧相对量不可行，我主要是通过一下三个方面来考虑的。

相对量求解完成后，需要更新为绝对量，也就是相对于世界坐标系的参数量，在更新绝对量过程中会出现冲突问题，假设有三个关键帧，那么相对位姿就会有T12、T13和T23三个，假设Tw1已知的情况下,Tw3可以通过两种方式表示，分别是Tw1 * T13和Tw1 * T12 * T23。这两种表示方法势必会产生冲突，这个冲突问题怎么解决呢？
就算可以通过某种方式，解决1中说明的更新冲突问题，那么当滑动窗口优化达到某个规模上限后，为了实时性势必会进行激活点和关键帧的边缘化操作，假设系统判定，当前需要边缘化$f_1$这个关键帧，对于绝对量的表示来讲，其边缘化所表达的概率模型为 $p(f_1,f_2,f_3,p_1,p_2,p_3,p_4,p_5)=p(f_2,f_3,p_1,p_2,p_3,p_4,p_5|f_1) * p(f_1)$，其中边缘化完成后$p(f_1)$为常量概率。但是对相对量来讲，如何进行边缘化操作呢？不能把$f_1$涉及的所有相对量都进行边缘化吧，这样的话肯定会造成信息丢失。
最后，即便不考虑相对量边缘化操作造成的信息丢失，在求解正规方程上，相对量的Hessian矩阵的规模也要比绝对量表示的Hessian矩阵大得多。因为$C_n^2 \geq n,n\geq 2$总是成立的。因此使用schur分解加速正规方程求解时，相对量表示的分解后得到的稠密矩阵H，要比绝对量表示的稠密矩阵H大得多。

从上面三方面的分析来看，从可行性，信息留存率和效率三方面来讲，绝对量表示的正规方程都优于相对量表示的正规方程。

4.1 残差构建仅包含关键帧的相对量，怎么办？

从上面的分析中可知，绝对量表示的正规方程要优于相对量表示的正规方程，DSO使用位姿伴随的性质，求解$\frac{\delta\xi_{ji}}{\delta\xi_{i}}$和$\frac{\delta \xi_{ji}}{\delta \xi_{j}}$实现位姿相对量到绝对量的变换。而仿射参数的$\frac{\partial a_{ji}}{\partial a_i}$、$\frac{\partial a_{ji}}{\partial a_j}$、$\frac{\partial b_{ji}}{\partial b_i}$、$\frac{\partial b_{ji}}{\partial b_j}$可以根据残差方程直接推导出来，下面是推导流程。

首先，需要说明一个位姿矩阵的伴随性质，$Exp(Ad_T \times \xi)=TExp(\xi)T^{-1}$，其中$Ad_T$为位姿矩阵$T$的伴随矩阵，$\xi$为一个李代数上的小扰动。现在，考虑一个相对量位姿$T_{ji}$的左扰动$\delta \xi_{ji}$，它势必会造成$T_{jw}$和$T_{iw}$的小的左扰动，假设它们分别是$\delta \xi_j$和$\delta \xi_i$，它们的公式描述如下：

$$ \begin{align*} Exp(\delta \xi_{ji})T_{ji}&=Exp(\delta \xi_j) T_{jw} T_{iw}^{-1} \\ &\to Exp(\delta \xi_{ji})=Exp(\delta \xi_j) \to \frac{ \partial \delta \xi_{ji}}{\partial \delta \xi_j}=I\\ Exp(\delta \xi_{ji})T_{ji}&=T_{jw} (Exp(\delta \xi_i)T_{iw})^{-1}=T_{jw}T_{iw}^{-1}Exp(-\delta \xi_i)\\ &\to Exp(\delta \xi_{ji})=T_{ji}Exp(-\delta \xi_i)T_{ji}^{-1}=-Ad_{T_{ji}} \times \delta \xi_i \to \frac{ \partial \delta \xi_{ji}}{\partial \delta \xi_i}=-Ad_{T_{ji}} \end{align*} $$

相对仿射参数到绝对仿射参数之间的转换比较简单，可以根据残差公式直接推导：

$$ r_k=I_j[p_j]-\frac{t_je^{a_i}}{t_ie^{a_j}}I_i[p_i]-(b_j-\frac{t_je^{a_i}}{t_ie^{a_j}}b_i)\\ a_{ji}=\frac{t_je^{a_i}}{t_ie^{a_j}} \quad b_{ji}=(b_j-\frac{t_je^{a_i}}{t_ie^{a_j}}b_i)\\ \frac{\partial a_{ji}}{\partial a_i}=\frac{t_je^{a_j}}{t_ie^{a_i}}\\ \frac{\partial a_{ji}}{\partial a_j} = -\frac{t_je^{a_j}}{t_ie^{a_i}} \\ \frac{\partial b_{ji}}{\partial b_i}=-\frac{t_je^{a_j}}{t_ie^{a_i}}\\ \frac{\partial b_{ji}}{\partial b_j}= 1\\ $$

上面通过推导的形式，构建了$\frac{\partial \delta \xi_{ji}}{\partial \xi_j}$、$\frac{\partial \delta \xi_{ji}}{\partial \xi_i}$、$\frac{\partial a_{ji}}{\partial a_i}$、$\frac{\partial a_{ji}}{\partial a_j}$、$\frac{\partial b_{ji}}{\partial b_i}$、$\frac{\partial b_{ji}}{\partial b_j}$，因此可以计算残差相对于绝对量的雅可比矩阵，也就能完成基于相对量的Hessian矩阵到绝对量Hessian矩阵的转换。

4.2 为什么会有不一致性问题，该怎么解决？

DSO的后端使用的是滑动窗口的优化策略，因此DSO使用边缘化策略来控制整个后端滑动窗口中的规模。当然这个边缘化的操作是在后端优化完成后做的，其详细的策略我会在第5小节中描述，这里我们只需要知道，每次进行后端的滑动窗口优化时，都需要考虑最新边缘化操作得到的HM和bM矩阵。这代表着边缘化后剩余的信息，以最大似然估计的角度看，HM和bM中的信息代表着系统的条件概率，由于HM和bM会提供一个边缘化先验信息，它们所表达的约束为$H_M \delta x = -b_M$，这个约束可以为后续的优化操作提供方向。

与想象中不同的是，我们不能以下面这种方式使用边缘化得到的$H_M$和$b_M$：

$$ (H_A + H_M + H_P) \delta x = -(b_A + b_M + b_P) $$

其中：

$H_A$，$b_A$代表由滑动窗口中所有残差构建的H矩阵和b矩阵；
$H_M$，$b_M$代表边缘化后得到的H矩阵和b矩阵；
$H_P$，$b_P$代表先验信息的H矩阵和b矩阵；

不能直接进行相加的原因主要有两点，首先$H_A$和$H_M$线性化点的位置不同，这样直接相加会导致系统出现不一致性问题，其次$b_M$在优化中代表的是能量在线性化点$x_M$处的能量梯度（能量对待优化量的雅可比矩阵，在线性化点$x_M$处），但是随着更新的进行，两个线性化点$x_A$和$x_M$的距离可能逐渐变大，这样$b_M$就不能近似$x_A$处的能量梯度了，其中$x_A$代表的是残差的线性化点，$x_M$代表的是边缘化信息的线性化点，值得注意的是，$x_A$随着优化的进行，会逐渐改变，而$x_M$则不能改变。

对不一致性问题的详细说明：在论文 [1] 中，作者讨论了不同线性化点处的H矩阵直接相加导致了$H$矩阵的秩增加的情况（系统的某些状态由不客观变得可观了），也就是说这种不正确的操作低估了系统状态的不确定性，认定这种改变系统状态不确定性的问题为系统不一致性问题。下图清晰也表述了系统的不一致性问题，图中表示，系统的解为$xy=1$，具有一个状态是不可观的。如果将$E_1$在$x=0.5$处线性化，$E_2$在$x=1.2$处线性化后求解，导致系统的解坍缩成了一个点，从而失去了系统的不确定性。

DSO使用了FEJ（First Estimate Jacobian）来解决不同线性化点导致的问题，FEJ是这样描述的，由于边缘化提供的$H_M$和$b_M$线性化点不能改变了，但是由线性化残差构建的$H_A$和$b_A$的线性化点却可以改变，为了保证H矩阵的一致性，残差构建的$H_A$也使用$x_M$进行线性化，不会改变系统的能观性，但是这样做势必会引入线性误差。也就是说，残差$r_k$值的计算，使用$x_A$线性化点计算，$\frac{\partial r_k}{\partial x}$使用$x_M$线性化点处计算，这样可以构建一个新的$H_A$和$b_A$。除此之外，为了避免$b_M$描述的能量梯度不准确的问题，FEJ使用一阶泰勒展开对$b_M$进行修正，FEJ的公式描述如下：

$$ E_A(x) = E(x_A) + b_A(r_k(x_A), \frac{\partial r_k}{\partial x}|_{x=x_M})(x-x_A) + (x-x_A)^TH_A(x_M)(x-x_A) \\ E_M(x) = E(x_M) + b_M(x_M + x_A - x_M)(x-x_A) + (x-x_A)^TH_M(x_M)(x-x_A)\\ \frac{\partial b_M}{\partial x}|_{x=x_M}=H_M(x_M) \\ b_M(x_M + x_A - x_M) = b_M(x_M) + H_M(x_M)(x_A - x_M) \\ $$

其中：

$x_A$为某次优化中，残差线性化点；
$x_M$为marg的线性化点；
$E_A(x)$为残差对应能量值；
$E_M(x)$为marg对应的能量值；

从FEJ的公式描述可以发现，为了保证H矩阵的一致性，$H_A(x_M)$部分引入了线性化误差，在$b_A$计算过程中，$\frac{\partial r_k}{\partial x}|_{x=x_M}$部分引入了线性化误差，而$b_M(x_M+x_A - x_M)$部分使用泰勒展开进行了修正。

FEJ 非用不可？

通过上面公式的分析，可以发现，使用FEJ会不可避免的引入线性化误差（对残差部分来讲），而不引入FEJ又会导致系统的不一致性问题。貌似使用FEJ或者不使用FEJ都做不到完美。我认为FEJ并不是非使用不可，比如VINS中就没有使用FEJ，当然如果不使用FEJ的话，还是建议使用FEJ的$b_M$更新策略来维护$b_M$。甚至可以部分使用FEJ，DSO也是部分使用FEJ，来规避引入大的线性误差的风险。在DSO中，激活点的逆深度状态完全没有使用FEJ，这可能是因为激活点在经过一轮的滑动窗口优化后，可能不太稳定，如果固定逆深度的话，可能会引入较大的线性化误差。除此之外，DSO对$\frac{\partial r_k}{\partial \xi_{ji}}$、$\frac{\partial r_k}{\partial d_{pi}}$和$\frac{\partial r_k}{\partial C}$分阶段求解，$\frac{\partial r_k}{\partial \xi_{ji}}=\frac{\partial r_k}{\partial p_{j}}|_{x=x_A} \times \frac{\partial p_j}{\partial \xi_{ji}}|_{x=x_M} \quad \frac{\partial r_k}{\partial d_{pi}} = \frac{\partial r_k}{\partial p_j}|_{x=x_A} \times \frac{\partial p_j}{\partial d_{pi}}|_{x=x_M} \quad \frac{\partial r_k}{\partial C} = \frac{\partial r_k}{\partial p_j}|_{x=x_A} \times \frac{\partial p_j}{\partial C}|_{x=x_M}$，可能考虑到图像为非线性比较强的函数，因此对所有残差雅可比涉及图像梯度的部分都没有使用FEJ。

DSO在滑动窗口优化中的残差线性化阶段计算$\frac{\partial p_j}{\partial \delta \xi_{ji}}$、$\frac{\partial p_j}{\partial C}$和$\frac{\partial p_j}{\partial d_{pi}}$时使用了中心点的计算值代替整个pattern中每一个点的计算值，相当于减少了部分计算量，不过不清楚这么做是否会加重FEJ引入的线性化误差。

使用FEJ解决了系统不一致性问题后，就需要考虑先验的影响。DSO的系统求解中，主要有以下几种状态存在先验信息：

初始化完成后，加入到滑动窗口中的两个关键帧状态有先验。
初始化完成后，加入到滑动窗口中的激活点逆深度状态有先验。
由算法计算出的虚拟内参，有先验。

在给定某个状态的先验信息矩阵$H_P$后，先验信息构建的正规方程可以由下面的公式进行描述：

$$ E_P=\frac{1}{2}(x_A+\Delta x-x_P)^TH_P(x_P)(x_A+\Delta x-x_P)\\ b_P(x_P)=H_P(x_P)(x_A+\Delta x-x_P) $$

$E_P$为先验信息所对应的能量值；
$H_P$和$b_P$为先验信息构建的正规方程矩阵；
$x_P$为先验状态；

综上所述，当残差构建完成后，系统会根据提供的先验信息矩阵和最新边缘化得到的边缘化信息构建本次滑动窗口优化的正规方程，并进行迭代求解，构建的正规方程可以由下面的公式进行描述：

$$ H = H_A(x_M) + H_P(x_P) + H_M(x_M)\\ b = b_A(r_k(x_A), \frac{\partial r_k}{\partial x}|_{x=x_M}) + b_P(x_P) + b_M(x_M + x_A - x_M)\\ H \Delta x = -b $$

FEJ减少的计算量

在上面对FEJ的探讨中，我们知道了DSO中使用了部分FEJ的方案来解决系统的不一致性问题，又避免了引入大的线性化误差。使用这种方案竟还意外的减少了部分计算量，在4.1节中我们知道DSO需要将绝对量变换为相对量，需要求解$\frac{\partial {\xi_{ji}}}{\partial \xi_j}$、$\frac{\partial {\xi_{ji}}}{\partial \xi_i}$、$\frac{\partial {a_{ji}}}{\partial a_j}$、$\frac{\partial {a_{ji}}}{\partial a_i}$、$\frac{\partial {b_{ji}}}{\partial b_i}$和$\frac{\partial {b_{ji}}}{\partial b_j}$。而DSO的部分FEJ策略又将$\frac{\partial p_j}{\partial \xi_{ji}}$、$\frac{\partial r_k}{\partial \a_{ji}}$、$\frac{\partial r_k}{\partial \b_{ji}}$限制在线性化$x_M$处，因此相对量到绝对量转换的中间量的求解只需要在线性化点$x_M$处求解一次即可，在一定程度上可以减少一些计算量。

4.3 在解正规方程时，如何防止解在零空间中漂移？

对于某个等式$H \Delta x = -b$来讲，如果矩阵$H$不是满秩阵，那么必定存在$H x_{ns} = 0, x \ne \overrightarrow{0}$。其中$x_{ns}$为由$H$矩阵在零空间上的基组成的向量。这个$x_{ns}$也被称为解在零空间上的漂移。因为$H x_{ns} = 0, x \ne \overrightarrow{0}$总是成立的，因此不论$x_{ns}$漂移多少，$H (\Delta x + x_{ns}) = -b$总是成立的。也就是说在解正规方程时，如果不对零空间的漂移加以限制，那么解与不含漂移的解之间的误差就会很大。然而对于单目的VO系统来讲，系统总是有7个自由度，也就是说，H矩阵一定非满秩，并且与其行数差7，需要进行一些处理手段来保证正规方程得到的解不漂移。

DSO的源码里面定义了两种防止零空间漂移的方法，一种是基于SVD分解截断小奇异值求矩阵伪逆的方案，另一种是基于位姿矩阵伴随的性质求解世界坐标系零空间到局部坐标系零空间映射，并使用正交基投影的方式去掉零空间漂移的影响。

首先，我们先看下DSO求伪逆是如何防止零空间漂移的，假设目前完成了某次线性化，得到正规方程$H \Delta x = -b$，那么$H$的伪逆可以由下面的公式表示：

$$ H = U \Sigma V^T\\ H^{-1} = V \Sigma^{+}U^T\\ \Delta x^+ = H^{-1}b $$

其中：

$U$和$V$为酉矩阵（代表等距变换，列正交）；
$\Sigma$矩阵为对角矩阵，其对角上的值为奇异值；
奇异值为0的对应的 $V$矩阵的列为矩阵$H$的零空间的基，并且是正交基；
对$\Sigma$矩阵中的小奇异值置零后，将其非0奇异值取倒数后，转置得到$\Sigma^{+}$；
$H^{-1}$为$H$的伪逆；
$\Delta x^+$为去掉零空间漂移的正规方程的解；

通过对$\Sigma^+$矩阵和$V$矩阵的描述可知，$V$矩阵中0奇异值对应的列向量为零空间的一组正交基，然而$\Sigma^+$矩阵将小奇异值置零，在伪逆计算中，$\Sigma ^+$作用到了矩阵$V$的右侧，因此$V$矩阵0奇异值对应的列向量被$\Sigma ^+$矩阵作用后置零，因此得到的结果消除了零空间的影响。在DSO的源码里面有两种截断方式，一种是强制的7自由度截断，还有一种是定阈值截断法（小于这个阈值的奇异值置零）。

还有一种方式是通过世界坐标系零空间到相机坐标系零空间基转换的方式，直接求解出来相机坐标系对应的零空间的基，然后通过正交投影的方式得到零空间分量，最后减去即可。我们知道VO系统有7个自由度，即如果在世界坐标系上添加扰动，那么VO系统的约束仍然成立，因此世界坐标系上的零空间的基是恒定的，而相机坐标系上的零空间的基需要世界坐标系进行转换，下面的公式描述了这种转换关系：

$$ Exp(\delta \xi_c) T_{cw} = (Exp(\delta \xi_w) T_{wc})^{-1} \to Exp(\delta \xi_c) = T_{cw} Exp(-\delta \xi_w) T_{cw}^{-1} \to \delta \xi_c = -Ad_{T_{cw}} \times \delta \xi_w \\ $$

在DSO中使用世界坐标系零空间基上的正负两个扰动来推导出DSO相机坐标系对应的零空间基上的扰动，这里以$x$轴旋转$\phi_x$和比例因子$s$为例进行推导：

$$ \delta {\xi_{w}^{\phi_x}}_p = \begin{bmatrix} 10^{-3} & 0 & 0 & 0 & 0 & 0 \end{bmatrix}\\ \delta {\xi_{w}^{\phi_x}}_n = \begin{bmatrix} 10^{-3} & 0 & 0 & 0 & 0 & 0 \end{bmatrix}\\ \delta \xi_c^{\phi_x} = -\frac{1}{2\times 10^{-3}}Ad_{T_{cw}} \times (\delta {\xi_{w}^{\phi_x}}_p - \delta {\xi_{w}^{\phi_x}}_n)\\ \delta {s_w}_p = 1.0001\\ \delta {s_w}_p = \frac{1}{1.0001}\\ \delta \xi_c^s = \frac{1}{2\times 10^{-4}}Log(\begin{bmatrix} R_{cw} & \delta {s_w}_p t_{cw}\\ 0 & 1 \end{bmatrix} T_{cw}^{-1}) - Log(\begin{bmatrix} R_{cw} & \delta {s_w}_n t_{cw}\\ 0 & 1 \end{bmatrix} T_{cw}^{-1}) $$

在得到世界坐标系下的零空间基到相机坐标系下的零空间基转换后，通过正交投影的方式可以求得解在零空间中的分量，然后减去这个分量即可，下图是正交投影的例子，在 [2] 中推导得出$P_{\mathcal{M}}=M(M^TM)^{-1}M^T$，其中$M$为零空间中的基组成的矩阵，$P_{\mathcal{M}}v$代表的是$v$在零空间中的分量。

4.4 如何保证H矩阵非病态？

文章 [3] 中描述了病态矩阵出现的原因，针对正规方程$H\delta x=-b$这个公式，病态的$H$矩阵由于列相关性较高导致条件数过大，从而使得解$\delta x$收$b$矩阵的扰动较大。出现病态矩阵的根本原因是$H$矩阵中存在相关性较高的列，SVD分解后表现在$\Sigma$矩阵中存在极小的特征值，这与正规方程的解在零空间漂移存在一些关系，解在零空间漂移出现的原因是系统存在一定的自由度，即可以明确存在一些完全相关的列，而病态矩阵的出现是因为系统存在一些高度相关的列（不完全相关），从而在求解$\delta_x -H^{-1}b$时，小奇异值作用到$H^{-1}$上后，会产生较大的scale作用，从而难以应对$b$矩阵扰动情况。

普通病态矩阵可以通过截断SVD的方案来解决，即使用SVD对矩阵$H$进行分解，然后求$H$矩阵的逆时，可以将$\Sigma$矩阵中的小奇异值置零，将奇异值部分完全看做矩阵的零空间零空间来处理。DSO中的某个solver也使用了截断SVD的方式处理零空间问题，即给定某个阈值，小于这个阈值的奇异值都置零，这样可以同时解决零空间问题和病态矩阵问题。

DSO另外使用了一种对角线预处理的方式，来解决病态矩阵问题，对$H$和$b$矩阵进行预处理，旨在降低$H$矩阵的条件数。对角预处理的公式描述如下：

$$ W = \sqrt{diag(H) + 10 \times I}\\ H'=WHW\\ b'=Wb\\ x = -WH'^{-1}b'\\ $$

其中，$diag(H)$代表取H的对角线组成新的对角矩阵，使用这种对角预处理的手段确实可以减少矩阵H的条件数，但是比较有限。

5. 滑动窗口中的帧和点超出限定范围怎么办？

当滑动窗口的优化完成后，就到了边缘化判断和实施边缘化操作的阶段，我们知道DSO后端使用的是滑动窗口优化策略，因此为了保证优化的实时性，需要控制后端滑动窗口的规模，在DSO的源码中规定了理想的后端优化的规模，即5~7个关键帧和2000个左右的激活点。也就是说，当后端窗口中的关键帧或者激活点超出这个范围时，DSO就会使用边缘化的策略来控制滑动窗口的规模，以下是DSO的点管理和帧管理策略：

帧管理策略：

从优化提供的作用角度考虑，当某一帧$\frac{n_{i} + n_{p}}{n_{m} + n_{d} + n_i + n_p} < 0.05$则判断当前帧需要被边缘化，其中$n_{i}$代表未成熟点的个数，$n_{p}$代表该帧中激活点的数目，$n_m$代表该帧被边缘化点的数目，$n_d$代表该帧被丢掉点的数目，如果可以对优化起作用的点数目（$n_i + n_p$）占所有点的比例小于5%时，则判断该帧对优化提供的作用较少，标记为边缘化；
从曝光参数上考虑，计算某一帧和最新帧之间的光度参数$a_{ji}>=0.7$（这里的$a_{ji}$和初始化中的$a_{ji}$意义相同），则认为该帧和最新帧之间的亮度变化太大（环境光变化大），则标记当前帧为边缘化；
在时间轴上考虑，保证距离最新帧newFH较近的3帧不被边缘化；
在距离轴上考虑，提出启发式距离评价方法，距离评分公式为$s(I_i)=-\sqrt{d(i, 1)}\times \sum_{j\in[3,n],j\ne i}{d(i,j)^{-1}}$，将评分最小的帧标记为边缘化帧；
值得注意的是，每次只边缘化一帧，上面的条件从上到下依次判断，如果满足某一个条件则标记边缘化帧后退出。

点管理策略：

逆深度小于0或者没有参与优化的点，被丢掉drop
判断某个点对后续滑动优化的作用：
- 某一帧被边缘化后，点残差保留太少，则认定对后续滑窗优化作用不大
- 最新帧看不到该点，则认定为对后续滑窗优化作用不大
- 针对那些残差比较少的点，是新加入的点，认定对后续滑窗优化的作用较大（源码里面还做了单独的判断，这个判断是否不需要做，仅对作用不大的条件进行判断不就好了？）
- 在最近两次滑窗优化中都被判断为是outlier的点，则认定对后续滑窗优化作用不大
判断某个点是否是内点：
- 要求残差数量要大于某个阈值
- 要求被判断为是inlier的残差数量要大于某个阈值
如果判断某个点对后续滑窗优化的作用不大或者某个激活点的host帧为待被边缘化的帧
- 如果这个点被判断为内点
  - 判断该点的$H_{dd} > 50$，$H_{dd}$是该点在滑窗优化中计算的Hessian，如果不满足则标记为丢掉；
  - 否则标记为边缘化掉；
- 如果这个点被判断为外点，则标记为丢掉。

假设下图中，点$p_1$需要被丢掉，点$p_2$、$p_3$、帧$kf_1$需要被边缘化掉。

具体执行边缘化的流程可以由下图进行描述，首先根据要被边缘化的残差构建Hessian和b，值得注意的是，线性化点为$x_M$，在实施边缘化操作之前，还需要考虑点的先验信息，需要加上确定被边缘化的状态的先验Hessian和先验b，然后先边缘化点，最后边缘化帧。使用schur分解的方式实现，下面的图描述了这一过程：

为什么先边缘化点再边缘化帧？

在点管理策略中可以发现，某个帧如果被判断为要被边缘化掉的话，该帧上的所有激活点都要被丢掉或者边缘化掉。并且实施边缘化时需要先边缘化掉点，然后边缘化掉帧。这么做的原因在于，防止边缘化帧时导致点与点之间的Hessian变稠密。如果点与点之间的Hessian不在稀疏，那么整个优化过程将变的极其复杂，schur分解加速优化将不再成立，求解正规方程将非常缓慢。

Reference

[1] T. -C. Dong-Si and A. I. Mourikis, “Consistency analysis for sliding-window visual odometry,” 2012 IEEE International Conference on Robotics and Automation, Saint Paul, MN, USA, 2012, pp. 5202-5209, doi: 10.1109/ICRA.2012.6225246.

[2] Meyer C D. Matrix analysis and applied linear algebra[M]. Society for Industrial and Applied Mathematics, 2023.

[3] 机器学习中的矩阵方法(附录A)：病态矩阵与条件数

DSO的前端跟踪

2025-02-18T00:00:00+08:00

DSO中的前端跟踪过程相对简单，其主要涵盖跟踪前处理、跟踪过程和跟踪后处理三部分内容。下面我会对这三部分进行拆解并对其中的细节进行介绍。

1. 参考帧确定和跟踪参考点构建（跟踪前处理）

当DSO系统的初始化完成后，目前整个系统中会有两个关键帧和若干激活地图点，当然激活地图点的host帧为初始化参考帧。和初始化过程类似，由于新来的一帧frame的位姿不明确，需要一个相对鲁邦的位姿估计方法，和初始化过程一样，DSO的前端跟踪部分采用的也是基于金字塔由粗至精的跟踪方法，因此需要确定一个跟踪参考帧和若干跟踪地图点。

在介绍参考点构建之前，我先明确一个问题，就是DSO系统会维护一个关键帧队列，即DSO中的关键帧滑动窗口，至于这个滑窗的维护策略，我会在后续文章中进行介绍。

1.1 金字塔参考点初构建

DSO使用了最新关键帧作为跟踪过程中的参考关键帧。为了提高跟踪鲁棒性，滑动窗口中的所有激活地图点都会投影到最新关键帧的第0层上。正如下面这个图所描述的那样，来初始化跟踪参考关键帧上第0层的像素点逆深度。

在投影过程中，DSO系统使用了四舍五入的方式来计算投影点的像素位置，因此存在多个点投影到同一个像素位置的情况，这里使用DSO中的初始化的3.3节讲到的高斯归一化积解决同像素位置不同逆深度的问题。

根据跟踪参考帧第0层上的像素逆深度点，向上层进行投影得到参考帧第1层的像素逆深度点，以此类推最终得到初步的跟踪参考点逆深度，这里同样会存在多对一的情况，依然使用高斯归一化积解决。

1.2 金字塔参考点膨胀

除了使用上述投影方式，DSO的源码中使用了膨胀手段来进一步增加参考点的数目，正如下图所示，紫色的位置代表某个金字塔层级上没有逆深度点的像素位置，而青色位置代表膨胀遍历位置，即遍历青色位置试图寻找一个有逆深度的像素点，然后使用取均值的方式再紫色位置膨胀出逆深度点出来。下图是DSO中采用的两种膨胀遍历方式，左侧是金字塔0层和1层的膨胀方式，右侧是其他层的膨胀方式。

膨胀点原因猜测

这里膨胀点的方式和我在DSO中的优化模型文章中提到的pattern类似，pattern的作用是使用周围像素共享中心点的逆深度，以此来增强灰度不变形假设的可行性。个人猜测DSO在这里使用膨胀的原因和pattern类似，尝试以这种方式提高假设的可靠性。

2. 实施跟踪过程（构建模型并优化）

有了参考帧和待跟踪的参考点后，就可以实施前端跟踪过程了。除了使用图像金字塔从粗到精的跟踪手段外，DSO还拓展了不同的运动假设来构建优化初值假设库，试图找到一个最优初值，从而加速优化过程，提高达到最优值的可能性。

2.1 构建优化初值假设库

DSO考虑了恒速运动、倍数运动、半速运动和相对参考帧静止的模型假设，除此之外DSO还做了小旋转运行假设，所有的运动假设模型我都在下面的图片里面表示了出来，共有$(3 * 3 * 3 - 1) * 3 + 4$种假设结果。其中待优化帧的光度仿射参数$a_j$和$b_j$直接使用的上一帧光度仿射参数$a_{last}$和$b_{last}$作为初值，这是因为自然条件下，相邻帧之间的光度仿射参数变化非常小。

2.2 基于某种假设的跟踪尝试

2.2.1 跟踪模型

某种位姿假设初值$T_{cr}$确定后，DSO前端跟踪部分会使用下面的公式构造优化目标函数。

$$ E_{l}=\sum_{p_i \in ref}{||r_k||_{\gamma}} \\ r_k=I_j[p_j]-\frac{t_j e^{a_i}}{t_i e^{a_j}}I_i[p_i]-(b_j - \frac{t_j e^{a_i}}{t_i e^{a_j}} b_i) $$

其中：

$p_i$为跟踪参考关键帧ref frame$l$层上，带有逆深度的参考点。
$p_j$为使用当前优化参数投影到当前帧$l$层上的点。
$E_l$为$l$层金字塔上的能量值，同样也是优化目标函数。

与DSO中的优化模型文章中的模型对比发现，跟踪模型并没有使用pattern，这部分也印证了我在1.2小节中的猜测。

2.2.2 雅可比矩阵构建

虽然模型没有使用pattern，但是残差部分是一致的，因此雅可比矩阵的构建也完全一致。在前端位姿估计部分，不涉及地图点逆深度的优化问题，因此待优化量主要有当前帧相对于跟踪参考帧的位姿$T_{cr}$以及当前帧的光度仿射参数$a_j$和$b_j$，对于$a_j$和$b_j$来讲，它们的求导是平凡的，而相对位姿$T_{cr}$的求导，我在文章DSO中的优化模型中做了详细描述，这里只写出相关结论。

$$ \begin{align*} \frac{\partial{r_k}}{\partial{\xi_{ji}}}&= \frac{1}{P_Z'} \begin{bmatrix} d_x&d_y \end{bmatrix} \begin{bmatrix} f_x & 0 \\ 0 & f_y \end{bmatrix} \begin{bmatrix} 1 & 0 & -\frac{P_X'}{P_Z'} \\ 0 & 1 & -\frac{P_Y'}{P_Z'} \\ \end{bmatrix} \begin{bmatrix} d_{pi}&-P'^{\wedge} \end{bmatrix}\\ &= \begin{bmatrix} d_xf_x&d_yf_y\end{bmatrix} \begin{bmatrix} \frac{d_{pi}}{P_{Z}'} & 0 & -\frac{d_{pi}}{P_{Z}'}\frac{P_{X}'}{P_{Z}'} & -\frac{P_{X}'P_{Y}'}{P_{Z}'^2} & 1+\frac{P_{X}^{2}}{P_{Z}^{2}} & -\frac{P_{Y}'}{P_{Z}'} \\ 0 & \frac{d_{pi}}{P_{Z}'} & -\frac{d_{pi}}{P_{Z}'}\frac{P_{Y}'}{P_{Z}'} & -1-\frac{P_{Y}^{2}}{P_{Z}^{2}} & \frac{P_{X}'P_{Y}'}{P_{Z}^{2}} & \frac{P_{X}'}{P_{Z}'} \end{bmatrix}\\ \frac{\partial r_k}{\partial a_j}&=\frac{t_j e^{a_i}}{t_i e^{a_j}}(b_i-I_i[p_i])\\ \frac{\partial r_k}{\partial b_j}&=-1 \end{align*} $$

跟踪部分的细节性问题

计算残差时，采用动态阈值调节的手段，即先确定一个相对可控的残差阈值，然后根据内点比例适当地调整倍率来确保有足够的内点进行优化。当这个可控阈值调整过后，说明某层优化的残差相对较大，为了保证收敛，会对这一层进行再次优化。（调整过阈值的层会优化两次，怀疑内点比例低）
当某一层优化收敛后，会判断最终的优化RMSE和所有尝试的最小RMSE * 1.5做判断，如果大于1.5倍的最小值，则认定这次尝试直接失败，退出这次尝试，以节省时间，源码中维护这一行为的变量为achieveRes。
如何判断某次尝试失败与否？DSO从光度仿射参数的相对量和绝对量上判断是否优化成功，$a_j > 1.2$ 或者 $b_j > 200$则失败，$a_{ji}>1.5$或者$b_{ji}>200$则认为失败。
如何提前退出尝试？从2.1节的介绍中可以发现，一共有$(3 * 3 * 3 - 1) * 3 + 4 = 82$次尝试，DSO为了避免多次尝试导致的时间浪费问题，会使用上一帧优化能量值 * 1.5 作为退出尝试的条件，当然这样就可能导致动态阈值一直增大的情况。DSO在关键帧判断中对这个问题做了约束，保证了动态阈值增长的上限（见本章第3节）。

3. 关键帧判断（跟踪后处理）

在关键帧的判断中，除了通常考虑的时间间隔和空间间隔外，DSO还考虑了光度变化和用来提前退出尝试的跟踪能量阈值。

DSO中考虑的时间间隔相对普通，会人为限定一个创建关键帧的最大时间间隔，在DSO的源码中默认是不开启这个配置的。
DSO考虑的空间间隔相对新颖，不是所谓的真实距离，而是考虑了某些位姿间隔条件下的平均光流大小（金字塔第0层）。我们知道两张图像的平均光流大小可以近似的代表两张图像之间的视差，光流越大，视差越大，两张图片的重叠部分越少，反之亦然。DSO使用$T_{cr}$中的$t_{cr}$、$-t_{cr}$、$T_{cr}$和$[R_{cr},t_{cr}]$四种位姿导致的平均光流，配合自定义权重判断是否需要创建关键帧。使用四种位姿的平均光流而不是仅仅$T_{cr}$造成的平均光流，我猜测可能是因为使用四种位姿可以在一定程度上模拟之后一段时间内的运动状态，从而做到提前判断。
DSO使用了基于光度仿射参数的直接法，因此当相对仿射参数$a_{ji}$变化较大时，代表着当前帧和参考关键帧之间的光照条件发生了相对较大的变化，因此需要及时创建关键帧防止跟踪丢失情况的发生。
在第2小节的末尾，做了一些跟踪细节方面的讨论，提到了提前退出尝试的动态阈值可能会持续增大，从而导致阈值限定意义失效，DSO为了防止这种情况的发生，会提前记录基于当前参考关键帧第一次跟踪成功的能量值，如果当前帧跟踪阈值超过了这个值的2倍，则认定需要创建关键帧了，这么做相当于对动态阈值的上限做了规定。

DSO中的初始化

2025-01-16T00:00:00+08:00

在这篇文章中，我会对DSO初始化的过程和初始化中的一些要点进行讲解说明。作为一个单目视觉VO系统，DSO也采用了多帧初始化的策略，即首先在初始化参考帧上进行提点操作，然后根据不同的帧来优化初始化参考帧上点的逆深度估计，初始化的优化模型是在DSO中的优化模型文章中提到的优化模型基础上，考虑了逆深度的连续性和平移距离构建的，并且使用了Schur消元来加速初始化中的优化过程。

在DSO的初始化过程中还使用了图像金字塔（从粗到精的优化）来提高鲁棒性，并构建点的同层相邻点和上层父点之间的联系，实现点的逆深度传递。除此之外，DSO在初始化过程中还对相对光度仿射参数进行了处理，下面是我对这些要点的详细说明：

1. 初始化中对仿射参数的处理

我在DSO中的优化模型文章中提到了DSO的优化残差，可以使用下面的公式进行描述。

$$ r_k = (I_j(p') - b_j) - \frac{t_je^{a_j}}{t_ie^{a_i}}(I_i(p) - b_i) $$

可以看到，公式中涉及到了参考帧$i$待求解帧$j$的绝对光度仿射参数，为了简化这部分对求导的影响，DSO构造了相对仿射参数来代替绝对仿射参数，即令$e^{aji}=\frac{t_je^{a_j}}{t_ie^{a_i}}$，$b_{ji}=b_j - a_{ji}b_i$得公式(2)：

$$ r_k = I_j(p')-e^{a_{ji}}I_i(p)-b_{ji} $$

为什么不使用$a_{ji}$而是使用$e^{a_{ji}}$？

首先，如果使用$a_{ji}$来代替$\frac{t_je^{a_j}}{t_ie^{a_i}}$话，就会要求$a_{ji}>0$的，因此模型会从原来的无约束优化问题变成了部分变量的有约束优化问题，会增加整个优化过程中的难度。因此为了避免这个问题，DSO中使用了$e^{a_{ji}}$来代替$\frac{t_je^{a_j}}{t_ie^{a_i}}$而不是$a_{ji}$。

为了解决这个优化问题，除了DSO中的优化模型文章中求解的一些雅可比矩阵外，还需要求解残差对相对光度仿射参数的雅可比矩阵，从构建的残差公式中不难得到：

$$ \frac{\partial r_k}{\partial a_{ji}} = e^{a_{ji}}I_i(p)\\ \frac{\partial r_k}{\partial b_{ji}} = -1 $$

2. 初始化优化模型中的惩罚项

接下来，我们需要讨论的是，为什么DSO在初始化过程中需要一个相对大并且准确估计的相对平移距离$t_{ji}$。现在看下图的一个位姿估计过程，对比大平移距离$t_{ji}$和小平移距离$t_{ji}$的两张优化结果。可以发现，如果投影到$j$帧上的像素点存在1个像素的误差，大平移距离对应的点$p_i$的深度变化可以由图像左侧的绿色大括号表示，而小平移距离对应的点$p_i$的深度变化由右侧的绿色大括号表示。可以看出，小平移距离估计的点$p_i$的深度受投影像素误差影响比较大。因此DSO才会针对平移距离构建不同的优化目标函数，试图找到一个准确且足够大的$t_{ji}$，来保证初始化参考帧上的点$p_i$逆深度的稳定性和准确性。

2.1 平移距离不足时的惩罚项

DSO在初始化过程中还考虑了平移距离和逆深度连续性的影响，DSO认为在初始化过程中，输入的普通帧$j$和初始化参考帧$i$的平移距离应该达到某个阈值才认为$j$帧对参考帧上点逆深度起作用，为了确保在优化过程中得到的相对平移距离足够可靠，DSO在未达到足够的平移距离之前，会对平移距离添加惩罚项，除此之外还会对点的逆深度做一个尺度约束，因此在平移距离不够的条件下，DSO构造的优化目标函数如下：

$$ E_f=\sum_{p_i\in R_f}{\sum_{p\in \mathcal{N}_i(p_i)}{||I_j(p')-e^a_{ji}I_{R_f}(p)-b_{ji}||_{\gamma}}}+E_l\\ E_l=\frac{\alpha_W}{2} \sum_{p_i\in R_f}{((d_{pi}-1)^2+||t_{ji}||^2_2)} $$

其中：

$E_f$部分为平移量不足条件下的优化目标函数；
$E_l$部分包含了逆深度尺度惩罚项$(d_{pi} - 1)^2$和相对平距离的惩罚项$||t_{ji}||^2_2$；
$\alpha_W$代表调节正则化项部分的超参数，从而在一定程度上改变优化的侧重方向；
$R_f$代表是初始化参考帧

针对$E_l$的惩罚项部分，$E_l^{p_j}$的整个组成部分相对简单，可以直接求解惩罚项的雅可比矩阵$J_l$和海塞矩阵$H_l$，而不必使用高斯牛顿法利用残差项的雅可比变换得到：

$$ \frac{\partial E_l^{p_j}}{\partial x} = J_{p_i}=\begin{bmatrix} t_{ji}^T & 0_{1 \times 3} & 0 & 0 & d_{pi}-1 \end{bmatrix} \\ \frac{\partial^2 E_l^{p_j}}{\partial x^2} = H_{pi}= \begin{bmatrix} \begin{bmatrix} I_{3\times 3} & \mathbf{0_{3\times3}}\\ \mathbf{0_{3\times3}} & \mathbf{0_{3\times3}} \end{bmatrix} & \mathbf{0_{6 \times 1}} & \mathbf{0_{6 \times 1}} & \mathbf{0_{6 \times 1}}\\ \mathbf{0_{1 \times 6}} & 0 & 0 & 0 \\ \mathbf{0_{1 \times 6}} & 0 & 0 & 0 \\ \mathbf{0_{1 \times 6}} & 0 & 0 & 1 \\ \end{bmatrix} $$

$E_l^{p_j}$和$E_l$的区别

值得注意的是，$E_l^{p_j}$的能量值，代表是初始化参考关键帧$R_f$上的点$p_i$向$p_j$部分投影这一约束贡献的系统能量值，或者说是贡献的优化目标函数值。惩罚项的海塞矩阵$H_l$可以由所有的$H_l^{p_j}$组合而成，即相同的约束部分可以加和，而不用的约束部分需要拓展组成一个大矩阵$H_l$，当然，雅可比矩阵也是如此。

2.2 平移距离足够时的惩罚项

当平移距离足够时，DSO认为初始化参考帧$i$中的像素点$p_i$的逆深度会有一个相对稳定、准确且尺度一致（因为使用了$(d_{pi}-1)^2$作为尺度惩罚项）的值，这时，DSO会去掉对平移距离的惩罚项，同时对逆深度进行期望约束，这里的期望值$iR$考虑了当前点$p_i$和周围点的逆深度关系，即逆深度的平滑性，得到的优化目标函数如下：

$$ E_f=\sum_{p_i\in R_f}{\sum_{p\in \mathcal{N}_i(p_i)}{||I_j(p')-e^a_{ji}I_{R_f}(p)-b_{ji}||_{\gamma}}}+E_l\\ E_l=\frac{\alpha}{2} \sum_{p_i\in R_f}{(d_{pi}-iR_{i})^2} $$

其中：

$E_f$部分为平移量足够条件下的优化目标函数；
$E_l$部分包含了逆深度期望惩罚项$(d_{pi} - iR_i)^2$，充分考虑了周围点逆深度的平滑性；
$\alpha$代表调节正则化项部分的超参数，从而在一定程度上改变优化的侧重方向；

针对$E_l$的惩罚项部分，求得其雅可比矩阵和海塞矩阵如下：

$$ \frac{\partial{E_l^{p_j}}}{\partial{x}}=J_l^{p_j}= \begin{bmatrix} \mathbf{0_{1\times6}} & 0 & 0 & d_{pi}-iR \end{bmatrix} \\ \frac{\partial^2{E_l^{p_j}}}{\partial{x^2}} = H_l^{p_j} = \begin{bmatrix} \mathbf{0_{6\times6}} & \mathbf{0_{6 \times 1}} & \mathbf{0_{6 \times 1}} & \mathbf{0_{6 \times 1}}\\ \mathbf{0_{1\times6}} & 0 & 0 & 0 \\ \mathbf{0_{1\times6}} & 0 & 0 & 0 \\ \mathbf{0_{1\times6}} & 0 & 0 & 1 \\ \end{bmatrix} $$

3. 基于金字塔的逆深度传递

为了能够方便的表述逆深度传递的重要性，在这部分我会先在3.1小节中梳理一下DSO初始化流程，然后在3.2小节中重点讲解基于金字塔和相邻点的逆深度传递过程。

3.1 DSO初始化过程

下图中，使用虚线框标注出来的部分是DSO初始化器的主要工作流程，值得注意的是，在初始化点选之后，参考帧$R_f$上会得到点$p_i\in R_f$，这些点$p_i$会分布在不同的金字塔层级上，为了方便后续这些点的逆深度传递，DSO构造了这些点的同层相邻点和上层父点之间的关系。

3.2 逆深度传递

DSO的初始化器构造了同层相邻点和上层父点之间的关系，这样会比较方便的执行下面的三种操作：

从上层到下层优化过程中，上层父点会向下层子点提供逆深度初值。
当优化完成后，下层子点会将充分优化的逆深度初值向上层父点传递，以修正父点逆深度值。
由于DSO初始化器在优化目标函数构建中，考虑了相邻点逆深度连续性，这部分连续性的考虑是由相邻点关系实现的。

在每一步优化结束后，DSO初始化器会更新逆深度值，同样会更新逆深度期望值，期望值的更新考虑了相邻点之间的连续性关系，下面的公式描述了逆深度期望的更新策略：

$$ iR_i=(1-\beta)\times{d_{pi}}_{new}+\beta\times{iR_{mid}^i}\\ iR_{mid}^i={Median}(neighbors(p_i)) $$

其中：

$iR_i$为点$p_i$逆深度的更新期望；
${d_{pi}}_{new}$为点$p_i$逆深度的优化更新值；
$\beta$为相邻点逆深度连续性置信度，在源码为0.8；
$iR_{mid}^i$为点$p_i$邻居的逆深度期望中值；

DSO初始化器使用同层金字塔K近邻的方式构建同层点和邻居点之间的关系，使用向上层金字塔投影寻找最近邻的方式构建与上层父点之间的关系。

3.3 高斯归一化积

考虑父点向子点传递逆深度过程：除了第一次优化过程外，子点都会有一个相对有效的逆深度优化值，这时父点还会向子点提供一个逆深度。DSO初始化器使用了高斯归一化来耦合这两个逆深度值，高斯归一化积的数学描述如下：

$$ \mu_{c} H_{c}=\sum_{i=0}^N \mu_iH_i\\ H_c=\sum_{i=0}^N H_i $$

父点向子点传递时，子点不仅仅是逆深度值$d_{pi}$发生了改变，子点的逆深度期望$iR$同样发生了改变，这个传递过程可以由下面的流程图进行描述：

3.2中的公式同样描述了，流程图中的考虑相邻点$iR$的连续性操作。通过这样的操作，可以人为的构造出一个考虑了连续性影响的逆深度期望值出来，从而能够完成后续初始化优化目标函数（会涉及点$p_i$的逆深度期望$iR_i$）的构建。

考虑子点向父点的逆深度传递过程：在3.2小节的描述中，父子点之间的关系构造是通过投影和最近邻实现，因此可能会出现一个父点对应多个子点的情况，这时又会出现多个逆深度值同时出现的问题，DSO仍然是通过高斯归一化积来解决这个问题，下面的流程图描述了具体的传递过程：

4. 初始化器的Schur优化加速

这里我打算以$t_{ji}$不足条件下的优化加速为例，初始化中$t_{ji}$不足时的优化目标函数如下：

$$ E_f=\sum_{p_i\in R_f}{\sum_{p\in \mathcal{N}_i(p_i)}{||I_j(p')-e^a_{ji}I_{R_f}(p)-b_{ji}||_{\gamma}}}+E_l\\ E_l=\alpha \sum_{p_i\in R_f}{(d_{pi}-iR_{i})^2} $$

首先，考虑非惩罚部分$E_{nl}=E_f-E_l$，这部分能量的残差为$r_k^{nl}=I_j(p')-e^a_{ji}I_{R_f}(p)-b_{ji}$，那么根据高斯牛顿法，在不考虑Huber核函数的情况下，可以构建优化方程：

$$ \begin{align*} H_{nl}^{p_j} & =\sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{x}})^T(\frac{\partial{r_k^{nl}}}{\partial{x}})} \\ & = \begin{bmatrix} \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{\delta\xi_{ji}}})^T(\frac{\partial{r_k^{nl}}}{\partial{\delta\xi_{ji}}})} & \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{\delta\xi_{ji}}})^T (\frac{\partial{r_k^{nl}}}{\partial{a_{ji}}})} & \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{\delta\xi_{ji}}})^T (\frac{\partial{r_k^{nl}}}{\partial{b_{ji}}})} & \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{\delta\xi_{ji}}})^T (\frac{\partial{r_k^{nl}}}{\partial{d_{pi}}})} \\ \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{a_{ji}}})^T(\frac{\partial{r_k^{nl}}}{\partial{\delta\xi_{ji}}})} & \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{a_{ji}}})^T\frac{\partial{r_k^{nl}}}{\partial{a_{ji}}}} & \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{a_{ji}}})^T(\frac{\partial{r_k^{nl}}}{\partial{b_{ji}}})} & \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{a_{ji}}})^T(\frac{\partial{r_k^{nl}}}{\partial{d_{pi}}})} \\ \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{b_{ji}}})^T(\frac{\partial{r_k^{nl}}}{\partial{\delta\xi_{ji}}})}& \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{b_{ji}}})^T(\frac{\partial{r_k^{nl}}}{\partial{a_{ji}}})}& \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{b_{ji}}})^T(\frac{\partial{r_k^{nl}}}{\partial{b_{ji}}})}& \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{b_{ji}}})^T(\frac{\partial{r_k^{nl}}}{\partial{d_{pi}}})} \\ \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{d_{pi}}})^T(\frac{\partial{r_k^{nl}}}{\partial{\delta\xi_{ji}}})} & \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{d_{pi}}})^T(\frac{\partial{r_k^{nl}}}{\partial{a_{ji}}})} & \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{d_{pi}}})^T(\frac{\partial{r_k^{nl}}}{\partial{b_{ji}}})} & \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{d_{pi}}})^T(\frac{\partial{r_k^{nl}}}{\partial{d_{pi}}})} \end{bmatrix} \end{align*} $$

$$ b_{nl}^{p_j}=-\sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{x}})^T \times r_k^{nl}}\quad= -\begin{bmatrix} \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{\delta\xi_{ji}}})^T \times r_k^{nl}} \\ \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{a_{ji}}})^T \times r_k^{nl}} \\ \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{b_{ji}}})^T \times r_k^{nl}} \\ \sum_{p \in \mathcal{N_i(p_i)}}{(\frac{\partial{r_k^{nl}}}{\partial{d_{pi}}})^T \times r_k^{nl}} \end{bmatrix} $$

$$ H_{nl}= \begin{bmatrix} \sum_{p_i \in R_f}{U_{p_j}} & W^{p_0} & W_{p_1} & ... & W_{p_m} \\ W_{p_0} & V_{p_0} \\ W_{p_1} & & V_{p_1}\\ ... & & & ... \\ W_{p_m} & & & & V_{p_m}\\ \end{bmatrix} \quad b_{nl}= -\begin{bmatrix} \sum_{p_i \in R_f}{b_{U}} \\ b_{V_{p_0}} \\ b_{V_{p_1}} \\ ... \\ b_{V_{p_m}} \\ \end{bmatrix} $$

其中：

$\frac{\partial{r_k^{nl}}}{\partial{\delta\xi_{ji}}}$、$\frac{\partial{r_k^{nl}}}{\partial{d_{pi}}}$的计算公式在DSO中的优化模型文章中推导过；
$\frac{\partial{r_k^{nl}}}{\partial{b_{ji}}}$、$\frac{\partial{r_k^{nl}}}{\partial{d_{pi}}}$的计算公式在本篇文章的第一小节中推导过；
$H_{nl}^{pj}$和$b_{nl}^{p_j}$，为某个点$p_i$投影到$p_j$这部分的优化约束；
$H_{nl}$和$b_{nl}$，为不含惩罚项部分的能量函数总约束，我使用了分块矩阵进行表示，这么做比较方便，矩阵分块结构图如下：

对惩罚项部分，根据文章的第2小节推导的公式，考虑超参数$\alpha_W$，并使用上图中的分块方式，可以得到惩罚项部分的优化约束矩阵：

$$ H_l= \alpha_W \begin{bmatrix} \sum_{p_i \in R_f}{\begin{bmatrix} I_{3\times 3} & \mathbf{0_{3\times3}} & \mathbf{0_{3 \times 1}} & \mathbf{0_{3 \times 1}}\\ \mathbf{0_{3\times3}} & \mathbf{0_{3\times3}} & \mathbf{0_{3 \times 1}} & \mathbf{0_{3 \times 1}}\\ \mathbf{0_{1\times3}} & \mathbf{0_{1\times3}} & 0 & 0\\ \mathbf{0_{1\times3}} & \mathbf{0_{1\times3}} & 0 & 0\\ \end{bmatrix}} & \mathbf{0_{8\times1}} & \mathbf{0_{8\times1}} & ... & \mathbf{0_{8\times1}}\\ \mathbf{0_{1\times8}} & 1\\ \mathbf{0_{1\times8}} & & 1\\ ... & & &...\\ \mathbf{0_{1\times8}} & & & & 1\\ \end{bmatrix} \\ b_l = -\alpha_W \begin{bmatrix} \begin{bmatrix}t_{ji}\\ \mathbf{0_{3\times1}} \\0\\0\end{bmatrix}\\d_{p0}-1\\d_{p1}-1\\...\\d_{pm}-1 \end{bmatrix} $$

因此整个系统的优化约束为：

$$ H=H_{nl}+H_{l}= \begin{bmatrix} \sum_{p_i \in R_f}{U_{p_j}'} & W^{p_0} & W_{p_1} & ... & W_{p_m} \\ W_{p_0} & V_{p_0}+\alpha_W \\ W_{p_1} & & V_{p_1}+\alpha_W \\ ... & & & ... \\ W_{p_m} & & & & V_{p_m}+\alpha_W \\ \end{bmatrix} \quad b=b_{nl}+b_l= -\begin{bmatrix} \sum_{p_i \in R_f}{b_{U}'} \\ b_{V_{p_0}} + \alpha_W(d_{p_0} - 1) \\ b_{V_{p_1}} + \alpha_W(d_{p_1} - 1) \\ ... \\ b_{V_{p_m}} + \alpha_W(d_{p_m} - 1) \\ \end{bmatrix} \\ U_{p_j}'=U_{p_j} + \alpha_W \begin{bmatrix} I_{3\times 3} & \mathbf{0_{3\times3}} & \mathbf{0_{3 \times 1}} & \mathbf{0_{3 \times 1}}\\ \mathbf{0_{3\times3}} & \mathbf{0_{3\times3}} & \mathbf{0_{3 \times 1}} & \mathbf{0_{3 \times 1}}\\ \mathbf{0_{1\times3}} & \mathbf{0_{1\times3}} & 0 & 0\\ \mathbf{0_{1\times3}} & \mathbf{0_{1\times3}} & 0 & 0\\ \end{bmatrix} \quad b_{U}'=b_{U} + \alpha_W \begin{bmatrix}t_{ji}\\ \mathbf{0_{3\times1}} \\0\\0\end{bmatrix} $$

最后，根据SLAM十四讲p249页描述的Schur分解公式，先将涉及逆深度部分分解掉，求解相对位姿和相对仿射参数的增量，然后将相对位姿增量和相对仿射参数增量再次带入涉及逆深度方程的部分，求解得到逆深度增量即可。

大矩阵Schur和小矩阵Schur

在上面的推导中，初始化器总优化约束的$H$矩阵和$b$矩阵由所有投影关系$p_j$-$p_i$的优化约束$H_{p_j}$和$b_{p_j}$得到。同样不难证明，大矩阵$H$和$b$进行Schur分解和所有小矩阵$H_{p_j}$和$b_{p_j}$分别Schur分解然后组合拼接（这里直接累加即可，因为分解后的矩阵的约束变量是相同的）的效果是一样的。DSO的源码里面也利用了这个结论，从而避免了大稀疏矩阵构建导致的高空间复杂度问题。

DSO中的优化模型

2025-01-15T00:00:00+08:00

在之前的DSO中的去畸变操作文章中，讲到DSO考虑了相机的成像过程对图像像素的影响：

在有光度参数的条件下，DSO使用$G^{-1}(I)$非线性响应函数的逆过程和渐晕函数$V(x)$进行图像的光度去畸变操作，可以得到由能量单位组成的去光度畸变的图像。
在没有光度参数的条件下，DSO使用仿射参数a和b来模拟光度参数的去畸变过程。
在DSO中的去畸变操作文章中，也进行了讨论，即去畸变得到的像素能量并不能保证同一点的一致性，因为还没有考虑曝光时间的影响。

1. 构建优化模型

SLAM十四讲中提到，直接法会直接使用像素的灰度值，构建优化模型的残差，其公式表示为$r_k=I_i(p_i)-I_j(p_j)$。由于DSO引入了仿射参数去光度畸变的操作，同时考虑了曝光时间对其一致性的影响，构建的残差应该为：

$$ r_k = \frac{1}{t_i}(e^{a_i} I_i(p_i) - b_i) - \frac{1}{t_j}(e^{a_j} I_i(p_i) - b_j) $$

通过一些移项和变换的操作，并考虑像素块pattern的影响，就可以得到DSO论文里面给出的残差模型了：

$$ E_{p_j} = \sum_{p \in \mathcal{N_{p_i}}}{\left | \left | (I_j(p') - b_j) - \frac{t_je^{a_j}}{t_ie^{a_i}}(I_i(p) - b_i) \right | \right | }_\gamma \\ P_{norm} = \pi^{-1}(p) \\ P' = R_{ji} P_{norm} + t_{ji} d_{pi} \\ P_{norm}' = \frac{P'}{P'_Z} \\ p' = \pi(P'_{norm}) $$

其中：

$t_i$和$t_j$分别为$i$帧和$j$帧的曝光时间，如果不存在曝光时间，则设定$t_i=t_j=1$；
$a_i$和$b_i$为$i$帧的仿射参数，$a_j$和$b_j$为$j$帧的仿射参数，如果存在光度参数和曝光时间，则设定$a_j = a_i = b_j = b_i = 0$；
$I_i$和$I_j$分别为$i$帧和$j$帧的去畸变图像，设定$i$帧为参考帧，而$j$帧为待估计帧；
$p$和$p'$分别为$i$帧上的像素点和$j$帧上的像素点，其中$p'$是$p$经过反向投影，位姿变换和正向投影得到的像素点；
$\pi$函数指的是从归一化坐标系到像素坐标系的投影，$\pi^{-1}$是$\pi$的反函数；
$\mathcal{N_{p_i}}$为以$i$帧上的像素点$p_i$为中心的像素块，在DSO的论文中，把他叫做一个pattern，DSO引入pattern的概念，认为在一个pattern上，所有像素的逆深度$d_{pi}$值保持一致，下图中是在DSO论文中讨论的一些pattern；

DSO的投影过程与普通BA之间存在区别？

在DSO构建的模型中，有一个与普通BA过程存在明显不同的地方，即在构建完成反向投影后，并不会使用$R_{ji}\frac{P_{norm}}{d_{pi}}+t_{ji}$来求解真实的3d点，而是使用$R_{ji}P_{norm}+t_{ji}d_{pi}$的方式，乘在了右边，构建了一个虚拟的3d点，这个虚拟3d点在坐标系原点真实3d的直线上，因此真实点和虚拟点之间对应着一个相同的归一化坐标系下的点。这么构建有一个比较明显的优势，即针对$d_{pi}$求导时，会变的比较简单。

2. 求解模型雅可比

现在，令$r_k=I_j(p')-\frac{t_je^{a_j}}{t_ie^{a_i}}I_i(p)+\frac{t_je^{a_j}}{t_ie^{a_i}}b_i-b_j$，考虑使用GN法或者LM法求解这个优化问题，因此需要求解残差$r_k$对待优化量的雅可比矩阵，后续无论是需要为优化模型添加核函数，或者是求解优化模型的海塞矩阵H，都可以通过残差对待优化量的雅可比矩阵进行变换得到。

2.1 残差对位姿的雅可比矩阵

根据链式求导法则，残差对位姿$T_{ji}$的左侧扰动$\xi$的雅可比矩阵$\frac{\partial{r_k}}{\partial{{\xi_{ji}}}}$：

$$ \frac{\partial{r_k}}{\partial{{\xi_{ji}}}} = \frac{\partial{r_k}}{\partial I_j} * \frac{\partial{I_j}}{\partial{p'}} * \frac{\partial{p'}}{\partial{P_{norm}'}} * \frac{\partial{P_{norm}'}}{\partial{P'}} * \frac{\partial{P'}}{\partial{\xi_{ji}}} $$

$\frac{\partial{r_k}}{\partial I_j}$，根据残差公式，可以看出来$\frac{\partial{r_k}}{\partial I_j}=1$；
$\frac{\partial{I_j}}{\partial{p'}}$，可以定义为$p'$在图像$I_j$上的像素梯度，以$[d_x,d_y]$进行表示；
$\frac{\partial{p'}}{\partial{P_{norm}'}}$，这部分表示的是归一化坐标系到像素坐标系的投影过程，其雅可比矩阵可以使用如下公式进行表示；

$$ \frac{\partial{p'}}{\partial{P_{norm}'}} = \begin{bmatrix}f_x&0\\0&f_y\end{bmatrix} $$

$\frac{\partial{P_{norm}'}}{\partial{P'}}$，这部分表示的是虚拟点$P'$到归一化坐标系的投影过程，根据公式不难推导出,其雅可比矩阵可以由下面的公式进行表示：
$$ \frac{\partial{P_{norm}'}}{\partial{P'}} = \begin{bmatrix}\frac{1}{P_Z'}&0&-\frac{P_X'}{P_Z'^2}\\0&\frac{1}{P_Z'}&-\frac{P_Y'}{P_Z'^2}\end{bmatrix} $$
$\frac{\partial{P'}}{\partial{\xi_{ji}}}$，这部分表示的是虚拟3d点位姿$T_{ji}$左乘扰动$\xi_{ji}$的雅可比矩阵，在视觉SLAM十四讲的中86页中推导过，这里只不过是针对虚拟点做了一些变换，可以得到$\frac{\partial{P'}}{\partial{\xi_{ji}}}=[d_{pi}I,-P'^{\wedge}]$

综上，通过链式法则将5部分的雅可比矩阵相乘可得残差对$T_{ji}$的左乘雅可比矩阵为:

2.2 残差对$p_i$点逆深度的雅可比矩阵

根据链式求导法则，残差对点的逆深度$d_{pi}$的雅可比矩阵$\frac{\partial{r_k}}{\partial{{d_{pi}}}}$：

$$ \frac{\partial{r_k}}{\partial{{d_{pi}}}} = \frac{\partial{r_k}}{\partial I_j} * \frac{\partial{I_j}}{\partial{p'}} * \frac{\partial{p'}}{\partial{P_{norm}'}} * \frac{\partial{P_{norm}'}}{\partial{P'}} * \frac{\partial{P'}}{\partial{d_{pi}}} $$

可以发现，残差对$p_i$点逆深度的链式求导的雅可比矩阵的前4部分都是相同的，因此只需要考虑$\frac{\partial{P'}}{\partial{d_{pi}}}$这部分的雅可比即可，从公式中不难推导出：

$$ \frac{\partial{P'}}{\partial{d_{pi}}}=t_{ji} $$

综上，通过链式法则将5部分的雅可比矩阵相乘可得残差对$d_{pi}$的雅可比矩阵为:

$$ \begin{align*} \frac{\partial{r_k}}{\partial{d_{pi}}}&= \frac{1}{P_Z'} \begin{bmatrix} d_x&d_y \end{bmatrix} \begin{bmatrix} f_x & 0 \\ 0 & f_y \end{bmatrix} \begin{bmatrix} 1 & 0 & -\frac{P_X'}{P_Z'} \\ 0 & 1 & -\frac{P_Y'}{P_Z'} \\ \end{bmatrix} \begin{bmatrix} t_{ji}^X\\t_{ji}^Y\\t_{ji}^Z \end{bmatrix}\\ &= \frac{1}{P_Z'}[d_xf_x(t_X^{ji}-\frac{P_X'}{P_Z'}t^Z_{ji})+d_yf_y(t_Y^{ji}-\frac{P_Y'}{P_Z'}t^Z_{ji})] \end{align*} $$

2.3 残差对光度仿射参数的雅可比矩阵

DSO在不同的阶段对光度仿射参数做了不同程度的处理，其主要表现在初始化阶段、前端跟踪阶段和后端滑窗阶段。其中，初始化阶段和前端跟踪阶段主要注重相对仿射参数$a_{ji}$和$b_{ji}$，而在后端滑窗优化阶段中则更加注重全局的仿射参数$a_j$、$b_j$、$a_i$和$b_i$。

我打算分别在初始化、前端跟踪和后端滑窗优化三篇文章中单独对”DSO对仿射参数处理”进行解析说明，在这篇文章中就不过多赘述了。

3. 模型在后端优化中的问题

正如我在2.3小节中所描述的，DSO的模型在后端优化中会产生一些问题，原因在于后端优化的参数量为全局量，而不是相对量。即是global而非local。

其中在后端中需要特殊处理的参数主要有相对位姿$T_{ji}$转变为绝对位姿$T_{jw}$和$T_{iw}$、相对光度仿射参数$a_{ji}$和$b_{ji}$转变为全局光度仿射参数$a_j$，$b_j$，$a_i$和$b_i$。

模型如何在后端优化中解决这个问题的说明，我会在DSO滑窗优化后端中单独进行说明。

提前透漏

可以提前说明的是，在相对位姿转绝对位姿部分，DSO使用的是位姿矩阵伴随的性质，而光度仿射参数的转换则采用的是雅可比矩阵中转变换的方法。

DSO中的点选策略

2025-01-14T00:00:00+08:00

DSO作为一个稀疏直接法，其中一个核心内容就是对图像上的部分点进行跟踪和逆深度估计，既然是对图像上的某些点进行跟踪估计操作，那么肯定会涉及到如何在图像上选择点的操作，在整个DSO的算法逻辑中，点选策略主要使用在两个阶段，一个是算法初始化阶段，还有一个是创建关键帧阶段。

1. 初始化阶段的点选策略

在DSO的初始化过程中，需要确定一个初始化参考帧，当然为了初始化的稳定和准确性，需要构建图像金字塔来完成初始化参考帧的构建。当然初始化部分我会单独出一个文章来进行讲解，这里主要是对初始化过程中的点选策略进行分析，大家只需要记住在初始化过程中需要对初始化关键帧进行金字塔多层级的点选操作，以供后续的初始化操作使用即可，至于为什么使用金字塔？或者为什么使用这样的点选策略，我会在后续的DSO初始化文章中进行阐述。

1.1 金字塔第零层点选

第0层的金字塔点的选择的要求主要有以下三点：

数量要求：第0层上选择的点的数量要满足要求，这因该是最基本的选点要求；
特殊性要求：第0层上选择的点要具有一定的特殊性，能够对模型提供比较大信息，在Advantages-of-DSO文章里面提到，直接法往往和像素梯度直接相关，因此大梯度点，往往能对模型优化提供一个相对多的信息；
均匀性要求：第0层上选择的点要求具有一定的均匀性，保证位姿估计的稳定性，因为大梯度点，往往在图像的极个别区域内集中分布，选择均匀分布的大梯度点，往往能规避集中分布的像素点导致位姿估计的不稳定性问题；

DSO首先使用块阈值计算策略，将输入图像（经过光度畸变矫正和像素畸变矫正）划分为32px * 32px的n多个小像素块，对每个块内独立的统计像素梯度的均方根，找到0.5分位数对应的均方根梯度值，然后加上一个超参数bias的方式，作为大梯度阈值，除此之外，为了保证整张图像的块梯度平滑性，使用一个3 * 3的均值滤波来对块梯度阈值进行操作。使用上述的块阈值计算策略，可以为后续提供不同区域下的梯度阈值，满足了点选要求的特殊性和均匀性，整个块阈值计算策略如下图所示。

DSO使用定pot选点策略，首先使用给定的pot，依次从4pot * 4pot到2pot * 2pot再到pot * pot的区域内，进行遍历，试图找到满足阈值条件的像素点。

首先，针对最内层的pot * pot区域内，进行逐像素的遍历，找到pot * pot区域内的最大梯度像素点，并且要满足最大像素梯度值要大于块阈值计算策略中计算的平滑阈值。
当完成了2pot * 2pot内的pot * pot的遍历后，如果区域内的4个pot * pot区域没有找到一个满足阈值条件的像素点（在DSO中，会将这部分像素点从0层投影到1层，使用1层的梯度进行阈值条件判断），那么这里将阈值要求降低0.75倍，然后重新遍历2pot * 2pot区域内的所有像素试图找到一个满足阈值条件的像素点（这里的阈值条件已经降低了0.75倍数）。
当完成了4pot * 4pot内的2pot * 2pot的遍历后，如果区域内的4个2pot * 2pot区域内没有找到一个满足阈值条件的像素点，那么这里将阈值要求再次降低0.75倍，然后重新遍历4pot * 4pot区域内的所有像素点（在DSO中，会将这部分像素点从0层投影到2层，使用2层的梯度进行阈值条件判断），试图找到一个满足阈值条件的像素点（这里的阈值条件已经降低了0.75 * 0.75）倍。
使用这种定pot选点策略，可以保证选点要求的均匀性和特殊性，除了可以保证均匀性外，该策略还可以在图像梯度值比较低的条件下完成特征的提取，因为这里涉及到梯度阈值的降低，该策略的示意图如下所示。

定pot选点策略的加速

DSO中的定pot选点策略的思路就是我在上面描述的那样，但是反映在代码逻辑上，存在着一些可以加速的策略，比如说在DSO的源码部分，当小区域pot * pot对应的像素点任何点达到梯度阈值要求的时候，会同步判断2pot * 2pot和4pot * 4pot大区域是否满足阈值条件的要求，只有当小区域出现满足阈值条件要求的像素点时，才放弃对大区域内像素点的梯度判断，从而避免了小区域内没选到点而再次遍历的尴尬逻辑。当然，除此之外，还可以使用多线程加速的策略，即每个区域的所有像素点对应一个线程，三个线程同时进行遍历和判断，然后对最后得到的结果进行统计，取小区域中满足阈值要求的点。当cpu的性能足够时，这也不失为一种加速策略。

DSO使用递归pot随动策略，来保证选点要求的数量要求。

通过分析，不难发现，调整pot的大小可以间接的提点数量进行控制。
DSO中，使用pot=3作为初始的pot大小，通过定pot选点策略来选择均匀且带有特殊性的像素点。
当选择到的点比期望的点多时，说明当前的pot比较小，得到的pot * pot的区域比较多，因此需要递增pot的大小。
当选择到的点比期望的点少时，说明当前的pot比较大，得到的pot * pot的区域比较少，因此需要递减pot的大小。

与期望数目之间差多少才算多？

DSO在源码中，规定了一个尺度范围ratio为$[0.25,1.25]$，注意这个尺度范围ratio指的是 期望值 / 提取值，当ratio < 0.25时，认定此时提取的点比较多，需要递增pot的大小，当ratio > 1.25时，认定此时提取的点比较少，需要递减pot的大小。

1.2 金字塔其他层点选

在初始化过程中，金字塔的其他层选择相对简单，也存在一些定pot选点策略和递归pot随动策略来保证特征点的均匀性要求和数量一致性要求。但是这两种策略与第0层选点使用的定pot选点策略和递归pot随动策略不太一样，这里仅单独的进行一些说明。

首先，在阈值方面，金字塔其它层的提取使用定阈值10和阈值缩放因子scaleTH一起决定，即阈值的大小与缩放因子相关。
定pot选点策略，不像第0层那样，由4pot * 4pot 到 2pot * 2pot 再到 pot * pot那样以此降低阈值来提点了，而是仅pot * pot区域的遍历，试图找到最多4个像素点，该像素点要求阈值大于给定的10 * scaleTH，并且其x轴梯度最大，或y轴梯度最大，或x轴 - y轴梯度最大，或x轴 + y轴梯度最大。
递归pot随动策略，同样不像第0层那样，它考虑了一个模型，认定可以提取的点与pot的大小成反比，即$pot^2*num=K$，也就是说，首先使用一个默认pot进行点选，然后根据点选的数量和默认pot的大小，确定$K$值，然后使用$K$值计算期望的pot大小，然后递归的调用即可，其原理可以由下面的公式解释。

$$ \frac{pot_d^2}{pot^2}=\frac{n_{have}}{n_{desir}} $$

其中，$pot_d$为期望的pot大小，$pot$为当前提取的pot大小，$n_{have}$为使用$pot$提取到的点的数量，$n_{desir}$为期望提取的点的数量。

递归停止条件

由于pot总是一个整数，因此使用整型的pot可能永远得到不期望的提取数量，因此这里需要设置一个递归的停止条件。接下来，需要确定一个比例范围ratio，来定义比期望值多或者比期望值少，DSO的源码中定义这个比例范围为$[0.8,1.2]$，这个比例是提取值 / 期望值（注意与第0层的比例意义不同）。首先考虑，pot=1时，提取到的点仍然远少于期望提取的点，那么需要考虑减小scaleTH来放宽梯度阈值条件。当使用的 (提取$pot-pot_{d}=0$) || ($n_{have}/n_{desir}\in[0.8,1.2]$) || (达到最大要求递归次数)，则退出递归。

2. 创建关键帧阶段的点选策略

DSO的点选策略除了用在初始化阶段外，还会用在新关键帧的创建阶段。当新关键帧被创建时，DSO会使用初始化过程中，金字塔第零层点选策略来进行像素点的选取，并初始化为未成熟点，以供后续跟踪过程的逆深度优化。

DSO中的去畸变操作

2025-01-09T00:00:00+08:00

1. 像素去畸变

1.1 畸变模型

DSO中引入了不同的畸变模型，来解决不同相机的像素畸变问题，除了我们在SLAM十四讲里面熟知的RadTan畸变模型外，还有FOV和KannalaBrandt畸变模型。不同的像素畸变模型的公式由下面的公式给出：

首先是我们熟知的RadTan径向切向畸变模型，可以根据畸变程度选择不同参数数量进行配置计算，常用的有三参数法（$k_1$,$p_1$,$p_2$）和五参数法（$k_1$,$k_2$,$k_3$,$p_1$,$p_2$）：

$$ \left\{\begin{matrix} x_{distorted} = x(1+k_1r^2+k_2r^4+k_3r^6) + 2p_1xy+p_2(r^2+2x^2)\\ y_{distorted} = y(1+k_1r^2+k_2r^4+k_3r^6) + 2p_2xy+p_1(r^2+2y^2)\\ r = \sqrt{x^2+y^2} \end{matrix}\right. $$

然后是FOV视野畸变模型，仅有一个参数$\omega$：

$$ \left\{\begin{matrix} x_{distorted} =\frac{r_{d}}{r}\cdot x\\ y_{distorted} =\frac{r_{d}}{r}\cdot y\\ r_{d} =\frac{1}{\omega }arctan(2\cdot r\cdot tan(\frac{\omega }{2}))\\ r = \sqrt{x^2 + y^2}\\ \end{matrix}\right. $$

最后是KannalaBrandt光度畸变模型，有四个参数$k_1$,$k_2$,$k_3$,$k_4$：

$$ \left\{\begin{matrix} x_{distorted} = \frac{\theta _{d}}{r}\cdot x_{c}\\ y_{distorted} = \frac{\theta _{d}}{r}\cdot y_{c}\\ \theta = atan(r)\\ \theta_{d}=\theta(1+k_1\theta^2+k_2\theta^4+k_3\theta^6+k_4\theta^8) \end{matrix}\right. $$

1.2 DSO中畸变模型转Pinhole逻辑

当然，这里的畸变模型并不是我想向大家进行解析的重点，DSO将这种像素畸变模型向小孔成像模型Pinhole进行了转换，而这个转换的逻辑，是我想给大家分享的重点！这里三种不同的畸变模型，我以$d(p,K)$来进行表示，这里的$p$表示归一化坐标系下的某个实际点，而$K$表示畸变模型的参数。

在归一化坐标系上，划定一块足够大的区域$S$，在DSO的源码里面是$x\in[-5,+5],y\in[-5,+5]$，试图通过在$x=0$和$y=0$的区域线段上，找到一组极限位置$X_{min}$、$X_{max}$、$Y_{min}$和$Y_{max}$，作为初始的粗略归一化区域（这个区域包含足够多的像素信息）。
在X=0轴方向上，以y划分为10万个刻度点，使用这些点向畸变坐标系进行投影，试图找到一个$Y_{min}$和$Y_{max}$，对应的这两个点的像素投影点都应在畸变像素坐标系内。
在Y=0轴方向上，使用上面一样的策略，寻找$X_{min}$和$X_{max}$。
在获得的极限位置轴上，取点（点的个数可以是畸变图像的宽度或者高度，以保证足够的分辨率），进行畸变和投影获得畸变像素坐标，判断像素坐标是否在图像范围内。
如果所有点都在像素图像范围内，则认为这个这个轴的位置是合理的。
如果存在点不在像素图像范围内，则需要将这个轴的位置进行向内移动，源码中是*0.995，以此来达到向内移动的效果。
注意，当X轴和Y轴部分都需要向内移动时，源码中为了防止额外的像素损失，仅尺寸较大的那一轴进行移动。
总的来说，DSO在实现畸变模型到纯小孔成像模型之间转换的时候，使用了两步法实现，首先，通过X=0和Y=0两个轴上的点确定一个较粗粒度的归一化区域，因为使用的是归一化中心点，因此这个区域肯定可以覆盖绝大部分的可视像素信息。然后再不断的投影四个极限轴上的点来实现轴的缩放，因此来获取一个精确的归一化区域。

使用上述说明的方式，可以获得一个归一化区域，这个区域内的所有点向畸变像素坐标系投影的结果都会在畸变图像范围内。在给定目标图像的宽度w和高度h后，可以计算获得pinhole条件下的内参矩阵K:

$$ \left\{\begin{matrix} f_xX_{min} + c_x = 0\\ f_yY_{min} + c_y = 0\\ f_xX_{max} + c_x = w\\ f_yY_{max} + c_y = h \end{matrix} \right. $$

最终可以得到：

$$ K = \begin{bmatrix} \frac{w}{X_{max}-X_{min}} & 0 & -\frac{wX_{min}}{X_{max}-X_{min}}\\ 0 & \frac{h}{Y_{max}-Y_{min}} & -\frac{hY_{min}}{Y_{max}-Y_{min}}\\ 0 & 0 & 1 \end{bmatrix} $$

在得到纯小孔成像模型的相机内参矩阵K后，为了更加方便的实现畸变图像到非畸变图像的转换，DSO维护了一组映射表remapX和remapY。这组映射表的构建逻辑也比较简单，可以通过下面的这张图进行表示：

针对无畸变坐标系下的某个像素点$p_i$，通过反向投影到归一化坐标系下得到$p_{norm}$，然后使用畸变参数模型在归一化坐标系下对点$p_{norm}$进行畸变处理得到点$p_{norm}^{dis}$，然后使用原来的相机内参$K_{org}$进行投影，得到无畸变像素坐标系点$p_i$和有畸变像素坐标系$p_i^{dis}$之间的映射关系。DSO将这部分对应起来的映射关系存储到remapX和remapY中，以便快速的进行无畸变图像的构建。这里还需要注意的是，使用映射表得到畸变像素坐标后，需要使用双线性插值实现像素值或者说是像素能量值（光度去畸变后）的精确获取。

2. 光度去畸变

2.1 光度畸变模型

DSO的作者考虑了相机的成像过程，针对形成漫反射的某个点p来讲,它会产生辐射$B_i(x)$，相机的感光器件会接收到这个这个点的辐射$B_i(x)$。但是由于辐射在相机的透镜中会发生一些扭曲，造成相机成像的渐晕现象（中间部分接收的辐射较高，而周围的较低），这种渐晕会被建模称$V(x)$，一般会用一张和成像尺寸相同的灰度图来表示，下面是TUM/sequence_29数据集中的渐晕图表示。

相机的感光器件在一定的曝光时间$t_i$内（很短的一段时间内），对辐射进行积分，假设在曝光时间内的辐射恒定那么感光器件获取到的能量值可以通过$IR_{acc}(x)=t_iV(x)B_i(x)$进行表示。

除此之外，相机并不会直接将能量作为像素值的输出，而是会经过相机的非线性响应函数$G$进行转换，$I_i(x)=G(t_iV(x)B_i(x))$，而$G^{-1}$一般通过单调递增的256个数值进行表示，即索引部分为像素值，而值部分对应的是某个适应的能量值。

因为DSO这种直接法会直接跟像素值打交道，为了避免渐晕$V(x)$和曝光时间$G$响应函数的干扰，DSO在这里进行了光度矫正处理，矫正处理的结果为：

$$ I'_i(x)=t_iB_i(x)=\frac{G^{-1}(I_i(x))}{V(x)} $$

通过深度思考发现，如果光照恒定的条件下，不同点产生的辐射值$B_i(x)$是相同的，但是这里的光度矫正为什么还会存在曝光时间$t_i$的影响呢？

我认为，这种疑问考虑是完全正确的，这里的光度矫正确实没有去掉曝光时间$t_i$的影响，但是曝光时间这部分的影响是在优化的能量函数中进行考虑的。能量函数的建模部分我会单独进行讲解，这里只需要记住DSO会在能量函数建模中考虑曝光时间的影响即可。

2.2 无光度参数的解决方案

在2.1部分，光度畸变模型中，要求需要对相机的光度参数部分进行标定，这会涉及到渐晕$V(x)$的标定、非线性响应函数$G(I)$的标定，以及曝光时间的获取。

然而在实际的数据集中，这些参数绝大部分的数据集是没有的，因此DSO提供了一种无光度参数的解决方案，即使用仿射函数的方式实现模拟光度校正，并且认定曝光时间$t_i$为1，即：

$$ I'_i(x) = e^{a_i}I_i(x) - b_i $$

DSO的论文里面还这样描述：这里乘法因子使用$e^{a_i}$而不是直接使用$a_i$的原因是，使用指数函数，既可以防止其乘法项变为负数，又可以避免因乘法漂移而产生的数值问题。

注意去畸变的先后顺序

DSO的畸变处理顺序是先进行光度去畸变，在进行像素去畸变，这么做的原因在于，针对渐晕$V(x)$的表达形式是与原图大小相同的灰度图，因此如果先进行像素去畸变，然后再进行光度去畸变的话，在消除渐晕部分可能会需要remapX和remapY的映射表来找到一个真实的渐晕位置，这样还会涉及到双线性插值，会导致计算量上的增加。

DSO的算法优势

2025-01-02T00:00:00+08:00

介绍

DSO 即 Direct Sparse Odometry，直接稀疏里程计。作为vSLAM直接法中开山鼻祖，在vSLAM方法中具有比较高的地位，并且其算法原理考虑内容比较多，代码的实现上更是接近底层（甚至底层到优化过程、字节对齐都是自己实现）。因此我认为DSO是一个非常值得学习和借鉴的vSLAM直接法。

在这本篇中，我打算着重解释什么是Direct，什么是Indirect；什么是Sparse，什么是Dense。它们之间到底有什么区别，具体的优劣势又在哪？

1. 直接法和非直接法

在SLAM十四讲中的第8章（视觉里程计2）中，提到了直接法的概念和基本原理，重点在于像素级别的灰度不变性，通过灰度不变假设构建优化模型。这里与非直接法，通过构建重投影误差有本质区别。

1.1 直接法

$$ r_k = I_j[p_j] - I_i[p_i] $$

$$ p_j = \pi(T_{ji} * \frac{\pi^{-1}(p_i)}{d_{pi}}) $$

正如上面的公式描述的一样，残差应该是ij两帧之间的像素差值。根据灰度不变假设，当$p_i$和$p_j$之间描述的是同一个3d空间中的点时，$r_k$最小，应该为0。为了重点突出灰度不变的假设，并没有考虑仅用一个点投影的非鲁棒性（实际情况下，需要考虑一个patch——像素块的灰度不变假设，这样更鲁邦一些）。

$$ \frac{\partial r_k}{\partial \delta \xi_{ji}} = \frac{\partial r_k}{\partial I_j} * \frac{\partial I_j}{\partial p_j} * \frac{\partial p_j}{\partial \delta \xi_{ji}} $$

$$ \frac{\partial r_k}{\partial d_{pi}} = \frac{\partial r_k}{\partial I_j} * \frac{\partial I_j}{\partial p_j} * \frac{\partial p_j}{\partial d_{pi}} $$

根据链式法则，可以看到残差对位姿的雅可比矩阵与$I_j$帧的像素梯度有关系。然而由于图像的非凸性比较明显，在经验上，图像的梯度仅能表示2个像素范围内的灰度变化。因此当使用直接法对图像进行跟踪时，需要较好的初始值，并且当跟踪丢失后，就很难进行重定位了。

但是由于直接法构建的是灰度之间的差异，而不是特征点的计算。因此直接法在算法运行速度上，要比非直接法快得多。

1.2 非直接法

我最先能想到的非直法就是 ORB-SLAM系列。经典的特征点匹配，通过BA构建重投影误差。针对非直接法来讲，像素的灰度变化并不会导致非直接法的强烈波动。原因在于，非直接法的特征点是由关键点和描述子两部分组成，具有相当强的稳定性。

非直接法之所以被称为非直接，是因为非直接法需要将得到的数据进行初步的加工处理，而不是直接使用传感器得到的结果。针对视觉SLAM来讲，传感器得到的数据就是像素点的灰度，而非直接法需要根据灰度和像素位置两方面的信息进行某种方式的计算，从而提取出特征点。而这部分计算任务往往是非直接法中最耗时的部分。

由于非直接法的特征点计算总是和像素梯度强相关，当图像中存在弱梯度时，特征点的提取可能失败，并且图像中的梯度越弱，提取出的特征点的鲁棒性和稳定性相对较差。因此特征点法对图像的梯度较敏感。

1.3 优缺点汇总

根据上面的分析，直接法的优劣势如下：

直接法的速度更快，因为不需要计算特征点。
直接法对图像的梯度不是那么敏感。
直接法在优化过程中会涉及到像素梯度，因此跟踪失败后，重定位相对困难。

除此之外，要是以回环闭合的角度去讨论的话，非直接法可能更具优势，因为目前视觉SLAM中比较常用的回环检测方法都是基于词袋的。而词袋方法的直接依据往往都是特征点的描述子。

2. 稀疏和稠密

在视觉SLAM中，稀疏和稠密主要是用来描述建图过程中是否会考虑点与点之间的关系。在视觉SLAM中，稀疏的方法往往占据主流，因为以优化的角度去讨论，稀疏方法往往不考虑点与点之间的关系，反映在优化的H矩阵中，表现为稀疏的矩阵，或者说引用视觉SLAM十四讲的内容，应该为一个箭头矩阵。只有这种稀疏的箭头矩阵，才能使用Schur消元的方式去加速事实求解。

当然，目前也有半稠密的视觉SLAM方法，比如说LSD-SLAM，LSD的方法考虑了点与点之间的连通性关系，但是LSD并没有使用所有的像素点来构建地图，因此在学术上称这种方法为半稠密。LSD使用了GPU，来实时的解一个稠密的H矩阵。

考虑像素中的所有点，并且考虑点与点之间的关系问题，在SFM问题中比较常见，即3D重建任务（从移动中恢复结构）。这种问题往往都是离线，使用多GPU的方式求解，重点在于恢复3D空间结构，而不是实时的给出相机位姿。