Loam_livox 代码阅读与总结

Loam_livox

https://github.com/hku-mars/loam_livox

Papper

Loam livox

Loam livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV

• 主要贡献:

  • 基于Loam算法适配 小视场角(FoV)和不规则采样的激光雷达 (Livox MID40 LiDAR为例)

    • 小视场角、非规则的扫描方式、非重复扫描、运动模糊

  • 非常有限视场角下的特征提取，鲁棒的野值剔除

  • 动态物体的过滤

  • 运动畸变的补偿

• 固态激光雷达 与传统机械雷达对比：

  以大疆投资的子公司生产的 Livox MID40 LiDar

  • 小视场，

    • 固态雷达：38.4度，特征更少，匹配性能下降，更容易受运动物体干扰
    • 传感机械雷达VLP-16: 360度

  • 不规则的扫描模式：

    • 固态激光雷达: 固态激光雷达是基于单激光束的连续扫描，图案是累积的，不规律的，MID-40是玫瑰状的扫描图案，相邻的两次扫描几乎完全不一样。
    • 传统机械雷达: 激光发射接收器在一行垂直排列，旋转的时候进行扫描，结果作为一组平行的环状扫描。角点可以通过对环形线上的点深度值插值得到，可以方便的进行特征提取。

  • 非重复扫描

    • 为了达到尽可能大的扫描覆盖率，激光点的连续扫描位置是不会与之前的轨迹重复的。

  • 运动模糊

    • 由于是基于单激光头的连续扫描，因此在同一帧里扫描的3D点是在不同的时刻采样的。而与此同时，帧内的雷达运动会使得点云发生畸变，从而引起运动模糊。

• 特征提取与筛选

  • 前端部分主要由特征点提取和筛选组成。特征点基于雷达的属性(比如激光斑大小，信噪比)将有效点定义为"good points"。

  • 3D 点的筛选

    • 计算激光雷达坐标下对应3D点$p=[x,y,z{]}^T$的特征，具体为：

      • 点的深度：$D(P)=\sqrt{(x^2+y^2+z^2)}$
      • 激光偏转角 $\varphi$，为x轴与激光束之间的夹角：$\varphi (P)={\tan }^{-1}(\sqrt{(y^2+z^2)}/x^2)$
      • 点的强度 I，``R` 是雷达返回的的反射率/强度，强度越低表示点越远或者物体的反射率越低：$I(P)=R/D(P{)}^2$
      • 入射角 $\theta$，为激光束和测量点附近平面的夹角： {} $\theta (P_b)={\cos }^{-1}\frac{(P_a-P_c)\cdot P_b}{|(P_a-P_c||P_b|}$

    • 为了提高定位和建图精度，将一下点剔除：

      • 剔除在Fov边缘位置的点(MID-40中偏转角 $\varphi >17$)。这些位置的扫描轨迹曲率较大，影响特征的提取可靠性
      • 剔除强度过大或者过小的点(对于MID-40：$I<7\times 10^{-3}orI>10^{-1}$)。信号强队太大容易导致接受端电路饱和或者受到干扰，导致探测精度下降，信号太弱则一般信噪比过低，也会探测精度下降。
      • 剔除入射角在$\theta <=5°or\theta >-175°$，在这些位置的点容易使得激光斑发生延长，探测距离是覆盖区域的平均值，因此导致探测精度下降。
      • 剔除隐藏在物体后面的点。这容易引起错误的边缘特征检测。基于下面公式判断点$P_e$是否是影藏点，$P_d$为激光扫描顺序上距离最近的另一个测量点：$|P_e-P_d|>=0.1and|P_e|>|P_d|$

  • 特征点提取

    • 基于上述有效点进行提取，与loam类似，通过点的平滑度来提取平面特征和边缘特征。由于使用的固态激光雷达FoV较小，因此引入雷达的反射率作为第4维度的的测量信息。
    • 如果一个3D点的反射率与相邻的点不同，将其也作为边缘点考虑(形状发生变化时会存在边缘点，同样材料属性不同也会因为反射率不同出现边缘点)。这在面对同时存在门和窗户的墙面等类似场景时可以起到积极的有益作用。

• 迭代优化：

  • 由于固态激光雷达的非重复扫描特性，提取的特征不能像传统Loam算法中那样进行正常跟踪。比如，即使激光雷达是静止不动的，则当前帧也与上一帧的扫描图案和特征点不一致。本文使用一种迭代姿态优化(Iterative pose optimization)方法来计算雷达的pose, 经过适当的优化，可以实现20Hz实时的里程计和建图。

  • 边缘点残差

    • ${\xi }_k$ 表示当前帧中所有边缘点特征的集合，${\xi }_m$表示地图中所有边缘点特征的集合。对于${\xi }_k$中的每一个点，在${\xi }_m$中查找距离最近的5个点。为了加快查找速度，将${\xi }_m$建立一个kd树，且kd树是当上一帧数据接收到并匹配成功后通过另外一个并行线程进行建立的。这能够保证当新的一帧数据到来时kd树能够立刻可用。
    • 文中查找的最近点数量为5。如果协方差矩阵的最大特征值比第二大特征值的3倍还要大，则认为这些查找到的最近点可以组成一条线，而且$P_w$ 应该也在这条线上。
    • 残差计算，面积法

  • 平面的残差

    • 与上述边缘点特征类似，同样基于当前帧中的平面点特征在地图中的平面点中查找对应最近的5个点。同样为了确保这些点在同一个平面上，需要计算这些点的协方差矩阵$\sum$，如果协方差矩阵的最小特征值的３倍比第二小特征值还要小，则计算当前帧中的此平面点到地图中这5个点组成的平面的距离。
    • 残差计算：体积法

  • 帧内运动补偿

    • 因为扫描的点是在雷达运动过程中不同的时间对应不同的pose下采样得到的，因此会存在运动模糊情况。一般有两种方法处理运动模糊：
    • 分段处理：作者将完整的一帧分为3个子帧序列，将这3个子帧分别与当时累积的相同的地图进行匹配。每一个子帧的时间间隔是原始帧的1/3。尽管方法比较简单，但是作者经过实验验证可用性极强，尤其是可以使用多核处理器并行处理多个子帧的时候。
    • 线性插值：原始Loam版本中就使用的线性插值方法。主要是根据上一帧的pose，当前帧的pose, 基于时间间隔进行插值。

  • 剔除外点，动态物体过滤

    • 为防止降低匹配精度，每次pose优化时，都重新查找地图中的最近点，并重新计算边缘点误差(6)和平面点误差(7)，然后将其增加到目标函数中（此时目标函数包含两次查找邻近点增加的残差）；
    • 小次数迭代(2次)后，重新计算每组误差，并移除最大残差的20%点作为外点
    • 最后进行完整的pose迭代优化，直到到达设置的迭代次数最大值或者目标函数收敛到一定的阈值，则pose优化结束。

• 结果评估

  • 建图评估

    • 不用任何运动补偿时，在楼梯和栏杆处会比较模糊，而且空间尺度较大时会发生轨迹畸变。
    • 分段法和线性插值法均可有效处理运动模糊的情况。但是线性插值法无法比较好的解决长时间的漂移问题，这是因为数据采集时是手持设备进行采集，运动比较剧烈，简单的线性插值模型不能很好地进行运动预测。

  • 里程计评估

    • 文中使用GPS的定位结果对里程计的性能进行评估，两组数据的定位精度在0.41%和0.65%。
    • 使用运动捕捉系统进行了旋转的评估，平均欧拉角误差<1.1°。

  • 评估性能

    • 文中使用A-Loam（两种方法处理运动模糊都是使用分段法）作为baseline, 进行性能评估，分别在不同的机器上进行评估。受益于并行化进行子帧配准，特征匹配，kd-tree的建立，文中算法比A-Loam的运行速度快2-3倍。
    • i7-9700k 35.68ms / 109.00ms
    • i7-8550U 54.60ms / 125.13ms

Loam loop closure

A fast, complete, point cloud based loop closure for LiDAR odometry and mapping

• 系统介绍：

  • 本篇论文相比较上一篇而言，最大的亮点是增加了2D直方图回环检测和后端优化了，不过很明显的是也无法保证实时性了。

  

  • 其回环检测实质是关键帧特征的匹配，而不是每接收一帧都进行回环检测，否则的话计算量太大。

• MAP AND CELL

  • cell

    • 一个cell 一个固定大小的立方体，由第一个点创建$P_i=[P_{ix},P_{iy},P_{iz}{]}^T$，在：

      $C_c=\left[\begin{array}{c}round(P_{ix}/S_x)\ast S_x+S_x/2\\ round(P_{iy}/S_y)\ast S_x+S_y/2\\ round(P_{iz}/S_z)\ast S_x+S_z/2\end{array}\right]$

    • 定义每个点的均值和协方差：该立方格中的多个点的

      • 均值： $C_u=\frac{1}{N}({\sum }_{i=1}^N{P}_i)$
      • 协方差： $C_{\Sigma }=\frac{1}{N-1}\left({\sum }_{i=1}^N(P_i-C_u)(P_i-C_u{)}^T\right)$

    • 立方体是固定的，并且不断添加新点，为了加快计算，给出递推形式：

      • 均值： $C_u=\frac{1}{N+1}(N{C}_u^{\prime }+P_{N+1})$
      • 协方差： $\begin{array}{c}{C}_{\Sigma }=\frac{1}{N}((N-1)C_{\Sigma }^{\prime }+(P_i-C_u^{\prime })(P_i-C_u^{\prime }{)}^T+(N+1)(C_u-C_u^{\prime })(C_u-C_u^{\prime }{)}^T\\ +2(C_u-C_u^{\prime })(P_{N+1}-C_u^{\prime }{)}^T)\end{array}$

  • Map

    • MAP包含了所有的CELL，为了快速查找CELL，MAP包含了一个哈希表和八叉树。主要介绍的就是如何把点加入哈希表和八叉树中。

• 具有旋转不变性的2D直方图

  • 闭环检测的主要思想是使用2D直方图近似的描述了关键帧
  • 对每个cell计算特征类型和方向 $c_d$
    • 有了方差矩阵以后，我们对其进行特征值分解，按照特征值大小顺序，得到${\lambda }_1⩾{\lambda }_2⩾{\lambda }_3$
    • 而每个cell的特征类型就是根据特征值的情况来定的：
      • 如果 ${\lambda }_2$ 远大于 ${\lambda }_3$，则认为是面特征，特征的方向是方差矩阵的第三列，也就是面的法向量
      • 如果不是面特征，且 ${\lambda }_1$ 远大于 ${\lambda }_2$ 则认为是线特征， 特征方向是方差矩阵的第一列，也就是线的方向
      • 不是以上两种，则木有特征
  • 计算旋转矩阵
    • 这里的旋转矩阵并不是点云位姿对应的旋转矩阵，而是 每个特征要给一个旋转矩阵，让特征按照这个矩阵旋转以后，能够使大多数线特征的方向，以及面特征边缘（面的边缘其实就是线）方向处于X轴上，而次 大多数在 Y 轴上。它的目的是为了使特征具有旋转不变性。
    • 由于平面特征比线特征更可靠（例如，平面特征的边缘被视为线特征），我们使用平面单元的特征方向来确定旋转矩阵R。
      • 与前几节类似，我们计算关键帧中所有平面特征方向的协方差：${\Sigma }_d={\sum }_{i=1}^N{c}_{i_d}{c}_{i_d}^T$
      • 特征值：${\lambda }_{d1}⩾{\lambda }_{d2}⩾{\lambda }_{d3}$
      • 特征向量：$V_d=\left[\begin{array}{ccc}{V}_{d1}& V_{d2}& V_{d3}\end{array}\right]$
      • 旋转矩阵：$R={\left[\begin{array}{ccc}{V}_{d1}& V_{d2}& V_{d1}\times V_{d2}\end{array}\right]}^T$
    • 在计算旋转矩阵 R 之后，我们将旋转变换应用于所有特征（平面和线）方向。
  • 2D 关键帧直方图
    • 利用关键帧中所有单元格的旋转不变特征方向，我们计算二维直方图如下：
    • 首先，对于给定的特征方向 $C_d=[c_{dx},c_{dy},c_{dz}]$，我们选择 $x$ 分量的方向，
      • 即$C_d=sign(c_{dx})\cdot C_d$
    • 然后，特征 欧拉角度方向计算
      • $\theta ={\sin }^{-1}(c_{dz})+90°\in [0,180]$
      • $\varphi ={\tan }^{-1}(c_{dy/c_{dx}})+90°\in [0,180]$
    • 直方图 $60\times 60$ 的矩阵（在pitch、yaw有3°的分辨率）

• 快速闭环检测

  • 闭环检测的过程
  • 两关键帧的相似度
    • $s(H_1,H_2)=\frac{{\sum }_I(H_1(I)-\overline{H_1})(H_2(I)-\overline{H_2})}{\sqrt{{\sum }_I(H_1(I)-\overline{H_1}{)}^2{\sum }_I(H_2(I)-\overline{H_2}{)}^2}}$
    • $\overline{H_k=\frac{1}{N}{\sum }_I{H}_k(I)}$
  • 地图对齐
  • 位姿图优化

ReadCamera

• function： 将 video 数据转为 Image数据，在通过 topic发出去

• 打开相机并发布数据（例如：笔记本的摄像头）：

  cv::VideoCapture cap(0); // open the default camera
  if (!cap.isOpened())     // check if we succeeded
  {
      ROS_ERROR("Open Camera error! exit node");
      return -1;
  }
      
  cap.set(CV_CAP_PROP_SETTINGS, 1); //opens camera properties dialog
  
  cap.set(CV_CAP_PROP_FRAME_WIDTH, 640);
  cap.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
  std::cout << "MJPG: " << cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G')) << std::endl;
  std::cout << "~~~ Read camera OK ~~~" << std::endl;
  cv::Mat frame;
  sensor_msgs::ImagePtr msg;
  for (;;)
  {
      cap >> frame; // get a new frame from camera
  
      // 0 表示绕 x 轴翻转。正值（例如 1）表示绕 y 轴翻转。负值（例如，-1）表示围绕两个轴翻转。
      //        cv::flip(frame, frame, 0);
      //        cv::flip(frame, frame, 1);
      msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", frame).toImageMsg();
      pub.publish(msg);
      msg->header.stamp = ros::Time::now();
      msg->header.frame_id = std::string("world");
      ros::spinOnce();
      std::this_thread::sleep_for(std::chrono::milliseconds(33));
  }
  

livox_scanRegistration

• 代码存在于：source/laser_feature_extractor.cpp

• 主函数 功能超级简单

  • 初始化ros节点：scanRegistration
  • 构造 Laser_feature 对象
  • ros::spin()

• 核心为类：Laser_feature

Laser_feature 构造

• 调用 init_ros_env() 即：初始化ros环境

init_ros_env()

• 创建句柄 ros::NodeHandle nh;
• 参数读取
  • 读取激光自身参数： init_livox_lidar_para(nh)
    • 读取激光类型参数：lidar_tpye_name，两种：livox，velodyne(默认)
    • 最小允许距离：m_livox_min_allow_dis，默认0.1
    • 最小sigma：m_livox_min_sigma，默认 7e-4
  • 特征提取参数：
    • 激光线束 m_laser_scan_number 16
    • 地图 平面特征 分辨率 m_plane_resolution, 0.8
    • 地图 线特征 分辨率 m_line_resolution, 0.8
    • 角点曲率：livox_corners, 0.05
    • 平面点曲率：livox_surface, 0.01
    • 最小可视角：minimum_view_angle, 10
  • common 参数：
    • 如果运动模糊：m_if_motion_deblur, 1
    • 分段计算：m_piecewise_number, 3
    • 里程计模型：m_odom_mode, 0
• 订阅激光数据：
  • 最多支持 3个激光雷达 m_maximum_input_lidar_pointcloud = 3
  • 每个激光话题：laser_points_ + std::to_string(i)
  • 回调函数：laserCloudHandler，带两个参数，数据 和 topic_name
• 发布话题：
  • 角点特征 + 平面特征 + 帅选后的好点

Velodyne 小知识

• 激光雷达数据并没有补偿畸变，所以直接用的话，可能里程计配准误差比较大。
• velodyne 雷达，扫描顺序是从上往下，顺时针扫描
• 每帧的扫描开始不一定是0°，结束角度也不一定是360°，每帧点的数量也是波动的。但基本都控制在0°附近，所以可以默认每一帧为0-360°

laserCloudHandler

• 1、数据前的转换

  • 首先确定当前数据属于哪个激光
    • 遍历所有激光名字，topic_name.compare，直到相同退出
    • 加check 校验未超出限制
  • 积累20帧数据后再处理，计算不过来，降频呀
  • 定义 每个scan的点云(多少线)、起始位置，终止位置
  • 将ros消息转换成pcl点云

• 2、点云的整理，分 livox 和 Velodyne 雷达

• 3、如果是 Livox雷达 ，计算特征并发布

  • 提取livox激光点云的边界和平面特征，并移除坏点extract_laser_features
  • 若为第一帧时，则发布第一帧激光时间
  • 若提取的花瓣少于5个，则return
  • 若发布 feature时
    • 将一个scan分为3段，减少 畸变(运动模糊)
    • 获得livox_corners（角点）, livox_surface（平面）, livox_full（好点）特征点
    • 发布特征点
      • 将所有雷达的特征点整合到一起
      • 只发布当前雷达的特征点

• 3、如果是Velodyne雷达, 其参考A-LOAM，计算特征并发布

  • 剔除nan的无效点 pcl::removeNaNFromPointCloud

  • 剔除最近的点 removeClosedPointCloud

  • 整理每个点，强度放水平+竖直信息：

    • velodyne是顺时针增大, 而坐标轴中的yaw是逆时针增加，基于几何关系算出

    • point.intensity = scanID + m_para_scanPeriod * relTime;

      即一个整数+一个小数，整数部分是线号，小数部分是该点的相对时间

  • 计算每个点的曲率

  • 去 遮挡点 和 直线与激光近似平行的点不要

  • 将scan平均分为6段，基于曲率排序，然后得到 角点和平面点

  • 发布

m_livox.extract_laser_features

• 数据当前时间确定
  • 若当前时间比上次最大时间小，则认为两次同一个时间，则赋值
  • 否则，认为一个新的数据
• clutter_size=projection_scan_3d_2d
• 提取特征 compute_features()
• 若花瓣簇为0，则直接返回
• 将点云分为不同的颜色，为rviz显示 split_laser_scan()

projection_scan_3d_2d

• 遍历 输入的点云，进行 数据赋值
  • 取每个点的下标，深度 赋值
  • 计算每个点的时间，初试时间+间隔
  • 赋值 上一帧最大时间戳 = 每个点的值
  • 判断点位置有inf，点剔除
  • 如果 x ==0时：
    • 若为第一个点时，点剔除
    • 否则，点剔除
  • 评估点：
    • 点深度过小，点剔除
    • 归一化点强度过小（点强度/距离），点剔除
  • 点位于视场角边界上，点剔除
  • 分割scans
    • 判断点是靠近花瓣中心，还是远离花瓣中心
      • 当前点与前一个点的距离，判断是大于0，还是小于0
    • 检测花瓣上的转折点，记录，并continue
• 计算每一个点的角度 scan_angle + pt_angle_index

compute_features

• 计算曲率
  • 找到该点的左右临近点，左右各2个
  • 计算曲率
• 基于曲率得到特征
  • 扫描平面与当前点夹角大于10，且曲率小于0.01，则认为是平面点
  • 扫描平面与当前点夹角大于10，且曲率大于0.05，则认为是角点

laserMapping

主函数：

• ros 节点初始化
• 定义 Laser_mapping 对象
• 创建 mapping_process 线程 process()

Laser_mapping 构造

• 初始化点云类型
• 初始化参数 init_parameters(m_ros_node_handle)
• 订阅topic：
  • 角点
  • 平面点
  • 整个点云
  • 第一帧时间戳
• 发布topic

callback

• 把三个回调函数中的laser_data合并起来，压入到

laserCloudCornerLastHandler

laserCloudSurfLastHandler

laserCloudFullResHandler

process

单独的一个std线程, 整个程序的入口

主要流程：

• 定义两个线程，std::future<void>(std::async(std::launch::async,&callback))
  • m_service_pub_surround_pts
  • m_service_loop_detection
• while ros::ok 循环，200hz
  • 取所处理数据
    • 若m_queue_avail_data中无数据时，0.1ms的周期循环等待查询
    • 若 m_queue_avail_data中数据超过最大个数时，弹出最早的
    • 取队列的最开始处理，并将其pop
  • 数据从ros转换为pcl格式
    • 角点，平面点，整个点云，整个颜色点云
  • 异步线程处理：
    • 线程list中保留最大线程数，maintain_maximum_thread_pool
      • 若当前线程数超出阈值时，erase减少线程数
        • 遍历所有线程，若该线程状态为ready，则erase
        • 否则， sleep_for 1ns 继续查询
    • 异步创建线程，回调函数 process_new_scan
  • sleep_for 10ns

process_new_scan

process 中开启的一个新的线程

• 数据赋值
  • 降采样后的 角点 和平面点
  • 当前帧的全部点云，角点，平面点
• 取本帧中最小，最大时间戳 find_min_max_intensity
  • 数据找最大，最小值，比较简单
• 获取当前帧时间
  • 初值：最小时间戳
  • 若当前时间戳大于 m_lastest_pc_income_time，则赋值 m_lastest_pc_income_time
  • 当前时间戳 = m_lastest_pc_income_time
• 初始化 点云寄存器 init_pointcloud_registration
  • local map与当前帧匹配前，set registration params
  • 注：用上一帧在map下位姿，作为初值。
• wait同步 matchbuff_and_pc_sync if_matchbuff_and_pc_sync
• 取当前帧的 角点和平面点
  • 先根据参数判断是否需要降采样，然后在赋值
• 局部地图 数据构建
  • 局部地图自身：角点、平面点及对应的kd_tree
• 将当前帧的角点和平面点转换到地图坐标系下
  • 与地图位姿关联：pointAssociateToMap
    • 若无畸变时，直接 基于全局位姿转换到地图坐标系中
    • 若有畸变时， 使用罗德里格斯进行快速计算。
      • 先基于时间线性差值运动，然后点云转换到世界坐标系
  • 将转换到地图坐标系的点云 保存
• 将数据放入历史帧中，若其未超过历史帧阈值时
• 在开启一个线程，进行 匹配 service_update_buff_for_matching
• 若闭环使能时，记录关键帧，及一些数据的转换

service_update_buff_for_matching

• while 1
  • 100hz 周期，等待buff为匹配 update_buff_for_matching

update_buff_for_matching

• 不断更新local map corners对应的kdtree_corners, localmap plannes对应的kdtree_plannes
• matching_mode = 0：用history数据,构建local map corners/plannes
• matching_mode = 1:在整个corners map中找离上一帧位置(x, y, z) 100m以内的所有points构建

service_pub_surround_pts

功能：

• 每隔100帧发布当前帧周围1000m范围内降采样后的points

m_service_loop_detection

功能：

• 线程，不断地检测是否有闭环，有闭环后进行对齐，然后进行pose graph更新每个finished keyframe位姿，更新地图发布到 “/pc_aft_loop_closure” 上

流程：

• while循环，循环频率为200hz
• 若无新关键帧时，continue，否则执行下列操作
• 取当前帧数据
  • Pose：q_curr,t_curr
  • 计算当前帧 每个cell的feature type
  • 计算当前帧 对应的line，planne histograms`
  • 将当前帧存入关键帧队列中 keyframe_vec
• 后面的与论文类似， 直方图统计找 闭环