• SR-LIO--手写紧耦合IESKF

    1.ESKF初始化

    1. void eskfEstimator::tryInit(const std::vectordouble, std::pair>> &imu_meas)
    2. {   //通过imu测量值初始化均值，协方差；(均值用于初始化零偏，协方差用于判断噪声是否太大)
    3.     initialization(imu_meas);
    4.     //imu初始化超过10次测量以及累积时间间隔大于3s 
    5.     if (num_init_meas > MIN_INI_COUNT && imu_meas.back().first - time_first_imu > MIN_INI_TIME)
    6.     {   
    7.         acc_cov *= std::pow(G_norm / mean_acc.norm(), 2);
    8.  
    9.         if (gyr_cov.norm() > MAX_GYR_VAR)
    10.         {
    11.             LOG(ERROR) << "Too large noise of gyroscope measurements. " << gyr_cov.norm() << " > " << MAX_GYR_VAR;
    12.             return;
    13.         }
    14.  
    15.         if (acc_cov.norm() > MAX_ACC_VAR)
    16.         {
    17.             LOG(ERROR) << "Too large noise of accelerometer measurements. " << acc_cov.norm() << " > " << MAX_ACC_VAR;
    18.             return;
    19.         }
    20.  
    21.         initial_flag = true;
    22.  
    23.         gyr_cov = gyr_cov_scale;
    24.         acc_cov = acc_cov_scale;
    25.         //初始化陀螺仪零偏
    26.         Eigen::Vector3d init_bg = mean_gyr;
    27.         //初始化重力加速度
    28.         Eigen::Vector3d init_gravity = mean_acc / mean_acc.norm() * G_norm;
    29.     
    30.         setBg(init_bg);
    31.         setGravity(init_gravity);
    32.         //协方差矩阵初始化
    33.         covariance.block<3, 3>(9, 9) *= 0.001;
    34.         covariance.block<3, 3>(12, 12) *= 0.0001;
    35.         covariance.block<2, 2>(15, 15) *= 0.00001;
    36.         //通过参数设定协方差初始化噪声(0.1,0.1,0.0001,0.0001)
    37.         initializeNoise();
    38.  
    39.         ROS_INFO("IMU Initialization Done.");
    40.  
    41.         std::cout << "init_gravity = " << init_gravity.transpose() << std::endl;
    42.         std::cout << "init_bg = " << init_bg.transpose() << std::endl;
    43.     }
    44.     else
    45.         ROS_INFO("Wait more IMU measurements...");
    46.  
    47.     return;
    48. }

    其中的initialization(imu_meas)函数为：

    1. void eskfEstimator::initialization(const std::vectordouble, std::pair>> &imu_meas)
    2. {   //如果为第一个imu测量，怎么判断是否是第一个？（默认就是第一个，之后置为false）
    3.     if (is_first_imu_meas)
    4.     {   //使用读入的第一个imu测量进行初始化
    5.         num_init_meas = 1;
    6.         is_first_imu_meas = false;
    7.         time_first_imu = imu_meas.front().first;
    8.         mean_gyr = imu_meas.front().second.first;
    9.         mean_acc = imu_meas.front().second.second;
    10.     }
    11.     //对每一个imu测量进行处理
    12.     for (const auto &imu : imu_meas)
    13.     {   //计算角速度以及加速度的均值，协方差
    14.         mean_gyr += (imu.second.first - mean_gyr) / num_init_meas;
    15.         mean_acc += (imu.second.second - mean_acc) / num_init_meas;
    16.  
    17.         gyr_cov = gyr_cov * (num_init_meas - 1.0) / num_init_meas 
    18.             + (imu.second.first - mean_gyr).cwiseProduct(imu.second.first - mean_gyr) * (num_init_meas - 1.0) / (num_init_meas * num_init_meas);
    19.  
    20.         acc_cov = acc_cov * (num_init_meas - 1.0) / num_init_meas 
    21.             + (imu.second.second - mean_acc).cwiseProduct(imu.second.second - mean_acc) * (num_init_meas - 1.0) / (num_init_meas * num_init_meas);
    22.  
    23.         num_init_meas++;
    24.     }
    25.     //陀螺仪，加速度的测量值
    26.     gyr_0 = imu_meas.back().second.first;
    27.     acc_0 = imu_meas.back().second.second;
    28. }

    初始化的目的是使用读入的第一个imu测量值来初始化第一帧imu时间戳、陀螺仪均值及加速度均值。对于后续帧，会继续计算陀螺仪和加速度均值，以及对应的协方差。最后，将最后一个测量值作为下一帧到来时的上一帧测量值。

    2.ESKF预测

      此时已经初始化完毕

    1.  //如果已经初始化
    2.         if (initial_flag)
    3.         {
    4.             imuState imu_state_temp;
    5.  
    6.             imu_state_temp.timestamp = current_time;
    7.             //imu状态量
    8.             //上一时刻无偏加速度
    9.             imu_state_temp.un_acc = eskf_pro->getRotation().toRotationMatrix() * (eskf_pro->getLastAcc() - eskf_pro->getBa());
    10.             //上一时刻无偏角速度
    11.             imu_state_temp.un_gyr = eskf_pro->getLastGyr() - eskf_pro->getBg();
    12.             //平移
    13.             imu_state_temp.trans = eskf_pro->getTranslation();
    14.             //旋转
    15.             imu_state_temp.quat = eskf_pro->getRotation();
    16.             //速度
    17.             imu_state_temp.vel = eskf_pro->getVelocity();
    18.  
    19.             imu_states.push_back(imu_state_temp);
    20.         }

    读取ESKF中上一帧末尾时刻的状态值（世界坐标系）作为当前帧起始时刻的状态值：a,w,T,R,v;将其存入imu_states容器中，用于后续畸变校准。

    然后处理当前帧imu读数：

    1. for (auto &imu_msg : measurement.first.first)
    2.         {
    3.             double time_imu = imu_msg->header.stamp.toSec();
    4.  
    5.             if (time_imu <= time_frame)
    6.             { 
    7.                 if(current_time < 0)
    8.                     //current_time：当前帧起始时刻
    9.                     current_time = measurement.second.first;
    10.                 double dt = time_imu - current_time;
    11.  
    12.                 if(dt < -1e-6) continue;
    13.                 assert(dt >= 0);
    14.                 current_time = time_imu;
    15.                 dx = imu_msg->linear_acceleration.x;
    16.                 dy = imu_msg->linear_acceleration.y;
    17.                 dz = imu_msg->linear_acceleration.z;
    18.                 rx = imu_msg->angular_velocity.x;
    19.                 ry = imu_msg->angular_velocity.y;
    20.                 rz = imu_msg->angular_velocity.z;
    21.                 
    22.                 imuState imu_state_temp;
    23.  
    24.                 imu_state_temp.timestamp = current_time;
    25.                 //当前时刻无偏加速度中值
    26.                 imu_state_temp.un_acc = eskf_pro->getRotation().toRotationMatrix() * (0.5 * (eskf_pro->getLastAcc() + Eigen::Vector3d(dx, dy, dz)) - eskf_pro->getBa());
    27.                 //当前时刻无偏角速度中值
    28.                 imu_state_temp.un_gyr = 0.5 * (eskf_pro->getLastGyr() + Eigen::Vector3d(rx, ry, rz)) - eskf_pro->getBg();
    29.  
    30.                 dt_sum = dt_sum + dt;
    31.                 //对测量值进行预测
    32.                 eskf_pro->predict(dt, Eigen::Vector3d(dx, dy, dz), Eigen::Vector3d(rx, ry, rz));
    33.  
    34.                 imu_state_temp.trans = eskf_pro->getTranslation();
    35.                 imu_state_temp.quat = eskf_pro->getRotation();
    36.                 imu_state_temp.vel = eskf_pro->getVelocity();
    37.  
    38.                 imu_states.push_back(imu_state_temp);
    39.             }
    40.             else
    41.             {
    42.                 double dt_1 = time_frame - current_time;
    43.                 double dt_2 = time_imu - time_frame;
    44.                 current_time = time_frame;
    45.                 assert(dt_1 >= 0);
    46.                 assert(dt_2 >= 0);
    47.                 assert(dt_1 + dt_2 > 0);
    48.                 double w1 = dt_2 / (dt_1 + dt_2);
    49.                 double w2 = dt_1 / (dt_1 + dt_2);
    50.                 dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;
    51.                 dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;
    52.                 dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;
    53.                 rx = w1 * rx + w2 * imu_msg->angular_velocity.x;
    54.                 ry = w1 * ry + w2 * imu_msg->angular_velocity.y;
    55.                 rz = w1 * rz + w2 * imu_msg->angular_velocity.z;
    56.  
    57.                 imuState imu_state_temp;
    58.  
    59.                 imu_state_temp.timestamp = current_time;
    60.  
    61.                 imu_state_temp.un_acc = eskf_pro->getRotation().toRotationMatrix() * (0.5 * (eskf_pro->getLastAcc() + Eigen::Vector3d(dx, dy, dz)) - eskf_pro->getBa());
    62.                 imu_state_temp.un_gyr = 0.5 * (eskf_pro->getLastGyr() + Eigen::Vector3d(rx, ry, rz)) - eskf_pro->getBg();
    63.  
    64.                 dt_sum = dt_sum + dt_1;
    65.                 eskf_pro->predict(dt_1, Eigen::Vector3d(dx, dy, dz), Eigen::Vector3d(rx, ry, rz));
    66.  
    67.                 imu_state_temp.trans = eskf_pro->getTranslation();
    68.                 imu_state_temp.quat = eskf_pro->getRotation();
    69.                 imu_state_temp.vel = eskf_pro->getVelocity();
    70.  
    71.                 imu_states.push_back(imu_state_temp);
    72.             }
    73.         }

    如以上代码所示，具体分为两种情况：

    1）time_imu <= time_frame：

    此时imu的时间戳小于等于雷达扫描帧的时间戳，则可以直接使用imu测量值进行前向预测；

    2）time_imu > time_frame：

    此时imu的时间戳大于雷达扫描帧的时间戳，需要对imu进行插值后才能进行前向预测。

    2.0前向预测函数predict(dt, Eigen::Vector3d(dx, dy, dz), Eigen::Vector3d(rx, ry, rz)):
    1. void eskfEstimator::predict(double dt_, const Eigen::Vector3d &acc_1_, const Eigen::Vector3d &gyr_1_)
    2. {
    3.     dt = dt_;
    4.     acc_1 = acc_1_;
    5.     gyr_1 = gyr_1_;
    6.  
    7.     Eigen::Vector3d avr_acc = 0.5 * (acc_0 + acc_1);
    8.     Eigen::Vector3d avr_gyr = 0.5 * (gyr_0 + gyr_1);
    9.  
    10.     Eigen::Vector3d p_before = p;
    11.     Eigen::Quaterniond q_before = q;
    12.     Eigen::Vector3d v_before = v;
    13.     Eigen::Vector3d ba_before = ba;
    14.     Eigen::Vector3d bg_before = bg;
    15.     Eigen::Vector3d g_before = g;
    16.  
    17.     Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + gyr_1) - bg;
    18.     Eigen::Vector3d un_acc = 0.5 * (acc_0 + acc_1) - ba;
    19.     //状态累积
    20.     q = q * numType::so3ToQuat(un_gyr * dt);
    21.     p = p + v * dt;
    22.     v = v + q_before.toRotationMatrix() * un_acc * dt - g * dt;
    23.  
    24.     Eigen::Matrix3d R_omega_x, R_acc_x;
    25.     //转为反对称矩阵
    26.     R_omega_x << 0, -un_gyr(2), un_gyr(1), un_gyr(2), 0, -un_gyr(0), -un_gyr(1), un_gyr(0), 0;
    27.     R_acc_x << 0, -un_acc(2), un_acc(1), un_acc(2), 0, -un_acc(0), -un_acc(1), un_acc(0), 0;
    28.  
    29.     Eigen::Matrix<double, 3, 2> B_x = numType::derivativeS2(g);
    30.  
    31.     Eigen::Matrix<double, 17, 17> F_x = Eigen::MatrixXd::Zero(17, 17);
    32.     F_x.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
    33.     F_x.block<3, 3>(0, 6) = Eigen::Matrix3d::Identity() * dt;
    34.     F_x.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() - R_omega_x * dt;
    35.     F_x.block<3, 3>(3, 12) = - Eigen::Matrix3d::Identity() * dt;
    36.     F_x.block<3, 3>(6, 3) = - q_before.toRotationMatrix() * R_acc_x * dt;
    37.     F_x.block<3, 3>(6, 6) = Eigen::Matrix3d::Identity();
    38.     F_x.block<3, 3>(6, 9) = - q_before.toRotationMatrix() * dt;
    39.     F_x.block<3, 2>(6, 15) = numType::skewSymmetric(g) * B_x * dt;
    40.     F_x.block<3, 3>(9, 9) = Eigen::Matrix3d::Identity();
    41.     F_x.block<3, 3>(12, 12) = Eigen::Matrix3d::Identity();
    42.     F_x.block<2, 2>(15, 15) = - 1.0 / (g.norm() * g.norm()) * B_x.transpose() * numType::skewSymmetric(g) * numType::skewSymmetric(g) * B_x;
    43.  
    44.     Eigen::Matrix<double, 17, 12> F_w = Eigen::MatrixXd::Zero(17, 12);
    45.     F_w.block<3, 3>(6, 0) = - q_before.toRotationMatrix() * dt;
    46.     F_w.block<3, 3>(3, 3) = - Eigen::Matrix3d::Identity() * dt;
    47.     F_w.block<3, 3>(9, 6) = - Eigen::Matrix3d::Identity() * dt;
    48.     F_w.block<3, 3>(12, 9) = - Eigen::Matrix3d::Identity() * dt;
    49.  
    50.     covariance = F_x * covariance * F_x.transpose() + F_w * noise * F_w.transpose();
    51.  
    52.     acc_0 = acc_1;
    53.     gyr_0 = gyr_1;
    54. }

    在预测过程中，首先获取本次imu的测量值x_{1}(w_{1},a_{1})，然后结合上一次imu的测量值x_{0}(w_{0},a_{0})求均值(以下所有表达式皆以下标1表示当前时刻，下标0表示上一时刻):

    \dot{w}=\frac{1}{2}\cdot (w_{0}+w_{1})-b_{g}

    \dot{a}=\frac{1}{2}\cdot (a_{0}+a_{1})-b_{a}

    2.1计算名义状态的预测，即对IMU进行积分(此处使用的是中值积分)：

    p_{1}=p_{0}+v_{0}\cdot \Delta _{t}

    v_{1}=v_{0}+q_{0}\cdot((\frac{_{1}}{2}\cdot (a_{0}+a_{1})-b_a{})\cdot\Delta _{t})-g\cdot\Delta_{t}

    q_{1}=q_{0}\cdot((\frac{1}{2}\cdot(w_{0}+w_{1})-b_{g})\cdot\Delta_{t})

    此时完成了名义状态的计算。需要注意的是，这里名义状态的递推忽略了一些量，完整的递推形式应该是这样的(加粗代表上式缺少的部分)：

    p_{1}=p_{0}+v_{0}\cdot \Delta _{t}\mathbf{+\frac{1}{2}\cdot(q_{0}\cdot(\frac{1}{2}\cdot(a_{0}+a_{1})-b_{a}))\cdot\Delta_{t}^{2}-\frac{1}{2}\cdot{g}\cdot\Delta_{t}^{2}}

    v_{1}=v_{0}+q_{0}\cdot((\frac{_{1}}{2}\cdot (a_{0}+a_{1})-b_a{})\cdot\Delta _{t})-g\cdot\Delta_{t}

    q_{1}=q_{0}\cdot((\frac{1}{2}\cdot(w_{0}+w_{1})-b_{g})\cdot\Delta_{t})

    \boldsymbol{b_{g}(t+\Delta_{t})=b_{g}(t)}

    \boldsymbol{b_{a}(t+\Delta_{t})=b_{a}(t)}

    \boldsymbol{g_{g}(t+\Delta_{t})=g_{g}(t)}

    2.2计算误差状态的预测(加粗代表矩阵形式)：

    \delta x_{pred}=\boldsymbol{F}\delta x\Leftrightarrow \delta x_{1}=\boldsymbol{F}\delta x _{0}

    \boldsymbol{P_{pred}}=\boldsymbol{F}\boldsymbol{P}\boldsymbol{F}^{T}+\boldsymbol{Q}\Leftrightarrow \boldsymbol{P_{1}}=\boldsymbol{F}\boldsymbol{P_{0}}\boldsymbol{F}^{T}+\boldsymbol{Q}

    计算协方差矩阵还有更完善的形式：

    \boldsymbol{P_{pred}}=\boldsymbol{F_{x}}\boldsymbol{P}\boldsymbol{F_{x}}^{T}+\boldsymbol{F_{w}}\boldsymbol{Q}\boldsymbol{F_{w}}^{T}\Leftrightarrow \boldsymbol{P_{1}}=\boldsymbol{F_{x}}\boldsymbol{P_{0}}\boldsymbol{F_{x}}^{T}+\boldsymbol{F_{w}}\boldsymbol{Q}\boldsymbol{F_{w}}^{T}

    需要注意的是，一般ESKF的误差状态在每次更新后会被重置为0，即\delta x _{0}=0。因此只需关注协方差部分即可。

    第一个重头戏来了！！！计算\boldsymbol{F}矩阵：

    首先贴一下高博书里的计算形式：

    高博这里没有考虑到重力的处理，因此贴出完整形式：

    3.ESKF更新

    存储完imu读数后进入处理函数：

    1.  
    2. void lioOptimization::process(std::vector &cut_sweep, double timestamp_begin, double timestamp_offset)
    3. {
    4.     state *cur_state = new state();//p,v,q,ba,bg
    5.  
    6.     stateInitialization(cur_state);
    7.  
    8.     std::vector const_frame;
    9.  
    10.     const_frame.insert(const_frame.end(), cut_sweep.begin(), cut_sweep.end());
    11.  
    12.     cloudFrame *p_frame = buildFrame(const_frame, cur_state, timestamp_begin, timestamp_offset);
    13.  
    14.     optimizeSummary summary = stateEstimation(p_frame);
    15.  
    16.     std::cout << "after solution: " << std::endl;
    17.     std::cout << "rotation: " << p_frame->p_state->rotation.x() << " " << p_frame->p_state->rotation.y() << " " 
    18.               << p_frame->p_state->rotation.z() << " " << p_frame->p_state->rotation.w() << std::endl;
    19.     std::cout << "translation: " << p_frame->p_state->translation.x() << " " << p_frame->p_state->translation.y() << " " << p_frame->p_state->translation.z() << std::endl;
    20.     std::cout << "gravity = " << G.x() << " " << G.y() << " " << G.z() << std::endl;
    21.  
    22.     dt_sum = 0;
    23.  
    24.     publish_odometry(pub_odom, p_frame);
    25.     publish_path(pub_path, p_frame);   
    26.  
    27.     if(debug_output)
    28.     {
    29.         pcl::PointCloud::Ptr p_cloud_temp;
    30.         p_cloud_temp.reset(new pcl::PointCloud());
    31.         point3DtoPCL(p_frame->point_frame, p_cloud_temp);
    32.  
    33.         std::string pcd_path(output_path + "/cloud_frame/" + std::to_string(index_frame) + std::string(".pcd"));
    34.         saveCutCloud(pcd_path, p_cloud_temp);
    35.     }
    36.  
    37.     int num_remove = 0;
    38.  
    39.     if (initial_flag)
    40.     {
    41.         if (index_frame > sweep_cut_num && index_frame % sweep_cut_num == 0)
    42.         {
    43.             while (all_cloud_frame.size() > std::max(2, sweep_cut_num))
    44.             {
    45.                 recordSinglePose(all_cloud_frame[0]);
    46.                 all_cloud_frame[0]->release();
    47.                 all_cloud_frame.erase(all_cloud_frame.begin());
    48.                 num_remove++;
    49.             }
    50.             assert(all_cloud_frame.size() == std::max(2, sweep_cut_num));
    51.         }
    52.     }
    53.     else
    54.     {
    55.         while (all_cloud_frame.size() > options.num_for_initialization)
    56.         {
    57.             recordSinglePose(all_cloud_frame[0]);
    58.             all_cloud_frame[0]->release();
    59.             all_cloud_frame.erase(all_cloud_frame.begin());
    60.             num_remove++;
    61.         }
    62.     }
    63.     
    64.  
    65.     for(int i = 0; i < all_cloud_frame.size(); i++)
    66.         all_cloud_frame[i]->id = all_cloud_frame[i]->id - num_remove;
    67.  
    68.     while (v_cut_sweep.size() > sweep_cut_num - 1)
    69.     {
    70.         std::vector().swap(v_cut_sweep.front());
    71.         assert(v_cut_sweep.front().size() == 0);
    72.         v_cut_sweep.erase(v_cut_sweep.begin());
    73.     }
    74. }

    首先进入stateInitialization(cur_state)函数对状态进行初始化：

    1. //状态初始化，1：匀速模型；2：IMU
    2. void lioOptimization::stateInitialization(state *cur_state)
    3. {
    4.     if (index_frame <= sweep_cut_num + 1)
    5.     {
    6.         cur_state->rotation = Eigen::Quaterniond::Identity();
    7.         cur_state->translation = Eigen::Vector3d::Zero();
    8.     }
    9.     else if (index_frame == sweep_cut_num + 2)
    10.     {   //匀速模型初始化
    11.         if (options.initialization == INIT_CONSTANT_VELOCITY)
    12.         {
    13.             //q(k) = [q(k-1) * q(k-2)^-1] * q(k-1)
    14.             Eigen::Quaterniond q_next_end = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation * 
    15.                     all_cloud_frame[all_cloud_frame.size() - 2]->p_state->rotation.inverse() * all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation;
    16.             //t(k) = t(k-1) + [q(k-1) * q(k-2)^-1] * [t(k-1) - t(k-2)]
    17.             Eigen::Vector3d t_next_end = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation + 
    18.                                          all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation * 
    19.                                          all_cloud_frame[all_cloud_frame.size() - 2]->p_state->rotation.inverse() * 
    20.                                          (all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation - 
    21.                                          all_cloud_frame[all_cloud_frame.size() - 2]->p_state->translation);
    22.  
    23.             cur_state->rotation = q_next_end;
    24.             cur_state->translation = t_next_end;
    25.         }
    26.         //IMU初始化
    27.         else if (options.initialization == INIT_IMU)
    28.         {
    29.             if (initial_flag)
    30.             {
    31.                 cur_state->rotation = eskf_pro->getRotation();
    32.                 cur_state->translation = eskf_pro->getTranslation();
    33.             }
    34.             else
    35.             {   //q(k) = [q(k-1) * q(k-2)^-1] * q(k-1)
    36.                 Eigen::Quaterniond q_next_end = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation * 
    37.                         all_cloud_frame[all_cloud_frame.size() - 2]->p_state->rotation.inverse() * all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation;
    38.                 //t(k) = t(k-1) + [q(k-1) * q(k-2)^-1] * [t(k-1) - t(k-2)]
    39.                 Eigen::Vector3d t_next_end = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation + 
    40.                                              all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation * 
    41.                                              all_cloud_frame[all_cloud_frame.size() - 2]->p_state->rotation.inverse() * 
    42.                                              (all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation - 
    43.                                              all_cloud_frame[all_cloud_frame.size() - 2]->p_state->translation);
    44.  
    45.                 cur_state->rotation = q_next_end;
    46.                 cur_state->translation = t_next_end;
    47.             }
    48.         }
    49.         else
    50.         {
    51.             cur_state->rotation = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation;
    52.             cur_state->translation = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation;
    53.         }
    54.     }
    55.     else
    56.     {
    57.         if (options.initialization == INIT_CONSTANT_VELOCITY)
    58.         {
    59.             Eigen::Quaterniond q_next_end = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation * 
    60.                     all_cloud_frame[all_cloud_frame.size() - 2]->p_state->rotation.inverse() * all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation;
    61.  
    62.             Eigen::Vector3d t_next_end = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation + 
    63.                                          all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation * 
    64.                                          all_cloud_frame[all_cloud_frame.size() - 2]->p_state->rotation.inverse() * 
    65.                                          (all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation - 
    66.                                          all_cloud_frame[all_cloud_frame.size() - 2]->p_state->translation);
    67.  
    68.             cur_state->rotation = q_next_end;
    69.             cur_state->translation = t_next_end;
    70.         }
    71.         else if (options.initialization == INIT_IMU)
    72.         {
    73.             if (initial_flag)
    74.             {
    75.                 cur_state->rotation = eskf_pro->getRotation();
    76.                 cur_state->translation = eskf_pro->getTranslation();
    77.             }
    78.             else
    79.             {
    80.                 Eigen::Quaterniond q_next_end = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation * 
    81.                         all_cloud_frame[all_cloud_frame.size() - 2]->p_state->rotation.inverse() * all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation;
    82.  
    83.                 Eigen::Vector3d t_next_end = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation + 
    84.                                              all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation * 
    85.                                              all_cloud_frame[all_cloud_frame.size() - 2]->p_state->rotation.inverse() * 
    86.                                              (all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation - 
    87.                                              all_cloud_frame[all_cloud_frame.size() - 2]->p_state->translation);
    88.  
    89.                 cur_state->rotation = q_next_end;
    90.                 cur_state->translation = t_next_end;
    91.             }
    92.         }
    93.         else
    94.         {
    95.             cur_state->rotation = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation;
    96.             cur_state->translation = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation;
    97.         }
    98.     }
    99. }

    这里的初始化形式主要分为匀速模型及直接使用imu初始化两种形式，其中匀速模型初始化即当前时刻状态等于上一时刻状态加上上上时刻状态到上一时刻状态的增量。

    随后进入主要部分，即状态估计stateEstimation(p_frame)：

    1. optimizeSummary lioOptimization::stateEstimation(cloudFrame *p_frame)
    2. {
    3.     icpOptions optimize_options = options.optimize_options;
    4.     const double kSizeVoxelInitSample = options.voxel_size;//1.5
    5.  
    6.     const double kSizeVoxelMap = optimize_options.size_voxel_map;//1.0
    7.     const double kMinDistancePoints = options.min_distance_points;//0.1
    8.     const int kMaxNumPointsInVoxel = options.max_num_points_in_voxel;//20
    9.  
    10.     optimizeSummary optimize_summary;
    11.  
    12.     if(p_frame->frame_id > sweep_cut_num)
    13.     {
    14.         bool good_enough_registration = false;
    15.         double sample_voxel_size = p_frame->frame_id < options.init_num_frames ? options.init_sample_voxel_size : options.sample_voxel_size;
    16.         double min_voxel_size = std::min(options.init_voxel_size, options.voxel_size);
    17.  
    18.         optimize_summary = optimize(p_frame, optimize_options, sample_voxel_size);
    19.  
    20.         if(!optimize_summary.success)
    21.         {
    22.             return optimize_summary;
    23.         }
    24.     }
    25.     else
    26.     {
    27.         p_frame->p_state->translation = eskf_pro->getTranslation();
    28.         p_frame->p_state->rotation = eskf_pro->getRotation();
    29.         p_frame->p_state->velocity = eskf_pro->getVelocity();
    30.         p_frame->p_state->ba = eskf_pro->getBa();
    31.         p_frame->p_state->bg = eskf_pro->getBg();
    32.         G = eskf_pro->getGravity();
    33.         G_norm = G.norm();
    34.     }
    35.     //将p_frame点云添加到地图
    36.     addPointsToMap(voxel_map, p_frame, kSizeVoxelMap, kMaxNumPointsInVoxel, kMinDistancePoints);
    37.  
    38.     const double kMaxDistance = options.max_distance;
    39.     const Eigen::Vector3d location = p_frame->p_state->translation;
    40.  
    41.     removePointsFarFromLocation(voxel_map, location, kMaxDistance);
    42.  
    43.     return optimize_summary;
    44. }

    状态估计函数中主要调用optimize(p_frame, optimize_options, sample_voxel_size)函数：

    1. optimizeSummary lioOptimization::optimize(cloudFrame *p_frame, const icpOptions &cur_icp_options, double sample_voxel_size)
    2. {
    3.     std::vector keypoints;
    4.     //从point_frame中采样关键点keypoints(雷达坐标系)
    5.     gridSampling(p_frame->point_frame, keypoints, sample_voxel_size);
    6.  
    7.     optimizeSummary optimize_summary;
    8.  
    9.     optimize_summary = updateIEKF(cur_icp_options, voxel_map, keypoints, p_frame);
    10.  
    11.     if (!optimize_summary.success) {
    12.         return optimize_summary;
    13.     }
    14.  
    15.     Eigen::Quaterniond q_end = p_frame->p_state->rotation;
    16.     Eigen::Vector3d t_end = p_frame->p_state->translation;
    17.     //将所有雷达坐标系下的点转到世界坐标系
    18.     for (auto &point_temp: p_frame->point_frame) {
    19.         transformPoint(point_temp, q_end, t_end, R_imu_lidar, t_imu_lidar);
    20.     }
    21.  
    22.     return optimize_summary;
    23. }

    该函数首先对输入点云进行下采样出关键点keypoints，然后通过updateIEKF函数进行卡尔曼滤波的更新：

    1. optimizeSummary lioOptimization::updateIEKF(const icpOptions &cur_icp_options, const voxelHashMap &voxel_map_temp, std::vector &keypoints, cloudFrame *p_frame)
    2. {    
    3.     int max_num_iter = p_frame->frame_id < cur_icp_options.init_num_frames ? 
    4.     std::max(15, cur_icp_options.num_iters_icp) : cur_icp_options.num_iters_icp;
    5.     std::cout<<"@@@max_num_iter:"<//begin15 later5
    6.     //获取预测状态变量
    7.     Eigen::Vector3d p_predict = eskf_pro->getTranslation();
    8.     Eigen::Quaterniond q_predict = eskf_pro->getRotation();
    9.     Eigen::Vector3d v_predict = eskf_pro->getVelocity();
    10.     Eigen::Vector3d ba_predict = eskf_pro->getBa();
    11.     Eigen::Vector3d bg_predict = eskf_pro->getBg();
    12.     Eigen::Vector3d g_predict = eskf_pro->getGravity();
    13.  
    14.     optimizeSummary summary;
    15.  
    16.     for (int i = -1; i < max_num_iter; i++)
    17.     {
    18.         std::vector plane_residuals;
    19.  
    20.         double loss_old = 0.0;
    21.         //构建点到面残差
    22.         summary = buildPlaneResiduals(cur_icp_options, voxel_map_temp, keypoints, plane_residuals, p_frame, loss_old);
    23.  
    24.         if (summary.success == false)
    25.             return summary;
    26.  
    27.         int num_plane_residuals = plane_residuals.size();
    28.  
    29.         Eigen::Matrix<double, Eigen::Dynamic, 6> H_x;
    30.         Eigen::Matrix<double, Eigen::Dynamic, 1> h; 
    31.         //H_x:距离残差对状态变量的雅可比
    32.         H_x.resize(num_plane_residuals, 6);
    33.         //h:距离残差
    34.         h.resize(num_plane_residuals, 1);
    35.  
    36.         for (int i = 0; i < num_plane_residuals; i++)
    37.         {
    38.             H_x.block<1, 6>(i, 0) = plane_residuals[i].jacobians;
    39.             h.block<1, 1>(i, 0) = Eigen::Matrix<double, 1, 1>(plane_residuals[i].distance * plane_residuals[i].weight);
    40.         }
    41.         //计算误差状态变量
    42.         Eigen::Vector3d d_p = eskf_pro->getTranslation() - p_predict;
    43.         Eigen::Quaterniond d_q = q_predict.inverse() * eskf_pro->getRotation();
    44.         Eigen::Vector3d d_so3 = numType::quatToSo3(d_q);
    45.         Eigen::Vector3d d_v = eskf_pro->getVelocity() - v_predict;
    46.         Eigen::Vector3d d_ba = eskf_pro->getBa() - ba_predict;
    47.         Eigen::Vector3d d_bg = eskf_pro->getBg() - bg_predict;
    48.  
    49.         Eigen::Vector3d g = eskf_pro->getGravity();
    50.  
    51.         Eigen::Vector3d g_predict_normalize = g_predict;
    52.         Eigen::Vector3d g_normalize = g;
    53.  
    54.         g_predict_normalize.normalize();
    55.         g_normalize.normalize();
    56.         //归一化后的重力向量的叉乘积
    57.         Eigen::Vector3d cross = g_predict_normalize.cross(g_normalize);
    58.         //归一化后的重力向量的点乘积
    59.         double dot = g_predict_normalize.dot(g_normalize);
    60.  
    61.         Eigen::Matrix3d R_dg;
    62.         //根据点乘积的值，计算旋转矩阵R_dg。如果点乘积接近1，矩阵设为单位矩阵。否则，使用叉乘积计算一个Skew对称矩阵，并使用Rodrigues'公式计算旋转矩阵
    63.         if (fabs(1.0 - dot) < 1e-6)
    64.             R_dg = Eigen::Matrix3d::Identity();
    65.         else
    66.         {
    67.             Eigen::Matrix3d skew = numType::skewSymmetric(cross);
    68.             R_dg = Eigen::Matrix3d::Identity() + skew + skew * skew * (1.0 - dot) 
    69.                 / (cross(0) * cross(0) + cross(1) * cross(1) + cross(2) * cross(2));
    70.         }
    71.         //SO3旋转 δθ_dg
    72.         Eigen::Vector3d so3_dg = numType::rotationToSo3(R_dg);
    73.         //计算导数矩阵B
    74.         Eigen::Matrix<double, 3, 2> B_x_predict = numType::derivativeS2(g_predict);
    75.         //B^T * δθ_dg
    76.         Eigen::Vector2d d_g = B_x_predict.transpose() * so3_dg;
    77.  
    78.         //17维误差状态变量
    79.         Eigen::Matrix<double, 17, 1> d_x;
    80.         d_x.head<3>() = d_p;
    81.         d_x.segment<3>(3) = d_so3;
    82.         d_x.segment<3>(6) = d_v;
    83.         d_x.segment<3>(9) = d_ba;
    84.         d_x.segment<3>(12) = d_bg;
    85.         d_x.tail<2>() = d_g;
    86.  
    87.         //I - 1/2 * δθ （3*3）
    88.         Eigen::Matrix3d J_k_so3 = Eigen::Matrix3d::Identity() - 0.5 * numType::skewSymmetric(d_so3);
    89.         //J0_gn = I + 1/2 * B^T * δθ_dg * B （2*2）
    90.         Eigen::Matrix2d J_k_s2 = Eigen::Matrix2d::Identity() + 0.5 * B_x_predict.transpose() * numType::skewSymmetric(so3_dg) * B_x_predict;
    91.         //整个操作就是计算J0_n * dx
    92.         Eigen::Matrix<double, 17, 1> d_x_new = d_x;
    93.         //(I - 1/2 * δθ) * δθ 在旋转误差状态左乘了(I - 1/2 * δθ)
    94.         d_x_new.segment<3>(3) = J_k_so3 * d_so3;
    95.         //J0_gn * (B^T * δθ_dg) 在重力状态分量上左乘了J0_gn：(I + 1/2 * B^T * δθ_dg * B)
    96.         d_x_new.segment<2>(15) = J_k_s2 * d_g;
    97.  
    98.         Eigen::Matrix<double, 17, 17> covariance = eskf_pro->getCovariance();
    99.         //计算J0_n * P * J0_n^T
    100.         for (int j = 0; j < covariance.cols(); j++)
    101.             covariance.block<3, 1>(3, j) = J_k_so3 * covariance.block<3, 1>(3, j);
    102.  
    103.         for (int j = 0; j < covariance.cols(); j++)
    104.             covariance.block<2, 1>(15, j) = J_k_s2 * covariance.block<2, 1>(15, j);
    105.  
    106.         for (int j = 0; j < covariance.rows(); j++)
    107.             covariance.block<1, 3>(j, 3) = covariance.block<1, 3>(j, 3) * J_k_so3.transpose();
    108.  
    109.         for (int j = 0; j < covariance.rows(); j++)
    110.             covariance.block<1, 2>(j, 15) = covariance.block<1, 2>(j, 15) * J_k_s2.transpose();
    111.         //(J0_n * (P/V) * J0_n^T)^-1
    112.         Eigen::Matrix<double, 17, 17> temp = (covariance/laser_point_cov).inverse();
    113.         //H^T * H
    114.         Eigen::Matrix<double, 6, 6> HTH = H_x.transpose() * H_x;
    115.         //H^T * H + (J0_n * (P/V) * J0_n^T)^-1
    116.         temp.block<6, 6>(0, 0) += HTH;
    117.         //(H^T * V^-1 * H + (J0_n * (P/V) * J0_n^T)^-1)^-1
    118.         Eigen::Matrix<double, 17, 17> temp_inv = temp.inverse();
    119.         //计算Kh
    120.         Eigen::Matrix<double, 17, 1> K_h = temp_inv.block<17, 6>(0, 0) * H_x.transpose() * h;
    121.  
    122.         Eigen::Matrix<double, 17, 17> K_x = Eigen::Matrix<double, 17, 17>::Zero();
    123.         //计算KH
    124.         K_x.block<17, 6>(0, 0) = temp_inv.block<17, 6>(0, 0) * HTH;
    125.         //dx = -Kh - (I - KH) * J0_n * dx
    126.         d_x = - K_h + (K_x - Eigen::Matrix<double, 17, 17>::Identity()) * d_x_new;
    127.  
    128.         Eigen::Vector3d g_before = eskf_pro->getGravity();
    129.  
    130.         if ((d_x.head<3>()).norm() > 100.0 || AngularDistance(d_x.segment<3>(3)) > 100.0)
    131.         {
    132.             continue;
    133.         }
    134.  
    135.         eskf_pro->observe(d_x);
    136.         //状态更新
    137.         p_frame->p_state->translation = eskf_pro->getTranslation();
    138.         p_frame->p_state->rotation = eskf_pro->getRotation();
    139.         p_frame->p_state->velocity = eskf_pro->getVelocity();
    140.         p_frame->p_state->ba = eskf_pro->getBa();
    141.         p_frame->p_state->bg = eskf_pro->getBg();
    142.         G = eskf_pro->getGravity();
    143.         G_norm = G.norm();
    144.  
    145.         bool converage = false;
    146.  
    147.         if (p_frame->frame_id > sweep_cut_num &&        
    148.             (d_x.head<3>()).norm() < cur_icp_options.threshold_translation_norm && //0.001
    149.             AngularDistance(d_x.segment<3>(3)) < cur_icp_options.threshold_orientation_norm) //0.0001
    150.         {
    151.             converage = true;
    152.         }
    153.         //协方差更新
    154.         if (converage || i == max_num_iter - 1)
    155.         {
    156.             Eigen::Matrix<double, 17, 17> covariance_new = covariance;
    157.  
    158.             Eigen::Matrix<double, 3, 2> B_x_before = numType::derivativeS2(g_before);
    159.  
    160.             J_k_so3 = Eigen::Matrix3d::Identity() - 0.5 * numType::skewSymmetric(d_x.segment<3>(3));
    161.             J_k_s2 = Eigen::Matrix2d::Identity() + 0.5 * B_x_before.transpose() * numType::skewSymmetric(B_x_before * d_x.tail<2>()) * B_x_before;
    162.  
    163.             for (int j = 0; j < covariance.cols(); j++)
    164.                 covariance_new.block<3, 1>(3, j) = J_k_so3 * covariance.block<3, 1>(3, j);
    165.  
    166.             for (int j = 0; j < covariance.cols(); j++)
    167.                 covariance_new.block<2, 1>(15, j) = J_k_s2 * covariance.block<2, 1>(15, j);
    168.  
    169.             for (int j = 0; j < covariance.rows(); j++)
    170.             {
    171.                 covariance_new.block<1, 3>(j, 3) = covariance.block<1, 3>(j, 3) * J_k_so3.transpose();
    172.                 covariance.block<1, 3>(j, 3) = covariance.block<1, 3>(j, 3) * J_k_so3.transpose();
    173.             }
    174.  
    175.             for (int j = 0; j < covariance.rows(); j++)
    176.             {
    177.                 covariance_new.block<1, 2>(j, 15) = covariance.block<1, 2>(j, 15) * J_k_s2.transpose();
    178.                 covariance.block<1, 2>(j, 15) = covariance.block<1, 2>(j, 15) * J_k_s2.transpose();
    179.             }
    180.  
    181.             for (int j = 0; j < 6; j++)
    182.                 K_x.block<3, 1>(3, j) = J_k_so3 * K_x.block<3, 1>(3, j);
    183.  
    184.             for (int j = 0; j < 6; j++)
    185.                 K_x.block<2, 1>(15, j) = J_k_s2 * K_x.block<2, 1>(15, j);
    186.  
    187.             covariance = covariance_new - K_x.block<17, 6>(0, 0) * covariance.block<6, 17>(0, 0);
    188.  
    189.             eskf_pro->setCovariance(covariance);
    190.  
    191.             break;
    192.         }
    193.     }
    194.  
    195.     return summary;
    196. }

    这里主要涉及到点到面残差的计算、雅可比的计算、卡尔曼增益的计算以及协方差矩阵的更新。这里为了简便直接将公式贴出来：

    1）计算点到面残差：

    2）计算雅可比：

    3）计算卡尔曼增益：

    其中H矩阵的计算为残差对状态量的雅可比：

    4）更新协方差矩阵及状态量：

    协方差矩阵的更新是卡尔曼滤波迭代结束才进行更新：

    而状态量是每次迭代都会进行更新：

    至此，IESKF状态估计的全部过程结束。

  • 相关阅读:
    【工具】SSH端口转发管理器，专门管理SSH Port Forwarding
    Langchain-chatchat本地部署
    SQL 优化
    【开源】JAVA+Vue.js实现APK检测管理系统
    数据结构与算法之排序: Leetcode 922. 按奇偶排序数组 II (Typescript版)
    UE4 材质多张图片拼接成一张图片（此处用2×2拼接）
    Ubuntu系统升级
    能链科技完成重点项目签约
    HummerRisk V0.6.0发布：升级列表高级搜索功能，扩充对象存储和操作审计支持范围等
    《宝塔面板教程1》：宝塔面板安装命令及安装要求
  • 原文地址：https://blog.csdn.net/qq_43527694/article/details/134466229