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 
#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 info = pre_integration_->get_information();
        sqrt_info_ = Eigen::LLT>(info.cast()).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 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 info = pre_integration_->get_information();
        sqrt_info_ = Eigen::LLT>(info.cast()).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 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 omega(x);
    Eigen::Map position(x + 3);

    // delta = [ dtheta (3), dp (3) ]
    Eigen::Map dtheta(delta);
    Eigen::Map 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 omega_new(x_plus_delta);
    Eigen::Map 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> 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.