PoseGraph公式研究与应用

位姿优化问题建模

通常可获取的资料主要聚焦于SE(3)的PoseGraph构建，但在某些情况下，状态变量被设定为四元数q与位置向量t，此时该如何实施PoseGraph呢？
对SE(3)开展PoseGraph优化时，残差方程如下所示：
e_{6 \times 1} = Log({^{c_i}}\hat{T}_{c_{i+1}} {^{c_{i+1}}}T_{w}^{-1} {^{c_i}}T_{w})
同样地，针对R和t进行PoseGraph处理时，残差方程由两个部分构成：
关于SO(3)的部分：
e_{q} = Log({^{c_i}}\hat{R}_{c_{i+1}}^{-1} {^{c_{i}}}R_{w} {^{c_{i+1}}}R_{w}^{-1})
关于平移量的部分：
e_t = {^{c_i}}\hat{t}_{c_{i+1}} - ({^{c_i}}t_w - {^{c_i}}R_w {^{c_{i+1}}}R_w^{-1} {^{c_{i+1}}}t_w)

接下来对e_q关于\delta \theta_i求Jacobian:
$\begin{aligned} e_q({\delta \theta_i}) &= Log({^{{c_i}}\hat{R}_{c_{i+1}}}{