SLAM十四讲--最小二乘问题

解决最小二乘问题，通常遵循以下步骤：

1. 构建残差函数：根据位姿图的节点和边，我们定义残差函数 $e_{ij}$。这通常涉及到两个节点之间的相对运动的计算，或者是通过传感器测量得到的观测值与预测值之间的差异。

2. 线性化残差函数：将非线性残差函数 $e_{ij}$ 在当前估计值 $\xi$ 处进行泰勒展开，得到其在当前估计值附近的线性近似。这可以通过计算残差函数的雅可比矩阵 $J_{ij}$ 来实现，得到：

$$e_{ij}(\xi )\approx e_{ij}({\xi }^{(0)})+J_{ij}\delta \xi$$

其中，$J_{ij}$ 是残差函数 $e_{ij}$ 关于位姿变量 $\xi$ 的雅可比矩阵，$\delta \xi$ 是位姿变量的增量。

3. 构建正规方程：基于线性化的残差函数，我们构建正规方程来表示最小化误差的加权和。正规方程通常可以表示为：

$$\underset{\delta \xi }{\min }\frac{1}{2}\sum _{i,j\in E}{e}_{ij}^T{\Sigma }_{ij}^{-1}{e}_{ij}$$

通过将线性化的残差函数代入，我们得到：

$$\underset{\delta \xi }{\min }\frac{1}{2}\sum _{i,j\in E}(e_{ij}({\xi }^{(0)})+J_{ij}\delta \xi {)}^T{\Sigma }_{ij}^{-1}(e_{ij}({\xi }^{(0)})+J_{ij}\delta \xi )$$

4. 解正规方程：求解上述正规方程，得到位姿变量的增量 $\delta \xi$，然后更新当前的估计值：

$$\xi \leftarrow \xi +\delta \xi$$

5. 迭代优化：重复步骤2-4，直到满足收敛条件（例如，位姿变量的增量 $\delta \xi$ 很小或残差函数的变化很小）。