Loam_livox的算法解析可以参考论文和相关资料,这里只对某些关键函数的功能进行说明。写这篇博客的原因是发现网上关于部分函数,特别是论文中没有提到的函数的说明甚少,所以把自己阅读过程中的理解加以记录,其中难免出现理解错误,希望大家留言指正。
laser_feature_extractor.cpp
只有一个main函数。
laser_feature_extractor.hpp
init_ros_env(),一些初始化和话题的订阅发布,直接找订阅原始点云的回调函数,继续往下看。
laserCloudHandler(),回调函数,对激光点云的主要处理流程。跳过开始的20帧,然后将每帧数据分成多个scan,提取平面点和角点并发布,具体实现都在文件livox_feature_extractor.hpp里。
livox_feature_extractor.hpp
projection_scan_3d_2d()函数用于激光点云预处理,将点云投影到前方(x轴正方向)一米处,寻找扫描转折点。因为livox固态激光雷达是花瓣式扫描,所以目的就是找到距中心最远点进行去除,并分割为多个scan。
split_laser_scan(),将一帧激光点分割成多束scan,每个“花瓣”被分成两个线束,类似于velodyne激光雷达按线束分割。
compute_features(),计算曲率,并按曲率大小对平面点和角点分类打标签。
extract_laser_features(),特征提取,主要就是调用了上面三个函数。
get_features(), 根据特征分类,将平面点和角点分别放到不同的pcl::PointCloud中。
point_cloud_registration.hpp
这个函数主要是用ceres优化点线、点面距离的残差函数,就是scan to map的优化过程。
pointAssociateToMap(),对点云去运动畸变,根据时间戳做了线性插值。
find_out_incremental_transfrom(),估计每帧点云的变换增量,构建点线、点面距离的残差函数,然后用ceres估计位姿变换。