SVO学习笔记

上下标定义：
c -> current；r -> reference；w -> world；$T_a^b$ -> T_ba -> 从a系到b系的变换阵

Sparse Model-based Image Alignment

frame_handler_mono.cpp：

processFrame：

img_align建立新类SparseImgAlign，初始化相关参数（哪些参数？）
img_align.run：输入为上一针last_fram_和当前帧new_frame_；输出为稀疏图像跟踪匹配点数img_align_n_tracked，跳转到sparse_img_align.cpp文件中的run函数

sparse_img_align.cpp 继承vk::NLLSSolver<6, SE3>

run： 输入的是参考帧和当前帧，即连续的两帧

patch大小为patch_area_ = 4×4 = 16；
ref_patch_cache_ 为参考帧上所有特征点的跟踪块为，即参考帧上所有特征点 × patch大小(16)的矩阵，行=参考帧上特征点数量，列 = 16，参考帧patch的缓存，即每行为一个特征patch的16个像素灰度值；
申请跟踪块的雅克比缓冲区(矩阵)：jacobian_cache_，行为6，列为特征patch的数目即特征点数量，大小为ref_patch_cache_行(参考帧上的所有特征点)×patch大小(16), 每一个特征patch的每个像素对应一个6×1的雅克比；
申请visible_fts_(bool)，大小为ref_patch_cache_行(参考帧上的所有特征点)；
计算T_cr(T_cur_from_ref)的变换阵 = T_cw × T_rw的逆，帧间位姿初始化；
金字塔迭代，从最高层（即分辨率最低）开始迭代，到最低层（原始图像）；
迭代优化函数optimize(nlls_solver_impl.hpp中实现)->optimizeLevenbergMarquardt(nnls_solver.h初始化优化方法为LevenbergMarquardt)
返回的是平均特征数量。

nlls_solver_impl.hpp

optimize: 迭代优化函数，nlls_solver.h中定义，输入为ModelType模板

判断采用那种优化算法(GN/LM)，这里NLLSSolver构造函数(nlls_solver.h)初始化method_为LevenbergMarquardt，LM优化optimizeLevenbergMarquardt；
如果初始化method_为GaussNewton，GN优化optimizeGaussNewton。

optimizeLevenbergMarquardt

optimizeGaussNewton

computeResiduals： 残差计算，输入为T_cr帧间位姿，

取出当前迭代的(level_层)金字塔图像 cur_img；
预计算(precomputeReferencePatches() )参考帧特征patch的缓存，将ref_patch_cache_开辟的存储空间填上相应的值，可以暂时认为ref_patch_cache_中已经有值了；
创建误差项errors,数量为特征点个数；
利用输入的T_cr和相机投影模型，遍历参考帧上的特征点，计算当次遍历的特征点在当前图像中的像素坐标：1) 通过特征的世界坐标pos_和参考帧的世界坐标ref_pos计算深度depth (z_r)；2) 通过参考帧计算 $$p_{w_r}=xy{z}_{ref}=K^{-1}\times z_r\times p_{u{v}_r}=z_r\times (K^{-1}\times p_{u{v}_r})=depth\times f_r=depth\times [u_r,v_r,1{]}^T$$；3) 参考帧特征通过初始化的或上次迭代估计的帧间位姿计算在当前帧相机坐标系下的坐标 $$xy{z}_{cur}=p_{w_c}=T_{cr}\times p_{w_r}=K^{-1}\times z_c\times p_{u{v}_c}$$；4) 通过相机投影变换获得图像像素坐标，金字塔scale $$p_{u{v}_c}=xy{z}_{cur}/z_c$$ floorf计算像素坐标整型，为后面做双线性插值，通过双线性插值计算像素光度值；
判断当前的像素坐标是否小于0以及此像素对应的patch是否在当前图像的范围内；
对当前图像进行双线性插值算法，计算双线性插值参数，左上，右上，左下，右下；
指向当前特征patch的指针：头指针ref_patch_cache_.data (总体特征跟踪块) + 当前特征数feature_counter×每个特征patch的像素个数patch_area_(16)；
从当前特征patch的左上角开始遍历16个像素，进行雅克比计算：1) 从4*4 patch的左上角开始，像素数组和像素坐标之间存在一个转换（按行存储），指向当前特征块上当前像素值的指针：cur_img_ptr = v × cols + u；2) 根据双线性插值计算当前遍历的pixel的像素值intensity_cur； 3) 计算光度误差res = 当前pixel像素值 (intensity_cur)-参考帧像素值 (*ref_patch_cache_ptr)；4) 取出预计算中存储的雅克比矩阵J，jacobian_cache_中对应于此pixel(feature_counter × patch_area_ + pixel_counter)的列；5) 计算Hessian矩阵：$$H^{=}J\times J^T\times (weight)$$ 其中weight默认为0； 6) 计算最速下降的增量 $$\Delta x=-J\times f(x)=\Delta res=-J\times res\times (weight)$$

precomputeReferencePatches：

不明白的地方

computeResiduals：

1. 函数中compute_weight_scale，use_weights_，scale_相关的代码
2. value函数，compute函数的实现

参考：

https://www.cnblogs.com/mafuqiang/p/9373707.html
https://blog.csdn.net/datase/article/details/74273227