在上文中我们从ROS的节点出发，一步步介绍了SVO ros节点的运行流程，下面我们将深入介绍SVO大的核心代码。SVO的核心主要分为3方面内容 - spase image alignment - Feature alignment - pose and Structure optimization - map 首先给出总体的结构图。
首先参考论文中的内容，进行分析。使用直接法最小化图像块重投影残差来获取位姿。如图所示：其中红色的 Tk,k−1 为位姿，即优化变量。 直接法具体过程如下： 1. 准备工作。假设相邻帧之间的位姿 Tk,k−1 已知，一般初始化为上一相邻时刻的位姿或者假设为单位矩阵。通过之前多帧之间的特征检测以及深度估计，我们已经知道第k-1帧中特征点位置以及它们的深度。 2. 重投影。知道 Ik−1 中的某个特征在图像平面的位置 (u,v) ，以及它的深度 d ，能够将该特征投影到三维空间 pk−1 ，该三维空间的坐标系是定义在 Ik−1 摄像机坐标系的。所以，我们要将它投影到当前帧 Ik 中，需要位姿转换 Tk,k−1 ，得到该点在当前帧坐标系中的三维坐标 pk 。最后通过摄像机内参数，投影到 Ik 的图像平面 (u′,v′) ，完成重投影。 3. 迭代优化更新位姿 。按理来说对于空间中同一个点，被极短时间内的相邻两帧拍到，它的亮度值应该没啥变化。但由于位姿是假设的一个值，所以重投影的点不准确，导致投影前后的亮度值是不相等的。不断优化位姿使得这个残差最小，就能得到优化后的位姿 Tk,k−1 。 将上述过程公式化如下：通过不断优化位姿 Tk,k−1 最小化残差损失函数。
Tk,k−1=argminTk,k−112∑i∈R∥δI(Tk,k−1,ui)∥2δI(Tk,k−1,ui)=Ik(π(Tk,k−1⋅π(u,du)))−Ik−1(u) 第一步为根据图像位置和深度逆投影到三维空间，第二步将三维坐标点旋转平移到当前帧坐标系下，第三步再将三维坐标点投影回当前帧图像坐标。当然在优化过程中，残差的计算方式不止这一种形式：有前向(forwards)，逆向(inverse)之分，并且还有叠加式(additive)和构造式(compositional)之分。这方面可以读读光流法方面的论文，Baker的大作《Lucas-Kanade 20 Years On: A Unifying Framework》。选择的方式不同，在迭代优化过程中计算雅克比矩阵的时候就有差别，一般为了减小计算量，都采用的是inverse compositional algorithm。 上面的非线性最小化二乘问题，可以用高斯牛顿迭代法求解，位姿的迭代增量 ξ (李代数)可以通过下述方程计算：
JTJξ=−JTδI(0) 其中雅克比矩阵为图像残差对李代数的求导，可以通过链式求导得到： ∂δI(ξ,ui)∂ξ=∂Ik−1(a)∂a∣∣a=ui⋅∂π(b)∂b∣∣b=pi⋅∂T(ξ)∂ξ∣∣ξ=0⋅Pi 我们已经能够估计位姿了，但是这个位姿肯定不是完美的。导致重投影预测的特征点在 Ik 中的位置并不和真正的吻合，也就是还会有残差的存在。如下图所示：
图中灰色的特征块为真实位置，蓝色特征块为预测位置。幸好，他们偏差不大，可以构造残差目标函数，和上面直接法类似，不过优化变量不再是相机位姿，而是像素的位置 (u',v') ，通过迭代对特征块的预测位置进行优化。这就是svo中提到的Feature Alignment。
接下来我们在看代码中的情况。这里我们首先介绍image spase alignment ,这个类继承自类 vk::NLLSSolver<6, SE3> 该类的声明和实现在rpg-vikit包中metapackage vikit-common 中的文件nlls_sovler.h和nlls_sovler_impl.h 中。该类中定义了若干虚函数，
virtual double computeResiduals (const ModelType& model, bool linearize_system, bool compute_weight_scale) = 0; virtual int solve () = 0; virtual void update (const ModelType& old_model, ModelType& new_model) = 0; virtual void startIteration () { } virtual void finishIteration () { }这里回顾一下（纯）虚函数的用法
公开继承意味这is-a的关系。 derived class，类D; base class 类B， 每个类D 的对象也是一个类B 的对象。反之不成立。B比D表现出更为一般化的概念， D对象比B表现出更为特殊化的概念。
pure virtual 函数的目的是为了让derived class制继承接口。这意味这derived class必须提供一个对应该接口的实现，但base class不关心使怎样实现的。 声明非纯虚函数的目的是让derived class继承该函数的接口和缺省实现。这意味这derived class必须支持该接口，可以自己实现，也可以使用默认版本。
这里可以看出优化函数optimize， optimizeGaussNewton， optimizeLevenbergMarquardt 是基类中定义和实现，不是虚函数，因此最终运行过成中调用的是这里的函数，而计算残差更新等与自己的数据类型相关的函数，需要派生类中实现，覆盖基类中的定义。派生类spaseImgAlign中的run函数传入的参数参考帧和当前帧的frame，计算两者之间的变换T_cur_from_ref，最后调用基类的optimize函数，这里需要指定优化的方法是Gauss-Newton 还是LM，这个是在类frame-handler-mono中指定的。
SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), 30, SparseImgAlign::GaussNewton, false, false);所以我们进入优化函数optimizeGaussNewton中去看具体的过程如何。
首先是计算残差，这个函数时调用了spasre-img-align中重新定义的函数computeResiduals，然后是开始迭代。每一次迭代中包含一下步骤 - 执行开始函数startIteration - computeResiduals - applyPrior - solve - update - finishIteration 执行开始函数startIteration和finishIteration没有内容，update中完成了
T_curnew_from_ref = T_curold_from_ref * SE3::exp(-x_);solve中代用Eigen中的ldlt的solver函数。
主要内容在computeResiduals，首先映射（扭曲）到参考图像的空间，注意该函数分为四种模式，是否是线性系统和是否应用权重不同类型，如果是首次运行的化需要计算权迭代的权重，然后对于参考帧中的每一特征进行 - 判断该特征是否在该图像中 - 计算当前帧的像素位置 - 计算残差 - 如果是非线性系统那么计算雅克比，权重hessian和梯度下降的权值。 - 如果是首次迭代，那么计算用于第一次迭代的权值。
地图模型通常来说保存的就是三维空间点，因为每一个Key frame通过深度估计能够得到特征点的三维坐标，这些三维坐标点通过特征点在Key Frame中进行保存。所以SVO地图上保存的是Key Frame 以及还未插入地图的KF中的已经收敛的3d点坐标，也就是说地图map不需要自己管理所有的3d点，它只需要管理KF就行了。先看看选取KF的标准是啥？KF中保存了哪些东西？当新的帧new frame和相邻KF的平移量超过场景深度平均值的12%时(比如四轴上升)，new frame就会被当做KF，它会被立即插入地图。同时，又在这个新的KF上检测新的特征点作为深度估计的seed，这些seed会不断融合新的new frame进行深度估计。但是，如果有些seed点3d点位姿通过深度估计已经收敛了，怎么办？map用一个point_candidates来保存这些尚未插入地图中的点。所以map这个数据结构中保存了两样东西，以前的KF以及新的尚未插入地图的KF中已经收敛的3d点。 通过地图我们保存了很多三维空间点，很明显，每一个new frame都是可能看到地图中的某些点的。由于new frame的位姿通过上一步的直接法已经计算出来了，按理来说这些被看到的地图上的点可以被投影到这个new frame中，即图中的蓝色方框块。上图中分析了，所有位姿误差导致这个方框块在new frame中肯定不是真正的特征块所处的位置。所以需要Feature Alignment来找到地图中特征块在new frame中应该出现的位置，根据这个位置误差为进一步的优化做准备。基于光度不变性假设，特征块在以前参考帧中的亮度应该和new frame中的亮度差不多。所以可以重新构造一个残差，对特征预测位置进行优化：
u′i=argminu′i12∥Ik(u′i)−AiIr(u′i∥2 述注意这里的优化变量是像素位置，这过程就是光流法跟踪嘛。并且注意，光度误差的前一部分是当前图像中的亮度值，后一部分不是 Ik−1 而是 Ir ，即它是根据投影的3d点追溯到的这个3d点所在的key frame中的像素值，而不是相邻帧。由于是特征块对比并且3d点所在的KF可能离当前帧new frame比较远，所以光度误差和前面不一样的是还加了一个仿射变换，需要对KF帧中的特征块进行旋转拉伸之类仿射变换后才能和当前帧的特征块对比。 这时候的迭代量计算方程和之前是一样的，只不过雅克比矩阵变了，这里的雅克比矩阵很好计算： J=[∂r∂u′∂r∂v′] 这不就是图像横纵两个方向的梯度嘛。 通过这一步我们能够得到优化后的特征点预测位置，它比之前通过相机位姿预测的位置更准，所以反过来，我们利用这个优化后的特征位置，能够进一步去优化相机位姿以及特征点的三维坐标。所以位姿估计的最后一步就是Pose and Structure Refinement。然后继续看代码，这里首先介绍介个基础的类，其中frame类是地图的关键组成，上述的理论的实现在reprojecter类中，首先给出示意图 从processFrame的函数流程中顺序执行到这里，上一步是使用Gauss-Newton方法优化相邻帧间的变化，使用链式求导的方法计算雅克比矩阵。那么接下来，把地图中跟当前帧有重叠的关键帧拿出来，遍历对应的特征点优化当前的位姿。 - 通过当前帧查询到地图中当前帧最近的n各关键帧图像。 - 按照与 当前帧的距离（靠近程度）排序 - 对于地图中当前帧最近的n帧图像的每一个，遍历其中的特征点，构造一个overlay关键帧容器（std::vector< std::pair
struct Candidate { EIGEN_MAKE_ALIGNED_OPERATOR_NEW Point* pt; //!< 3D point. Vector2d px; //!< projected 2D pixel location. Candidate(Point* pt, Vector2d& px) : pt(pt), px(px) {} };可以看出这是一个一个点对，包含了世界中的3D点和图像的2D点。
struct Grid { CandidateGrid cells; vector<int> cell_order; int cell_size; int grid_n_cols; int grid_n_rows; };接下来就是重投影candidate points，这些点在map中定义。 Grid除了包含Candidate另外增加了一个cell的索引，以及图像的宽和高。 最后是执行projectcell，这这其中首先按照pointquality 进行排序，对于每个cell，findMatchDirect，如果找到，构造新的特征，在当前帧中增加特征。findMatchDirect中首先找到有类似视角的帧，然后根据当前帧和参考帧的camera信息计算affine matrix，并使用该矩阵warp on a large patch. 最后是使用fature_alignment中的函数align2D函数。该函数包含了针对SSE2指令和ARM的的分别优化，因此在编译的时候要分来处理。 为了理解其中原理，我们来看未优化的版本 - 计算梯度和hessian - 亚像素计算直至收敛 关于ARM-neon和x86的SSE指令集可以参考，这里不在展开 1. http://blog.csdn.net/hemmingway/article/details/44828303 2. https://developer.arm.com/docs/dui0472/l/using-neon-support/neon-intrinsics-for-operations-with-a-scalar-value 3. https://community.arm.com/groups/processors/blog/2010/03/17/coding-for-neon–part-1-load-and-stores
