当前位置：网站首页>aloam 代码阅读与总结

aloam 代码阅读与总结

2022-07-02 15:56:00 【xiaoma_bk】

文章目录

Aloam

Aloam

A-LOAM 是 LOAM的高级实现，它使用 Eigen 和 Ceres Solver 来简化代码结构。此代码由 LOAM 和 LOAM_NOTEA 修改而来。这段代码简洁明了，没有复杂的数学推导和冗余操作。对于 SLAM 初学者来说是一本很好的学习资料。
节点示意图：
KITTI Example
- 下载 KITTI Odometry dataset 数据集到你数据集文件夹，设置 dataset_folder 和 sequence_number 参数。
- 请注意，您还可以通过在 kitti_helper.launch 中设置适当的参数来将 KITTI 数据集转换为包文件以便于使用。

Summary

优点：

代码确实简洁了不少，如果有loam基础在看这个，确实一目了然。

建议：

计算曲率时，每一线间的转变未考虑进去，不过不影响，因为提取特征时都跳过了每一线的起始多少个点
基于曲率大小筛选特征时，因为点云已经基于曲率排过序了
1. 角点：if 判断曲率需大于0.1成立，否则直接break不好吗，一直遍历完不浪费时间
2. 平面点：if 判断曲率需小于0.1成立，否则直接break不好吗，一直遍历完不浪费时间
找临近后确定直线和平面时感觉没lio-sam中精度高
ceres优化，来个手动求导不好吗

rosbag read_write

读写rosbag 用到 bag 类，具体使用见下例子：

// 定义读写对象，并申明
rosbag::Bag i_bag, o_bag;
i_bag.open(src_bag, rosbag::bagmode::Read);
o_bag.open(new_bag, rosbag::bagmode::Write);

// 定义一个数组
std::vector<std::string> topics;
topics.push_back(std::string(imu_topic));
topics.push_back(std::string(pcd_topic));
// 申明 rosbag::View对象
rosbag::View view(i_bag, rosbag::TopicQuery(topics));

///< 读取方法一：
for (auto m : view) {
      // 遍历view
    sensor_msgs::Imu::ConstPtr imu = m.instantiate<sensor_msgs::Imu>();
    if (imu == nullptr) {
      // 是否有imu
        std::cerr << "imu null " << std::endl;
    } else {
    
        std::cout << "imu stamp:" << imu->header.stamp << std::endl;
        o_bag.write(imu_topic, imu->header.stamp, imu);
    }
    sensor_msgs::PointCloud2::ConstPtr pcd =
        m.instantiate<sensor_msgs::PointCloud2>();
    if (pcd == nullptr) {
    
        std::cerr << "pcd null " << std::endl;
    } else {
    
        std::cout << "pcd stamp:" << pcd->header.stamp << std::endl;
        o_bag.write(pcd2_topic, pcd->header.stamp, pcd);
    }
}

///< 读取方法二：
rosbag::View view(bag);
for (const rosbag::ConnectionInfo* c : view.getConnections()) {
    

    const std::string& topic = c->topic;
    if (topic_to_publisher.count(topic) == 0) {
    
        ros::AdvertiseOptions options(c->topic, kQueueSize, c->md5sum,
                                      c->datatype, c->msg_def);
        topic_to_publisher[topic] = node_handle.advertise(options);
    }
}

scanRegistration

主函数：

Ros节点
- ros 节点初始化 scanRegistration
- 读取参数：scan_line、minimum_range
只支持 16,32,64 线的 velodyne，不对则 return
订阅话题：
- /velodyne_pointslaserCloudHandler
发布话题：
- sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100);
- sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100);
- sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100);
- sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);
- sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100);
- sensor_msgs::PointCloud2>("/laser_remove_points", 100);
基于参数确定是否发布每条线正常不执行

laserCloudHandler

上来一个系统初始化，就是单纯跳过多少帧
点云转 pcl格式，pcl::removeNaNFromPointCloud
移除最近点，removeClosedPointCloud
- 遍历每个点，求点到激光的距离，若距离小于阈值，则 continue
- 否则将点放入新一个容器中
起始角度计算：
- 开始角：startOri = -atan2(pointp[0].y,pointp[0].x)
- 结束角：endOri = -atan2(pointp[size-1].y,pointp[size-1].x)+2π
- 若 endOri-startOri > 3π endOri -= 2π
- 若 endOri-startOri < 3π endOri += 2π
计算 scanID + relTime
- scanID 哪一线，好计算
- relTime 与初始点的时间差算与初始点的水平角*周期/总角度
计算每个点的曲率
- 跟 loam计算一样
- 每一线间的转变未考虑进去，不过不影响，因为提取特征时都跳过了每一线的起始多少个点
遍历多线N_SCANS，对于每一线，将点云均分成6份：
- 计算每一份的起始下标： sp，ep
- 将每一份的点云按曲率从小到大排序 std::sort 函数
- 找角点，反向遍历已排好序的点云
  - 若曲率大于 0.1 且周围点可以为特征点时：
    - 找曲率最大的2 点放入 cornerPointsSharp 点云标记：2
    - 找曲率最大的20个放入 cornerPointsLessSharp 点云标记：1
    - 若该点标为特征点时，周围的5个点不可以为特征点
- 找平面点，正向遍历已排好序的点云
  - 若曲率小于 0.1 且周围点可以为特征点时：
    - 找曲率最大的4 点放入 surfPointsFlat 点云标记：-1
    - 若该点标为特征点时，周围的5个点不可以为特征点
- 若标记为-1 则放入 surfPointsLessFlatScan

laserOdometry

主函数：

Ros节点
- ros 节点初始化 laserOdometry
- 读取参数：mapping_skip_frame 默认2
订阅话题：
- /velodyne_cloud_2laserCloudFullResHandler
  - 这个点云就是去了最近点和 nan的
  - 这个回调也比较简单，将数据放入队列中
- 四种特征，角点，角点less，平面点，平面点less
  - 这四个回调都比较简单，就把数据放入队列中而已
发布：
- 角点和平面点
- velodyne_cloud_3
- laser_odom_to_init
- laser_odom_path
while 主线程，ros::spin0nce，100hz
- 上面5种数据都到且取第一个数据时间都一致时
  - 当5种数据都到后，时间肯定一致，不一致ros报错
- 5种数据进行 pcl数据转换
- 若 systemInited== flase时：
  - 直接赋值 systemInited=true
- 否则，优化两次，对于每一次都执行下列操作：
  - 优化变量为位姿 para_q，para_t，4+3个自由度
  - 遍历角点特征：
    - 将角点转换到里程计坐标系，TransformToStart
    - 通过kdTree找到直线
      - 通过kdTree 找到上一帧中的最近角点下标 pointSearchInd
      - 该点下标上下两range中最近的一个点作为另一个点
      - 两点构成一条直线
    - 通过 LidarEdgeFactor 构造ceres直接约束
    - 并添加残差
  - 遍历平面特征：
    - 将平面点转换到里程计坐标系，TransformToStart
    - 通过kdTree找到平面
      - 通过kdTree 找到上一帧中的最近角点下标 pointSearchInd
      - 该点下标上下两range中最近的一个点作为另一个点
      - 同一ring中往前伸一个平面特征作为第三个点
    - 通过 LidarPlaneFactor 构造ceres直接约束
    - 并添加残差
- 构建求解器：迭代4次，QR分解，得到解
- 更新 q_w_curr 和 t_w_curr
- 更新上一帧角点和平面点kdTree

TransformToStart

记录了上一次里程计坐标系的关系，直接按上次的关系进行转换
即认为当前帧初值与上一帧初值一样，方便计算

LidarEdgeFactor

点到直线的距离，自动求导

LidarPlaneFactor

点到平安的距离，自动求导

laserMapping

主函数：

ros节点初始化
- 定义节点 laserMapping
- 读取参数：
  - mapping_line_resolution 0.4
  - mapping_plane_resolution 0.8
订阅话题：
- /laser_cloud_corner_lastlaserCloudCornerLastHandler
- /laser_cloud_surf_lastlaserCloudSurfLastHandler
- /laser_odom_to_initlaserOdometryHandler
- /velodyne_cloud_3laserCloudFullResHandler
发布话题：
主线程 mapping_process process

callback

3个激光回调都将数据放入各自的队列中，有互斥锁
激光里程计回调除了将数据放入队列中后，还基于里程计漂移将数据从世界坐标系发出

process

while 1循环，2s执行一次

数据同步，并转换成pcl格式
- 若 3种点云数据和里程计数据都不为空时：
  - 若某一种据比cornerLastBuf.front() 数据早时，则 pop
- 取各个数据的第一个数据，应该这四种数据的时间一致
  - 同一节点发的，且四种数据的时间戳给的一样
- 将3种点云数据通过pcl 转换为各自对象，并从队列中 pop_front
- 若 cornerLastBuf非空时， pop其数据，保证实时性
得到当前帧在世界坐标系的位姿 transformAssociateToMap()
- ${ _L^WT=_O^WT*_L^OT}$
1、优化处理找当前估计的lidar位姿属于哪个cube，I/J/K对应cube的索引
- cube中心位置索引，50m的分辨率，初始值 [10,10,5]
2、如果当前帧lidar位姿对应的cube在整个大cube边缘则将索引向中心方向挪动一个单位
- width 方向：centerCubeI 和中心位置索引都相应改变
  - centerCubeI 在 width 方向的小端，将帧cube指针向中心方向平移,即 i = i-1
  - centerCubeI 在 width 方向的大端，将帧cube指针向中心方向平移,即 i = i+1
- height 方向：centerCubeJ 和中心位置索引都相应改变
  - centerCubeJ 在 height 方向的小端，将帧cube指针向中心方向平移,即 i = i-1
  - centerCubeJ 在 height 方向的大端，将帧cube指针向中心方向平移,即 i = i+1
- depth 方向：centerCubeK 和中心位置索引都相应改变
  - centerCubeK 在 depth 方向的小端，将帧cube指针向中心方向平移,即 i = i-1
  - centerCubeK 在 depth 方向的大端，将帧cube指针向中心方向平移,即 i = i+1
3、取 centerCube周围的点云组成局部地图
- 当前 centerCube 的三个方向各扩展2个各自的有效数组
- 将 centerCube周围有效数组的角点、平面点地图集点云想加起来
4、当前帧角点和平面点特征进行降采样
5、得到当前帧与地图匹配的位姿
- 如果满足角点地图个数大于10 且平面点地图点个数大于50，才执行
- 将地图点云放入 kdtree中方便查找
- 迭代两次求解：
  - 遍历当前帧角点特征，构建ceres误差模型
    - 当前角点转到世界坐标系，并找5个最近点
    - 若5个点都小于1m时
      - 得到 5个点的协方差，并计算其特征值和特征向量
      - 如果确实是线特征，构建线特征模型（注意特征库按升序对特征值进行排序）
  - 遍历当前帧面点特征，构建ceres误差模型
    - 转到世界坐标系，并找5个最近点
    - 若5个点都小于1m时，构建5个点的方程： ax+by+c=1
    - 若平面5个点到平面的距离都小于0.2m，证明平面成立，构建面特征模型
      - 构建面特征模型
  - 求解得到匹配结果
6、优化完成后，更新数据
- 进行位姿更新
- 跟新当前帧中角点在 cube，并加入地图
- 跟新当前帧中平面点在 cubeI，并放入地图
7、对地图进行体素滤波降采样
8、发布相应的数据