图优化以及如何将信息矩阵添加到残差

变量：
1、预积分量：$\alpha 、\theta 、\beta 、b_k^a、b_k^g$
2、前一次优化后的状态：$q_k、p_k、v_k、b_k^g、b_k^a$，优化过程$b_k^g、b_k^a$设为常数
3、预积分预测的后一次的位姿：$q_{k+1}^{pre}、p_{k+1}^{pre}、v_{k+1}^{pre}、b_{k+1}^g、b_{k+1}^a$
4、点云匹配得到的后一次的位姿：$q_{k+1}^{pc}、p_{k+1}^{pc}$

残差的求取：
1、imu自身的约束：
（1）每次计算残差之前需要使用零偏更新一次预积分
（2）
$$\begin{array}{rl}{E}_{\alpha }& =q_k\ast (p_{k+1}^{pre}-p_k-v_k\ast deltat-\frac{1}{2}g\ast \delta t^2)-\alpha \\ E_{\theta }& =\ln (\exp (-[\theta {]}_{\times })\ast q_k\ast (q_{k+1}^{pre}{)}^{-1}{)}^{\vee }\\ E_{\beta }& =q_k\ast (v_{k+1}^{pre}-v_k-g\ast \delta t)-\beta \\ E_{b^a}& =b_{k+1}^a-b_k^a\\ E_{b^g}& =b_{k+1}^g-b_k^g\end{array}$$
（3）在ceres中没有提供直接添加信息矩阵的接口（g2o有），因此需要手动添加信息矩阵到残差中。
在最小二乘法问题中，当我们讨论带信息矩阵的最小二乘法时，通常是指加权最小二乘法（Weighted Least Squares, WLS），其中权重矩阵通常是根据测量误差的逆协方差矩阵来选择的。这种情况下，信息矩阵就是测量误差的逆协方差矩阵。

假设我们要最小化的残差向量为 $\mathbf{r}(\mathbf{x})=\mathbf{y}-f(\mathbf{x})$，其中 $\mathbf{y}$ 是观测值向量，$f(\mathbf{x})$ 是由参数向量 (\mathbf{x}) 定义的模型预测值。如果观测数据的误差服从已知分布且其协方差矩阵为 $\Sigma$，则我们可以定义一个加权的残差向量，使得每个观测值的重要性与其误差的大小成反比。这样可以得到如下目标函数：

$$\underset{\mathbf{x}}{\min }\frac{1}{2}\mathbf{r}(\mathbf{x}{)}^T{\Sigma }^{-1}\mathbf{r}(\mathbf{x})$$

在这个公式中，${\Sigma }^{-1}$就是我们所说的信息矩阵，它反映了观测值的精度信息。如果误差是独立同分布的，那么 $\Sigma$ 可能是单位矩阵或对角矩阵，此时加权最小二乘就退化为普通最小二乘。

${\Sigma }^{-1}$有两个作用：
1、对残差加权，衡量不同残差之间的重要性，避免某个残差过大，导致难以优化
2、使不同单位之间可以比较，例如面积和体积是不可比的，因此需要引入信息矩阵

当 $\mathbf{r}(\mathbf{x})$ 是 $\mathbf{x}$ 的非线性函数时，我们可以用泰勒级数展开来近似 $\mathbf{r}(\mathbf{x})$，从而得到线性化后的最小二乘问题。如果当前参数估计为 ${\mathbf{x}}_k$，则线性化后的残差可以表示为：

$\mathbf{r}({\mathbf{x}}_k+\Delta \mathbf{x})\approx \mathbf{r}({\mathbf{x}}_k)+J({\mathbf{x}}_k)\Delta \mathbf{x}$

其中 $J({\mathbf{x}}_k)$ 是雅可比矩阵。将此代入目标函数中，我们得到：

${\min }_{\Delta \mathbf{x}}\frac{1}{2}(\mathbf{r}({\mathbf{x}}_k)+J({\mathbf{x}}_k)\Delta \mathbf{x}{)}^T{\Sigma }^{-1}(\mathbf{r}({\mathbf{x}}_k)+J({\mathbf{x}}_k)\Delta \mathbf{x})$

对上式关于 $\Delta \mathbf{x}$ 求导并令导数为零，可以得到正规方程：

$J({\mathbf{x}}_k{)}^T{\Sigma }^{-1}J({\mathbf{x}}_k)\Delta \mathbf{x}=-J({\mathbf{x}}_k{)}^T{\Sigma }^{-1}\mathbf{r}({\mathbf{x}}_k)$

现在需要手动把${\Sigma }^{-1}$加入到残差中：
$$\underset{\mathbf{x}}{\min }\frac{1}{2}\mathbf{r}(\mathbf{x}{)}^T{\Sigma }^{-1}\mathbf{r}(\mathbf{x})=\underset{\mathbf{x}}{\min }\frac{1}{2}\mathbf{r}(\mathbf{x}{)}^{T}L{L}^T\mathbf{r}(\mathbf{x})=\underset{\mathbf{x}}{\min }\frac{1}{2}(L^T\mathbf{r}(\mathbf{x}){)}^T(L^T\mathbf{r}(\mathbf{x}))$$

因此可以使用Cholesky分解（https://blog.csdn.net/ergevv/article/details/139260380）将${\Sigma }^{-1}$分解成$L{L}^T$，则残差$\mathbf{r}(\mathbf{x})$变成了 $L^T\mathbf{r}(\mathbf{x})$。

重新推导高斯牛顿验证一下：
$$\mathbf{r}(\mathbf{x}{)}^{\prime }=L^T\mathbf{r}(\mathbf{x})$$
$$L^T\mathbf{r}({\mathbf{x}}_k+\Delta \mathbf{x})\approx L^T\mathbf{r}({\mathbf{x}}_k)+L^{T}J({\mathbf{x}}_k)\Delta \mathbf{x}$$
则
$$\underset{{\mathbf{x}}_{\mathbf{k}}}{\min }\frac{1}{2}(L^T\mathbf{r}({\mathbf{x}}_k)+L^{T}J({\mathbf{x}}_k)\Delta \mathbf{x}{)}^T(L^T\mathbf{r}({\mathbf{x}}_k)+L^{T}J({\mathbf{x}}_k)\Delta \mathbf{x})$$

对上式关于 $\Delta \mathbf{x}$ 求导并令导数为零，可以得到正规方程：

$$(L^{T}J({\mathbf{x}}_k){)}^T{L}^{T}J({\mathbf{x}}_k)\Delta \mathbf{x}=-(L^{T}J({\mathbf{x}}_k){)}^T{L}^T\mathbf{r}({\mathbf{x}}_k)$$
则
$$J({\mathbf{x}}_k{)}^{T}L{L}^{T}J({\mathbf{x}}_k)\Delta \mathbf{x}=-J({\mathbf{x}}_k{)}^{T}L{L}^T\mathbf{r}({\mathbf{x}}_k)$$
即
$$J({\mathbf{x}}_k{)}^T{\Sigma }^{-1}J({\mathbf{x}}_k)\Delta \mathbf{x}=-J({\mathbf{x}}_k{)}^T{\Sigma }^{-1}\mathbf{r}({\mathbf{x}}_k)$$

2、点云和imu之间的约束：
$$\begin{array}{rl}{E}_{\theta }^{pc}& =(q_{k+1}^{pc}{)}^{-1}\ast q_{k+1}^{pre}\\ E_p^{pc}& =p_{k+1}^{pre}-p_{k+1}^{pc}\end{array}$$

这里就不分析残差对变量的雅可比矩阵了，因为ceres和g2o都支持自动求导