LiDar_IMU_init代码走读

主程序在laserMapping.cpp中。

1. main函数

1.1 初始化与读取参数

首先在main函数中读取了ros parameter，初始化一些变量。
订阅laser到standard_pcl_cbk或livox_pcl_cbk中处理（由于我使用VELO，只看了前者）。
订阅IMU消息到imu_cbk中处理。

1.2 signal(SIGINT, SigHandle);

获取键盘消息，比如ctrl+c后，在SigHadle中进行保存等操作。

1.3 以5000Hz运行while循环

如果SigHandle接收到了退出指令，break；
ros::spinOnce() 处理所有callback函数，即IMU和lidar数据。

sync_packages(Measures)

判断是否来了一帧lidar，同时有IMU数据，收集一次处理数据。
1） 如果lidar还未放入，把lidar buffer中的front放到MeasrueGroup中，并赋值lidar_end_time；
2） 把imu buffer中lidar_end_time（最后一个点对应的时间）之前的imu数据取出，放到MeasureGroup中。
如果lidar和IMU拼装完成，则进入如下处理：

• ImuProcess::Process 获取点云中最后一个点对应时刻到位姿，及运动畸变矫正后到点云
  • 如果IMu运动充分了，TODO 首先根据上次位姿、速度按照IMU推算到最后一个点对应时间处的位姿、速度，然后将本帧点云中的点补偿到最后一个点对应的时间处。
  • 否则，Forward_propagation_without_imu，预测+点云运动矫正，不使用IMU的角速度和加速度，使用状态量中的角速度和速度预测。
• lasermap_fov_segment
  • 判断是否需要更新local map范围，当点离边界的距离小于 1.5*det_range 时，更新local map 范围LocalMap_Points，同时更新需要删除的地图点边界到cub_needrm。
  • points_cache_collect 不知道什么作用？？？
  • ikdtree.Delete_Point_Boxes 删除边界外的local map点。
• 对畸变矫正后的scan点云进行降采样。
• 首次启动初始化ikdtree，把降采样点转换到word系下，build到ikdtree。然后continue
• ICP、Kalman迭代计算位姿，其中用到了多线程加速
  • 对降采样点进行多线程加速处理：
    • 转到world系坐标
    • 当nearest_search_en为true时，查找kdtree中5m内最近的5个点，如果5m内点数小于5，该点point_selected_surf为false，不进行后续处理。
    • 用最近的5个5m内的点构建平面约束，当点到平面的距离小于改点扫描距离的1/9时，改点与平面构成匹配关系，保存平面的法向量和点到平面距离。
  • 将建立平面约束的点保存到laserCloudOri（body坐标），平面法向量保存到corr_normvect。
  • 对所有约束点计算H矩阵、观测（点到平面误差），进行如下处理：
    • 将点的body坐标通过安装角和杆臂转换到相对IMU的坐标。
    • calcBodyVar 不知道干啥的，待研究 var = R* var * R^T
    • calculate the Measurement Jacobian matrix H，使用IMU与不使用的H计算方式不同。
  • 迭代一步Kalman滤波，更新state。
  • 当角度增量小于0.01度&&位置增量小于0.015cm时，flg_EKF_converged置为true。
  • flg_EKF_converged为true或（未进行重新匹配平面 且 本次迭代次数等于NUM_MAX_ITERATIONS - 2）时，再次重新匹配平面nearest_search_en=true。
  • 进行收敛判定和方差更新。
• 发布当前位置的odometry
• map_incremental 将scan降采样的点加入到ikdtree中。
  • 遍历该scan中点，用最新的pose将body下的lidar点转换到world系下。
  • 如果当前点附近有5m内的点，计算当前点对应体素的中心点坐标。最近的一个点不在当前体素内是，放入PointNoNeedDownsample；如果最近点在体素内，5m内的点数<5或5个点中有点到体素的中心距离>该点到体素中心的距离，则将该点放入PointToAdd。
  • 否则，将该点直接放入PointToAdd。
  • 将PointToAdd中的所有点加入到ikdtree中，（降采样的模式加入）。
  • 将PointNoNeedDownsample中的点直接加入到ikdtree中。

2. standard_pcl_cbk

• mtx_buffer上锁，scan_count计数。
• 如果lidar时间回退，清空lidar和time buffer。
• 如果lidar时间与最新IMU时间差1s以上，timediff_imu_wrt_lidar=last_timestamp_imu - last_timestamp_lidar，这个作为粗时间同步结果。需要与time_lag_IMU_wtr_lidar配合使用。
• 如果cut_frame为true，p_pre->process_cut_frame_pcl2
  • 将符合条件 i % point_filter_num == 0 && added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z > blind 的点放入pl_surf中。其中i为点added_pt对应的index。相当于把点云降采样后放到pl_surf中。
  • 将pl_surf中的点按照时间排序。
  • 对pl_surf中的点按照required_cut_num分组，分组后的点放到pcl_out。前20帧不进行分组。其中是否存在一个bug？
• 将分好组的pcl_out点云和各组点云的起始时间放到lidar_buffer和time_buffer中。
• mtx_buffer解锁。

3. imu_cbk

• mtx_buffer上锁。
• 计算前100个IMU的加计均值mean_acc和周期，频率小于100Hz报警。
• IMU时间补偿，msg->header.stamp.toSec() - timediff_imu_wrt_lidar - time_lag_IMU_wtr_lidar，其中time_lag_IMU_wtr_lidar为Init_LI估计出的时间同步结果？
• 如果IMU出现时间回退现象，清空imu_buffer和Init_LI中的IMU buffer。
• 将时间补偿后的imu message放入imu_buffer中。
• Init_LI->push_ALL_IMU_CalibState(msg, mean_acc_norm);把IMU信息传入初始化模块，其中mean_acc_norm相当于一个acc的放大系数（在函数内将加计数据/mean_acc_norm*9.81处理）。
• mtx_buffer解锁。