0. 简介

上一节我们将while内部的IKD-Tree部分全部讲完，下面将是最后一部分，关于后端优化更新的部分。

1. 迭代更新

最后一块主要做的就是，拿当前帧与IKD-Tree建立的地图算出的残差，然后去计算更新自己的位置，并将更新后的结果通过map_incremental函数插入到由ikd-Tree表示的映射中。

// 外参，旋转矩阵转欧拉角V3D ext_euler = SO3ToEuler(state_point.offset_R_L_I);fout_pre << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_point.pos.transpose() << " " << ext_euler.transpose() << " " << state_point.offset_T_L_I.transpose() << " " << state_point.vel.transpose()<< " " << state_point.bg.transpose() << " " << state_point.ba.transpose() << " " << state_point.grav << endl; //输出预测的结果if (0) // If you need to see map point, change to "if(1)"{// 释放PCL_Storage的内存PointVector().swap(ikdtree.PCL_Storage);// 把树展平用于展示ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD);featsFromMap->clear();featsFromMap->points = ikdtree.PCL_Storage;}pointSearchInd_surf.resize(feats_down_size); //搜索索引Nearest_Points.resize(feats_down_size);      //将降采样处理后的点云用于搜索最近点int rematch_num = 0;bool nearest_search_en = true; //t2 = omp_get_wtime();/*** 迭代状态估计 ***/double t_update_start = omp_get_wtime();double solve_H_time = 0;//迭代卡尔曼滤波更新，更新地图信息kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time);state_point = kf.get_x();euler_cur = SO3ToEuler(state_point.rot);pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;geoQuat.x = state_point.rot.coeffs()[0];geoQuat.y = state_point.rot.coeffs()[1];geoQuat.z = state_point.rot.coeffs()[2];geoQuat.w = state_point.rot.coeffs()[3];double t_update_end = omp_get_wtime();/******* 发布里程计 *******/publish_odometry(pubOdomAftMapped);/*** 向映射kdtree添加特性点 ***/t3 = omp_get_wtime();map_incremental();t5 = omp_get_wtime();/******* 发布轨迹和点 *******/publish_path(pubPath);if (scan_pub_en || pcd_save_en)publish_frame_world(pubLaserCloudFull);if (scan_pub_en && scan_body_pub_en){publish_frame_body(pubLaserCloudFull_body);publish_frame_lidar(pubLaserCloudFull_lidar);}// publish_effect_world(pubLaserCloudEffect);// publish_map(pubLaserCloudMap);/*** Debug 参数 ***/if (runtime_pos_log){frame_num++;kdtree_size_end = ikdtree.size();aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num;aver_time_icp = aver_time_icp * (frame_num - 1) / frame_num + (t_update_end - t_update_start) / frame_num;aver_time_match = aver_time_match * (frame_num - 1) / frame_num + (match_time) / frame_num;aver_time_incre = aver_time_incre * (frame_num - 1) / frame_num + (kdtree_incremental_time) / frame_num;aver_time_solve = aver_time_solve * (frame_num - 1) / frame_num + (solve_time + solve_H_time) / frame_num;aver_time_const_H_time = aver_time_const_H_time * (frame_num - 1) / frame_num + solve_time / frame_num;T1[time_log_counter] = Measures.lidar_beg_time;s_plot[time_log_counter] = t5 - t0;                         //整个流程总时间s_plot2[time_log_counter] = feats_undistort->points.size(); //特征点数量s_plot3[time_log_counter] = kdtree_incremental_time;        // kdtree增量时间s_plot4[time_log_counter] = kdtree_search_time;             // kdtree搜索耗时s_plot5[time_log_counter] = kdtree_delete_counter;          // kdtree删除点数量s_plot6[time_log_counter] = kdtree_delete_time;             // kdtree删除耗时s_plot7[time_log_counter] = kdtree_size_st;                 // kdtree初始大小s_plot8[time_log_counter] = kdtree_size_end;                // kdtree结束大小s_plot9[time_log_counter] = aver_time_consu;                //平均消耗时间s_plot10[time_log_counter] = add_point_size;                //添加点数量time_log_counter++;printf("[ mapping ]: time: IMU + Map + Input Downsample: %0.6f ave match: %0.6f ave solve: %0.6f  ave ICP: %0.6f  map incre: %0.6f ave total: %0.6f icp: %0.6f construct H: %0.6f \n", t1 - t0, aver_time_match, aver_time_solve, t3 - t1, t5 - t3, aver_time_consu, aver_time_icp, aver_time_const_H_time);ext_euler = SO3ToEuler(state_point.offset_R_L_I);fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_point.pos.transpose() << " " << ext_euler.transpose() << " " << state_point.offset_T_L_I.transpose() << " " << state_point.vel.transpose()<< " " << state_point.bg.transpose() << " " << state_point.ba.transpose() << " " << state_point.grav << " " << feats_undistort->points.size() << endl;dump_lio_state_to_log(fp);}}status = ros::ok();rate.sleep();}/**************** save map ****************//* 1. make sure you have enough memories/* 2. pcd save will largely influence the real-time performences **/if (pcl_wait_save->size() > 0 && pcd_save_en){string file_name = string("scans.pcd");string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name);pcl::PCDWriter pcd_writer;cout << "current scan saved to /PCD/" << file_name << endl;pcd_writer.writeBinary(all_points_dir, *pcl_wait_save);}fout_out.close();fout_pre.close();if (runtime_pos_log){vector<double> t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7;FILE *fp2;string log_dir = root_dir + "/Log/fast_lio_time_log.csv";fp2 = fopen(log_dir.c_str(), "w");fprintf(fp2, "time_stamp, total time, scan point size, incremental time, search time, delete size, delete time, tree size st, tree size end, add point size, preprocess time\n");for (int i = 0; i < time_log_counter; i++){fprintf(fp2, "%0.8f,%0.8f,%d,%0.8f,%0.8f,%d,%0.8f,%d,%d,%d,%0.8f\n", T1[i], s_plot[i], int(s_plot2[i]), s_plot3[i], s_plot4[i], int(s_plot5[i]), s_plot6[i], int(s_plot7[i]), int(s_plot8[i]), int(s_plot10[i]), s_plot11[i]);t.push_back(T1[i]);s_vec.push_back(s_plot9[i]);s_vec2.push_back(s_plot3[i] + s_plot6[i]);s_vec3.push_back(s_plot4[i]);s_vec5.push_back(s_plot[i]);}fclose(fp2);}return 0;
}

前面获取的传播状态xk^\hat{x_k}xk^和协方差Pk^\hat{P_k}Pk^对未知状态xkx_kxk施加了一个先验高斯分布。具体来说，Pk^\hat{P_k}Pk^表示以下误差状态的协方差:

式中，Jκ为(x^kπ田x^kκ)日x^k( \widehat {x}_ {k}^ {\pi } 田 \widehat {x}_ {k}^ {\kappa } )日 \widehat {x} k(xkπ田xkκ)日xk对x^kk\hat{x}^k_kx^kk=0处的偏微分。

式中，A(.)−1A(.)^{-1}A(.)−1 定义于公式(6)，如下。

如下的δGθIkδ^Gθ_{I_k}δGθIk和δIθLkδ^Iθ{L_k}δIθLk分别为IMU的姿态和转动的误差状态。

对于第一次迭代（以拓展的卡尔曼滤波器为例），有 xkκ^=xk\hat{x^κ_k} =x_kxkκ^=xk, Jκ=IJ_κ =IJκ=I。

除了先验分布外，我们也有一个源于测量(8)的状态分布：

结合(10)的先验分布和(12)的测量模型，可得到状态xkx_kxk的后验分布，其等价表示为xkkx^k_kxkk及其最大后验估计:

式中，有∣∣x∣∣M2=xTM−1x||x||^2_M = x^TM^{−1} x∣∣x∣∣M2=xTM−1x。该最大后验估计问题可由下面的迭代卡尔曼滤波器解决：

需注意，卡尔曼增益K计算需要对状态维数矩阵求逆，而不是在之前的工作中使用的测量维数矩阵。上述过程将重复进行，直到收敛(即∣∣xκ(k+1)日xkκ∣∣<||x_κ^{(k+1)} 日 x^κ_k||<∣∣xκ(k+1)日xkκ∣∣<无穷小)。收敛后的最优状态和协方差估计为:

通过状态更新xkx_kxk，第k次扫描中的每个LiDAR点(Lpj)将通过(16)被转换到全局框架:

转换后的LiDAR点{Gp¯j}将被插入到由ikd-Tree表示的映射中。我们的状态估计在算法1中进行了总结。

…详情请参照古月居

FAST-LIO2代码解析(六)相关推荐

视觉SLAM开源算法ORB-SLAM3 原理与代码解析
来源:深蓝学院,文稿整理者:何常鑫,审核&修改:刘国庆本文总结于上交感知与导航研究所科研助理--刘国庆关于[视觉SLAM开源算法ORB-SLAM3 原理与代码解析]的公开课. ORB-SLA ...
Linux-0.00 代码解析（四）
Linux 0.00 的编译.运行.源码下载: http://blog.csdn.net/longintchar/article/details/78757065 Linux 0.00 Makefil ...
iOS 编写高质量Objective-C代码（六）
级别: ★★☆☆☆ 标签:「iOS」「Block」「Objective-C」作者: MrLiuQ 审校: QiShare团队前言: 这几篇文章是小编在钻研<Effective Objecti ...
Celery 源码解析六：Events 的实现
序列文章: Celery 源码解析一:Worker 启动流程概述 Celery 源码解析二:Worker 的执行引擎 Celery 源码解析三: Task 对象的实现 Celery 源码解析四: 定时 ...
Mina Kimchi SNARK 代码解析
1. 引言 Mina系列博客有: Mina概览 Mina的支付流程 Mina的zkApp Mina中的Pasta(Pallas和Vesta)曲线 Mina中的Schnorr signature Min ...
Compact Multi-Signatures for Smaller Blockchains代码解析
1. 引言 Boneh等人2018年论文<Compact Multi-Signatures for Smaller Blockchains>,论文解读参见博客 Compact Multi- ...
STARK中的FRI代码解析
1. 引言前序博客有: STARK入门知识 STARKs and STARK VM: Proofs of Computational Integrity Fast Reed-Solomon Inte ...
番外篇15：libevent简单理解（附libevent官方代码解析，和跨平台服务器、客户端链接代码）
文章目录一.事件event和事件管理器event_base介绍二.libevent流程简介(注册->检测->分派) 三.libevent的好处四.代码比较 4.1 原来reactor ...
[从零手写VIO|第五节]——后端优化实践——单目BA求解代码解析
长篇警告⚠⚠⚠ 目录 solver 全流程回顾 Solver三要素 Solver求解中的疑问核心问题代码解析 1. TestMonoBA.cpp 2. 后端部分: 2.1 顶点 2.2 边(残差) ...

FAST-LIO2代码解析(六)

0. 简介

1. 迭代更新

…详情请参照古月居

FAST-LIO2代码解析(六)相关推荐

最新文章

热门文章