BA优化中Jacobian矩阵的计算

BA优化中的边（重投影误差）

Bundle Adjustment，中文翻译有捆集调整、光束法平差。它的主要思想是最小化重投影误差。

BA优化的误差边也就是重投影误差边。

• 投影过程是将一个地图点（三维坐标为(X,Y,Z)）投影到当前帧（假设位姿为）的像素平面上（得到投影坐标为）

• 误差边的定义是测量坐标（该帧上提取到的此地图点的位置）减去投影得到坐标

BA优化中的节点（投影点与关键帧）

通常我们最经常放入BA中进行优化的节点有两个

1. 地图点的三维坐标为

2. 被投影帧的位姿

当然了，BA优化时只要观测到的约束足够多，方程就足够多，因此可以优化更多节点。比如我们还可以放入的节点有相机内参等。

jacobian的推导

前面已经列出来了重投影误差的定义为：

然后被投影帧的位姿为，地图点在世界坐标系中的位置为，则将其变换到该帧的相机坐标系下的坐标为

然后将这个点投影到像素平面上的操作为

即

重投影误差对地图点三维坐标的jacobian

由上，我们首先计算重投影误差对地图点相机坐标系下坐标的偏导，

接着根据如下链式法则与坐标变换关系

可以得到最终的重投影误差对地图点三维坐标的jacobian

重投影误差对被投影帧位姿的jacobian

此处需要说明的是，对于位姿变换（对应的李群是）的求导我们一般将其变换到对应的李代数ξ上使用扰动模型来求导。

根据前面的推导我们可以发现，重投影误差项e中与位姿相关的只有，因此可以将求解过程分解为如下两步

其中第一部分在前面已经求解出来了，第二部分可以通过使用扰动模型左乘扰动量来求得（具体步骤见附录1）

由于上面是使用的的齐次坐标形式，所以从上式结果中取出前三维就是（这是一个3∗6大小的矩阵）

然后将两项相乘就可以得到我们想要的结果–重投影误差对被投影帧位姿的jacobian

 

附录1

假设如下求导中的左乘的扰动项的李代数为，那么：

上面最后一行矩阵除法，与矩阵乘法规则类似，只是乘号变成了除号。其使用一个矩阵除以一个矩阵得到一个矩阵

附录2

雅克比矩阵即是偏导数矩阵，由于一般待求导的目标函数的因变量是列向量，自变量也是列向量，在求雅克比矩阵时遵循如下规则：

其实只需要明确一点，上述公式便很容易记起来了。对于输入为n维向量，输出为m维向量的变换（即从的变换），它的雅克比矩阵为维。即行数由输出向量的维度决定，列数由输入向量的维度决定