主要内容
1. 概述
1）考虑k时刻的状态估计，用过去0到k中的数据来估计现在的状态分布：
2）按照贝叶斯法则：
注：从左到右，一次为后验概率，似然概率 和 先验概率；其中，似然有量测信息给定，先验概率基于过去所有的状态估计和量测得来的
3）对先验概率进行进一步展开，按照
为条件概率展开：
注：如果考虑更久之前的状态，可以对上式进行进一步的展开，现在只关注k时刻和k-1时刻。
4） 继续处理先验概率
一种方法是假设马尔科夫性，即认为k时刻的状态只与k-1时刻的状态有关，而与再之前的无关，以扩展卡尔曼滤波（EKF）为代表的滤波器方法；
二是依然考虑k时刻的状态与之前所有状态的关系，以非线性优化为主体的 优化框架。（SLAM的主流）
2. EKF
1） 基于马尔科夫性，先验概率公式展开可以表示为：
从这个公式中可以看出，k时刻的先验概率可以表示描述为：基于k-1时刻的后验概率
，结合输入信息以及k-1时刻到k时刻的转移概率，可以求解出k时刻的先验概率。
在线性系统和高斯噪声下，卡尔曼滤波器构成了该系统中的最大后验概率估计，即无偏最优估计；而在SLAM这种非线性的情况下，EKF给出了单次线性近似下的最大后验估计（MAP）。
2） 讨论
2.1）优点：形式简洁，应用在计算资源受限，或待估计量比较简单的场合
2.2）典型方法及文献：
IF(信息滤波)，IKF(Iterated KF)，UKF(Unscented KF)，和粒子滤波器，SWF(Sliding Window Filter)，等等，或者用分治法等思路改进EKF的效率。相关文献如下：
[1] Sujan V A, Dubowsky S. Efficient information-based visual robotic mapping in unstructured environments[J]. The International Journal of Robotics Research, 2005, 24(4): 275-293.
[2] A kalman-filter-based method for pose estimation in visual servoing
F Janabi-Sharifi, M Marey - IEEE transactions on Robotics, 2010 - ieeexplore.ieee.org
[3] Li S, Ni P. Square-root unscented Kalman filter based simultaneous localization and mapping[C]//The 2010 IEEE International Conference on Information and Automation. IEEE, 2010: 2384-2388.
[4] Gil A, Reinoso Ó, Ballesta M, et al. Multi-robot visual SLAM using a Rao-Blackwellized particle filter[J]. Robotics and Autonomous Systems, 2010, 58(1): 68-80.
[5] Sliding window filter with application to planetary landing
G Sibley, L Matthies, G Sukhatme - Journal of Field Robotics, 2010 - Wiley Online Library
[6] Chen S Y. Kalman filter for robot vision: a survey[J]. IEEE Transactions on Industrial Electronics, 2011, 59(11): 4409-4420.
3) EKF局限性
3.1） 滤波器方法在一定程度上假设了马尔科夫性；
3.2） EKF仅在某一点处做了一次线性化，即认为该点处的线性化近似在后验概率处仍然是有效的。——存在非线性误差
3.3） EKF需要存储状态量的均值和方差，由于SLAM中路标数量很大，因此存储的量比较大，因此，EKF SLAM被普遍认为不适用于大型场景。
3. BA与图优化
3.1) 投影模型和BA代价函数
3.2) BA求解
增量线性方程的求解
3.3）稀疏性和边缘化
矩阵H的稀疏结构；
H矩阵当中的非对角部分的非零矩阵块可以理解为其对应的两个变量之间存在联系，或者可以称之为约束——图优化结构与增量方程的稀疏性存在着明显的联系
通常有路标的数量远远大于相机位姿的数量，因此H矩阵又称为箭头形矩阵或镐形矩阵。
3.4） 利用H矩阵的稀疏性加速计算的方法——Schur消元（在SLAM亦称为Marginalization, 边缘化）
对线性方程组进行高斯消元，目标是消去右上角的非对角部分E.
求解过程相当于做了条件概率展开，结果是求出了关于xc的边缘分布，所以又称为边缘化。
3.5） S的稀疏性是不规则的，取决于实际观测的结果。
S矩阵非对角线上的非零矩阵块，表示了该处对应的两个相机变量之间存在着共同观测的路标点,有时候称为共视（Co-visibility）；
ORB-SLAM 选择具有共同观测的帧作为关键帧，因此Schur消元后得到的S是稠密的
3.6） 鲁棒核函数
核函数保证每条边的误差不会大的没边而掩盖掉其他的边
常用的核函数：Huber核 Cauch核 Tukey核
Schur消元也并不意味着将所有路标消元，将相机变量消元也是SLAM当中采用的手段。
4. BA实现：G2O
4.1） 参数的配置和解析：（command_args.cpp BundleParams.h ）
可以初始设置参数的名字，默认值，以及各种不同的类型参数
可以解析可执行传入的参数，解析并保存到对应的变量中，进行使用
可通过-h选项，输出当前已有的参数名以及相关默认的值
eg: -input ~qifarui/code/slambook-master/ch10/g2o_custombundle/data/problem-16-22106-pre.txt -h (先设置输入的文件名，然后通过 -h进行输出)
4.2）BALProblem.cpp
PLY点云文件生成（包括相机中心点的写入，以及 空间特征点的写入）
相机，路边以及量测信息读入（特征点位置以及相机位姿信息的读入，以及相机和特征点量测信息）
4.3）利用轴角进行坐标变换
罗德里格斯公式实现坐标变换
利用相机位姿信息求解相机原点在世界坐标系下的坐标（ c = -R't）。
轴角和四元数的转化
利用相机中心在世界坐标系下的坐标生成相机的位姿信息
（Eigen中的Map类——Eigen如何与原生raw C/C++ 数组混合编程）
4.4） Normalize 函数实现：（作用？？？？）
特征点的归一化实现（相对于中间点的比例缩放）
相机中心点按照特征点， 在几何体中心点等比例进行缩放。
4.5） Perturb（）扰动误差添加
根据设定的标准差，对输入的特征点坐标，相机位姿（平移加旋转）添加白噪声
4.6） g2o 优化设置
4.6.1） SetSolverOptionsFromFlags 求解设置：
使用何种方法（列文伯格， DogLeg等）来定义非线性优化的下降策略
使用哪类线性求解器（稠密 or稀疏（额外需要对求解器进行设置-矩阵排序））
4.6.2） BuildProblem 构造最优化问题，增加边和顶点：
a 相机顶点类VertexCameraBAL定义，及顶点初始添加（包括相机位姿（旋转+平移），相机参数）
b 路边定点VertexCameraBAL定义，及顶点初始添加
注意：
each vertex should have an unique id, no matter it is a camera vertex, or a point vertex ；
pPoint→setMarginalized(true); // 设置路标点的schur消元
4.6.3) 边EdgeObservationBAL定义, 以及添加
a 误差计算函数实现
b （）运算付重载，用于投影误差计算
c CamProjectionWithDistortion:根据坐标点以及相机参数（内参 外参以及畸变系数）得到像素坐标点
d ceres库的自动求导功能使用：AutoDiff，获得雅克比矩阵;
注：自动求导功能需要类型将需要求导的公式实现在括号运算符里。
e 量测信息获取，并添加（根据相机和路标点ID设置边连接的顶点，对应边的量测信息设置）
f 量测信息协方差矩阵设置
g 根据配置信息，设置鲁棒核函数huber
4.6.4) 优化器初始化，根据设置的迭代次数，开启优化
4.7） 将优化器优化的结果写入的本地优化变量中，包括相机的参数信息以及路标点的位置信息，通过memcpy的形式
4.8） 根据配置的文件名，将最终的结果写入到点云ply文件中。
注： g2o库说明：
1） 由于g2o库本身公开API说明较少，只能通过网上的开源项目及其本身的源代码来了解它
2） g2o在做BA的优化时，必须将其所有点云全部Schur掉，否则会报错。其原因在于使用了g2o::LinearSolver<BalBlockSolver::PoseMatrixType>* linearSolver的线性求解器，其中模板参数当中的 PoseMatrixType在程序中为相机姿态参数的维度，于是BA当中Schur消元后解的线性方程组必须是只含有相机的姿态变量。
BA实现g2o结果如下：
bal problem have 16 cameras and 22106 points. Forming 83718 observatoins. beginning problem... Normalization complete... begin optimizaiton .. iteration= 0 chi2= 1593976.900329 time= 0.430721 cumTime= 0.430721 edges= 83718 schur= 1 lambda= 9370.114177 levenbergIter= 1 iteration= 1 chi2= 1503859.189085 time= 0.327325 cumTime= 0.758047 edges= 83718 schur= 1 lambda= 3123.371392 levenbergIter= 1 iteration= 2 chi2= 1340948.148116 time= 0.322999 cumTime= 1.08105 edges= 83718 schur= 1 lambda= 1041.123797 levenbergIter= 1 iteration= 3 chi2= 1104714.717556 time= 0.335624 cumTime= 1.41667 edges= 83718 schur= 1 lambda= 347.041266 levenbergIter= 1 iteration= 4 chi2= 879105.141579 time= 0.323237 cumTime= 1.73991 edges= 83718 schur= 1 lambda= 115.680422 levenbergIter= 1 iteration= 5 chi2= 723703.972964 time= 0.318097 cumTime= 2.058 edges= 83718 schur= 1 lambda= 38.560141 levenbergIter= 1 iteration= 6 chi2= 615709.929828 time= 0.317493 cumTime= 2.3755 edges= 83718 schur= 1 lambda= 12.853380 levenbergIter= 1 iteration= 7 chi2= 503775.361750 time= 0.316782 cumTime= 2.69228 edges= 83718 schur= 1 lambda= 4.284460 levenbergIter= 1 iteration= 8 chi2= 381980.060879 time= 0.320198 cumTime= 3.01248 edges= 83718 schur= 1 lambda= 1.428153 levenbergIter= 1 iteration= 9 chi2= 267385.644652 time= 0.34434 cumTime= 3.35682 edges= 83718 schur= 1 lambda= 0.476051 levenbergIter= 1 iteration= 10 chi2= 170314.069365 time= 0.342958 cumTime= 3.69977 edges= 83718 schur= 1 lambda= 0.158684 levenbergIter= 1 iteration= 11 chi2= 108750.143596 time= 0.325639 cumTime= 4.02541 edges= 83718 schur= 1 lambda= 0.052895 levenbergIter= 1 iteration= 12 chi2= 79228.387608 time= 0.33411 cumTime= 4.35952 edges= 83718 schur= 1 lambda= 0.035263 levenbergIter= 1 iteration= 13 chi2= 62903.760510 time= 0.359433 cumTime= 4.71896 edges= 83718 schur= 1 lambda= 0.023509 levenbergIter= 1 iteration= 14 chi2= 51637.899996 time= 0.332307 cumTime= 5.05126 edges= 83718 schur= 1 lambda= 0.015672 levenbergIter= 1 iteration= 15 chi2= 42491.525780 time= 0.352168 cumTime= 5.40343 edges= 83718 schur= 1 lambda= 0.010448 levenbergIter= 1 iteration= 16 chi2= 37296.044380 time= 0.32017 cumTime= 5.7236 edges= 83718 schur= 1 lambda= 0.005843 levenbergIter= 1 iteration= 17 chi2= 36145.312948 time= 0.375634 cumTime= 6.09924 edges= 83718 schur= 1 lambda= 0.001948 levenbergIter= 1 iteration= 18 chi2= 36069.306555 time= 0.334505 cumTime= 6.43374 edges= 83718 schur= 1 lambda= 0.000649 levenbergIter= 1 iteration= 19 chi2= 36067.831214 time= 0.32886 cumTime= 6.7626 edges= 83718 schur= 1 lambda= 0.000216 levenbergIter= 1 optimization complete..
生成的点云结果通过 meshlab 打开以后，结果如下：（上为initial， 下为final）
5. BA实现：Ceres
该部分将介绍Ceres实现，其中和g2o相同部分的输入，配置解析等代码讲不会介绍。
5.1）最小二乘优化问题构建：Problem
void BuildProblem(BALProblem* bal_problem, Problem* problem, const BundleParams& params) { const int point_block_size = bal_problem->point_block_size(); const int camera_block_size = bal_problem->camera_block_size(); double* points = bal_problem->mutable_points(); double* cameras = bal_problem->mutable_cameras(); // Observations is 2 * num_observations long array observations // [u_1, u_2, ... u_n], where each u_i is two dimensional, the x // and y position of the observation. const double* observations = bal_problem->observations(); for(int i = 0; i < bal_problem->num_observations(); ++i){ CostFunction* cost_function; // Each Residual block takes a point and a camera as input // and outputs a 2 dimensional Residual cost_function = SnavelyReprojectionError::Create(observations[2*i + 0], observations[2*i + 1]); // If enabled use Huber's loss function. LossFunction* loss_function = params.robustify ? new HuberLoss(1.0) : NULL; // Each observatoin corresponds to a pair of a camera and a point // which are identified by camera_index()[i] and point_index()[i] // respectively. double* camera = cameras + camera_block_size * bal_problem->camera_index()[i]; double* point = points + point_block_size * bal_problem->point_index()[i]; problem->AddResidualBlock(cost_function, loss_function, camera, point); } }
5.1.1） 向问题中添加残差，各个观测的残差信息：
problem->AddResidualBlock(cost_function, loss_function, camera, point);
其中参数包括代价函数（残差计算函数） 损失函数（鲁棒核函数——根据配置项进行设定），
待估计的参数位姿和路标点
5.1.2） 代价函数：ceres::AutoDiffCostFunction<SnavelyReprojectionError,2,9,3>
自动求导；
模板参数：残差计算定义类SnavelyReprojectionError， 残差维度，待估计相机参数维度， 待估计路标点维度；
new的构造参数： SnavelyReprojectionError变量
class SnavelyReprojectionError { public: SnavelyReprojectionError(double observation_x, double observation_y):observed_x(observation_x),observed_y(observation_y){} template<typename T> bool operator()(const T* const camera, const T* const point, T* residuals)const{ // camera[0,1,2] are the angle-axis rotation T predictions[2]; CamProjectionWithDistortion(camera, point, predictions); residuals[0] = predictions[0] - T(observed_x); residuals[1] = predictions[1] - T(observed_y); return true; } static ceres::CostFunction* Create(const double observed_x, const double observed_y){ return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError,2,9,3>( new SnavelyReprojectionError(observed_x,observed_y))); } private: double observed_x; double observed_y; };
5.1.3） 根据相机和路标点的连接索引ID，将对应的量测信息和待估计参数添加到Problem中
（其中待估计参数直接复用原有变量的地址，不会额外进行申请变量，而在g2o中将这些信息以边，顶点这种额外的变量类型添加到优化问题中，同时优化完成以后，又需要将这些信息返回到之前的变量中）
5.2） 求解器设置：Solver::Options
5.2.1） 最大迭代次数，最小化进度输出，线程个数设置等
options->max_num_iterations = params.num_iterations; options->minimizer_progress_to_stdout = true; options->num_threads = params.num_threads;
5.2.2）优化方法增量方程求解中：下降策略 or 信赖区域策略
StringToTrustRegionStrategyType(params.trust_region_strategy, &options->trust_region_strategy_type);
5.2.3）线性求解器设置
ceres::StringToLinearSolverType(params.linear_solver, &options->linear_solver_type); ceres::StringToSparseLinearAlgebraLibraryType(params.sparse_linear_algebra_library, &options->sparse_linear_algebra_library_type); ceres::StringToDenseLinearAlgebraLibraryType(params.dense_linear_algebra_library, &options->dense_linear_algebra_library_type); options->num_threads = params.num_threads;
5.2.4） Schur消元顺序管理类型ceres::ParameterBlockOrdering设置：
通过编号大小设置消元的优先级，优先消元编号最小的变量。
void SetOrdering(BALProblem* bal_problem, ceres::Solver::Options* options, const BundleParams& params) { const int num_points = bal_problem->num_points(); const int point_block_size = bal_problem->point_block_size(); double* points = bal_problem->mutable_points(); const int num_cameras = bal_problem->num_cameras(); const int camera_block_size = bal_problem->camera_block_size(); double* cameras = bal_problem->mutable_cameras(); if (params.ordering == "automatic") return; ceres::ParameterBlockOrdering* ordering = new ceres::ParameterBlockOrdering; // The points come before the cameras for(int i = 0; i < num_points; ++i) ordering->AddElementToGroup(points + point_block_size * i, 0); for(int i = 0; i < num_cameras; ++i) ordering->AddElementToGroup(cameras + camera_block_size * i, 1); options->linear_solver_ordering.reset(ordering); }
5.2.5) 梯度阈值，相邻两次迭代之间目标函数差值的阈值设置
options.gradient_tolerance = 1e-16; options.function_tolerance = 1e-16;
5.3 求解函数：Solve
参数输入：配置信息，优化问题，输出优化信息
Solver::Summary summary; Solve(options, &problem, &summary); std::cout << summary.FullReport() << "\n";
Ceres优势：
1）能够在原数组上进行操作
2）优化设置非常便捷（Solver::Options类型成员变量赋值， schur消元顺序等）
3）API说明详细，可读性高，多读读源码，加深理解和应用
输出结果：
bal problem have 16 cameras and 22106 points. Forming 83718 observatoins. beginning problem... Normalization complete... the problem is successfully build.. iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time 0 4.185660e+06 0.00e+00 2.16e+07 0.00e+00 0.00e+00 1.00e+04 0 1.27e-01 3.83e-01 1 1.980525e+05 3.99e+06 5.34e+06 2.40e+03 9.60e-01 3.00e+04 1 2.56e-01 6.39e-01 2 5.086543e+04 1.47e+05 2.11e+06 1.01e+03 8.22e-01 4.09e+04 1 2.41e-01 8.80e-01 3 1.859667e+04 3.23e+04 2.87e+05 2.64e+02 9.85e-01 1.23e+05 1 2.43e-01 1.12e+00 4 1.803857e+04 5.58e+02 2.69e+04 8.66e+01 9.93e-01 3.69e+05 1 2.39e-01 1.36e+00 5 1.803391e+04 4.66e+00 3.11e+02 1.02e+01 1.00e+00 1.11e+06 1 2.41e-01 1.60e+00 6 1.803390e+04 1.85e-03 2.12e+00 4.60e-01 1.01e+00 3.32e+06 1 2.41e-01 1.84e+00 7 1.803390e+04 1.01e-06 9.61e-02 8.99e-03 1.03e+00 9.95e+06 1 2.39e-01 2.08e+00 8 1.803390e+04 1.62e-09 4.97e-03 1.98e-04 1.05e+00 2.98e+07 1 2.40e-01 2.32e+00
结果如下：
参考链接
http://www.ceres-solver.org/index.html
[ceres-solver] AutoDiff
Ceres（一）自动求导（AutomaticDiff）
SLAM十四讲 解决第10讲 num_linear_solver_threads找不到的问题