SR-LIO--手写紧耦合IESKF

1.ESKF初始化

void eskfEstimator::tryInit(const std::vector<std::pair<double, std::pair<Eigen::Vector3d, Eigen::Vector3d>>> &imu_meas)
{   //通过imu测量值初始化均值，协方差；(均值用于初始化零偏，协方差用于判断噪声是否太大)initialization(imu_meas);//imu初始化超过10次测量以及累积时间间隔大于3s if (num_init_meas > MIN_INI_COUNT && imu_meas.back().first - time_first_imu > MIN_INI_TIME){   acc_cov *= std::pow(G_norm / mean_acc.norm(), 2);if (gyr_cov.norm() > MAX_GYR_VAR){LOG(ERROR) << "Too large noise of gyroscope measurements. " << gyr_cov.norm() << " > " << MAX_GYR_VAR;return;}if (acc_cov.norm() > MAX_ACC_VAR){LOG(ERROR) << "Too large noise of accelerometer measurements. " << acc_cov.norm() << " > " << MAX_ACC_VAR;return;}initial_flag = true;gyr_cov = gyr_cov_scale;acc_cov = acc_cov_scale;//初始化陀螺仪零偏Eigen::Vector3d init_bg = mean_gyr;//初始化重力加速度Eigen::Vector3d init_gravity = mean_acc / mean_acc.norm() * G_norm;setBg(init_bg);setGravity(init_gravity);//协方差矩阵初始化covariance.block<3, 3>(9, 9) *= 0.001;covariance.block<3, 3>(12, 12) *= 0.0001;covariance.block<2, 2>(15, 15) *= 0.00001;//通过参数设定协方差初始化噪声(0.1,0.1,0.0001,0.0001)initializeNoise();ROS_INFO("IMU Initialization Done.");std::cout << "init_gravity = " << init_gravity.transpose() << std::endl;std::cout << "init_bg = " << init_bg.transpose() << std::endl;}elseROS_INFO("Wait more IMU measurements...");return;
}

其中的initialization(imu_meas)函数为：

void eskfEstimator::initialization(const std::vector<std::pair<double, std::pair<Eigen::Vector3d, Eigen::Vector3d>>> &imu_meas)
{   //如果为第一个imu测量，怎么判断是否是第一个？（默认就是第一个，之后置为false）if (is_first_imu_meas){   //使用读入的第一个imu测量进行初始化num_init_meas = 1;is_first_imu_meas = false;time_first_imu = imu_meas.front().first;mean_gyr = imu_meas.front().second.first;mean_acc = imu_meas.front().second.second;}//对每一个imu测量进行处理for (const auto &imu : imu_meas){   //计算角速度以及加速度的均值，协方差mean_gyr += (imu.second.first - mean_gyr) / num_init_meas;mean_acc += (imu.second.second - mean_acc) / num_init_meas;gyr_cov = gyr_cov * (num_init_meas - 1.0) / num_init_meas + (imu.second.first - mean_gyr).cwiseProduct(imu.second.first - mean_gyr) * (num_init_meas - 1.0) / (num_init_meas * num_init_meas);acc_cov = acc_cov * (num_init_meas - 1.0) / num_init_meas + (imu.second.second - mean_acc).cwiseProduct(imu.second.second - mean_acc) * (num_init_meas - 1.0) / (num_init_meas * num_init_meas);num_init_meas++;}//陀螺仪，加速度的测量值gyr_0 = imu_meas.back().second.first;acc_0 = imu_meas.back().second.second;
}

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

2.ESKF预测

此时已经初始化完毕

 //如果已经初始化if (initial_flag){imuState imu_state_temp;imu_state_temp.timestamp = current_time;//imu状态量//上一时刻无偏加速度imu_state_temp.un_acc = eskf_pro->getRotation().toRotationMatrix() * (eskf_pro->getLastAcc() - eskf_pro->getBa());//上一时刻无偏角速度imu_state_temp.un_gyr = eskf_pro->getLastGyr() - eskf_pro->getBg();//平移imu_state_temp.trans = eskf_pro->getTranslation();//旋转imu_state_temp.quat = eskf_pro->getRotation();//速度imu_state_temp.vel = eskf_pro->getVelocity();imu_states.push_back(imu_state_temp);}

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

然后处理当前帧imu读数：

for (auto &imu_msg : measurement.first.first){double time_imu = imu_msg->header.stamp.toSec();if (time_imu <= time_frame){ if(current_time < 0)//current_time：当前帧起始时刻current_time = measurement.second.first;double dt = time_imu - current_time;if(dt < -1e-6) continue;assert(dt >= 0);current_time = time_imu;dx = imu_msg->linear_acceleration.x;dy = imu_msg->linear_acceleration.y;dz = imu_msg->linear_acceleration.z;rx = imu_msg->angular_velocity.x;ry = imu_msg->angular_velocity.y;rz = imu_msg->angular_velocity.z;imuState imu_state_temp;imu_state_temp.timestamp = current_time;//当前时刻无偏加速度中值imu_state_temp.un_acc = eskf_pro->getRotation().toRotationMatrix() * (0.5 * (eskf_pro->getLastAcc() + Eigen::Vector3d(dx, dy, dz)) - eskf_pro->getBa());//当前时刻无偏角速度中值imu_state_temp.un_gyr = 0.5 * (eskf_pro->getLastGyr() + Eigen::Vector3d(rx, ry, rz)) - eskf_pro->getBg();dt_sum = dt_sum + dt;//对测量值进行预测eskf_pro->predict(dt, Eigen::Vector3d(dx, dy, dz), Eigen::Vector3d(rx, ry, rz));imu_state_temp.trans = eskf_pro->getTranslation();imu_state_temp.quat = eskf_pro->getRotation();imu_state_temp.vel = eskf_pro->getVelocity();imu_states.push_back(imu_state_temp);}else{double dt_1 = time_frame - current_time;double dt_2 = time_imu - time_frame;current_time = time_frame;assert(dt_1 >= 0);assert(dt_2 >= 0);assert(dt_1 + dt_2 > 0);double w1 = dt_2 / (dt_1 + dt_2);double w2 = dt_1 / (dt_1 + dt_2);dx = w1 * dx + w2 * imu_msg->linear_acceleration.x;dy = w1 * dy + w2 * imu_msg->linear_acceleration.y;dz = w1 * dz + w2 * imu_msg->linear_acceleration.z;rx = w1 * rx + w2 * imu_msg->angular_velocity.x;ry = w1 * ry + w2 * imu_msg->angular_velocity.y;rz = w1 * rz + w2 * imu_msg->angular_velocity.z;imuState imu_state_temp;imu_state_temp.timestamp = current_time;imu_state_temp.un_acc = eskf_pro->getRotation().toRotationMatrix() * (0.5 * (eskf_pro->getLastAcc() + Eigen::Vector3d(dx, dy, dz)) - eskf_pro->getBa());imu_state_temp.un_gyr = 0.5 * (eskf_pro->getLastGyr() + Eigen::Vector3d(rx, ry, rz)) - eskf_pro->getBg();dt_sum = dt_sum + dt_1;eskf_pro->predict(dt_1, Eigen::Vector3d(dx, dy, dz), Eigen::Vector3d(rx, ry, rz));imu_state_temp.trans = eskf_pro->getTranslation();imu_state_temp.quat = eskf_pro->getRotation();imu_state_temp.vel = eskf_pro->getVelocity();imu_states.push_back(imu_state_temp);}}

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

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)):

void eskfEstimator::predict(double dt_, const Eigen::Vector3d &acc_1_, const Eigen::Vector3d &gyr_1_)
{dt = dt_;acc_1 = acc_1_;gyr_1 = gyr_1_;Eigen::Vector3d avr_acc = 0.5 * (acc_0 + acc_1);Eigen::Vector3d avr_gyr = 0.5 * (gyr_0 + gyr_1);Eigen::Vector3d p_before = p;Eigen::Quaterniond q_before = q;Eigen::Vector3d v_before = v;Eigen::Vector3d ba_before = ba;Eigen::Vector3d bg_before = bg;Eigen::Vector3d g_before = g;Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + gyr_1) - bg;Eigen::Vector3d un_acc = 0.5 * (acc_0 + acc_1) - ba;//状态累积q = q * numType::so3ToQuat(un_gyr * dt);p = p + v * dt;v = v + q_before.toRotationMatrix() * un_acc * dt - g * dt;Eigen::Matrix3d R_omega_x, R_acc_x;//转为反对称矩阵R_omega_x << 0, -un_gyr(2), un_gyr(1), un_gyr(2), 0, -un_gyr(0), -un_gyr(1), un_gyr(0), 0;R_acc_x << 0, -un_acc(2), un_acc(1), un_acc(2), 0, -un_acc(0), -un_acc(1), un_acc(0), 0;Eigen::Matrix<double, 3, 2> B_x = numType::derivativeS2(g);Eigen::Matrix<double, 17, 17> F_x = Eigen::MatrixXd::Zero(17, 17);F_x.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();F_x.block<3, 3>(0, 6) = Eigen::Matrix3d::Identity() * dt;F_x.block<3, 3>(3, 3) = Eigen::Matrix3d::Identity() - R_omega_x * dt;F_x.block<3, 3>(3, 12) = - Eigen::Matrix3d::Identity() * dt;F_x.block<3, 3>(6, 3) = - q_before.toRotationMatrix() * R_acc_x * dt;F_x.block<3, 3>(6, 6) = Eigen::Matrix3d::Identity();F_x.block<3, 3>(6, 9) = - q_before.toRotationMatrix() * dt;F_x.block<3, 2>(6, 15) = numType::skewSymmetric(g) * B_x * dt;F_x.block<3, 3>(9, 9) = Eigen::Matrix3d::Identity();F_x.block<3, 3>(12, 12) = Eigen::Matrix3d::Identity();F_x.block<2, 2>(15, 15) = - 1.0 / (g.norm() * g.norm()) * B_x.transpose() * numType::skewSymmetric(g) * numType::skewSymmetric(g) * B_x;Eigen::Matrix<double, 17, 12> F_w = Eigen::MatrixXd::Zero(17, 12);F_w.block<3, 3>(6, 0) = - q_before.toRotationMatrix() * dt;F_w.block<3, 3>(3, 3) = - Eigen::Matrix3d::Identity() * dt;F_w.block<3, 3>(9, 6) = - Eigen::Matrix3d::Identity() * dt;F_w.block<3, 3>(12, 9) = - Eigen::Matrix3d::Identity() * dt;covariance = F_x * covariance * F_x.transpose() + F_w * noise * F_w.transpose();acc_0 = acc_1;gyr_0 = gyr_1;
}

在预测过程中，首先获取本次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})$

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

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

$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读数后进入处理函数：


void lioOptimization::process(std::vector<point3D> &cut_sweep, double timestamp_begin, double timestamp_offset)
{state *cur_state = new state();//p,v,q,ba,bgstateInitialization(cur_state);std::vector<point3D> const_frame;const_frame.insert(const_frame.end(), cut_sweep.begin(), cut_sweep.end());cloudFrame *p_frame = buildFrame(const_frame, cur_state, timestamp_begin, timestamp_offset);optimizeSummary summary = stateEstimation(p_frame);std::cout << "after solution: " << std::endl;std::cout << "rotation: " << p_frame->p_state->rotation.x() << " " << p_frame->p_state->rotation.y() << " " << p_frame->p_state->rotation.z() << " " << p_frame->p_state->rotation.w() << std::endl;std::cout << "translation: " << p_frame->p_state->translation.x() << " " << p_frame->p_state->translation.y() << " " << p_frame->p_state->translation.z() << std::endl;std::cout << "gravity = " << G.x() << " " << G.y() << " " << G.z() << std::endl;dt_sum = 0;publish_odometry(pub_odom, p_frame);publish_path(pub_path, p_frame);   if(debug_output){pcl::PointCloud<pcl::PointXYZINormal>::Ptr p_cloud_temp;p_cloud_temp.reset(new pcl::PointCloud<pcl::PointXYZINormal>());point3DtoPCL(p_frame->point_frame, p_cloud_temp);std::string pcd_path(output_path + "/cloud_frame/" + std::to_string(index_frame) + std::string(".pcd"));saveCutCloud(pcd_path, p_cloud_temp);}int num_remove = 0;if (initial_flag){if (index_frame > sweep_cut_num && index_frame % sweep_cut_num == 0){while (all_cloud_frame.size() > std::max(2, sweep_cut_num)){recordSinglePose(all_cloud_frame[0]);all_cloud_frame[0]->release();all_cloud_frame.erase(all_cloud_frame.begin());num_remove++;}assert(all_cloud_frame.size() == std::max(2, sweep_cut_num));}}else{while (all_cloud_frame.size() > options.num_for_initialization){recordSinglePose(all_cloud_frame[0]);all_cloud_frame[0]->release();all_cloud_frame.erase(all_cloud_frame.begin());num_remove++;}}for(int i = 0; i < all_cloud_frame.size(); i++)all_cloud_frame[i]->id = all_cloud_frame[i]->id - num_remove;while (v_cut_sweep.size() > sweep_cut_num - 1){std::vector<point3D>().swap(v_cut_sweep.front());assert(v_cut_sweep.front().size() == 0);v_cut_sweep.erase(v_cut_sweep.begin());}
}

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

//状态初始化，1：匀速模型；2：IMU
void lioOptimization::stateInitialization(state *cur_state)
{if (index_frame <= sweep_cut_num + 1){cur_state->rotation = Eigen::Quaterniond::Identity();cur_state->translation = Eigen::Vector3d::Zero();}else if (index_frame == sweep_cut_num + 2){   //匀速模型初始化if (options.initialization == INIT_CONSTANT_VELOCITY){//q(k) = [q(k-1) * q(k-2)^-1] * q(k-1)Eigen::Quaterniond q_next_end = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation * all_cloud_frame[all_cloud_frame.size() - 2]->p_state->rotation.inverse() * all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation;//t(k) = t(k-1) + [q(k-1) * q(k-2)^-1] * [t(k-1) - t(k-2)]Eigen::Vector3d t_next_end = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation + all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation * all_cloud_frame[all_cloud_frame.size() - 2]->p_state->rotation.inverse() * (all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation - all_cloud_frame[all_cloud_frame.size() - 2]->p_state->translation);cur_state->rotation = q_next_end;cur_state->translation = t_next_end;}//IMU初始化else if (options.initialization == INIT_IMU){if (initial_flag){cur_state->rotation = eskf_pro->getRotation();cur_state->translation = eskf_pro->getTranslation();}else{   //q(k) = [q(k-1) * q(k-2)^-1] * q(k-1)Eigen::Quaterniond q_next_end = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation * all_cloud_frame[all_cloud_frame.size() - 2]->p_state->rotation.inverse() * all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation;//t(k) = t(k-1) + [q(k-1) * q(k-2)^-1] * [t(k-1) - t(k-2)]Eigen::Vector3d t_next_end = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation + all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation * all_cloud_frame[all_cloud_frame.size() - 2]->p_state->rotation.inverse() * (all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation - all_cloud_frame[all_cloud_frame.size() - 2]->p_state->translation);cur_state->rotation = q_next_end;cur_state->translation = t_next_end;}}else{cur_state->rotation = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation;cur_state->translation = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation;}}else{if (options.initialization == INIT_CONSTANT_VELOCITY){Eigen::Quaterniond q_next_end = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation * all_cloud_frame[all_cloud_frame.size() - 2]->p_state->rotation.inverse() * all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation;Eigen::Vector3d t_next_end = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation + all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation * all_cloud_frame[all_cloud_frame.size() - 2]->p_state->rotation.inverse() * (all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation - all_cloud_frame[all_cloud_frame.size() - 2]->p_state->translation);cur_state->rotation = q_next_end;cur_state->translation = t_next_end;}else if (options.initialization == INIT_IMU){if (initial_flag){cur_state->rotation = eskf_pro->getRotation();cur_state->translation = eskf_pro->getTranslation();}else{Eigen::Quaterniond q_next_end = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation * all_cloud_frame[all_cloud_frame.size() - 2]->p_state->rotation.inverse() * all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation;Eigen::Vector3d t_next_end = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation + all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation * all_cloud_frame[all_cloud_frame.size() - 2]->p_state->rotation.inverse() * (all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation - all_cloud_frame[all_cloud_frame.size() - 2]->p_state->translation);cur_state->rotation = q_next_end;cur_state->translation = t_next_end;}}else{cur_state->rotation = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->rotation;cur_state->translation = all_cloud_frame[all_cloud_frame.size() - 1]->p_state->translation;}}
}

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

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

optimizeSummary lioOptimization::stateEstimation(cloudFrame *p_frame)
{icpOptions optimize_options = options.optimize_options;const double kSizeVoxelInitSample = options.voxel_size;//1.5const double kSizeVoxelMap = optimize_options.size_voxel_map;//1.0const double kMinDistancePoints = options.min_distance_points;//0.1const int kMaxNumPointsInVoxel = options.max_num_points_in_voxel;//20optimizeSummary optimize_summary;if(p_frame->frame_id > sweep_cut_num){bool good_enough_registration = false;double sample_voxel_size = p_frame->frame_id < options.init_num_frames ? options.init_sample_voxel_size : options.sample_voxel_size;double min_voxel_size = std::min(options.init_voxel_size, options.voxel_size);optimize_summary = optimize(p_frame, optimize_options, sample_voxel_size);if(!optimize_summary.success){return optimize_summary;}}else{p_frame->p_state->translation = eskf_pro->getTranslation();p_frame->p_state->rotation = eskf_pro->getRotation();p_frame->p_state->velocity = eskf_pro->getVelocity();p_frame->p_state->ba = eskf_pro->getBa();p_frame->p_state->bg = eskf_pro->getBg();G = eskf_pro->getGravity();G_norm = G.norm();}//将p_frame点云添加到地图addPointsToMap(voxel_map, p_frame, kSizeVoxelMap, kMaxNumPointsInVoxel, kMinDistancePoints);const double kMaxDistance = options.max_distance;const Eigen::Vector3d location = p_frame->p_state->translation;removePointsFarFromLocation(voxel_map, location, kMaxDistance);return optimize_summary;
}

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

optimizeSummary lioOptimization::optimize(cloudFrame *p_frame, const icpOptions &cur_icp_options, double sample_voxel_size)
{std::vector<point3D> keypoints;//从point_frame中采样关键点keypoints(雷达坐标系)gridSampling(p_frame->point_frame, keypoints, sample_voxel_size);optimizeSummary optimize_summary;optimize_summary = updateIEKF(cur_icp_options, voxel_map, keypoints, p_frame);if (!optimize_summary.success) {return optimize_summary;}Eigen::Quaterniond q_end = p_frame->p_state->rotation;Eigen::Vector3d t_end = p_frame->p_state->translation;//将所有雷达坐标系下的点转到世界坐标系for (auto &point_temp: p_frame->point_frame) {transformPoint(point_temp, q_end, t_end, R_imu_lidar, t_imu_lidar);}return optimize_summary;
}

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

optimizeSummary lioOptimization::updateIEKF(const icpOptions &cur_icp_options, const voxelHashMap &voxel_map_temp, std::vector<point3D> &keypoints, cloudFrame *p_frame)
{    int max_num_iter = p_frame->frame_id < cur_icp_options.init_num_frames ? std::max(15, cur_icp_options.num_iters_icp) : cur_icp_options.num_iters_icp;std::cout<<"@@@max_num_iter:"<<max_num_iter<<std::endl;//begin15 later5//获取预测状态变量Eigen::Vector3d p_predict = eskf_pro->getTranslation();Eigen::Quaterniond q_predict = eskf_pro->getRotation();Eigen::Vector3d v_predict = eskf_pro->getVelocity();Eigen::Vector3d ba_predict = eskf_pro->getBa();Eigen::Vector3d bg_predict = eskf_pro->getBg();Eigen::Vector3d g_predict = eskf_pro->getGravity();optimizeSummary summary;for (int i = -1; i < max_num_iter; i++){std::vector<planeParam> plane_residuals;double loss_old = 0.0;//构建点到面残差summary = buildPlaneResiduals(cur_icp_options, voxel_map_temp, keypoints, plane_residuals, p_frame, loss_old);if (summary.success == false)return summary;int num_plane_residuals = plane_residuals.size();Eigen::Matrix<double, Eigen::Dynamic, 6> H_x;Eigen::Matrix<double, Eigen::Dynamic, 1> h; //H_x:距离残差对状态变量的雅可比H_x.resize(num_plane_residuals, 6);//h:距离残差h.resize(num_plane_residuals, 1);for (int i = 0; i < num_plane_residuals; i++){H_x.block<1, 6>(i, 0) = plane_residuals[i].jacobians;h.block<1, 1>(i, 0) = Eigen::Matrix<double, 1, 1>(plane_residuals[i].distance * plane_residuals[i].weight);}//计算误差状态变量Eigen::Vector3d d_p = eskf_pro->getTranslation() - p_predict;Eigen::Quaterniond d_q = q_predict.inverse() * eskf_pro->getRotation();Eigen::Vector3d d_so3 = numType::quatToSo3(d_q);Eigen::Vector3d d_v = eskf_pro->getVelocity() - v_predict;Eigen::Vector3d d_ba = eskf_pro->getBa() - ba_predict;Eigen::Vector3d d_bg = eskf_pro->getBg() - bg_predict;Eigen::Vector3d g = eskf_pro->getGravity();Eigen::Vector3d g_predict_normalize = g_predict;Eigen::Vector3d g_normalize = g;g_predict_normalize.normalize();g_normalize.normalize();//归一化后的重力向量的叉乘积Eigen::Vector3d cross = g_predict_normalize.cross(g_normalize);//归一化后的重力向量的点乘积double dot = g_predict_normalize.dot(g_normalize);Eigen::Matrix3d R_dg;//根据点乘积的值，计算旋转矩阵R_dg。如果点乘积接近1，矩阵设为单位矩阵。否则，使用叉乘积计算一个Skew对称矩阵，并使用Rodrigues'公式计算旋转矩阵if (fabs(1.0 - dot) < 1e-6)R_dg = Eigen::Matrix3d::Identity();else{Eigen::Matrix3d skew = numType::skewSymmetric(cross);R_dg = Eigen::Matrix3d::Identity() + skew + skew * skew * (1.0 - dot) / (cross(0) * cross(0) + cross(1) * cross(1) + cross(2) * cross(2));}//SO3旋转 δθ_dgEigen::Vector3d so3_dg = numType::rotationToSo3(R_dg);//计算导数矩阵BEigen::Matrix<double, 3, 2> B_x_predict = numType::derivativeS2(g_predict);//B^T * δθ_dgEigen::Vector2d d_g = B_x_predict.transpose() * so3_dg;//17维误差状态变量Eigen::Matrix<double, 17, 1> d_x;d_x.head<3>() = d_p;d_x.segment<3>(3) = d_so3;d_x.segment<3>(6) = d_v;d_x.segment<3>(9) = d_ba;d_x.segment<3>(12) = d_bg;d_x.tail<2>() = d_g;//I - 1/2 * δθ （3*3）Eigen::Matrix3d J_k_so3 = Eigen::Matrix3d::Identity() - 0.5 * numType::skewSymmetric(d_so3);//J0_gn = I + 1/2 * B^T * δθ_dg * B （2*2）Eigen::Matrix2d J_k_s2 = Eigen::Matrix2d::Identity() + 0.5 * B_x_predict.transpose() * numType::skewSymmetric(so3_dg) * B_x_predict;//整个操作就是计算J0_n * dxEigen::Matrix<double, 17, 1> d_x_new = d_x;//(I - 1/2 * δθ) * δθ 在旋转误差状态左乘了(I - 1/2 * δθ)d_x_new.segment<3>(3) = J_k_so3 * d_so3;//J0_gn * (B^T * δθ_dg) 在重力状态分量上左乘了J0_gn：(I + 1/2 * B^T * δθ_dg * B)d_x_new.segment<2>(15) = J_k_s2 * d_g;Eigen::Matrix<double, 17, 17> covariance = eskf_pro->getCovariance();//计算J0_n * P * J0_n^Tfor (int j = 0; j < covariance.cols(); j++)covariance.block<3, 1>(3, j) = J_k_so3 * covariance.block<3, 1>(3, j);for (int j = 0; j < covariance.cols(); j++)covariance.block<2, 1>(15, j) = J_k_s2 * covariance.block<2, 1>(15, j);for (int j = 0; j < covariance.rows(); j++)covariance.block<1, 3>(j, 3) = covariance.block<1, 3>(j, 3) * J_k_so3.transpose();for (int j = 0; j < covariance.rows(); j++)covariance.block<1, 2>(j, 15) = covariance.block<1, 2>(j, 15) * J_k_s2.transpose();//(J0_n * (P/V) * J0_n^T)^-1Eigen::Matrix<double, 17, 17> temp = (covariance/laser_point_cov).inverse();//H^T * HEigen::Matrix<double, 6, 6> HTH = H_x.transpose() * H_x;//H^T * H + (J0_n * (P/V) * J0_n^T)^-1temp.block<6, 6>(0, 0) += HTH;//(H^T * V^-1 * H + (J0_n * (P/V) * J0_n^T)^-1)^-1Eigen::Matrix<double, 17, 17> temp_inv = temp.inverse();//计算KhEigen::Matrix<double, 17, 1> K_h = temp_inv.block<17, 6>(0, 0) * H_x.transpose() * h;Eigen::Matrix<double, 17, 17> K_x = Eigen::Matrix<double, 17, 17>::Zero();//计算KHK_x.block<17, 6>(0, 0) = temp_inv.block<17, 6>(0, 0) * HTH;//dx = -Kh - (I - KH) * J0_n * dxd_x = - K_h + (K_x - Eigen::Matrix<double, 17, 17>::Identity()) * d_x_new;Eigen::Vector3d g_before = eskf_pro->getGravity();if ((d_x.head<3>()).norm() > 100.0 || AngularDistance(d_x.segment<3>(3)) > 100.0){continue;}eskf_pro->observe(d_x);//状态更新p_frame->p_state->translation = eskf_pro->getTranslation();p_frame->p_state->rotation = eskf_pro->getRotation();p_frame->p_state->velocity = eskf_pro->getVelocity();p_frame->p_state->ba = eskf_pro->getBa();p_frame->p_state->bg = eskf_pro->getBg();G = eskf_pro->getGravity();G_norm = G.norm();bool converage = false;if (p_frame->frame_id > sweep_cut_num &&        (d_x.head<3>()).norm() < cur_icp_options.threshold_translation_norm && //0.001AngularDistance(d_x.segment<3>(3)) < cur_icp_options.threshold_orientation_norm) //0.0001{converage = true;}//协方差更新if (converage || i == max_num_iter - 1){Eigen::Matrix<double, 17, 17> covariance_new = covariance;Eigen::Matrix<double, 3, 2> B_x_before = numType::derivativeS2(g_before);J_k_so3 = Eigen::Matrix3d::Identity() - 0.5 * numType::skewSymmetric(d_x.segment<3>(3));J_k_s2 = Eigen::Matrix2d::Identity() + 0.5 * B_x_before.transpose() * numType::skewSymmetric(B_x_before * d_x.tail<2>()) * B_x_before;for (int j = 0; j < covariance.cols(); j++)covariance_new.block<3, 1>(3, j) = J_k_so3 * covariance.block<3, 1>(3, j);for (int j = 0; j < covariance.cols(); j++)covariance_new.block<2, 1>(15, j) = J_k_s2 * covariance.block<2, 1>(15, j);for (int j = 0; j < covariance.rows(); j++){covariance_new.block<1, 3>(j, 3) = covariance.block<1, 3>(j, 3) * J_k_so3.transpose();covariance.block<1, 3>(j, 3) = covariance.block<1, 3>(j, 3) * J_k_so3.transpose();}for (int j = 0; j < covariance.rows(); j++){covariance_new.block<1, 2>(j, 15) = covariance.block<1, 2>(j, 15) * J_k_s2.transpose();covariance.block<1, 2>(j, 15) = covariance.block<1, 2>(j, 15) * J_k_s2.transpose();}for (int j = 0; j < 6; j++)K_x.block<3, 1>(3, j) = J_k_so3 * K_x.block<3, 1>(3, j);for (int j = 0; j < 6; j++)K_x.block<2, 1>(15, j) = J_k_s2 * K_x.block<2, 1>(15, j);covariance = covariance_new - K_x.block<17, 6>(0, 0) * covariance.block<6, 17>(0, 0);eskf_pro->setCovariance(covariance);break;}}return summary;
}

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

1）计算点到面残差：