关于Ceres中IMU预积分因子的SO(3)+R³局部参数化（尤其是ComputeJacobian）正确性的验证问询

云原生阿岚

2026-4-13

提问内容

I am trying to add IMU preintegration factor to my optimization in VIO. I just want to be sure if I am doing everything correctly, especially LocalParameterization part. Below is given the IMU preintegration factor. I just pasted .h file, so you can have an idea on parametrization used in factor.

#ifndef IMU_INTEG_FACTOR_H
#define IMU_INTEG_FACTOR_H
#include <ceres/ceres.h>
#include "IMU/basicTypes.hpp"
#include "IMU/imuPreintegrated.hpp"

namespace vio {

class ImuIntegFactor : public ceres::SizedCostFunction<15, 6, 9, 6, 9> {
  public:
    ImuIntegFactor(IMUPreintegrated* pre_integration) : pre_integration_(pre_integration) {
        Eigen::Matrix<double, 15, 15> info = pre_integration_->get_information();
        sqrt_info_ = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(info.cast<double>()).matrixL().transpose();
    }

    void ScaleSqrtInfo(double scale) {
        // only scale the sub information matrix for rotation, velocity, pose.
        sqrt_info_.block<9, 9>(0, 0) *= scale;
        sqrt_info_.block<6, 6>(9, 9) *= 1e-1;    // scale bias information.
    }

    virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const;
    void check(double** parameters);

    Eigen::Matrix<double, 15, 15> sqrt_info_;
    IMUPreintegrated* pre_integration_;
};

class ImuIntegGdirFactor : public ceres::SizedCostFunction<15, 6, 9, 6, 9, 3> {
  public:
    ImuIntegGdirFactor(IMUPreintegrated* pre_integration) : pre_integration_(pre_integration) {
        Eigen::Matrix<double, 15, 15> info = pre_integration_->get_information();
        sqrt_info_ = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(info.cast<double>()).matrixL().transpose();
    }

    void ScaleSqrtInfo(double scale) {
        // only scale the sub information matrix for rotation, velocity, pose.
        sqrt_info_.block<9, 9>(0, 0) *= scale;
        sqrt_info_.block<6, 6>(9, 9) *= 1e-1;    // scale bias information.
    }

    virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const;
    void check(double** parameters);

    Eigen::Matrix<double, 15, 15> sqrt_info_;
    IMUPreintegrated* pre_integration_;
};

}
#endif /* IMU_INTEG_FACTOR_H */

Here is a small part from Evaluate method:

bool ImuIntegFactor::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const {
    /// step 1: prepare data
    Eigen::Vector3d omegai(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Vector3d Pi(parameters[0][3], parameters[0][4], parameters[0][5]);
    Sophus::SO3d Ri = Sophus::SO3d::exp(omegai);
    Sophus::SO3d invRi = Ri.inverse();

    Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]);
    Eigen::Vector3d Bgi(parameters[1][3], parameters[1][4], parameters[1][5]);
    Eigen::Vector3d Bai(parameters[1][6], parameters[1][7], parameters[1][8]);

    Eigen::Vector3d omegaj(parameters[2][0], parameters[2][1], parameters[2][2]);
    Eigen::Vector3d Pj(parameters[2][3], parameters[2][4], parameters[2][5]);
    Sophus::SO3d Rj = Sophus::SO3d::exp(omegaj);

    Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);
    Eigen::Vector3d Bgj(parameters[3][3], parameters[3][4], parameters[3][5]);
    Eigen::Vector3d Baj(parameters[3][6], parameters[3][7], parameters[3][8]);
...

And this is how I added it into ceres optimization problem:

// Add preintegration factors for each keyframe inside sliding window
    for (size_t i = 0; i < drt_vio_init_ptr_->local_active_frames.size(); i++)
    {
        ceres::LocalParameterization* pose_param = new PoseSO3LocalParameterization();
        problem.AddParameterBlock(para_pose[i], SIZE_PARAMETERIZATION::SIZE_POSE, pose_param);
        problem.AddParameterBlock(para_speed_bias[i], SIZE_PARAMETERIZATION::SIZE_SPEEDBIAS);
    }

    for (size_t i = 0; i < drt_vio_init_ptr_->local_active_frames.size() - 1; i++)
    {
        size_t j = i + 1;

        if (drt_vio_init_ptr_->imu_meas[i].sum_dt_ > 10)
            continue;

        ImuIntegFactor* imu_factor = new ImuIntegFactor(&drt_vio_init_ptr_->imu_meas[i]);
        problem.AddResidualBlock(imu_factor, NULL, para_pose[i], para_speed_bias[i], para_pose[j], para_speed_bias[j]);
    }

I created local paramterization for IMU preintegration factor, as addition operation for so3 should be handled differently.

Here is my local parametrization implementation:

#include "factor/pose_so3_local_parametrization.h"

bool PoseSO3LocalParameterization::Plus(
    const double* x,
    const double* delta,
    double* x_plus_delta
) const
{
    // x = [ omega (3), position (3) ]
    Eigen::Map<const Eigen::Vector3d> omega(x);
    Eigen::Map<const Eigen::Vector3d> position(x + 3);

    // delta = [ dtheta (3), dp (3) ]
    Eigen::Map<const Eigen::Vector3d> dtheta(delta);
    Eigen::Map<const Eigen::Vector3d> dp(delta + 3);

    // Convert so3 -> SO3
    Sophus::SO3d R = Sophus::SO3d::exp(omega);

    // Right-multiplicative update
    Sophus::SO3d R_new = R * Sophus::SO3d::exp(dtheta);

    // Write back
    Eigen::Map<Eigen::Vector3d> omega_new(x_plus_delta);
    Eigen::Map<Eigen::Vector3d> position_new(x_plus_delta + 3);

    omega_new = R_new.log();      // back to so3
    position_new = position + dp;

    return true;
}

bool PoseSO3LocalParameterization::ComputeJacobian(
    const double* /*x*/,
    double* jacobian
) const
{
    // Jacobian of Plus(x, delta) w.r.t delta at delta = 0
    // For so3 ⊕ R3, this is identity
    Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor>> J(jacobian);
    J.setIdentity();
    return true;
}

My main concern is related implementation of local parametrization. So, I would like to get your thoughts on that if I am doing it correctly or not. Thank you very much for your attention and time.

专家解答

嗨，我仔细看了你的局部参数化实现和整个IMU预积分因子的代码，核心问题出在ComputeJacobian函数上，下面分点给你拆解说明：

1. Plus函数：基本正确，但要注意一致性

你的Plus函数逻辑是没问题的：

把参数块的前3维作为SO3的李代数转成旋转矩阵，用右乘更新（R * exp(dtheta)）得到新旋转，再转回李代数
位置部分直接做欧几里得加法

这里要强调：右乘更新本身没有错，只要你的整个VIO系统（比如预积分残差的推导、其他视觉/IMU因子的更新逻辑）都统一用右乘的风格就好，和左乘只是更新方向的差异，关键是全流程的一致性。

2. ComputeJacobian函数：致命错误！

你当前直接返回单位矩阵是完全错误的！

Ceres的LocalParameterization要求ComputeJacobian返回的是：当局部扰动delta趋近于0时，更新后参数x⊕delta对delta的导数矩阵，也就是d(Plus(x, delta))/d(delta)|_{delta=0}。

对于SO3旋转部分，根据李群的BCH公式，当delta_theta→0时：
$$\log(R \cdot \exp(\delta\theta)) \approx \omega + J_r(\omega) \cdot \delta\theta$$
其中J_r(ω)是SO3的右雅可比矩阵，它绝对不是单位矩阵（只有当旋转接近单位阵时才近似为单位阵）。

位置部分的导数确实是单位矩阵，因为位置是欧几里得空间，pos + delta_pos对delta_pos的导数就是单位矩阵。

3. 修正后的局部参数化实现

第一步：实现SO3的右雅可比矩阵

先写一个辅助函数计算SO3的右雅可比：

#include "Sophus/SO3.hpp"

Eigen::Matrix3d SO3RightJacobian(const Eigen::Vector3d& omega) {
    double theta = omega.norm();
    // 数值保护：当旋转极小时直接返回单位矩阵
    if (theta < 1e-8) {
        return Eigen::Matrix3d::Identity();
    }
    Eigen::Vector3d omega_normalized = omega / theta;
    Eigen::Matrix3d omega_hat = Sophus::SO3d::hat(omega_normalized); // 构造反对称矩阵

    Eigen::Matrix3d Jr = Eigen::Matrix3d::Identity()
                       - (1 - cos(theta)) / theta * omega_hat
                       + (theta - sin(theta)) / theta * omega_hat * omega_hat;
    return Jr;
}

第二步：修复ComputeJacobian函数

bool PoseSO3LocalParameterization::ComputeJacobian(
    const double* x,
    double* jacobian
) const
{
    Eigen::Map<const Eigen::Vector3d> omega(x); // 取出当前旋转的李代数
    Eigen::Matrix3d Jr = SO3RightJacobian(omega);

    Eigen::Map<Eigen::Matrix<double, 6, 6, Eigen::RowMajor>> J(jacobian);
    J.setZero();
    J.block<3, 3>(0, 0) = Jr; // 旋转扰动对更新后旋转李代数的雅可比
    J.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity(); // 位置扰动对更新后位置的雅可比
    return true;
}

4. 额外的关键注意事项

（1）左乘vs右乘的一致性

如果你的IMU预积分残差是基于左乘更新推导的（比如R_new = exp(dtheta) * R），那你需要：

修改Plus函数的旋转更新逻辑为左乘
把右雅可比换成左雅可比，左雅可比的实现和右雅可比只有一个符号差异：

Eigen::Matrix3d SO3LeftJacobian(const Eigen::Vector3d& omega) {
    double theta = omega.norm();
    if (theta < 1e-8) {
        return Eigen::Matrix3d::Identity();
    }
    Eigen::Vector3d omega_normalized = omega / theta;
    Eigen::Matrix3d omega_hat = Sophus::SO3d::hat(omega_normalized);

    Eigen::Matrix3d Jl = Eigen::Matrix3d::Identity()
                       + (1 - cos(theta)) / theta * omega_hat
                       + (theta - sin(theta)) / theta * omega_hat * omega_hat;
    return Jl;
}

然后ComputeJacobian的前3x3块换成Jl(omega)即可。

（2）残差雅可比的转换

在你的ImuIntegFactor::Evaluate函数中，如果你计算的是残差对参数块x（即omega和pos）的雅可比，需要手动转换为对局部参数delta的雅可比：
$$J_{\text{delta}} = J_x \times J_p$$
其中J_x是残差对x的雅可比，J_p就是我们修复后的局部参数化雅可比矩阵。Ceres不会自动帮你做这个转换，必须手动处理，否则残差的雅可比会错误，导致优化不收敛。

5. 其他小建议

你在IMU因子中对bias的信息矩阵缩放了1e-1，这个系数可以根据你用的IMU的实际bias稳定性调整，如果IMU bias漂移很慢，可以调得更小一些。
可以在Plus函数中加入数值保护，比如当dtheta的范数极小时，直接用omega + dtheta近似，避免对数映射的数值误差。

备注：内容来源于stack exchange，提问作者BHOS

关于Ceres中IMU预积分因子的SO(3)+R³局部参数化（尤其是ComputeJacobian）正确性的验证问询

提问内容 #

专家解答 #

1. Plus函数：基本正确，但要注意一致性 #

2. ComputeJacobian函数：致命错误！ #

3. 修正后的局部参数化实现 #

第一步：实现SO3的右雅可比矩阵

第二步：修复ComputeJacobian函数

4. 额外的关键注意事项 #

（1）左乘vs右乘的一致性

（2）残差雅可比的转换

5. 其他小建议 #

提问内容

专家解答

1. Plus函数：基本正确，但要注意一致性

2. ComputeJacobian函数：致命错误！

3. 修正后的局部参数化实现

4. 额外的关键注意事项

5. 其他小建议