SLAM中的非线性优-3D图优化之绝对位姿SE3约束四元数形式（十九）

zl_vslam2026-03-25 15:16

本节开始展示四元数用以推导绝对位姿雅克比，这也将是绝对位姿理论部分的最后一节，理论后必接实战篇，让我们进入四元数推导的世界吧

完整版实现了在6.1 补充里的公式

完整版代码https://gitee.com/zl_vslam/slam_optimizer/blob/master/multi_sensor_slam/src/imu_factor_gtsam/run_gins_pre_integ_ch4.cc

注意这里的小旋转四元数可以参考《Quaternion Kinematics for Error-State Kalman Filter》式（193）的定义，表示符号略有差异

6. 代码实战证明

如图所示，可以看到雅克比为一个6x6的矩阵，其中右下角的3x3为单位矩阵，左上角为右雅克比的逆，之前多个章节都有过右雅克比矩阵的逆，再小角度残差作用下，近似为单位阵，因此上述推导中近似的单位矩阵是完全正确的，也从侧面说明了雅克比矩阵推导的正确性

6.1 补充

待使用的公式

证明，两个四元素的雅克比如下

代码已经过验证

复制代码

gtsam::Vector GPSPoseFactor::evaluateError(const gtsam::Pose3 &pose,
                                           boost::optional<gtsam::Matrix &> Jacobian) const
{
    gtsam::Pose3 delta_pose = measurement_.inverse()*pose;
    gtsam::Vector6 error = delta_pose.Logmap(delta_pose, Jacobian);
    
    if(Jacobian) {
        gtsam::Matrix6 jac;
        jac << gtsam::I_3x3, gtsam::Z_3x3, gtsam::Z_3x3, gtsam::I_3x3; // right
        *Jacobian = jac;
    }
    return error;
}