LI-Init源码学习笔记

LI-Init学习笔记

一、LiDAR里程计

1、匀速运动模型

TODO

2、LiDAR运动补偿

TODO

二、初始化计算

1、数据激活程度评估

bool LI_Init::data_sufficiency_assess(MatrixXd &Jacobian_rot, int &frame_num, V3D &lidar_omg, int &orig_odom_freq,
                                      int &cut_frame_num) {
    //Calculation of Rotation Jacobian
    // 计算旋转雅可比矩阵
    M3D lidar_omg_skew;
    lidar_omg_skew << SKEW_SYM_MATRX(lidar_omg);
    Jacobian_rot.block<3, 3>(3 * frame_num, 0) = lidar_omg_skew;
    bool data_sufficient = false;

    //Give a Data Appraisal every second
    // 每秒进行一次数据评估
    if (frame_num % (orig_odom_freq * cut_frame_num) == 0) {
        /// 计算信息矩阵（协方差矩阵）
        M3D Hessian_rot = Jacobian_rot.transpose() * Jacobian_rot;
        /// 求解信息矩阵的特征值和特征向量
        EigenSolver<M3D> es(Hessian_rot);
        V3D EigenValue = es.eigenvalues().real();
        M3D EigenVec_mat = es.eigenvectors().real();

        M3D EigenMatCwise = EigenVec_mat.cwiseProduct(EigenVec_mat);/// 逐元素求平方
        /// 提取三个特征向量的平方
        std::vector<double> EigenMat_1_col{EigenMatCwise(0, 0), EigenMatCwise(1, 0), EigenMatCwise(2, 0)};
        std::vector<double> EigenMat_2_col{EigenMatCwise(0, 1), EigenMatCwise(1, 1), EigenMatCwise(2, 1)};
        std::vector<double> EigenMat_3_col{EigenMatCwise(0, 2), EigenMatCwise(1, 2), EigenMatCwise(2, 2)};

        /// 找到最大值所在位置
        int maxPos[3] = {0};
        maxPos[0] = max_element(EigenMat_1_col.begin(), EigenMat_1_col.end()) - EigenMat_1_col.begin();
        maxPos[1] = max_element(EigenMat_2_col.begin(), EigenMat_2_col.end()) - EigenMat_2_col.begin();
        maxPos[2] = max_element(EigenMat_3_col.begin(), EigenMat_3_col.end()) - EigenMat_3_col.begin();

        /// 根据累积长度对特征值进行缩放，判断数据是否足够
        V3D Scaled_Eigen = EigenValue / data_accum_length;   //the larger data_accum_length is, the more data is needed

        V3D Rot_percent(Scaled_Eigen[1] * Scaled_Eigen[2], /// 第二轴和第三轴叉乘得到第一轴的激活程度
                        Scaled_Eigen[0] * Scaled_Eigen[2], /// 第一轴和第三轴叉乘得到第二轴的激活程度
                        Scaled_Eigen[0] * Scaled_Eigen[1]);/// 第一轴和第二轴叉乘得到第三轴的激活程度

        V3D Rot_percent_scaled(Rot_percent[0] < 0.99 ? Rot_percent[0] : 1,
                               Rot_percent[1] < 0.99 ? Rot_percent[1] : 1,
                               Rot_percent[2] < 0.99 ? Rot_percent[2] : 1);

        int axis[3];
        axis[2] = max_element(maxPos, maxPos + 3) - maxPos;
        axis[0] = min_element(maxPos, maxPos + 3) - maxPos;
        axis[1] = 3 - (axis[0] + axis[2]);

        /// 打印各方向数据激活进度
        clear(); //clear the screen
        printf("\033[3A\r");
        printProgress(Rot_percent_scaled[axis[0]], 88);
        printProgress(Rot_percent_scaled[axis[1]], 89);
        printProgress(Rot_percent_scaled[axis[2]], 90);
        fflush(stdout);

        /// 判断数据是否足够
        if (Rot_percent[0] > 0.99 && Rot_percent[1] > 0.99 && Rot_percent[2] > 0.99) {
            printf(BOLDCYAN "[Initialization] Data accumulation finished, Lidar IMU initialization begins.\n\n" RESET);
            printf(BOLDBLUE"============================================================ \n\n" RESET);
            data_sufficient = true;
        }
    }
    if (data_sufficient)
        return true;
    else
        return false;
}

2、数据预处理

（1）imu数据下采样并插值对齐Lidar状态数据

/**
 * @brief 对IMU状态数据进行下采样及插值。
 * @param param1 当前帧lidar结束时间。
 * @return 返回值的描述。
 * @function 1 剔除当前帧三秒前的数据；2 对加速度进行均值滤波； 3 反距离权重插值。
 */
void LI_Init::downsample_interpolate_IMU(const double &move_start_time) {

    /// 剔除当前帧三秒前的数据
    while (IMU_state_group_ALL.front().timeStamp < move_start_time - 3.0)
        IMU_state_group_ALL.pop_front();
    while (Lidar_state_group.front().timeStamp < move_start_time - 3.0)
        Lidar_state_group.pop_front();

    /// Original IMU measurements
    /// 将IMU_state_group_ALL容器中除最后一个元素外的所有元素，赋值给IMU_states_all_origin容器
    deque<CalibState> IMU_states_all_origin;
    IMU_states_all_origin.assign(IMU_state_group_ALL.begin(), IMU_state_group_ALL.end() - 1);

    /// Mean filter to attenuate noise
    /// 对加速度进行滑动窗口均值滤波，窗口尺寸为mean_filt_size=2
    int mean_filt_size = 2;
    for (int i = mean_filt_size; i < IMU_state_group_ALL.size() - mean_filt_size; i++) {
        V3D acc_real = Zero3d;
        for (int k = -mean_filt_size; k < mean_filt_size + 1; k++)
            acc_real += (IMU_states_all_origin[i + k].linear_acc - acc_real) / (k + mean_filt_size + 1);
        IMU_state_group_ALL[i].linear_acc = acc_real;
    }

    /// Down-sample and interpolation，Fig.4 in the paper
    /// 下采样及插值，反距离权重插值
    for (int i = 0; i < Lidar_state_group.size(); i++) {
        for (int j = 1; j < IMU_state_group_ALL.size(); j++) {
            if (IMU_state_group_ALL[j - 1].timeStamp <= Lidar_state_group[i].timeStamp
                && IMU_state_group_ALL[j].timeStamp > Lidar_state_group[i].timeStamp) {
                CalibState IMU_state_interpolation;
                double delta_t = IMU_state_group_ALL[j].timeStamp - IMU_state_group_ALL[j - 1].timeStamp;
                double delta_t_right = IMU_state_group_ALL[j].timeStamp - Lidar_state_group[i].timeStamp;
                double s = delta_t_right / delta_t;/// 反距离权重
                /// 权重插值
                IMU_state_interpolation.ang_vel = s * IMU_state_group_ALL[j - 1].ang_vel +
                                                  (1 - s) * IMU_state_group_ALL[j].ang_vel;
                IMU_state_interpolation.linear_acc = s * IMU_state_group_ALL[j - 1].linear_acc +
                                                     (1 - s) * IMU_state_group_ALL[j].linear_acc;
                /// 将插值后的数据压入容器中
                push_IMU_CalibState(IMU_state_interpolation.ang_vel, IMU_state_interpolation.linear_acc,
                                    Lidar_state_group[i].timeStamp);
                break;
            }
        }
    }
}

（2）IMU时间戳补偿

void LI_Init::IMU_time_compensate(const double &lag_time, const bool &is_discard) {
    /// 如果is_discard为true，则丢弃前10个Lidar估计值和对应的IMU测量值，以处理长时间间隔带来的误差。
    if (is_discard) {
        //Discard first 10 Lidar estimations and corresponding IMU measurements due to long time interval
        int i = 0;
        while (i < 10) {
            Lidar_state_group.pop_front();
            IMU_state_group.pop_front();
            i++;
        }
    }
    /// 遍历IMU状态组，将除最后一个元素外的每个元素的时间戳减去lag_time，完成粗标定后是lag_time_1
    auto it_IMU_state = IMU_state_group.begin();
    for (; it_IMU_state != IMU_state_group.end() - 1; it_IMU_state++) {
        it_IMU_state->timeStamp = it_IMU_state->timeStamp - lag_time;
    }
    /// 移除Lidar状态组和IMU状态组中时间戳不匹配的元素，
    /// 确保Lidar状态组的第一个元素时间戳小于或等于IMU状态组的第一个元素时间戳，
    /// 且Lidar状态组的第一个元素时间戳大于或等于IMU状态组的第二个元素时间戳。
    while (Lidar_state_group.front().timeStamp < IMU_state_group.front().timeStamp)
        Lidar_state_group.pop_front();
    while (Lidar_state_group.front().timeStamp > IMU_state_group[1].timeStamp)
        IMU_state_group.pop_front();

    //Align the size of two sequences
    /// 对Lidar状态组和IMU状态组进行大小对齐，
    /// 若IMU状态组大小大于Lidar状态组大小，则移除IMU状态组的尾部元素；
    /// 若IMU状态组大小小于Lidar状态组大小，则移除Lidar状态组的尾部元素。
    /// 这样保证两个状态组的大小相等，方便后续处理。
    while (IMU_state_group.size() > Lidar_state_group.size())
        IMU_state_group.pop_back();
    while (IMU_state_group.size() < Lidar_state_group.size())
        Lidar_state_group.pop_back();
}

（3）数据滤波（零相位低通滤波）

/**
 * @brief 对数据进行零相位滤波处理。
 * @param param1 滤波前数据。
 * @param param2 滤波后数据。
 * @return 返回值的描述。
 * @function 功能。
 */
void LI_Init::zero_phase_filt(const deque<CalibState> &signal_in, deque<CalibState> &signal_out) {
    deque<CalibState> sig_out1;
    Butter_filt(signal_in, sig_out1);

    deque<CalibState> sig_rev(sig_out1);
    /// 反转元素
    reverse(sig_rev.begin(), sig_rev.end()); //Reverse the elements

    Butter_filt(sig_rev, signal_out);
    reverse(signal_out.begin(), signal_out.end()); //Reverse the elements
}


/// 该函数是一个用于实现双向Butterworth滤波器的函数。它通过输入一个信号（deque<CalibState>类型），输出一个经过滤波处理的信号（也是deque<CalibState>类型）。
/// 首先，函数创建了一个Butterworth滤波器对象（butter）。
/// 接下来，函数计算了需要向前和向后扩展原始信号的样本数（extend_num），以便进行滤波处理。
/// 然后，函数使用这些样本数来从原始信号中提取前缀和后缀，并将它们存储在extend_front和extend_back两个deque中。
/// 接着，函数将原始信号与前后扩展的信号合并为一个新信号（sig_extended）。
/// 在完成信号扩展后，函数使用Butterworth滤波器的系数对新信号进行滤波处理，将结果存储在sig_out中。
/// 最后，函数将滤波后的信号（不包括前后扩展的部分）复制到输出信号（signal_out）中，并返回。
/// 该函数的主要作用是使用Butterworth滤波器对信号进行滤波处理，滤波器的类型和参数由Butterworth对象决定。
void LI_Init::Butter_filt(const deque<CalibState> &signal_in, deque<CalibState> &signal_out) {
    LI_Init::Butterworth butter;
    butter.extend_num = 10 * (butter.Coeff_size - 1);
    auto it_front = signal_in.begin() + butter.extend_num;
    auto it_back = signal_in.end() - 1 - butter.extend_num;

    deque<CalibState> extend_front;
    deque<CalibState> extend_back;

    for (int idx = 0; idx < butter.extend_num; idx++) {
        extend_front.push_back(*it_front);
        extend_back.push_front(*it_back);
        it_front--;
        it_back++;
    }

    deque<CalibState> sig_extended(signal_in);
    while (!extend_front.empty()) {
        sig_extended.push_front(extend_front.back());
        extend_front.pop_back();
    }
    while (!extend_back.empty()) {
        sig_extended.push_back(extend_back.front());
        extend_back.pop_front();
    }

    deque<CalibState> sig_out(sig_extended);
    //One-direction Butterworth filter Starts (all states)
    for (int i = butter.Coeff_size; i < sig_extended.size() - butter.extend_num; i++) {
        CalibState temp_state;
        for (int j = 0; j < butter.Coeff_size; j++) {
            auto it_sig_ext = *(sig_extended.begin() + i - j);
            temp_state += it_sig_ext * butter.Coeff_b[j];
        }
        for (int jj = 1; jj < butter.Coeff_size; jj++) {
            auto it_sig_out = *(sig_out.begin() + i - jj);
            temp_state -= it_sig_out * butter.Coeff_a[jj];
        }
        sig_out[i] = temp_state;
    }

    for (auto it = sig_out.begin() + butter.extend_num; it != sig_out.end() - butter.extend_num; it++) {
        signal_out.push_back(*it);
    }
}

/**
 * @brief 切割序列尾部
 *
 * 此函数用于调整和对齐Lidar状态和IMU状态两个序列的尾部，确保它们在时间上匹配并保持相对同步。
 * 该函数首先移除最近的20个Lidar和IMU状态，然后根据时间戳调整两个序列的长度，使其相等。
 */

void LI_Init::cut_sequence_tail() {
    // 移除最近的20个Lidar和IMU状态项
    for (int i = 0; i < 20; ++i) {
        Lidar_state_group.pop_back();
        IMU_state_group.pop_back();
    }

    // 移除Lidar序列中时间戳早于IMU序列最早时间戳的项
    while (Lidar_state_group.front().timeStamp < IMU_state_group.front().timeStamp)
        Lidar_state_group.pop_front();

    // 移除IMU序列中时间戳早于Lidar序列第二个时间戳的项
    while (Lidar_state_group.front().timeStamp > IMU_state_group[1].timeStamp)
        IMU_state_group.pop_front();

    // 对齐两个序列的长度，确保它们在数量上相等
    while (IMU_state_group.size() > Lidar_state_group.size())
        IMU_state_group.pop_back();
    while (IMU_state_group.size() < Lidar_state_group.size())
        Lidar_state_group.pop_back();
}

3、最大互相关法时差粗标定

/**
 * @brief 最大互相关法时差粗标定。
 * @param param1 lidar里程计频率。
 * @return 返回值的描述。
 * @function 通过计算每一个步长的互相关值，选取最大的作为时差。
 */
void LI_Init::xcorr_temporal_init(const double &odom_freq) {
    int N = IMU_state_group.size();
    //Calculate mean value of IMU and LiDAR angular velocity
    /// 计算平均角速度
    double mean_IMU_ang_vel = 0, mean_LiDAR_ang_vel = 0;
    for (int i = 0; i < N; i++) {
        mean_IMU_ang_vel += (IMU_state_group[i].ang_vel.norm() - mean_IMU_ang_vel) / (i + 1);
        mean_LiDAR_ang_vel += (Lidar_state_group[i].ang_vel.norm() - mean_LiDAR_ang_vel) / (i + 1);
    }

    //Calculate zero-centered cross correlation
    double max_corr = -DBL_MAX;
    for (int lag = -N + 1; lag < N; lag++) {
        double corr = 0;
        int cnt = 0;
        /// 计算在偏移步长为lag时的互相关值
        for (int i = 0; i < N; i++) {
            int j = i + lag;
            if (j < 0 || j > N - 1)
                continue;
            else {
                cnt++;
                /// 零中心互相关量
                corr += (IMU_state_group[i].ang_vel.norm() - mean_IMU_ang_vel) *
                        (Lidar_state_group[j].ang_vel.norm() - mean_LiDAR_ang_vel);  // Zero-centered cross correlation
            }
        }

        /// 获取最大互相关值
        if (corr > max_corr) {
            max_corr = corr;
            lag_IMU_wtr_Lidar = -lag;
        }
    }

    time_lag_1 = lag_IMU_wtr_Lidar / odom_freq;
    cout << "Max Cross-correlation: IMU lag wtr Lidar : " << -lag_IMU_wtr_Lidar << endl;
}

粗标定时差后利用该时差补偿IMU数据

4、差分计算

中心差分计算LiDAR的角加速度与线加速度，IMU的角加速度

/**
 * @brief 计算IMU状态组中每个元素的角加速度，lidar状态组中每个元素的角加速度和线加速度。
 * @param param1 参数1的描述。
 * @param param2 参数2的描述。
 * @return 返回值的描述。
 * @function 功能。
 */
void LI_Init::central_diff() {
    /// 通过中心差分法计算imu的角加速度，前后两个元素的角速度差除以时间差
    auto it_IMU_state = IMU_state_group.begin() + 1;
    for (; it_IMU_state != IMU_state_group.end() - 2; it_IMU_state++) {
        auto last_imu = it_IMU_state - 1;
        auto next_imu = it_IMU_state + 1;
        double dt_imu = next_imu->timeStamp - last_imu->timeStamp;
        it_IMU_state->ang_acc =
                (next_imu->ang_vel - last_imu->ang_vel) / dt_imu;
        fout_IMU_meas << setprecision(12)
                      << it_IMU_state->ang_vel.transpose() << " "
                      << it_IMU_state->ang_vel.norm() << " "
                      << it_IMU_state->linear_acc.transpose() << " "
                      << it_IMU_state->ang_acc.transpose() << " "
                      << it_IMU_state->timeStamp << endl;
    }

    /// 利用中心差分法计算lidar状态组的角加速度和线加速度
    auto it_Lidar_state = Lidar_state_group.begin() + 1;
    for (; it_Lidar_state != Lidar_state_group.end() - 2; it_Lidar_state++) {
        auto last_lidar = it_Lidar_state - 1;
        auto next_lidar = it_Lidar_state + 1;
        double dt_lidar = next_lidar->timeStamp - last_lidar->timeStamp;
        it_Lidar_state->ang_acc =
                (next_lidar->ang_vel - last_lidar->ang_vel) / dt_lidar;
        it_Lidar_state->linear_acc =
                (next_lidar->linear_vel - last_lidar->linear_vel) / dt_lidar;
        fout_LiDAR_meas << setprecision(12)
                        << it_Lidar_state->ang_vel.transpose() << " "
                        << it_Lidar_state->ang_vel.norm()<< " "
                        <<(it_Lidar_state->linear_acc - STD_GRAV).transpose() << " "
                        << it_Lidar_state->ang_acc.transpose()
                        << " " << it_Lidar_state->timeStamp << endl;
    }
}

5、对角加速度和线加速度滤波

/*** 第二次数据滤波,对角加速度和线加速度进行滤波 ***/
    deque<CalibState> IMU_after_2nd_zero_phase;
    deque<CalibState> Lidar_after_2nd_zero_phase;
    zero_phase_filt(get_IMU_state(), IMU_after_2nd_zero_phase);
    zero_phase_filt(get_Lidar_state(), Lidar_after_2nd_zero_phase);
    set_states_2nd_filter(IMU_after_2nd_zero_phase, Lidar_after_2nd_zero_phase);

6、参数解算

/// 求解旋转外参
    /// 只求解旋转外参，只考虑最小化v=R*omga_lidar-omga_imu
    solve_Rotation_only();
    /// 求解旋转外参、陀螺仪偏置
    /// 将上一步中计算的旋转外参作为初始值，求解旋转外参、陀螺仪偏置、时间差
    solve_Rot_bias_gyro(timediff_imu_wrt_lidar);

    /// 对IMU线加速度进行插值和lidar数据对齐
    acc_interpolate();

    /// 求解平移外参、加速度偏置、重力方向
    solve_trans_biasacc_grav();

（1）初始旋转外参求解

/// 求解旋转外参
void LI_Init::solve_Rotation_only() {
    double R_LI_quat[4];
    R_LI_quat[0] = 1;
    R_LI_quat[1] = 0;
    R_LI_quat[2] = 0;
    R_LI_quat[3] = 0;

    ceres::Manifold *quatParam = new ceres::EigenQuaternionManifold();
    ceres::Problem problem_rot;
    problem_rot.AddParameterBlock(R_LI_quat, 4, quatParam);

    for (int i = 0; i < IMU_state_group.size(); i++) {
        M3D Lidar_angvel_skew;
        Lidar_angvel_skew << SKEW_SYM_MATRX(Lidar_state_group[i].ang_vel);
        problem_rot.AddResidualBlock(Angular_Vel_Cost_only_Rot::Create(IMU_state_group[i].ang_vel, Lidar_state_group[i].ang_vel),
                                     nullptr,
                                     R_LI_quat);
    }
    ceres::Solver::Options options_quat;
    ceres::Solver::Summary summary_quat;
    ceres::Solve(options_quat, &problem_rot, &summary_quat);
    Eigen::Quaterniond q_LI(R_LI_quat[0], R_LI_quat[1], R_LI_quat[2], R_LI_quat[3]);
    Rot_Lidar_wrt_IMU = q_LI.matrix();
}
///
struct Angular_Vel_Cost_only_Rot {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Angular_Vel_Cost_only_Rot(V3D IMU_ang_vel_, V3D Lidar_ang_vel_) :
            IMU_ang_vel(IMU_ang_vel_), Lidar_ang_vel(Lidar_ang_vel_) {}

    template<typename T>
    bool operator()(const T *q, T *residual) const {
        Eigen::Matrix<T, 3, 1> IMU_ang_vel_T = IMU_ang_vel.cast<T>();
        Eigen::Matrix<T, 3, 1> Lidar_ang_vel_T = Lidar_ang_vel.cast<T>();
        Eigen::Quaternion<T> q_LI{q[0], q[1], q[2], q[3]};
        Eigen::Matrix<T, 3, 3> R_LI = q_LI.toRotationMatrix();  //Rotation
        Eigen::Matrix<T, 3, 1> resi = R_LI * Lidar_ang_vel_T - IMU_ang_vel_T;
        residual[0] = resi[0];
        residual[1] = resi[1];
        residual[2] = resi[2];
        return true;
    }

    static ceres::CostFunction *Create(const V3D IMU_ang_vel_, const V3D Lidar_ang_vel_) {
        return (new ceres::AutoDiffCostFunction<Angular_Vel_Cost_only_Rot, 3, 4>(
                new Angular_Vel_Cost_only_Rot(IMU_ang_vel_, Lidar_ang_vel_)));
    }

    V3D IMU_ang_vel;
    V3D Lidar_ang_vel;
};

（2）进一步求解旋转外参、陀螺偏置、时间差

/// 求解旋转外参、陀螺偏置、时间差
void LI_Init::solve_Rot_bias_gyro(double &timediff_imu_wrt_lidar) {
    /// 初始旋转外参
    Eigen::Quaterniond quat(Rot_Lidar_wrt_IMU);
    double R_LI_quat[4];
    R_LI_quat[0] = quat.w();
    R_LI_quat[1] = quat.x();
    R_LI_quat[2] = quat.y();
    R_LI_quat[3] = quat.z();
    /// 初始偏置设为0
    double bias_g[3]; //Initial value of gyro bias
    bias_g[0] = 0;
    bias_g[1] = 0;
    bias_g[2] = 0;
    /// 初始时间差为0
    double time_lag2 = 0; //Second time lag (IMU wtr Lidar)

    /*** Ceres求解 ***/
//    ceres::LocalParameterization *quatParam = new ceres::QuaternionParameterization();
    ceres::Manifold *quatParam = new ceres::EigenQuaternionManifold();
    /// 创建问题实例
    ceres::Problem problem_ang_vel;
    /// 添加参数块
    problem_ang_vel.AddParameterBlock(R_LI_quat, 4, quatParam);
    problem_ang_vel.AddParameterBlock(bias_g, 3);
    /// 设置残差块，Angular_Vel_Cost::Create函数为自定义代价函数
    for (int i = 0; i < IMU_state_group.size(); i++) {
        double deltaT = Lidar_state_group[i].timeStamp - IMU_state_group[i].timeStamp;
        problem_ang_vel.AddResidualBlock(Angular_Vel_Cost::Create(IMU_state_group[i].ang_vel,
                                                                  IMU_state_group[i].ang_acc,
                                                                  Lidar_state_group[i].ang_vel,
                                                                  deltaT),
                                         nullptr,
                                         R_LI_quat,
                                         bias_g,
                                         &time_lag2);
    }
    /// 创建求解器进行求解
    ceres::Solver::Options options_quat;
    ceres::Solver::Summary summary_quat;
    ceres::Solve(options_quat, &problem_ang_vel, &summary_quat);

    /// 获取优化结果（旋转外参、陀螺零偏、时间差）
    Eigen::Quaterniond q_LI(R_LI_quat[0], R_LI_quat[1], R_LI_quat[2], R_LI_quat[3]);
    Rot_Lidar_wrt_IMU = q_LI.matrix(); /// 旋转外参
    V3D euler_angle = RotMtoEuler(q_LI.matrix());
    gyro_bias = V3D(bias_g[0], bias_g[1], bias_g[2]); /// 陀螺零偏

    time_lag_2 = time_lag2;
    time_delay_IMU_wtr_Lidar = time_lag_1 + time_lag_2; /// 时间差
    cout << "Total time delay (IMU wtr Lidar): " << time_delay_IMU_wtr_Lidar + timediff_imu_wrt_lidar << " s" << endl;
    cout << "Using LIO: SUBTRACT this value from IMU timestamp" << endl
         << "           or ADD this value to LiDAR timestamp." << endl <<endl;

    //The second temporal compensation
    /// 第二次时间补偿
    IMU_time_compensate(get_lag_time_2(), false);

    for (int i = 0; i < Lidar_state_group.size(); i++) {
        fout_after_rot << setprecision(12) << (Rot_Lidar_wrt_IMU * Lidar_state_group[i].ang_vel + gyro_bias).transpose()
                       << " " << Lidar_state_group[i].timeStamp << endl;
    }
}

///
struct Angular_Vel_Cost {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    /**
     * Angular_Vel_Cost 构造函数。
     * @param IMU_ang_vel_ IMU测量的角速度。
     * @param IMU_ang_acc_ IMU测量的角加速度。
     * @param Lidar_ang_vel_ Lidar测量的角速度。
     * @param deltaT_LI_ IMU和Lidar时间戳之间的差值。
     */
    Angular_Vel_Cost(V3D IMU_ang_vel_, V3D IMU_ang_acc_, V3D Lidar_ang_vel_, double deltaT_LI_) :
            IMU_ang_vel(IMU_ang_vel_), IMU_ang_acc(IMU_ang_acc_), Lidar_ang_vel(Lidar_ang_vel_),
            deltaT_LI(deltaT_LI_) {}

    template<typename T>
    bool operator()(const T *q, const T *b_g, const T *t, T *residual) const {
        //Known parameters used for Residual Construction
        Eigen::Matrix<T, 3, 1> IMU_ang_vel_T = IMU_ang_vel.cast<T>();
        Eigen::Matrix<T, 3, 1> IMU_ang_acc_T = IMU_ang_acc.cast<T>();
        Eigen::Matrix<T, 3, 1> Lidar_ang_vel_T = Lidar_ang_vel.cast<T>();
        T deltaT_LI_T{deltaT_LI};

        //Unknown Parameters, needed to be estimated
        Eigen::Quaternion<T> q_LI{q[0], q[1], q[2], q[3]};
        Eigen::Matrix<T, 3, 3> R_LI = q_LI.toRotationMatrix();  //Rotation
        Eigen::Matrix<T, 3, 1> bias_g{b_g[0], b_g[1], b_g[2]};  //Bias of gyroscope
        T td{t[0]};                                             //Time lag (IMU wtr Lidar)

        //Residual
        /// paper func(18)
        Eigen::Matrix<T, 3, 1> resi =
                R_LI * Lidar_ang_vel_T - IMU_ang_vel_T - (deltaT_LI_T + td) * IMU_ang_acc_T + bias_g;
        residual[0] = resi[0];
        residual[1] = resi[1];
        residual[2] = resi[2];
        return true;
    }

    static ceres::CostFunction *
    Create(const V3D IMU_ang_vel_, const V3D IMU_ang_acc_, const V3D Lidar_ang_vel_, const double deltaT_LI_)
    {
        return (new ceres::AutoDiffCostFunction<Angular_Vel_Cost, 3, 4, 3, 1>
                (
                new Angular_Vel_Cost(IMU_ang_vel_, IMU_ang_acc_, Lidar_ang_vel_, deltaT_LI_)
                )
                );
    }

    V3D IMU_ang_vel;
    V3D IMU_ang_acc;
    V3D Lidar_ang_vel;
    double deltaT_LI;
};

（3）对IMU线加速度进行插值和LiDAR状态对齐

void LI_Init::acc_interpolate() {
    //Interpolation to get acc_I(t_L)
    for (int i = 1; i < Lidar_state_group.size() - 1; i++) {
        double deltaT = Lidar_state_group[i].timeStamp - IMU_state_group[i].timeStamp;
        if (deltaT > 0) {
            double DeltaT = IMU_state_group[i + 1].timeStamp - IMU_state_group[i].timeStamp;
            double s = deltaT / DeltaT;
            IMU_state_group[i].linear_acc = s * IMU_state_group[i + 1].linear_acc +
                                            (1 - s) * IMU_state_group[i].linear_acc;
            IMU_state_group[i].timeStamp += deltaT;
        } else {
            double DeltaT = IMU_state_group[i].timeStamp - IMU_state_group[i - 1].timeStamp;
            double s = -deltaT / DeltaT;
            IMU_state_group[i].linear_acc = s * IMU_state_group[i - 1].linear_acc +
                                            (1 - s) * IMU_state_group[i].linear_acc;
            IMU_state_group[i].timeStamp += deltaT;
        }
    }
}

（4）求解平移外参、加速度偏置、重力

/// 求解平移外参、加速度偏置、重力
void LI_Init::solve_trans_biasacc_grav() {
    M3D Rot_Init = Eye3d;
    Rot_Init.diagonal() = V3D(1, 1, 1);
    Eigen::Quaterniond quat(Rot_Init);
    double R_GL0_quat[4];
    R_GL0_quat[0] = quat.w();
    R_GL0_quat[1] = quat.x();
    R_GL0_quat[2] = quat.y();
    R_GL0_quat[3] = quat.z();

    double bias_aL[3]; //Initial value of acc bias
    bias_aL[0] = 0;
    bias_aL[1] = 0;
    bias_aL[2] = 0;

    double Trans_IL[3]; //Initial value of Translation of IL (IMU with respect to Lidar)
    Trans_IL[0] = 0.0;
    Trans_IL[1] = 0.0;
    Trans_IL[2] = 0.0;

//    ceres::LocalParameterization *quatParam = new ceres::QuaternionParameterization();
    ceres::Manifold *quatParam = new ceres::EigenQuaternionManifold();
    ceres::Problem problem_acc;

    problem_acc.AddParameterBlock(R_GL0_quat, 4, quatParam);
    problem_acc.AddParameterBlock(bias_aL, 3);
    problem_acc.AddParameterBlock(Trans_IL, 3);

    //Jacobian of acc_bias, gravity, Translation
    int Jaco_size = 3 * Lidar_state_group.size();
    MatrixXd Jacobian(Jaco_size, 9);
    Jacobian.setZero();

    //Jacobian of Translation
    MatrixXd Jaco_Trans(Jaco_size, 3);
    Jaco_Trans.setZero();

    for (int i = 0; i < IMU_state_group.size(); i++) {
        problem_acc.AddResidualBlock(Linear_acc_Cost::Create(Lidar_state_group[i],
                                                             Rot_Lidar_wrt_IMU,
                                                             IMU_state_group[i].linear_acc),
                                     nullptr,
                                     R_GL0_quat,
                                     bias_aL,
                                     Trans_IL);

        Jacobian.block<3, 3>(3 * i, 0) = -Lidar_state_group[i].rot_end;
        Jacobian.block<3, 3>(3 * i, 3) << SKEW_SYM_MATRX(STD_GRAV);
        M3D omg_skew, angacc_skew;
        omg_skew << SKEW_SYM_MATRX(Lidar_state_group[i].ang_vel);
        angacc_skew << SKEW_SYM_MATRX(Lidar_state_group[i].ang_acc);
        M3D Jaco_trans_i = omg_skew * omg_skew + angacc_skew;
        Jaco_Trans.block<3, 3>(3 * i, 0) = Jaco_trans_i;
        Jacobian.block<3, 3>(3 * i, 6) = Jaco_trans_i;
    }

    for (int index = 0; index < 3; ++index) {
        problem_acc.SetParameterUpperBound(bias_aL, index, 0.01);
        problem_acc.SetParameterLowerBound(bias_aL, index, -0.01);
    }

    ceres::Solver::Options options_acc;
    ceres::Solver::Summary summary_acc;
    ceres::Solve(options_acc, &problem_acc, &summary_acc);


    Eigen::Quaterniond q_GL0(R_GL0_quat[0], R_GL0_quat[1], R_GL0_quat[2], R_GL0_quat[3]);
    Rot_Grav_wrt_Init_Lidar = q_GL0.matrix();
    Grav_L0 = Rot_Grav_wrt_Init_Lidar * STD_GRAV;

    V3D bias_a_Lidar(bias_aL[0], bias_aL[1], bias_aL[2]);
    acc_bias = Rot_Lidar_wrt_IMU * bias_a_Lidar;

    V3D Trans_IL_vec(Trans_IL[0], Trans_IL[1], Trans_IL[2]);
    Trans_Lidar_wrt_IMU = -Rot_Lidar_wrt_IMU * Trans_IL_vec;

    for (int i = 0; i < IMU_state_group.size(); i++) {
        V3D acc_I = Lidar_state_group[i].rot_end * Rot_Lidar_wrt_IMU.transpose() * IMU_state_group[i].linear_acc -
                    Lidar_state_group[i].rot_end * bias_a_Lidar;
        V3D acc_L = Lidar_state_group[i].linear_acc +
                    Lidar_state_group[i].rot_end * Jaco_Trans.block<3, 3>(3 * i, 0) * Trans_IL_vec - Grav_L0;
        fout_acc_cost << setprecision(10) << acc_I.transpose() << " " << acc_L.transpose() << " "
                      << IMU_state_group[i].timeStamp << " " << Lidar_state_group[i].timeStamp << endl;
    }

    M3D Hessian_Trans = Jaco_Trans.transpose() * Jaco_Trans;
    EigenSolver<M3D> es_trans(Hessian_Trans);
    M3D EigenValue_mat_trans = es_trans.pseudoEigenvalueMatrix();
    M3D EigenVec_mat_trans = es_trans.pseudoEigenvectors();

}
///
struct Linear_acc_Cost {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    Linear_acc_Cost(CalibState LidarState_, M3D R_LI_, V3D IMU_linear_acc_) :
            LidarState(LidarState_), R_LI(R_LI_), IMU_linear_acc(IMU_linear_acc_) {}

    template<typename T>
    bool operator()(const T *q, const T *b_a, const T *trans, T *residual) const {
        //Known parameters used for Residual Construction
        Eigen::Matrix<T, 3, 3> R_LL0_T = LidarState.rot_end.cast<T>();
        Eigen::Matrix<T, 3, 3> R_LI_T_transpose = R_LI.transpose().cast<T>();
        Eigen::Matrix<T, 3, 1> IMU_linear_acc_T = IMU_linear_acc.cast<T>();
        Eigen::Matrix<T, 3, 1> Lidar_linear_acc_T = LidarState.linear_acc.cast<T>();

        //Unknown Parameters, needed to be estimated
        /// 待估计参数：R_GL0、bias_aL、T_IL
        Eigen::Quaternion<T> q_GL0{q[0], q[1], q[2], q[3]};
        Eigen::Matrix<T, 3, 3> R_GL0 = q_GL0.toRotationMatrix();   //Rotation from Gravitational to First Lidar frame
        Eigen::Matrix<T, 3, 1> bias_aL{b_a[0], b_a[1], b_a[2]};    //Bias of Linear acceleration
        Eigen::Matrix<T, 3, 1> T_IL{trans[0], trans[1], trans[2]}; //Translation of I-L (IMU wtr Lidar)

        //Residual Construction
        /// 构造残差
        M3D Lidar_omg_SKEW, Lidar_angacc_SKEW;
        Lidar_omg_SKEW << SKEW_SYM_MATRX(LidarState.ang_vel);
        Lidar_angacc_SKEW << SKEW_SYM_MATRX(LidarState.ang_acc);
        M3D Jacob_trans = Lidar_omg_SKEW * Lidar_omg_SKEW + Lidar_angacc_SKEW;
        Eigen::Matrix<T, 3, 3> Jacob_trans_T = Jacob_trans.cast<T>();

        Eigen::Matrix<T, 3, 1> resi = R_LL0_T * R_LI_T_transpose * IMU_linear_acc_T - R_LL0_T * bias_aL
                                      + R_GL0 * STD_GRAV - Lidar_linear_acc_T - R_LL0_T * Jacob_trans_T * T_IL;

        residual[0] = resi[0];
        residual[1] = resi[1];
        residual[2] = resi[2];
        return true;
    }

    static ceres::CostFunction *Create(const CalibState LidarState_, const M3D R_LI_, const V3D IMU_linear_acc_) {
        return (new ceres::AutoDiffCostFunction<Linear_acc_Cost, 3, 4, 3, 3>(
                new Linear_acc_Cost(LidarState_, R_LI_, IMU_linear_acc_)));
    }

    CalibState LidarState;
    M3D R_LI;
    V3D IMU_linear_acc;
};

三、进一步优化计算

TODO