SLAM6:BA优化

1. 投影模型与BA代价函数
   BA是非线性优化的一种特殊情况， 具体表现在优化过程中稀疏矩阵的数学处理。因此我们需要首先了解投影模型，然后利用我们上一节学习的最小二乘法来进行BA优化。
   a. 首先，把世界坐标P转化为相机坐标P’，用到相机的外参矩阵
   b. 然后，将坐标P’投至归一化平面，得到归一化坐标
   c. 考虑归一化坐标的畸变情况
   d. 最后，根据内参模型，计算像素坐标
   这就是之前的观测方程，内参和畸变都是在标定的时候即测量好了的，因此不算是变量，z = h(x, y)
   z是像素坐标，x即为相机外参，y为世界坐标，这是我们要优化的两个变量。
   因此观测误差为：e = z - h(s, p)
   s 为外参的李代数
   设zij为在位姿si处观测路标pj产生的数据，那么整体的代价函数就是所有误差的1/2平方和，对这个最小二乘进行求解，相当于对位姿和路标同时进行调整，这就是所谓的BA。
2. 稀疏性与边缘化（Schur消元）
3. 鲁棒核函数(消除错配带来的极大误差现象)
   Huber:
   H(e) =
   1/2 e^2 |e| <=a
   a(|e| - 1/2a) 其他
4. BA数据工具集合：
   g2o 图优化
   Ceres 消元