weixin_45533366的博客

多传感器融合中，用Ceres做后端优化，后端可能包含不同Factor，例如雷达的Factor，相机的Factor，或者一些约束例如地面约束的Factor。一般如果这个Factor的雅可比矩阵没有问题，这个Factor的残差都会收敛到接近0。

rq2∗Qimu−1⊗Qivecrq​2∗Qimu−1​⊗Qi​vec其中Qi是待优化变量rq对Qi求导其表达式为：其中Qi​是待优化变量rq​对Qi​求导RleftQimu−1⊗QiRleft​Qimu−1​⊗Qi​1000010000100000​1000​0100​001。

以四元数为例，QuaternionParameterization 的方法 bool ComputeJacobian(const double* x, double* jacobian) 计算得到一个 4x3 的矩阵。这些由 ComputeJacobian 计算得到的矩阵在 ceres 代码中被称作 “global_to_local”，含义是 Manifold 上变量对 Tangent Space 上变量的导数。例如四元数维度为4，其实际自由度为3。对于这类变量，Ceres在优化这类变量时，会定义一个。

时刻的姿态，有两种方法。具体推算过程这里就不放了，只放最后公式，会用就行了。在看下述内容之前，大家需要对四元数有一定基础。可以做下述线性近似，在一些计算时可以做简化。已知IMU的三轴角速度。已知IMU的三轴角速度。,就可以算出旋转的角度。

SLAM中相机姿态用李代数（SE3）表示， SE3是6维的，路边点的位置是三维的(X,Y,Z),因此状态矩阵的维度是60。雅克比矩阵为误差矩阵对状态矩阵求导，维度为误差矩阵维度x状态矩阵维度，即100x60。一个相机中10个点的误差维度为20，5个相机为100。一个路标点的误差的维度为2,误差即为实际像素坐标（状态矩阵：1x(5x6+10x3)=1x60。误差矩阵：2x5x10 = 100。雅克比矩阵维度：100x60。）与观测的路标点的像素{

【代码】位姿的插值怎么做。