Visual SLAM的BA问题：增加robust kernel

LS目标函数：
$$g(\mathbf{t})=\frac{1}{2}\sum _{<i,j>\in G}\rho \left({\mathbf{f}}_{ij}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{f}}_{ij}\right)$$

$\rho (x)$ 是一robust kernel 函数，用于限制测量误差的取值，避免由于outlier导致的测量误差非常大时对优化结果的消极影响。

常用的$\rho (x)$有

• huber kernel：
  $$\rho (x)=\{\begin{array}{ll}x& 0\le x<\delta \\ 2\delta (\sqrt{x}-\frac{\delta }{2})& x\ge \delta \end{array}$$

$${\rho }^{\prime }(x)=\{\begin{array}{ll}1& 0\le x<\delta \\ \frac{\delta }{\sqrt{x}}& x\ge \delta \end{array}$$

$${\rho }^{\prime \prime }(x)=\{\begin{array}{ll}0& 0\le x<\delta \\ -\frac{1}{2}\frac{\delta }{(\sqrt{x}{)}^3}& x\ge \delta \end{array}$$

令$w_{ij}={\rho }^{\prime }\left({\mathbf{f}}_{ij}^{\top }{\mathbf{\Omega }}_{ij}{\mathbf{f}}_{ij}\right)$,
$$\mathbf{H}=\sum _{<i,j>\in G}{\mathbf{J}}_{ij}^{\top }\left(w_{ij}{\mathbf{\Omega }}_{ij}\right){\mathbf{J}}_{ij}$$

$$\mathbf{b}=\sum _{<i,j>\in G}{\mathbf{J}}_{ij}^{\top }\left(w_{ij}{\mathbf{\Omega }}_{ij}\right){\mathbf{f}}_{ij}$$