SLAM中的BA优化

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

根据相机模型求出空间点坐标

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

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

Y/Z=Y=(v−cy)/fyY/Z=Y=(v-cy)/fy Y/Z=Y=(v−cy)/fy

Z=1Z=1 Z=1

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

若相机为深度相机可以得到深度信息
X=Z∗(u−cx)/fxX=Z*(u-cx)/fx X=Z∗(u−cx)/fx

Y=Z∗(v−cy)/fyY=Z*(v-cy)/fy Y=Z∗(v−cy)/fy

Z=depth/sZ=depth/s Z=depth/s

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

PnP中的BA优化

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′/Z′+cxu=fx*X'/Z'+cx u=fx∗X′/Z′+cx

v=fy∗Y′/Z′+cyv=fy*Y'/Z'+cy v=fy∗Y′/Z′+cy

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