参考链接

Loam_livox 代码阅读与总结

livox_loam 第一部分详解

整体架构

解析Livox点云存储数据结构

从数据流视角解析 loam_livox

laser_feature_extractor

cpp文件

先看.cpp文件，注册了节点scanRegistration，创建了一个Laser_feature对象

int main(int argc, char **argv)
{
    ros::init(argc, argv, "scanRegistration"); // 注册scanRegistration节点
    Laser_feature laser_feature;

    ros::spin();

    return 0;
}

hpp文件

定义了Laser_feature类

init_ros_env

构造函数Laser_feature中调用了init_ros_env，嫌函数太长吗》//

• 先创建节点句柄，记录下此刻的时间

• 调用init_livox_lidar_para函数，初始化雷达类型m_lidar_type，m_livox.m_livox_min_allow_dis和m_livox_min_sigma若有参数就设置并输出，这俩是干啥的暂时不知道

• 调用了get_ros_parameter，从参数服务器获取一大堆参数，这个类中有以下成员

  Livox_laser m_livox;
  

  • 设置m_livox角点曲率阈值， 表面曲率阈值，最小视角，用于控制点云中点的视角范围

• 初始化log日志，m_map_pointcloud_full_vec_vec，surface_vec_vec，corner_vec_vec，这三个是vector里是vector，支持多个雷达m_maximum_input_lidar_pointcloud，但是我只有一个

• 遍历雷达数量，设置不同话题名称laser_points_i

  • 创建话题订阅者，订阅laser_points_i，存入m_sub_input_laser_cloud_vec，回调函数为laserCloudHandler
  • 初始化每一个雷达对应的那三个点云容器，m_piecewise_number为分段个数

问题

1. livox的点云到底是怎么扫描的，他与机械式雷达不一样啊，m_piecewise_number是啥

laserCloudHandler

回调函数，处理激光点云数据

/**
 * 激光雷达点云数据处理函数
 * 
 * @param laserCloudMsg 激光雷达点云数据消息
 * @param topic_name 订阅的主题名称
 */

数据转换成pcl点云。

• 先锁住lock(m_mutex_lock_handler)
• 获取当前雷达id current_lidar_index
• laserCloudScans是每个scan的点云
• 积累特定帧数据再处理，m_para_system_delay为帧数，超过后m_para_systemInited为true，实现一个降频的效果
• pcl::fromROSMsg(*laserCloudMsg, laserCloudIn); // 将ros消息转换成pcl点云

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

• m_livox.extract_laser_features函数提取livox激光点云的边界和平面特征，并移除坏点

  • 这个函数的返回值std::vector<pcl::PointCloud<pcl::PointXYZI>>，即所有的scan

• laserCloudScans花瓣数小于5，直接return。太少了！如果数量足够，那就初始化scanStartInd和scanEndInd为花瓣个数，并填充为0

• 调试模式？是啥，若开启，就进行后续操作，默认开启

• m_if_motion_deblur若为1，就会将一个scan作为一整段处理，不分段。

• 将一个scan分段，例如分三段，每次遍历，一次scan，半片花瓣，起始和结束位置start_scans和end_scans，再m_livox.find_pt_info找对应点的指针，这个指针类型是Pt_infos，具体见livox_feature_extractor.hpp定义

• 遍历分段次数

  • m_livox.get_features获取角点，平面点，和两者之和所有点，将以下赋值

    m_map_pointcloud_corner_vec_vec[current_lidar_index][i] = *livox_corners;
    m_map_pointcloud_surface_vec_vec[current_lidar_index][i] = *livox_surface;
    m_map_pointcloud_full_vec_vec[current_lidar_index][i] = *livox_full;
    

• 遍历分段次数，对每段处理，发布特征点

  • 遍历雷达设备个数，将所有雷达的特征点整合到一起；else只输入当前雷达的特征点。

    *livox_full += m_map_pointcloud_full_vec_vec[ii][i];
    *livox_surface += m_map_pointcloud_surface_vec_vec[ii][i];
    *livox_corners += m_map_pointcloud_corner_vec_vec[ii][i];
    

  • 用pcl::toROSMsg转换消息格式，设置时间戳和frame_id,发布消息。但是为什么要对平面点和角点进行体素网格降采样？定义了两个这个类型的变量

    pcl::VoxelGrid<PointType>
    

    • 发布的话题名称为 /pc2_corners，/pc2_surface，/pc2_full

如果是Velodyne雷达, 其参考A-LOAM，计算特征并发布。暂时不总结

话说转化成pcl点云，他数据格式是什么样的呢？

init_livox_lidar_para

livox_feature_extractor

定义了Livox_laser类

eval_point

1. 深度值特别小的点，e_pt_too_near
2. 求解 pt_info->sigma，小于阈值，e_pt_reflectivity_low，低反射率。见论文公式3

compute_features

1. 一些变量的定义，其中critical_rm_point用了按位或操作，空或无穷

2. neighbor_accumulate_xyz该点左右两个点的坐标累加，后面减去了四个该点坐标

3. 计算曲率m_pts_info_vec[idx].curvature

4. 计算平面角，就是vec_a和vec_b的夹角。更新m_pts_info_vec[idx].view_angle，论文的公式4

5. 大于10度的才进行标记

   • 曲率小于0.01, 认为时平面点

   • 曲率大于0.05，且判断与周围点深度的比较，且

     if (m_pts_info_vec[idx].depth_sq2 <= m_pts_info_vec[idx - curvature_ssd_size].depth_sq2 &&
                             m_pts_info_vec[idx].depth_sq2 <= m_pts_info_vec[idx + curvature_ssd_size].depth_sq2)
                         {
                             if (abs(m_pts_info_vec[idx].depth_sq2 - m_pts_info_vec[idx - curvature_ssd_size].depth_sq2) < sq2_diff * m_pts_info_vec[idx].depth_sq2 ||
                                 abs(m_pts_info_vec[idx].depth_sq2 - m_pts_info_vec[idx + curvature_ssd_size].depth_sq2) < sq2_diff * m_pts_info_vec[idx].depth_sq2)
                                 m_pts_info_vec[idx].pt_label |= e_label_corner;
                         }
     

projection_scan_3d_2d

1. 初始化各种数据结构和变量。

2. 遍历输入点云中的每个点，执行以下操作：

   • m_map_pt_idx，更新pt_info。结构体Pt_infos
   • 点值为inf，add_mask_of_point(pt_info, e_pt_nan); ，直接跳过
   • 根据论文，x方向在雷达正前方，为0表示距离太近
     • 若idx为0，更新pt_info的pt_2d_img和polar_dis_sq2，类型标记为e_pt_000
     • idx不为0，更新为idx - 1的信息？，类型同样标记为e_pt_000，跳出，后续不处理
   • 计算点的深度depth_sq2，pt_2d_img，极坐标距离polar_dis_sq2。
   • 评估点的坐标eval_point(pt_info);
   • 检查点是否在扫描边缘，将周围的几个点设置为e_pt_circle_edge。
   • 分割scans
     • 当前点与前一个点的距离dis_incre，判断是大于0，还是小于0，即远离还是靠近花瓣中心
     • 判断相邻的两个点的dis_incre检测花瓣上的转折点，若一个1一个-1，代表是转折点，记录，并continue

3. 记录本次调用中点的数量。

   split_idx.push_back(pts_size - 1);
   

4. split_idx如果小于6就返回0

5. 遍历所有点，

   • val_index < split_idx.size() - 2

     • 在一个花瓣内

       if (idx == 0 || idx > split_idx[val_index + 1])
       

       • idx超过下一个花瓣起始，val_index++;
       • 计算花瓣点数量internal_size
       • 根据下一个花瓣的起始点的极距，判断是用倒数0.2还是0.8计算scan_angle

   • 这两句不知道什么意思

     m_pts_info_vec[idx].polar_angle = scan_angle; // keep in mind the above, the if conditions skip points, therefore most scan_angle will be 0
     scan_id_index[idx] = scan_angle;              // only scan_angle of the starting point of a petal recorded
     

6. 返回花瓣数量。

7. 在遍历所有点之后，计算每个花瓣的扫描角度，并将其分配给相应的点。

8. 返回扫描中的花瓣数量。

• 一帧点云并不是同时获得的，由于时间戳是扫描到第一个点的时刻，因此所有点的时间戳会有细微不同，按下标分配时间戳，一帧10ms,10000个点，每个点的时间为10μs。因此m_time_internal_pts=10e-5s.
• 计算点的深度depth_sq2，pt_2d_img，极坐标距离polar_dis_sq2。这里不知道在干啥

split_laser_scan

1. 遍历点云
   • 分每个花瓣，分别存入laserCloudScans[scan_idx]点和pts_mask mask
   • 遍历laserCloudScans，
     • 遍历每个点laserCloudScans[i].size()
       • 按位与操作pts_mask[i][idx] & remove_point_pt_type
       • set_intensity
       • laser_clour_per_scan.points.push_back(temp_pt);
2. 返回的就是删除000、太近、nan类型的点，然后剩下的点。

不理解，为什么设置intensity

extract_laser_features

• m_current_time当前时间

  • 若当前时间比上次最大时间小，则认为两次同一个时间，则赋值
  • 否则，认为一个新的数据

• m_if_save_pcd_file，默认不保存原始点云数据

• 返回一个scan的花瓣数，并将一个scan的点云分成多个花瓣点云集合

  int clutter_size = projection_scan_3d_2d(laserCloudIn, scan_id_index); 
  

• 调用compute_features()

• 若花瓣簇为0，则直接返回

• 将点云分为不同的颜色，为rviz显示 split_laser_scan()