Bundle Adjustment

最小化投影误差

可以将前面讲的线性问题转变为非线性的最小二乘问题。非线性优化是不同于线性方法的，线性方法一般是先求去相机的位姿，然后再求空间点的坐标。而非线性优化问题则是将位姿和坐标都当作变量，带入优化器优化求解。
将旋转和平移矩阵表示成李代数，假设空间点P=[X,Y,Z],像素点u=[u,v]，可以得到入下的关系：

构建最小二乘：

该问题的误差为像素坐标与3D点按照当前估计的位姿进行投影得到的位置相比较得到的误差，称为 重投影误差。
在求解最小二乘的第一步时，需要知道误差项的导数：

将空间点向像素坐标系投影，先将空间点转变到相机坐标系下：

然后与相机内参，将相机坐标系的内的点投影到像素坐标系：

误差项e是u-su，可以知道u的坐标点（u1，v1）为像素坐标为常数。
而su是未知数，是包含李代数的空间坐标表达式。
优化位姿的误差项求导：
这里的求导为左乘扰动，第一项的导数为：
第二项的导数为：
两项的相乘们就是误差项的导数，这样就可以得到一个雅可比矩阵：

在求解位姿的优化后，还需要对空间点P进行优化，可以计算e关于P的导数：
于是可以得到：
而P’对P的导数可以表示成下式：