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基础在看这个，确实一目了然。
建议：

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

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

角点：if 判断曲率需大于0.1成立，否则直接break不好吗，一直遍历完不浪费时间
平面点：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_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执行一次

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、发布相应的数据