SLAM中的BA优化

SLAM中的BA优化，先根据相机模型和A,B图像特征匹配好的像素坐标，求出A图像上的像素坐标对应的归一化的空间点坐标，然后根据该空间点的坐标计算重投影到B图像上的像素坐标，重投影的像素坐标(估计值)与匹配好的B图像上的像素坐标(测量值),不会完全重合，BA的目的就是每一个匹配好的特征点建立方程，然后联立，形成超定方程，解出最优的位姿矩阵或空间点坐标(两者可以同时优化)。

? 相机模型–像素坐标与空间点坐标的对应关系

? 相机使用前先进行相机标定得到相机的内参K，通过特征点匹配可以得到，特征点的像素坐标，根据上述模型就可以得到像素像素点对应的归一化后的空间坐标，即：
$$X/Z=X=(u?cx)/fx$$

$$Y/Z=Y=(v?cy)/fy$$

$$Z=1$$

? 为什么是归一化后的空间坐标，由上面的公司可以看出，Z=1，所以是归一化后的坐标。

? 若相机为深度相机可以得到深度信息
$$X=Z?(u?cx)/fx$$

$$Y=Z?(v?cy)/fy$$

$$Z=depth/s$$

depth为与之对应的深度值，s为深度值与实际距离的转换参数。

PnP是求解3D点到2D点的运动方法，它描述的是在已知n个3D点的空间坐标以及他们的投影位置时，如何估计相机的位姿。特征点的3D位置可以由三角化或者RGB-D相机的深度图确定

当相机移动后，相机模型变为

T=(R|t)用李代数的形式表示，则

注意：exp(.)Pi结果为[4X1]，而K为[3X3],为了保证维数相同，取exp(.)Pi的前三维，变成三维的非其次坐标

则整体的优化方程为：

? 很方便地通过 G-N, L-M 等优化算法进行求解。不过，在使用 G-N 和 L-M 之前，我们需
要知道每个误差项关于优化变量的导数，也就是线性化：
?

由前面的内容可知
$$u=fx?X^{\prime }/Z^{\prime }+cx$$

$$v=fy?Y^{\prime }/Z^{\prime }+cy$$

然后就是求出误差对位姿的雅克比和误差对空间点的雅克比，具体求导参考《视觉SLAM14讲》P166-P167.