DSO附件

Direct Sparse Model

特点：优化时把相机内参，相机外参，和地图点的逆深度全放在一起优化，使用滑动窗口策略边缘化掉旧的变量，滑动窗口内用 BA 进行优化，BA 优化两个特点，一个是地图点优化只有逆深度，而不是3维坐标，另一个是损失函数是photometric error（像素值相减），而不是 geometric error（图像坐标相减）。

光度相机模型

对于直接法来说，在求解的时候把 photometric camera model（map real-world energy received by a pixel on the sensor(irradiance) to the respective intensity value）考虑进去比非直接法好处大，因为非直接法像提特征，特征对于光照明暗变化等因素本身就具备不变性。

photometric camera model 标定

Ii(x)=G(tiV(x)Bi(x)) 
其中 Ii(x) 表示像素 x 处相机获取到的像素值。ti 表示相机的曝光时间，V(x) 表示像素点处能量衰减函数，Bi(x) 表示辐照度（像素点处接收到的能量），G(x) 表示 gamma 矫正（将接收到的能量转换成像素值）。

photometric correction 关系式如下： 

I′i(x):=tiBi(x)=G−1(Ii(x))V(x)

矫正过的图像是 tiBi(x) 的形式，后续在计算 photometric derror 实际计算的是 Bi(x) 误差，所以还需要把曝光时间考虑在内。

Model Formulation

每个点的能量函数如下： 

其中 Np 是点 p 的邻域点，Np 分布如下： 
 
Note that in terms of the contained information, evaluating the SSD over such a small neighborhood of pixels is similar to adding first- and second-order irradiance derivative con-stancy terms (in addition to irradiance constancy) for the central pixel.

图像 Ii,Ij 的曝光时间为 ti,tj,||⋅||r 是 Huber norm。p′ 是点 P 根据逆深度参数 dp 得到的，如下： 
 
其中： 
 
公式 6 中，Ti,Tj 是待求的位姿参数。 
为了让方法能在不知道曝光时间的情况下工作，算法还加了一个 affine brightness transfer function: e−ai(Ii−bi)，ai,bi 也是待优化的参数。

除了用 robust Huber norm，还对能量函数加了权值，权值计算方式如下： 
 
权值会减弱梯度大的地方的影响。 
This weighting function can be probabilistically interpreted as adding small, independent geometric noise on the projected point position p , and immediately marginalizing it – approximating small geometric error. 
所以待优化的参数有：每个点的逆深度，相机内参，每帧图像的 pose，每帧图像的 brightness transfer function 参数 ai,bi,aj,bj。

系统总优化的能量函数包括： 
 
其中 i 是当期存在的图像，p 是图像 Pi 中的点，j 是点 p 在 active keyframe 中的观测。 
有上述关系式形成的 factor graph 如下： 

和一般的重投影误差不同，公式 4 中的重投影误差取决于两个图像的 pose，而不是一个图像的 pose。 
The only difference to the classical reprojection error is the additional dependency of each residual on the pose of the host frame, i.e., each term depends on two frames instead of only one. While this adds off-diagonal entries to the pose-pose block of the Hessian, it does not affect the sparsity pattern after application of the Schur complement to marginalize point parameters.

如果曝光时间已知，还会添加下面一项，放在能量函数中使得 brightness transfer function ai,bi 趋于 0（只有曝光时间就够了）。 
 
如果曝光时间不已知，则将 λa 和 λb 设为 0，这时 ai,bi 用来对变化的曝光时间进行建模。

Point Dimensionality

和 indirect 方法不同，算法只用逆深度一个参数来表示地图点待优化参数，如果像素坐标已知，地图点的未知量只有深度值，使用逆深度一个参数的好处：better suited to represent uncertainty from stereo-based depth estimation, in particular for far-away points.

consistency

对于地图点的同一个点可能会初始化好几次（尽管算法尽量把地图点均匀分布在空间中，这种情况也难以避免），这时对于一个图像中的点可能会有多个观测，这种情况在实际中也可以通过消除对同一个点的多个观测消除掉。

Windowed Optimization

Gauss-Newton Optimization

每个点优化的误差项： 
 
对于 additive increment x 雅各比： 
 
注意，这里是对增量的雅各比。

在边缘化的时候跟 okvis 一样也是使用 First Estimate Jacobians 形式。 
优化的时候用高斯牛顿法而不是 LM 算法，We found that since we never start far-away from the minimum – a Levenberg-Marquad dampening (which slows down convergence) is not required. 
LM 算法综合了梯度下降和高斯牛顿法，在距离极小值近时，高斯牛顿二阶近似比梯度下降一阶近似好，这里离极小值近，直接选用高斯牛顿法。

Marginalization

目标函数近似如下： 
 
上式中，H 是根据对 additive increment x 的雅各比计算的，所以写成 x−x0 形式。 
15 式去掉常数项化为： 
 
执行舍而补得到 Hααˆxα=b′αˆ 
其中： 
 
所以误差变为： 
 
误差 19 can be trivially added to the full photometric error during all subsequent optimization and marginalization operations. 这里的 b′αˆ 的表达式和 okvis 里更新 b 的表达式还不同。

Visual Odometry Front-End

Frame management

initial frame tracking

当一个新关键帧初始化时，所有的 active point 向新关键帧做投影，并且在新关键帧创建一个 semi-dense depht map，新来的帧和新关键帧匹配计算位姿时，用 constant motion model 初始化新来帧的位姿，和新关键帧用金字塔直接法匹配计算精确位姿。

如果新来的帧跟丢了，则在 27 个方向上初始化位姿，检验 27 个方向上初始化的位姿是否有有效的。

Keyframe creation

系统保留 5-10 个 active keyframe（系统用来匹配和参与优化的关键帧）。上一帧关键帧和上一帧图像相似度较小时，相机位移较大时，或者曝光时间变化较大时，创建新的关键帧。

Keyframe Marginalization

假设有 I1,I2,In... 个 active keyframe，假设 I1 是最新的一帧关键帧，In 是最老的一帧关键帧，active keyframe 管理方法如下： 
1，总是保留最近的两帧关键帧 I1,I2。 
2，边缘化掉和 I1 有小于 5% 共视点的关键帧。 
3，如果比系统设定的 active keyframe 关键帧数目多，则边缘掉除了 I1,I2 外的某个关键帧，某个关键帧是公式 20 得分最高的关键帧。 
 
公式 20 设计是为了把关键帧均与分布在空间中。 
选取关键帧示意图： 

在进行边缘化某一帧时，先边缘化掉帧里的所有点（如 Figure 5 所示，地图点是有从属关系的），然后再边缘掉帧的其他参数（位姿，affine transfer function）。为了保持 Hessian 矩阵的稀疏结构，被边缘化掉帧所有的观测被直接舍弃掉（观测对应损失函数中的误差项，有些点不属于被边缘掉的帧，但是在帧中有观测）。

Point Management

选取点的策略 
heavily sub-sample data to allow processing it in real time in a joint optimization framework. In fact, our experiments show that image data is highly redundant, and the benefit of simply using more data points quickly flattens off.

在优化的时候保持 2000 个 active points。开始的时候在新的关键帧中生成候选点，候选点并不立即放到优化中，而是和后面的帧跟踪初始化得到 3D 坐标。2，当需要插入新的优化点时（参与到目标函数中的点），从候选点中选取插入进来。

candidate point selection

选择的候选点尽量在图像中均与分布，而且是梯度值较大的点。

Candidate point tracking

候选点在新关键帧后用 discrete search 的策略在极线上搜索，选取使得 photometric error 最小的点计算深度和方差，深度和方差用于后续帧的匹配。

Candidata Point Activation

当旧的变量被边缘化掉后，初始化新的点来代替他们，将所有的候选点向最近的关键帧做投影，在候选点中选取和图像中存在的地图点最远的点初始化（为了使得选择的候选点均匀分布在图像和空间中）。

Outline and Occusion Detection

在极线搜索候选点跟踪时，误差不够小的点被剔除。 
地图点的观测 photometric error 大于一定值的被剔除。

参考文献： 
Engel J, Koltun V, Cremers D. Direct sparse odometry[J]. arXiv preprint arXiv:1607.02565, 2016.