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. 计算曲率时，每一线间的转变未考虑进去，不过不影响，因为提取特征时都跳过了每一线的起始多少个点

2. 基于曲率大小筛选特征时，因为点云已经基于曲率排过序了

   1. 角点：if 判断曲率需大于0.1成立，否则直接break不好吗，一直遍历完不浪费时间
   2. 平面点：if 判断曲率需小于0.1成立，否则直接break不好吗，一直遍历完不浪费时间

3. 找临近后确定 直线和平面时 感觉没lio-sam中精度高

4. 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_points laserCloudHandler
• 发布话题：
  • 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_2 laserCloudFullResHandler
    • 这个点云就是去了最近点和 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_last laserCloudCornerLastHandler
  • /laser_cloud_surf_last laserCloudSurfLastHandler
  • /laser_odom_to_init laserOdometryHandler
  • /velodyne_cloud_3 laserCloudFullResHandler
• 发布话题：
• 主线程 mapping_process process

callback

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

process

while 1循环，2s执行一次

• 数据同步，并转换成pcl格式
  • 若 3种点云数据 和 里程计数据都不为空时：
    • 若某一种据比cornerLastBuf.front() 数据早时，则 pop
  • 取各个数据的第一个数据，应该 这四种数据的时间一致
    • 同一节点发的，且四种数据的时间戳给的一样
  • 将3种点云数据通过pcl 转换为各自对象，并从队列中 pop_front
  • 若 cornerLastBuf非空时， pop其数据，保证实时性
• 得到当前帧在世界坐标系的位姿 transformAssociateToMap()
  • ${}_L^{W}T{=}_O^{W}T{\ast }_L^{O}T$
• 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、发布相应的数据