LINS代码阅读(2022.8.1)

image_projection_node.cpp

1、话题接收:

/points_raw 雷达原始点云,回调函数为cloudHandler

2、话题发布:

/full_cloud_projected ----转换成图片的点云,原始点云
/full_cloud_info ---- 带有距离信息的点云
/ground_cloud ---- 地面点云
/segmented_cloud ---- 已经分割的点云
/segmented_cloud_pure ----具有几何信息的分割点云
/segmented_cloud_info
/outlier_cloud ---- 过滤掉的点云

3、类:

ImageProjection:
allocateMemory()-- 缓存分配;
resetParameters()-- 设置参数;

4、回调函数cloudHandler:

copyPointCloud ---- 将消息转换为pcl数据格式,去除无限远点。
findStartEndAngle ---- 计算一帧消息起止角度和角度范围。
projectPointCloud ---- 投影到有序的image中,记录点的行列和深度。根据点的竖直方向角度划分竖直方向索引,对于16线激光雷达,-15~15,分辨率为2,索引范围[0,15];根据x,y方向夹角计算水平方向索引,对于16线激光雷达,分辨率为0.2,索引范围[0,1799];计算点的距离,将距离放入rangeMat中(rowIdn行,columnIdn列),设置点的intensity = rowIdn + columnIdn / 10000,index = columnIdn + rowIdn * SCAN_NUM。
groundRemoval ---- 移除地面点。groundScanInd 是在 utility.h 文件中声明的线数,groundScanInd=5,这个参数应该是根据雷达安装高度确定,跳过空点;当点与XoY平面夹角和雷达装配角差值小于一定范围时认为该点为地面点,将地面点与无穷远点的label赋为-1,不参与聚类。
cloudSegmentation ---- 非地面点聚类。使用广度优先搜索遍历帧内所有的点,找出较为平整的连续的点,孤立的点舍弃,不参与聚类;将地面点、参与聚类的点和被舍弃的点分别保存转发。
publishCloud ---- 话题发布。
resetParameters ---- 重置参数。

lins_fusion_node.cpp

1、话题接收:

/segmented_cloud ---- 已经分割的点云
/segmented_cloud_info
/outlier_cloud ---- 过滤掉的点
/imu_raw ---- IMU原始消息
/integrated_to_init---- 融合到初始

2、话题发布:

/undistorted_point_cloud
/laser_cloud_sharp
/laser_cloud_less_sharp
/laser_cloud_flat
/laser_cloud_less_flat
/laser_cloud_corner_last
/laser_cloud_surf_last
/outlier_cloud_last
/laser_odom_to_init

3、类:

parameter:这是个命名空间,规定了一些系统的主要参数(相关文件路径:”src/LINS—LiDAR-inertial-SLAM-dev/lins/include/parameters.h”、”src/LINS—LiDAR-inertial-SLAM-dev/lins/config/exp_config/exp_port.yaml”)
LinsFusion:命名空间fusion下的类(相关文件路径:”src/LINS—LiDAR-inertial-SLAM-dev/lins/include/Estimator.h”、”src/LINS—LiDAR-inertial-SLAM-dev/lins/src/lib/Estimator.cpp”)

4、回调函数:

mapOdometryCallback:只进行了坐标转换
imuCallback:提取当前帧六自由度加速度,把测量从IMU坐标系转换到车体坐标系。IMU和车体在yaw上存在偏角,把测量转换到车体坐标系下。
确认接收到第一帧点云数据后对其初始化,首先检测是否初始化,执行processFirstPointCloud函数进行初始化,对第一帧点云进行处理:首先获取最新一帧的点云和IMU信息,导入processPCL函数进行处理,将点云从雷达坐标系转换至车体坐标系,计算当前帧点云的曲率,去除被遮挡点和噪点,并进行特征点提取(线点和面点);初始化后,第二帧点云开始,与IMU数据进行匹配,策略是利用时间戳信息,找出可以被两个IMU帧包含的点云帧;找到与当前关键帧时间戳相近的IMU时间戳或雷达时间戳,计算上一关键帧到当前关键帧的时间差值,此时需要对扩展卡尔曼滤波器进行设置,具体步骤为:通过预积分计算出相对位姿,对线特征点和面特征点点云数据分别进行匹配,计算出jacobian矩阵(与LOAM不同,这边没有使用ceres进行求解),并对初始状态进行定义,假设车一开始静止,此时加速度应该为g,根据加速度在xy轴上的分量估计roll和pitch,根据点云匹配计算出的相对位置和速度,静止估计出的roll和pitch,给定的bias,进行运动畸变去除;在后续雷达帧,使用卡尔曼滤波进行状态变化估计(文件:src/LINS—LiDAR-inertial-SLAM-dev/lins/include/StateEstimator.hpp):假设在相邻关键帧之间bias和重力g不变,根据IMU线性模型计算出位移与速度变化量,更新误差状态转移矩阵和误差状态协方差。

lidar_mapping_node.cpp(与LEGO-LOAM的mapOptmization.cpp一样)

1、话题接收:

/laser_cloud_corner_last
/laser_cloud_surf_last
/outlier_cloud_last
/laser_odom_to_init

2、话题发布:

/key_pose_origin
/laser_cloud_surround
/aft_mapped_to_init
/aft_xyz_mapped_to_init
/history_cloud
/corrected_cloud
/recent_cloud

3、类:

MappingHandler:主要的流程写在run函数中,首先根据从上一帧到当前帧的偏移量,计算当前帧在世界坐标系最优位姿估计,transformBefMapped是上一次构图时在世界坐标系下的Lidar里程计信息,即上一时刻的transformSum;第二步,搜索周围关键帧,并拼接成map,对待是否回环存在不同的处理方式,并进行下采样;第三步,scan2map位姿优化,更新transformBefMapped和transformAftMapped;第四步,判断是否需要新增关键帧,如果需要那么利用因子图进行全局位姿优化,并新增关键帧;最后将优化后的信息发布。

4、回调函数:

laserOdometryHandler:保存里程计信息,transformSum前三个信息分别存三轴旋转信息,后三个存三轴平移量。

transform_fusion_node.cpp

1、话题接收:

/laser_odom_to_init ---- 雷达里程计到初始位置
/aft_mapped_to_init

2、话题发布:

/integrated_to_init

3、类:

TransformFusion

4、回调函数:

laserOdometryHandler
odomAftMappedHandler

参考链接

知乎专栏
论文地址
代码地址
大佬LEGO-LOAM代码地址,带详细注释

第一次写关于代码的文章,整体阅读了一周时间,可能有些地方有遗漏或者错误,希望各位大佬能给指出来。有些地方还不是很明白,在这边做一个学习记录,弄明白后进行更新。

本图文内容来源于网友网络收集整理提供,作为学习参考使用,版权属于原作者。
THE END
分享
二维码
< <上一篇
下一篇>>