A-LOAM(前端-4)的帧间lidar里程计-算法流程+代码注释

算法流程主要用作梳理整个雷达里程计，先在理解上知道代码会做什么，和这样做的原因，然后再看代码实现，代码实现中的注释和算法流程的梳理是一 一对应的，可以对比着代码看这个流程

算法流程

代码位于"laserOdometry.cpp"的main函数开始，以下为该函数的运行步骤

1. 从main函数开始，订阅提取出来的点云。

notic:订阅到的点云是经过特征点处理与均匀化的，具体订阅的消息见 A-LOAM中的特征提取及均匀化-算法流程+代码注释 中“每个scan提取特征”代码末尾的发布

2. 在while中调用 ros::spinOnce();

ros::spin() 和 ros::spinOnce() 区别及详解 总的来说就是，spinonce是当订阅的话题来一次消息时就执行一次回调函数，执行完回调函数后会回到主程序，spin就不会回到主程序，但是用spinonce需要考虑调用消息的时机，调用频率，以及消息池的大小，不然会造成数据丢包或者延迟的错误。

3. 分别求出各个数据队列的第一个时间，判断数据队列是否都属于同一帧，数据队列就是订阅的消息

数据队列的数据是在订阅到消息时的回调函数中执行放入的

4. 将5个点云消息提取出来，同时转成pcl格式

5种点云消息分别有：当前全部点云、曲率很大的点、曲率第2大的点、比较平坦的点、体素滤波后的平坦点

5. 设置ceres的参数
6. 下面7~14会迭代两次，两次迭代构成线的点和构成面的点可能都不一样，因为优化后可能发现和另外两个点构成的线更近了，面点同理
7. 提取当前帧曲率最大的点云中的点，并做运动补偿，详见A-LOAM中的雷达畸变及运动补偿-算法流程+代码注释
8. 通过kdtree在上一帧提取与该点最近的角点
9. 再找一个上一帧的与 7. 的角点不在同一根scan上并且与当前帧角点距离最近的角点，需要在scan的上下都分别找一个角点，然后再根据距离最小选择其中一个
10. 构建点线约束放入problem，原理见下图，具体实现见下面的代码

角点约束构建

至此：已完成角点约束的寻找与构建，用下图来具体描述点线约束的构建，p点为当前帧的角点，pa，pb为上一帧不同scan的角点，d就是构建的残差，待优化的变量是旋转和平移

---

10. 提取当前帧的面点，并做运动补偿
11. 通过kdtree寻找上一帧与当前帧这个面点距离最近的面点
12. 获取该面点的scan id 在同一个scan上再提取一个面点，再在不是同一个scan上面提取一个面点，这里共提取了两个面点
13. 增量寻找和降序寻找都完成一遍，选取距离最短的两个点
14. 构建点面约束放入problem，原理见下图，具体实现见下面的代码

点面约束构建

j，l点在同一条scan上，j、l、m都是属于上一帧的面点，求出法向量就是为了投影求得距离d，距离d就是残差，待优化变量是旋转和平移之，之所以定义距离d为残差可以这样理解，当得到两帧间正确的变换时，当前帧的点转换到上一帧，与上一帧的点是重合的，用点面和点线求距离是因为激光里面不好做特征点匹配。这里的初值是通过匀速模型根据 时间比例s 进行插值得到初值位姿

15. 得出优化后的四元数和平移就发布出去，topic为 laserOdometry ，后端也会收到
16. 发布轨迹，topic为 laserPath，这个轨迹是给ROS看的，没有其它作用
17. kdtree设置当前帧，用来下一帧lidar odom使用
18. 将当前的角点、当前的面点、所有的点以一定的频率发送给后端，这里可以降频发送，频率由skipFrameNum决定，意思为每隔多少帧发送一次
19. 计算发送频率

好了，A-LOAM的雷达里程计到此结束

代码注释

int main(int argc, char **argv)
{
   
    ros::init(argc, argv, "laserOdometry");
    ros::NodeHandle nh;

    nh.param<int>("mapping_skip_frame", skipFrameNum, 2);

    printf("Mapping %d Hz \n", 10 / skipFrameNum);
    // 订阅提取出来的点云
    ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100, laserCloudSharpHandler);

    ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler);

    ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100, laserCloudFlatHandler);

    ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100, laserCloudLessFlatHandler);

    ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100, laserCloudFullResHandler);

    ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 100);

    ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 100);

    ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 100);

    ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 100);

    ros::Publisher pubLaserPath = nh.advertise<nav_msgs::Path>("/laser_odom_path", 100);

    nav_msgs::Path laserPath;

    int frameCount = 0;
    ros::Rate rate(100);

    while (ros::ok())
    {
   
        ros::spinOnce();    // 触发一次回调，参考https://www.cnblogs.com/liu-fa/p/5925381.html

        // 首先确保订阅的五个消息都有，有一个队列为空都不行
        if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() &&
            !surfFlatBuf.empty() && !surfLessFlatBuf.empty() &&
            !fullPointsBuf.empty())
        {
   
            // 分别求出队列第一个时间
            timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.toSec();
            timeCornerPointsLessSharp = cornerLessSharpBuf.front()->header.stamp.toSec();
            timeSurfPointsFlat = surfFlatBuf.front()->header.stamp.toSec();
            timeSurfPointsLessFlat = surfLessFlatBuf.front()->header.stamp.toSec();
            timeLaserCloudFullRes = fullPointsBuf.front()->header.stamp.toSec();
            // 因为同一帧的时间戳都是相同的，因此这里比较是否是同一帧
            if (timeCornerPointsSharp != timeLaserCloudFullRes ||
                timeCornerPointsLessSharp != timeLaserCloudFullRes ||
                timeSurfPointsFlat != timeLaserCloudFullRes ||
                timeSurfPointsLessFlat != timeLaserCloudFullRes)
            {
   
                printf("unsync messeage!");
                ROS_BREAK();
            }
            // 分别将五个点云消息取出来，同时转成pcl的点云格式
            mBuf.lock();
            cornerPointsSharp->clear();
            pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp);
            cornerSharpBuf.pop();

            cornerPointsLessSharp->clear();
            pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp);
            cornerLessSharpBuf.pop();

            surfPointsFlat->clear();
            pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat);
            surfFlatBuf.pop();

            surfPointsLessFlat->clear();
            pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat);
            surfLessFlatBuf.pop();

            laserCloudFullRes->clear();
            pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes);
            fullPointsBuf.pop();
            mBuf.unlock();

            TicToc t_whole;
            // initializing
            // 一个什么也不干的初始化
            if (!systemInited)
            {
   
                systemInited = true;
                std::cout << "Initialization finished \n";
            }
            else
            {
   
                // 取出比较突出的特征
                int cornerPointsSharpNum = cornerPointsSharp->points.size();
                int surfPointsFlatNum = surfPointsFlat->points.size();

                TicToc t_opt;
                // 进行两次迭代 两次迭代构成线的点可能不一样，因为优化后可能发现和另外两个点构成的线更近了
                for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)
                {
   
                    corner_correspondence = 0;
                    plane_correspondence = 0;

                    //ceres::LossFunction *loss_function = NULL;
                    // 定义一下ceres的核函数
                    ceres::LossFunction *loss_function = new ceres