1 SLAM问题求解框架
步骤说明：
- 1. 构建误差函数（跟待求状态量有关的误差，视觉上：空间点的重投影误差/逆深度误差1/d，IMU的状态量：位置p,旋转q,速度v,加速计的bias,陀螺仪的bias）；
- 2. 计算上述误差相对于状态量的雅克比矩阵J；从而构建出信息矩阵H，然后使用LM/高斯牛顿等方法求解该最小二乘问题（对于非线性问题使用最小二乘，线性问题使用直接线性变换(如SVD/QR/PCG)）；
- 3. 对于像SLAM这样的问题，信息矩阵具有稀疏性，故可采用舒尔补(schur)的方式进行矩阵变换，然后使用基于边际概率的滑动窗口算法，将丢
