一、理论部分
学习过VINS的小伙伴应该知道，在SFM（structure from motion）的计算中 光束法平差法 - BA（Bundle Adjustment）
的重要性！本人也是学习过一段时间后对其有了更加深刻的理解，那咱们就 先理论 后程序！
1、 光束平差法
光束： 源于 bundle of light， 指的是三维空间中的点投影到像平面上的光束，而重投影误差正是利用这些光束来构建的，
因此强调光束也正是描述其优化模型的来源！
平差：感觉像测绘里面的内容！由于测量仪器的精度不完善和人为因素及外界条件的影响，测量误差总是不可避免的。
为此，观测值的个数往往要多于确定未知量所必须观测的个数。有了多余观测，势必在观测结果之间产生矛盾，测量平
差的目的就在于消除这些矛盾而求得观测量的最可靠结果并评定测量成果的精度，测量平差采用的原理就是“最小二乘法”
2、 BA模型到底是怎么来的？
BA到底是干嘛的？ 用一句话来描述BA那就是，BA的本质是一个优化模型，其目的是最小化重投影误差
本质是一个优化模型应该很容易理解，那么什么是重投影误差呢？
来个图自然就是最棒了！
这些五颜六色的线就是我们讲的光束啦！那现在就该说下什么叫重投影误差了，重投影也就是指的第二次投影，那到底是怎么投影的呢？
第一次投影：指的就是相机在拍照的时候三维空间点投影到图像上，然后我们利用这些图像对一些特征点进行三角定位
（triangulation，显然是利用几何信息构建三角形来确定三维空间点的位置嘛 ）
第二次投影：利用计算得到的三维点的坐标和我们计算得到的相机矩阵 进行第二次投影
现在我们知道什么是重投影了，那重投影误差到底是什么样的误差呢？
这个误差是指的真实三维空间点在图像平面上的投影（测量值）和重投影（预测值） 的差值，噪声等原因的存在，使得两者不会相等！
也就是这个差值不可能恰好为0，而优化的目的就是：找出最优的相机参数及三维空间点的坐标，使得将这些 差值的和
数学模型如下：
BA是一个图优化模型，那首先肯定要构造一个图模型了 。既然是图模型那自然就有节点和边了，
这个图模型的节点由相机 Pi 和三维空间点构成 Xj 构成，如果点 Xj 投影到相机 Pi
数学形式如下：
既然是优化模型，那自然就应该用各种优化算法来进行计算了！
利用BA模型的稀疏性质来进行计算的，LM算法是最速下降法（梯度下降法）和Gauss-Newton的结合体。
假设优化函数模型如下：
为什么用平方呢？ 还不是因为不用计算平方根了嘛！
最速下降法
如果你对梯度比较熟悉的话，那你应该知道梯度方向是函数上升最快的方向，而此时我们需要解决的问题是让函数最小化。
那就顺着梯度的负方向去迭代寻找使函数最小的变量值就好了嘛。梯度下降法就是用的这种思想，用数学表达如下：
其中 λ 为步长，最速下降法保证了每次迭代函数都是下降的，在初始点离最优点很远的时候刚开始下降的速度非常快，
但是最速下降法的迭代方向是折线形的导致了收敛非常非常的慢。
Newton型方法
给定一个开口向上的一元二次函数，如何知道该函数何处最小？这个应该很容易就可以答上来了，对该函数求导，导数为 0 处就是函数最小处。
Newton型方法也就是这种思想，首先将函数利用泰勒展开到 二次项：
H 为Hessian矩阵，是二次偏导矩阵。也就是说Newton型方法将函数 局部近似
接下来自然就对这个函数求导了：
Gauss-Newton方法
既然Newton型方法计算Hessian矩阵太困难了，那有没有什么方法可以不计算Hessian矩阵呢？在范数符号内将 e 一阶泰勒展开，我们可以得到
其中 J 为Jacobi矩阵，函数 e 对 x 求一次偏导而来，梯度也是对向量函数求一次偏导而来。将标量考虑为 1×1的矩阵，将向量考虑为 n×1的矩阵，
其实这些求导都是求Jacobi矩阵。此时不需要计算Hessian矩阵。同样，二次函数导数为0时取极值，则有
由此 x 在 δx 方向上迭代直至 ||e|| 最小，同时我们可以发现一个等式，会在下面的LM方法中用到
LM方法
LM方法就是在以上方法基础上的改进，通过参数的调整使得优化能在最速下降法 和 Gauss-Newton法之间自由的切换，
在保证下降的同时也能保证快速收敛。Gauss-Newton最后需要求解的方程为
LM算法在此基础上做了更改，变成了
通过参数 λ 的调节在最速下降法和Gauss-Newton法之间切换。做个不很数学的直观分析吧，当 λ 很小时，
显然和Gauss-Newton法是一样的；当 λ很大时，就变成了这样：
然后再看看前面的最速下降法？
其实LM算法的具体形式就笔者看到的就有很多种，但是本质都是通过参数 λ 在最速下降法和Gauss-Newton法之间切换。
这里选用的是维基百科上的形式，LM算法就由此保证了每次迭代都是下降的，并且可以快速收敛。
解方程
稠密矩阵的最小二乘解
对于形如 Ax=b 的 超定 参数方程而言，有很多求解方式，伪逆、QR分解、SVD 等等 。这些方式都有一个共同的特点，
都是将 A 看作一般的稠密矩阵，主要得到的解自然非常鲁棒，但是计算量却是和维数的三次方成正比（ O ( n 3 ) 。
面对BA这种超大规模的优化似乎有点不太实用。
稀疏矩阵的Cholesky分解
稠密矩阵计算起来那么复杂，如果是稀疏矩阵的话利用其稀疏的性质可以大幅减少计算量， 对于稀疏矩阵的 Cholesky分解 就是这样。
其分解形式为一个上 三角矩阵 的转置乘上自身：
为什么说我们的矩阵是稀疏的？
考虑相机位置（图像数量）和空间点都非常多的情况，不难想象Jacobi矩阵不光是一个稀疏矩阵而且还可以写成形如 [A∣B] 的分块矩阵。
接下来就该利用这些性质正式进入计算了！
开始计算吧
现在再回到 Gauss-Newton 最后的超定参数方程吧。既然Jacobi 矩阵可以分块那我们就先分块，分块可以有效降低需要计算的矩阵的维度并以此减少计算量。
注意：李群及李代数
不知道有没有人注意到，在优化迭代的过程中，我们求的值为 δx，然后利用 x+δx 来更新 x 的值。这里就应该出现一个问题了，
对于空间点的坐标和平移向量这么处理自然没有什么问题，但是对于旋转矩阵呢？难道用 R + δ R 来更新 R 的值吗？好像不太对吧。
对于旋转矩阵 R 而言是不存在加法的，按理讲应该用 RδR来更新 R 的值，但是优化算法的迭代过程又不能是乘法，这就出现了矛盾。
这里 旋转矩阵及相关运算属于李群，此时将旋转矩阵变换到其对应的李代数上进行计算，然后再变回李群。
协方差矩阵
在我们的推导中是求解方程
但常常加入信息矩阵（协方差矩阵的逆），令求解方程变为
其中 Σ x
二、程序部分
Ceres 对 BA 的介绍如下：
给定一系列测得的图像，包含特征点位置和对应关系。BA的目标就是，通过最小化重投影误差，确定三维空间点的位置和相机参数。
这个优化问题通常被描述为非线性最小二乘法问题，要最小化的目标函数 即为测得图像中特征点位置 和 相机成像平面上的对应的投影
之间的差的平方。Ceres例程使用了BAL数据集 。
第一步依然使定义一个模板functor来计算残差，在这个问题中也就是的重投影误差。每个残差值依赖于 空间点的位置（3个参数）
和相机参数（9个参数）。九个相机参数包含用 Rodriques’ axis-angle表示的 旋转向量 R（3个参数），平移参数 T（3个参数），
以及焦距 f 和两个径向畸变参数 k1, k2。相机模型的详细描述也可以在BAL的主页找到;
理解：
不知道你们有没有理解上面这个图的意思？上面的 X1是实际环境中的点，相机观察到特征点 X1 后，会在相机平面上得到 像素点x1,
这个 x1 那可能会问，X1' 是什么？X1' 是 P1 相机 前面的相机 用三角法估计出来的三维点，但是这个
估计出来的三维点位置和真正环境中的X1是不重和的！因为噪声等误差的存在嘛！现在我们知道了这个虚拟的三维点 X1' ，然后投影到
P1 相机上，得到了 像素点 x1' P1 相机下 像素点 x1和 x1'， 那我们就 进行优化，不断调整 R，T等参数 是 他俩的误差最小！
/--------------------------------------------------- 程序 ------------------------------------------------------------/
BA.h
struct SnavelyReprojectionError {
SnavelyReprojectionError(double observed_x, double observed_y)
: observed_x(observed_x), observed_y(observed_y) {}
template <typename T>
bool operator()(const T* const camera,
const T* const point,
T* residuals) const {
// camera[0,1,2] 是三个方向上的旋转
T p[3];
//根据旋转，预测空间特征点在 相机的坐标
ceres::AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] 是XYZ三个方向上的平移
p[0] += camera[3];
p[1] += camera[4];
p[2] += camera[5];
//归一化坐标
T xp = -p[0] / p[2];
T yp = -p[1] / p[2];
// 下面的程序是为了得到 经过修正后的 坐标
const T& l1 = camera[7];
const T& l2 = camera[8];
T r2 = xp*xp + yp*yp;
T distortion = 1.0 + r2 * (l1 + l2 * r2);
const T& focal = camera[6];
T predicted_x = focal * distortion * xp;
T predicted_y = focal * distortion * yp;
// 计算两个归一化 坐标的差：预测值 和 实际的测量值
residuals[0] = predicted_x - observed_x;
residuals[1] = predicted_y - 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)));
}
double observed_x;
double observed_y;
};以上部分定义了反向投影误差是怎么计算的，即residuals的计算，以及损失函数是怎么定义的，损失函数中的几个参数，
第一个为代价函数的类型，第二个为代价的维度，第三个为每张像片外方位元素和相机内参，demo使用的相机内参为一
个焦距加两个畸变参数，再加上3个旋转矩阵参数3个平移矩阵参数，正好9个，第四个为每一个空间三维点的坐标为3维。
main.cpp
if (!bal_problem.LoadFile("C:\\Users\\sggzg\\Desktop\\problem-49-7776-pre.txt")) {
std::cerr << "ERROR: unable to open file " << "C:\\Users\\sggzg\\Desktop\\problem - 49 - 7776 - pre.txt" << "\n";
}
const double* observations = bal_problem.observations();
ceres::Problem problem;
for (int i = 0; i < bal_problem.num_observations(); ++i)
{
ceres::CostFunction* cost_function =SnavelyReprojectionError::Create(observations[2 * i + 0],observations[2 * i + 1]);
problem.AddResidualBlock(cost_function,NULL ,bal_problem.mutable_camera_for_observation(i),bal_problem.mutable_point_for_observation(i));}
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.max_num_iterations = 3;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";
这一部分中for循环主要就是添加每一个前面所说的光束法平差单元，AddResidualBlock函数即添加每一个平差单元的需要优化的参数，
Create函数通过具体的像点观测值实例化前面定义的损失函数。平差的一些选项设置，平差报告的打印。
结合自己的实验数据进行调整：
主要是对反向投影误差计算的修改，根据自己的误差定义修改计算residuals的过程，以及根据相机的不同重新定义损失函数，如下：
ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<
BundleAdjustment_CostFunction, 2, 4, 3, 3, 3>(
new BundleAdjustment_CostFunction(point2D.X(), point2D.Y()));
problem.AddResidualBlock(cost_function, loss_function, psfmreconstruction->ImageDatas[img_idx].image.Qvec().data(),
psfmreconstruction->ImageDatas[img_idx].image.Tvec().data(), psfmreconstruction->params_.data(),
psfmreconstruction->points3Ds_[point2D.Point3DId()].XYZ().data());这时可以看出损失函数中的几个参数，第一个为代价函数的类型，第二个为代价的维度，第三个为每张像片外方位元素和相机内参，
我使用的相机内参为一个焦距加两个像主点参数，再加上4元数表示的qvec4个参数以及平移向量3个参数，第四个为每一个空间三维点的坐标为3维。
实验数据
原始代码针对的数据集下载地址：http://grail.cs.washington.edu/projects/bal/
