代码结构很清晰，从最外层看包含两个文件夹，一个是fast-lio,另外一个是加上scan-context的回环检测与位姿图优化。

fast-lio

主要是论文的fast-lio2论文的实现，包括前向处理和ikd-tree的实现

 1.先从cmakelist入手看代码结构：

#这是定义代码中的ROOT_DIR
add_definitions(-DROOT_DIR=\\"${CMAKE_CURRENT_SOURCE_DIR}/\\")#寻找机器的cpu核数，来选择是否采用多核计算，且留一个核的余量
if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" )include(ProcessorCount)ProcessorCount(N)message("Processer number:  ${N}")if(N GREATER 4)  add_definitions(-DMP_EN)add_definitions(-DMP_PROC_NUM=3)message("core for MP: 3")elseif(N GREATER 3)add_definitions(-DMP_EN)add_definitions(-DMP_PROC_NUM=2)message("core for MP: 2")else()add_definitions(-DMP_PROC_NUM=1)endif()
else()add_definitions(-DMP_PROC_NUM=1)
endif() #依赖openMP  PythonLibs  MATPLOTLIB_CPP_INCLUDE_DIRS绘图库#自定义了 Pose6D.msg
add_message_files(FILESPose6D.msg
)#主要程序是
src/laserMapping.cpp 
include/ikd-Tree/ikd_Tree.cpp 
src/preprocess.cpp

 Pose6D.msg：

雷达在IMU坐标系下的预积分值

float64   IMU 和 第一帧雷达点的时延
float64[3] acc       # the preintegrated total acceleration (global frame) at the Lidar origin
float64[3] gyr       # the unbiased angular velocity (body frame) at the Lidar origin
float64[3] vel       # the preintegrated velocity (global frame) at the Lidar origin
float64[3] pos       # the preintegrated position (global frame) at the Lidar origin
float64[9] rot       # the preintegrated rotation (global frame) at the Lidar origin

 主程序入口在src/laserMapping.cpp 中，其他的两个cpp以库的形式给它使用

main()程序流程：

ros节点初始化-》参数读取--》参数初始化、指针初始化---》读取的雷达和IMU外参矩阵---》IMU积分参数设置，如测量协方差 ----》设置卡尔曼滤波器的参数，如迭代精度设置、迭代次数，迭代卡尔曼滤波器模型等-----》日志记录初始化

1. 获取激光雷达类型之后，开始订阅standard_pcl_cbk() 、    imu_cbk()

time_buffer为基于激光时间戳的队列，安装激光时间进行处理

void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) //velodyne回调
{mtx_buffer.lock();scan_count ++;double preprocess_start_time = omp_get_wtime();//可以理解为当前时间戳if (msg->header.stamp.toSec() < last_timestamp_lidar)  //检测激光时间戳是否异常{ROS_ERROR("lidar loop back, clear buffer");lidar_buffer.clear();}PointCloudXYZI::Ptr  ptr(new PointCloudXYZI());p_pre->process(msg, ptr);      //激光雷达预处理，获得特征点云lidar_buffer.push_back(ptr);  //激光雷达预处理完的雷达数据time_buffer.push_back(msg->header.stamp.toSec());  //time_buffer是以激光雷达时间戳为基准的时间戳队列last_timestamp_lidar = msg->header.stamp.toSec();s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; //用于绘图显示处理时间mtx_buffer.unlock();sig_buffer.notify_all();  //信号量的提示 唤醒线程
}void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in)
{publish_count ++;// cout<<"IMU got at: "<<msg_in->header.stamp.toSec()<<endl;sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in));if (abs(timediff_lidar_wrt_imu) > 0.1 && time_sync_en) //timediff_lidar_wrt_imu仅在使用lovix雷达时才会使用{msg->header.stamp = \\ros::Time().fromSec(timediff_lidar_wrt_imu + msg_in->header.stamp.toSec());}double timestamp = msg->header.stamp.toSec(); //经过补偿的IMU时间戳，如果是lovix雷达才需要补偿，其他不需要mtx_buffer.lock();if (timestamp < last_timestamp_imu)  //校验IMU时间戳的一维性，检测跳变{ROS_WARN("imu loop back, clear buffer");imu_buffer.clear();}last_timestamp_imu = timestamp;  //最新IMU的时间imu_buffer.push_back(msg);  //数据插入队列中mtx_buffer.unlock();sig_buffer.notify_all();  //有信号时，唤醒线程
}

       此次的激光点云回调会调用预处理类，获得特征点云的输出。

         然后开启ros的无限循环，当然，此处添加了信号处理，通常终端结束进程时是通过发送信号的，当收到信号时，唤醒所以线程。

2.这里需要先看测量量的定义：

包括了当前帧点云和imu数据队列

struct MeasureGroup     // Lidar data and imu dates for the curent process
{MeasureGroup(){lidar_beg_time = 0.0;this->lidar.reset(new PointCloudXYZI());};double lidar_beg_time;PointCloudXYZI::Ptr lidar;deque<sensor_msgs::Imu::ConstPtr> imu;
};

3.然后看数据同步：bool sync_packages(MeasureGroup &meas)

//这部分主要处理了buffer中的数据，将两帧激光雷达点云数据时间内的IMU数据从缓存队列中取出，进行时间对齐，并保存到meas中
bool sync_packages(MeasureGroup &meas)
{if (lidar_buffer.empty() || imu_buffer.empty()) {return false;}/*** push a lidar scan ***/if(!lidar_pushed)   //如果程序初始化时没指定，默认值是false， 是否已经将测量值插入雷达帧数据{meas.lidar = lidar_buffer.front();   //将雷达队列最前面的数据塞入测量值if(meas.lidar->points.size() <= 1)  //保证塞入的雷达数据点都是有效的{lidar_buffer.pop_front();return false;}meas.lidar_beg_time = time_buffer.front(); //雷达的时间按照time_buffer队首处理，因为它存的就是雷达的时间戳//雷达帧头的时间戳是帧头的时间戳，这和驱动有关系，通过公式推导该帧激光的帧尾时间戳lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000);lidar_pushed = true;  // 成功提取到lidar测量的标志}if (last_timestamp_imu < lidar_end_time) //如果最新的IMU时间戳都闭雷达帧尾的时间早，则这一帧不处理了{return false;}/*** push imu data, and pop from imu buffer ***/double imu_time = imu_buffer.front()->header.stamp.toSec(); //从最早的IMU队列开始，初始化imu_timemeas.imu.clear();while ((!imu_buffer.empty()) && (imu_time < lidar_end_time)){imu_time = imu_buffer.front()->header.stamp.toSec(); //从最早的IMU队列开始if(imu_time > lidar_end_time) break;     //没有跳出循环的话就会将IMU数据添加进去测量量meas.imu.push_back(imu_buffer.front());  imu_buffer.pop_front();  //弹出已经塞进测量量的IMU数据}//从这出来的,测量数据中包含了当前帧的激光数据， 当前帧帧尾结束前的新增IMU数据lidar_buffer.pop_front(); //处理过的数据出栈time_buffer.pop_front();lidar_pushed = false;  //又重新置位，这样下一帧雷达来了又可以刷新时间，获取点云帧头和帧尾的时间return true;
}

        这个同步是基于激光雷达的数据存入测量量，获得帧头和帧尾之间的IMU数据队列，存入测量量中。

4.上面用到了激光的预处理，这里先插播激光预处理的内容：

        通过实例 shared_ptr<Preprocess> p_pre(new Preprocess());进行预处理，预处理仅在激光回调中使用，激光回调前是读取参数设置预处理的参数。

preprocess.h/cpp

#define IS_VALID(a)  ((abs(a)>1e8) ? true : false)  //定义一个数字是否有效

//使用枚举变量描述激光的几个特征，

enum LID_TYPE{AVIA = 1, VELO16, OUST64}; //{1, 2, 3}

enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint};

enum Surround{Prev, Next};

enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind};

    //ROS循环的主要流程signal(SIGINT, SigHandle);ros::Rate rate(5000);bool status = ros::ok();while (status){if (flg_exit) break;ros::spinOnce();if(sync_packages(Measures)){if (flg_reset){ROS_WARN("reset when rosbag play back");p_imu->Reset();flg_reset = false;Measures.imu.clear();continue;}double t0,t1,t2,t3,t4,t5,match_start, solve_start, svd_time;match_time = 0;kdtree_search_time = 0.0;solve_time = 0;solve_const_H_time = 0;svd_time   = 0;t0 = omp_get_wtime();p_imu->Process(Measures, kf, feats_undistort);state_point = kf.get_x();pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I;if (feats_undistort->empty() || (feats_undistort == NULL)){first_lidar_time = Measures.lidar_beg_time;p_imu->first_lidar_time = first_lidar_time;// cout<<"FAST-LIO not ready"<<endl;continue;}flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? \\false : true;/*** Segment the map in lidar FOV ***/lasermap_fov_segment();/*** downsample the feature points in a scan ***/downSizeFilterSurf.setInputCloud(feats_undistort);downSizeFilterSurf.filter(*feats_down_body);t1 = omp_get_wtime();feats_down_size = feats_down_body->points.size();/*** initialize the map kdtree ***/if(ikdtree.Root_Node == nullptr){if(feats_down_size > 5){ikdtree.set_downsample_param(filter_size_map_min);feats_down_world->resize(feats_down_size);for(int i = 0; i < feats_down_size; i++){pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));}ikdtree.Build(feats_down_world->points);}continue;}int featsFromMapNum = ikdtree.validnum();kdtree_size_st = ikdtree.size();// cout<<"[ mapping ]: In num: "<<feats_undistort->points.size()<<" downsamp "<<feats_down_size<<" Map num: "<<featsFromMapNum<<"effect num:"<<effct_feat_num<<endl;/*** ICP and iterated Kalman filter update ***/normvec->resize(feats_down_size);feats_down_world->resize(feats_down_size);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)"{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();/*** iterated state estimation ***/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 *******/publish_odometry(pubOdomAftMapped);/*** add the feature points to map kdtree ***/t3 = omp_get_wtime();map_incremental();t5 = omp_get_wtime();/******* Publish points *******/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 variables ***/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;s_plot4[time_log_counter] = kdtree_search_time;s_plot5[time_log_counter] = kdtree_delete_counter;s_plot6[time_log_counter] = kdtree_delete_time;s_plot7[time_log_counter] = kdtree_size_st;s_plot8[time_log_counter] = kdtree_size_end;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();}