Visual SLAM的BA问题：构建最小二乘问题

BA问题归结为一个LS（Least Square）问题：
$$\min g(\mathbf{t})=\frac{1}{2}\sum _{<i,j>\in G}{\mathbf{f}}_{ij}^{\top }{\mathbf{f}}_{ij}$$

令
$$\mathbf{g}\stackrel{\Delta }{=}\left[\begin{array}{c}⋮\\ {\mathbf{f}}_{ij}\\ ⋮\end{array}\right]$$

$\mathbf{g}$的雅可比矩阵：
$$\mathbf{J}\stackrel{\Delta }{=}\frac{\partial \mathbf{g}}{\partial \mathbf{t}}=\left[\begin{array}{ccccccccccc}\cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots \\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ 0& \cdots & 0& \frac{\partial {\mathbf{f}}_{ij}}{\partial {\mathbf{x}}_i}& 0& \cdots & 0& \frac{\partial {\mathbf{f}}_{ij}}{\partial {\mathbf{y}}_j}& 0& \cdots & 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots & \cdots \end{array}\right]$$

$\mathbf{H}\stackrel{\Delta }{=}{\mathbf{J}}^{\top }\mathbf{J}$：
$${\mathbf{J}}_{ij}\stackrel{\Delta }{=}\left[\begin{array}{ccccccccccc}0& \cdots & 0& \frac{\partial {\mathbf{f}}_{ij}}{\partial {\mathbf{x}}_i}& 0& \cdots & 0& \frac{\partial {\mathbf{f}}_{ij}}{\partial {\mathbf{y}}_j}& 0& \cdots & 0\end{array}\right]$$

$$\mathbf{H}={\mathbf{J}}^{\top }\mathbf{J}=\sum _{<i,j>\in G}{\mathbf{J}}_{ij}^{\top }{\mathbf{J}}_{ij}$$

$${\mathbf{J}}_{ij}^{\top }{\mathbf{J}}_{ij}=\left[\begin{array}{ccccc}\cdots & 0& \cdots & 0& \cdots \\ ⋮& ⋮& ⋮& ⋮& ⋮\\ \cdots & 0& \cdots & 0& \cdots \\ \cdots & (\frac{\partial {\mathbf{f}}_{ij}}{\partial {\mathbf{x}}_i}{)}^{\top }\frac{\partial {\mathbf{f}}_{ij}}{\partial {\mathbf{x}}_i}& \cdots & (\frac{\partial {\mathbf{f}}_{ij}}{\partial {\mathbf{x}}_i}{)}^{\top }\frac{\partial {\mathbf{f}}_{ij}}{\partial {\mathbf{y}}_j}& \cdots \\ \cdots & 0& \cdots & 0& \cdots \\ ⋮& ⋮& ⋮& ⋮& ⋮\\ \cdots & 0& \cdots & 0& \cdots \\ \cdots & (\frac{\partial {\mathbf{f}}_{ij}}{\partial {\mathbf{y}}_j}{)}^{\top }\frac{\partial {\mathbf{f}}_{ij}}{\partial {\mathbf{x}}_i}& \cdots & (\frac{\partial {\mathbf{f}}_{ij}}{\partial {\mathbf{y}}_j}{)}^{\top }\frac{\partial {\mathbf{f}}_{ij}}{\partial {\mathbf{y}}_j}& \cdots \\ \cdots & 0& \cdots & 0& \cdots \\ ⋮& ⋮& ⋮& ⋮& ⋮\\ \cdots & 0& \cdots & 0& \cdots \end{array}\right]$$

遍历observation edges, 根据edge连接的pose vertex id和landmark vertex id去确定在$\mathbf{H}$里的位置，然后朝里累加数据. 所以看着很简单就能得到 $\mathbf{H}$。

$\mathbf{b}={\mathbf{J}}^{\top }\mathbf{g}$:
$$\mathbf{b}=\sum _{<i,j>\in G}{\mathbf{J}}_{ij}^{\top }{\mathbf{f}}_{ij}$$

$${\mathbf{J}}_{ij}^{\top }{\mathbf{f}}_{ij}=\left[\begin{array}{c}0\\ ⋮\\ 0\\ (\frac{\partial {\mathbf{f}}_{ij}}{\partial {\mathbf{x}}_i}{)}^{\top }{\mathbf{f}}_{ij}\\ 0\\ ⋮\\ 0\\ (\frac{\partial {\mathbf{f}}_{ij}}{\partial {\mathbf{y}}_i}{)}^{\top }{\mathbf{f}}_{ij}\\ 0\\ ⋮\\ 0\end{array}\right]$$

这样，同样也是遍历observation edges，到edge连接的pose vertex id和landmark vertex id所标识的$\mathbf{b}$里的位置上去累加数据，即可，也是很easy。

LS问题的核心是求解如下的线性方程组：
$$\mathbf{H}\Delta \mathbf{t}=\mathbf{b}$$