A-LOAM源码详解（二）：laserOdometry.cpp

本文旨在帮助读者快速理解经典的3D激光SLAM方法：A-LOAM！笔者在阅读时主要参考了LOAM笔记及A-LOAM源码阅读。废话不多说，直接上代码！

//基于前述的4种feature进行帧与帧的点云特征配准，即简单的Lidar Odometry
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

#include "aloam_velodyne/common.h"
#include "aloam_velodyne/tic_toc.h"
#include "lidarFactor.hpp"

#define DISTORTION 0


int corner_correspondence = 0, plane_correspondence = 0;

constexpr double SCAN_PERIOD = 0.1;   //建议凡是「常量」语义的场景都使用 constexpr，只对「只读」语义使用 const
constexpr double DISTANCE_SQ_THRESHOLD = 25;
constexpr double NEARBY_SCAN = 2.5;

int skipFrameNum = 5;
bool systemInited = false;

double timeCornerPointsSharp = 0;
double timeCornerPointsLessSharp = 0;
double timeSurfPointsFlat = 0;
double timeSurfPointsLessFlat = 0;
double timeLaserCloudFullRes = 0;
//创建KdtreeFLANN对象
pcl::KdTreeFLANN::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN());
pcl::KdTreeFLANN::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN());

//typedef pcl::PointXYZI PointType;  以下定义的4种点云为scanRegisteration中或取到的4种特征点云
pcl::PointCloud::Ptr cornerPointsSharp(new pcl::PointCloud());
pcl::PointCloud::Ptr cornerPointsLessSharp(new pcl::PointCloud());
pcl::PointCloud::Ptr surfPointsFlat(new pcl::PointCloud());
pcl::PointCloud::Ptr surfPointsLessFlat(new pcl::PointCloud());

pcl::PointCloud::Ptr laserCloudCornerLast(new pcl::PointCloud());
pcl::PointCloud::Ptr laserCloudSurfLast(new pcl::PointCloud());
pcl::PointCloud::Ptr laserCloudFullRes(new pcl::PointCloud());

int laserCloudCornerLastNum = 0;
int laserCloudSurfLastNum = 0;

// Lidar Odometry线程估计的frame在world坐标系下的位姿P，Transformation from current frame to world frame
Eigen::Quaterniond q_w_curr(1, 0, 0, 0); //旋转四元数q
Eigen::Vector3d t_w_curr(0, 0, 0); //位移t

// 点云特征匹配时的待优化变量， q_curr_last(x, y, z, w), t_curr_last
double para_q[4] = {0, 0, 0, 1};
double para_t[3] = {0, 0, 0};

//下面两个分别是优化变量para_q和para_t的映射：表示的是两个world坐标系下的位姿P之间的增量，例如，△P = P0.inverse() * P1
Eigen::Map q_last_curr(para_q);//Eigen::Map可以理解为映射/引用
Eigen::Map t_last_curr(para_t);

std::queue cornerSharpBuf;
std::queue cornerLessSharpBuf;
std::queue surfFlatBuf;
std::queue surfLessFlatBuf;
std::queue fullPointsBuf;
std::mutex mBuf; //定义互斥锁

// undistort lidar point
void TransformToStart(PointType const *const pi, PointType *const po)
// TransformToStart：将当前帧Lidar坐标系下的点云变换到上一帧Lidar坐标系下
{
 //interpolation ratio
 //如果点云没有去除畸变，用slerp差值的方式计算出每个点在fire时刻的位姿，然后进行TransformToStart的坐标变换，一方面实现了变换到上一帧Lidar坐标系下；另一方面也可以理解成点都将fire时刻统一到了开始时刻，即去除了畸变，完成了运动补偿
    double s;
    if (DISTORTION)
        s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD; //SCAN_PERIOD=0.1，intensity的整数部分存放scanID，小数部分存放归一化后的fireID，int强转向下取整。因此s=fireID/0.1
    else
        s = 1.0;
    //s = 1;
    Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);//Eigen::Map q_last_curr(para_q)，表示坐标变换的旋转增量
    Eigen::Vector3d t_point_last = s * t_last_curr; //Eigen::Map t_last_curr(para_t)，表示坐标变换的位移增量
    Eigen::Vector3d point(pi->x, pi->y, pi->z);
    Eigen::Vector3d un_point = q_point_last * point + t_point_last;

    po->x = un_point.x();
    po->y = un_point.y();
    po->z = un_point.z();
    po->intensity = pi->intensity;
}

// transform all lidar points to the start of the next frame
//将输入点转为下一帧下的坐标
void TransformToEnd(PointType const *const pi, PointType *const po)
{
    // undistort point first
    pcl::PointXYZI un_point_tmp;
    TransformToStart(pi, &un_point_tmp); //pi转到上一帧Lidar坐标系下，转为un_point_tmp

    Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);//un_point_tmp转为un_point向量
    Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);//Eigen::Map t_last_curr(para_t)

    po->x = point_end.x();
    po->y = point_end.y();
    po->z = point_end.z();

    //Remove distortion time info
    po->intensity = int(pi->intensity);
}

//接下来五个Handler函数为接受5个topic的回调函数，作用是将消息缓存到对应的queue中
//receive sharp pointcloud
void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2)
{
    mBuf.lock(); //互斥锁被锁定。线程申请该互斥锁，如果未能获得该互斥锁，则调用线程将阻塞(block)在该互斥锁上；如果成功获得该互诉锁，该线程一直拥有该互斥锁直到调用unlock解锁；如果该互斥锁已经被当前调用线程锁住，则产生死锁(deadlock)。
    cornerSharpBuf.push(cornerPointsSharp2); //将corner_sharp点加入到cornerSharpBuf中
    mBuf.unlock(); //解锁，释放调用线程对该互斥锁的所有权。
}
//receive less_sharp pointcloud
void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsLessSharp2)
{
    mBuf.lock();
    cornerLessSharpBuf.push(cornerPointsLessSharp2);//将corner_less_sharp点加入到CornerLessSharpBuf中
    mBuf.unlock();
}
//receive flat pointcloud
void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsFlat2)
{
    mBuf.lock();
    surfFlatBuf.push(surfPointsFlat2);//将surf_flat点加入到surfFlatBuf中
    mBuf.unlock();
}
//receive less_flat pointcloud
void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsLessFlat2)
{
    mBuf.lock();
    surfLessFlatBuf.push(surfPointsLessFlat2);//将surf_less_flat点加入到surfLessFireBuf中
    mBuf.unlock();
}
//receive all pointcloud
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
{
    mBuf.lock();
    fullPointsBuf.push(laserCloudFullRes2);//将所有点laserCloud点加入到fullPointsBuf中
    mBuf.unlock();
}

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

    nh.param("mapping_skip_frame", skipFrameNum, 2); //在节点初始化之后获取参数服务器的参数，并设置初始值，launch文件中param name调用

    printf("Mapping %d Hz \n", 10 / skipFrameNum);

    ros::Subscriber subCornerPointsSharp = nh.subscribe("/laser_cloud_sharp", 100, laserCloudSharpHandler);

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

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

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

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

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

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

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

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

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

    nav_msgs::Path laserPath;

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

    while (ros::ok())
    {
        ros::spinOnce();

        if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() &&
            !surfFlatBuf.empty() && !surfLessFlatBuf.empty() &&
            !fullPointsBuf.empty())
        {
            //记录四种特征点云中的获取到的第一个点的时间戳
            timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.toSec();
            timeCornerPointsLessSharp = cornerLessSharpBuf.front()->header.stamp.toSec(); //queue.front() 返回第一个元素
            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();
            }

            mBuf.lock();
            cornerPointsSharp->clear(); //清空cornerPointSharp中的所有点云
            pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp);//将ROS系统接收到的cornerSharpBuf中的首元素（一帧点云）转存到cornerPointsSharp
            cornerSharpBuf.pop();//删除cornerSharpBuf中的首元素

            cornerPointsLessSharp->clear();
            pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp);//将CornerLessSharpBuf中的首元素转存到CornerPointsLessSharp
            cornerLessSharpBuf.pop(); //删除CornerLessSharpBuf中的首元素

            surfPointsFlat->clear();
            pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat);//将surfFlatBuf中的首元素转存到surfPointsFlat
            surfFlatBuf.pop();//删除surfFlatBuf的首元素

            surfPointsLessFlat->clear();
            pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat);//将surfLessFlatBuf中的首元素转存到surfPointsLessFlat
            surfLessFlatBuf.pop();//删除surfLessFlatBuf的首元素

            laserCloudFullRes->clear();
            pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes);//将fullPointsBuf中的首元素转存到laserCloudFullRes
            fullPointsBuf.pop();//删除fullPointsBuf的首元素
            mBuf.unlock();
            //这三句：.clear()、.fromROSmsg与.pop()都处在循环之中，每次循环都会清空A，将B首元素添加到A，再删除B的首元素，这样就实现了B的遍历并且保证每次A中只有一个元素

            TicToc t_whole;
            // initializing
            if (!systemInited)  //第一帧不进行匹配，仅仅将CornerPointsLessSharp保存到laserCloudCornerLast，将surfPointLessFlat保存至laserCloudSurfLast，为下次匹配提供target
            {
                systemInited = true;
                std::cout << "Initialization finished \n";
            }
            else
            {
                int cornerPointsSharpNum = cornerPointsSharp->points.size();
                int surfPointsFlatNum = surfPointsFlat->points.size(); 

                TicToc t_opt;
                //点到线以及点到面的ICP，迭代两次
                for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)  
                {
                    corner_correspondence = 0;
                    plane_correspondence = 0;
                
                    ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1); //定义ceres使用Huber损失函数
                    //对于四元数或者旋转矩阵这种使用过参数化表示旋转的方式，它们是不支持广义的加法
                    //（因为使用普通的加法就会打破其constraint，比如旋转矩阵加旋转矩阵得到的就不再是旋转矩阵），
                    //所以我们在使用ceres对其进行迭代更新的时候就需要自定义其更新方式了，具体的做法是实现一个参数本地化的子类，
                    //需要继承于LocalParameterization，LocalParameterization是纯虚类，所以我们继承的时候要把所有的纯虚函数都实现一遍才能使用该类生成对象.
                    ceres::LocalParameterization *q_parameterization =  new ceres::EigenQuaternionParameterization();
                    ceres::Problem::Options problem_options;

                    ceres::Problem problem(problem_options); //定义优化问题
                    problem.AddParameterBlock(para_q, 4, q_parameterization); //待优化参数para_q，内含4个变量，对旋转四元数需要自定义参数本地化的子类q_parameterization
                    problem.AddParameterBlock(para_t, 3);//待优化参数para_t

                    pcl::PointXYZI pointSel; //定义kdTree查询点
                    std::vector pointSearchInd; //定义kdTree搜索到的查询点近邻的索引
                    std::vector pointSearchSqDis;//定义kdTree对应近邻点中心距离平方

                    TicToc t_data;
                    // 基于最近邻(只找2个最近邻点)原理建立corner特征点之间关联，find correspondence for corner features
                    for (int i = 0; i < cornerPointsSharpNum; ++i) //对于每个corner_sharp点,注意是++i
                    {
                        TransformToStart(&(cornerPointsSharp->points[i]), &pointSel); //将当前帧的corner_sharp特征点从当前帧的Lidar坐标系下变换到上一帧的Lidar坐标系下（作为kdTree的查询点）
                        kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); // kdtree中的点云是上一帧的corner_less_sharp，所以这是在上一帧
                                                                                                                                                                                                      // 的corner_less_sharp中寻找当前帧corner_sharp特征点O的最近邻点（记为A）
                        //pointSel：查询点；1：邻近个数；pointSearchInd：储存搜索到的近邻点的索引；pointSearchSqDis：储存查询点与对应近邻点中心距离平方

                        int closestPointInd = -1, minPointInd2 = -1; 
                        if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)  //如果最近邻的corner特征点之间距离平方小于阈值(25)，则最近邻点A有效
                        {
                            closestPointInd = pointSearchInd[0];
                            int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity); //强转int向下取整，表示scanID

                            double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
                            // 向上搜索，寻找另外一个最近邻的点（记为B1），search in the direction of increasing scan line
                            for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)  //laserCloudCornerLast 来自上一帧的corner_less_sharp特征点，由于提取特征时是按照scan的顺序提取的，所以laserCloudCornerLast中的点也是按照scanID递增的顺序存放的
                            {
                                // if in the same scan line, continue
                                if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)  //intensity整数部分存放的是scanID
                                    continue;

                                // if not in nearby scans, end the loop
                                if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
                                    break;

                                double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
                                                        (laserCloudCornerLast->points[j].x - pointSel.x) +
                                                    (laserCloudCornerLast->points[j].y - pointSel.y) *
                                                        (laserCloudCornerLast->points[j].y - pointSel.y) +
                                                    (laserCloudCornerLast->points[j].z - pointSel.z) *
                                                        (laserCloudCornerLast->points[j].z - pointSel.z);

                                if (pointSqDis < minPointSqDis2)  //第二个最近邻点有效，更新点B1
                                {
                                    // find nearer point
                                    minPointSqDis2 = pointSqDis;
                                    minPointInd2 = j;  //找到一个像样的点B1，缩小阈值，继续搜索更近的点B来替换上一轮搜索的B，直到遍历所有的laserCloudCornerLast，即上一帧的corner_less_sharp点
                                }
                            }

                            // 向下搜索，寻找点另外一个最近邻的点B2，search in the direction of decreasing scan line
                            for (int j = closestPointInd - 1; j >= 0; --j)
                            {
                                // if in the same scan line, continue
                                if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)
                                    continue;

                                // if not in nearby scans, end the loop
                                if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
                                    break;

                                double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
                                                        (laserCloudCornerLast->points[j].x - pointSel.x) +
                                                    (laserCloudCornerLast->points[j].y - pointSel.y) *
                                                        (laserCloudCornerLast->points[j].y - pointSel.y) +
                                                    (laserCloudCornerLast->points[j].z - pointSel.z) *
                                                        (laserCloudCornerLast->points[j].z - pointSel.z);

                                if (pointSqDis < minPointSqDis2) //第二个最近邻点有效，更新点B
                                {
                                    // find nearer point
                                    minPointSqDis2 = pointSqDis;
                                    minPointInd2 = j;
                                }
                            }
                        }

                        if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid
                        {                                            // 即特征点0的两个最近邻点A和B都有效
                            Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x,    //当前点
                                                       cornerPointsSharp->points[i].y,
                                                       cornerPointsSharp->points[i].z);
                            Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x, //上一帧中搜索到的近邻点A
                                                         laserCloudCornerLast->points[closestPointInd].y,
                                                         laserCloudCornerLast->points[closestPointInd].z);
                            Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x,  //上一帧中搜索到的近邻点B
                                                         laserCloudCornerLast->points[minPointInd2].y,
                                                         laserCloudCornerLast->points[minPointInd2].z);

                            double s; //运动补偿系数
                            if (DISTORTION)
                                s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;//  fireID/0.1
                            else
                                s = 1.0;
                            // 用0，A，B构造点到线的距离的残差，注意这三个点都是在上一帧的Lidar坐标系下，即，残差=点0到直线AB的距离
                            //具体计算方法在lidarFactor.hpp
                            ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
                            problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
                            corner_correspondence++;
                        }
                    }
                    //下面说的点符号与上述相同
                    //与上面建立corner特征点之间的关联类似，寻找平面特征点的最近邻点ABC（只找3个最近邻点），即基于最近邻原理建立surf特征点之间的关联
                    // find correspondence for plane features
                    for (int i = 0; i < surfPointsFlatNum; ++i)
                    {
                        TransformToStart(&(surfPointsFlat->points[i]), &pointSel);//将当前帧的corner_sharp特征点从当前帧的Lidar坐标系下变换到上一帧的Lidar坐标系下（作为kdTree的查询点）
                        kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);  // kdtree中的点云是上一帧的corner_less_sharp，所以这是在上一帧
                                                                                                                                                                                                 // 的corner_less_sharp中寻找当前帧corner_sharp特征点O的最近邻点（记为A）
                        //pointSel：查询点；1：邻近个数；pointSearchInd：储存搜索到的近邻点的索引；pointSearchSqDis：储存查询点与对应近邻点中心距离平方

                        int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
                        if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD) //如果最近邻的corner特征点之间距离平方小于阈值(25)，则找到的最近邻点A有效
                        {
                            closestPointInd = pointSearchInd[0];

                            // get closest point's scan ID
                            int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);//强转int向下取整，表示scanID
                            double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;

                            // 向上搜索，寻找另外一个最近邻的点（记为B1），search in the direction of increasing scan line
                            for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)
                            {
                                // if not in nearby scans, end the loop
                                if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
                                    break;

                                double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
                                                        (laserCloudSurfLast->points[j].x - pointSel.x) +
                                                    (laserCloudSurfLast->points[j].y - pointSel.y) *
                                                        (laserCloudSurfLast->points[j].y - pointSel.y) +
                                                    (laserCloudSurfLast->points[j].z - pointSel.z) *
                                                        (laserCloudSurfLast->points[j].z - pointSel.z);

                                // if in the same or lower scan line
                                if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
                                {
                                    minPointSqDis2 = pointSqDis;  //找到的第2个最近邻有效，更新B，注意如果scanID准确的话，一般A与B的scanID相同
                                    minPointInd2 = j;
                                }
                                // if in the higher scan line
                                else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
                                {
                                    minPointSqDis3 = pointSqDis;  //找到的第3个最近邻点有效，更新点C，注意如果scanID准确的话，一般点A和点B的scanID相同，且与点C的scanID不同
                                    minPointInd3 = j;
                                }
                            }

                            // search in the direction of decreasing scan line
                            for (int j = closestPointInd - 1; j >= 0; --j)
                            {
                                // if not in nearby scans, end the loop
                                if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
                                    break;

                                double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
                                                        (laserCloudSurfLast->points[j].x - pointSel.x) +
                                                    (laserCloudSurfLast->points[j].y - pointSel.y) *
                                                        (laserCloudSurfLast->points[j].y - pointSel.y) +
                                                    (laserCloudSurfLast->points[j].z - pointSel.z) *
                                                        (laserCloudSurfLast->points[j].z - pointSel.z);

                                // if in the same or higher scan line
                                if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2)
                                {
                                    minPointSqDis2 = pointSqDis;
                                    minPointInd2 = j;
                                }
                                else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3)
                                {
                                    // find nearer point
                                    minPointSqDis3 = pointSqDis;
                                    minPointInd3 = j;
                                }
                            }

                            if (minPointInd2 >= 0 && minPointInd3 >= 0) //A、B、C都有效
                            {

                                Eigen::Vector3d curr_point(surfPointsFlat->points[i].x,  //当前点
                                                            surfPointsFlat->points[i].y,
                                                            surfPointsFlat->points[i].z);
                                Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x,  //上一帧中的近邻点A
                                                                laserCloudSurfLast->points[closestPointInd].y,
                                                                laserCloudSurfLast->points[closestPointInd].z);
                                Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x,    //上一帧中的近邻点B
                                                                laserCloudSurfLast->points[minPointInd2].y,
                                                                laserCloudSurfLast->points[minPointInd2].z);
                                Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x,  //上一帧中的近邻点C
                                                                laserCloudSurfLast->points[minPointInd3].y,
                                                                laserCloudSurfLast->points[minPointInd3].z);

                                double s;
                                if (DISTORTION)
                                    s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD;
                                else
                                    s = 1.0;
                                //用点0，A，B，C构造点到面的距离的残差，注意这三个点都是在上一帧的Lidar坐标系下，即残差=点0到平面ABC的距离
                                ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);
                                problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
                                plane_correspondence++;
                            }
                        }
                    }

                    //printf("coner_correspondance %d, plane_correspondence %d \n", corner_correspondence, plane_correspondence);
                    printf("data association time %f ms \n", t_data.toc());

                    if ((corner_correspondence + plane_correspondence) < 10)
                    {
                        printf("less correspondence! *************************************************\n");
                    }

                    TicToc t_solver;
                    ceres::Solver::Options options;
                    options.linear_solver_type = ceres::DENSE_QR;//QR分解
                    options.max_num_iterations = 4;
                    options.minimizer_progress_to_stdout = false;
                    ceres::Solver::Summary summary;
                    //基于构造的所有残差项，求解最优的当前帧位姿与上一帧位姿的位姿增量：para_q和para_t
                    ceres::Solve(options, &problem, &summary);
                    printf("solver time %f ms \n", t_solver.toc());
                }
                printf("optimization twice time %f \n", t_opt.toc());
                //用最新计算出的位姿增量，更新上一帧的位姿，得到当前帧的位姿，注意这里说的位姿都是世界坐标系下的位姿
                t_w_curr = t_w_curr + q_w_curr * t_last_curr;
                q_w_curr = q_w_curr * q_last_curr;
            }

            TicToc t_pub;

            // publish odometry
            //publish由当前Odometry线程计算出的当前帧在世界坐标系的位姿、corner_less_sharp特征点、surf_less_flat特征点、滤波后的点云（原封不动的转发上一个node处理出的当前帧点云）
            nav_msgs::Odometry laserOdometry;
            laserOdometry.header.frame_id = "/camera_init";
            laserOdometry.child_frame_id = "/laser_odom";
            laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
            laserOdometry.pose.pose.orientation.x = q_w_curr.x();
            laserOdometry.pose.pose.orientation.y = q_w_curr.y();
            laserOdometry.pose.pose.orientation.z = q_w_curr.z();
            laserOdometry.pose.pose.orientation.w = q_w_curr.w();//当前帧的旋转四元数
            laserOdometry.pose.pose.position.x = t_w_curr.x();
            laserOdometry.pose.pose.position.y = t_w_curr.y();
            laserOdometry.pose.pose.position.z = t_w_curr.z();//当前帧的位移量
            pubLaserOdometry.publish(laserOdometry);

            geometry_msgs::PoseStamped laserPose;
            laserPose.header = laserOdometry.header;
            laserPose.pose = laserOdometry.pose.pose;
            laserPath.header.stamp = laserOdometry.header.stamp;
            laserPath.poses.push_back(laserPose);
            laserPath.header.frame_id = "/camera_init";
            pubLaserPath.publish(laserPath);

            // transform corner features and plane features to the scan end point
            //用cornerPointsLessSharp和surfPointsLessFlat更新laserCloudCornerLast和laserCloudSurfLast以及相应的kdtree，为下一次点云特征匹配提供target
            if (0)
            {
                int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();
                for (int i = 0; i < cornerPointsLessSharpNum; i++)
                {
                    TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);
                }

                int surfPointsLessFlatNum = surfPointsLessFlat->points.size();
                for (int i = 0; i < surfPointsLessFlatNum; i++)
                {
                    TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);
                }

                int laserCloudFullResNum = laserCloudFullRes->points.size();
                for (int i = 0; i < laserCloudFullResNum; i++)
                {
                    TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
                }
            }

            pcl::PointCloud::Ptr laserCloudTemp = cornerPointsLessSharp;
            cornerPointsLessSharp = laserCloudCornerLast; 
            laserCloudCornerLast = laserCloudTemp;   //将上一节点中获取到的less_sharp点云赋给laserCloudCornerLast，，这就是为啥//277行//第一帧不进行匹配，仅仅将CornerPointsLessSharp保存到laserCloudCornerLast，将surfPointLessFlat保存至laserCloudSurfLast，为下次匹配提供target

            laserCloudTemp = surfPointsLessFlat;
            surfPointsLessFlat = laserCloudSurfLast;
            laserCloudSurfLast = laserCloudTemp;

            laserCloudCornerLastNum = laserCloudCornerLast->points.size();
            laserCloudSurfLastNum = laserCloudSurfLast->points.size();

            // std::cout << "the size of corner last is " << laserCloudCornerLastNum << ", and the size of surf last is " << laserCloudSurfLastNum << '\n';

            kdtreeCornerLast->setInputCloud(laserCloudCornerLast); //设置kdtreeCornerLast输入点云为上一帧的less_sharp
            kdtreeSurfLast->setInputCloud(laserCloudSurfLast);//设置 kdtreeSurfLast输入点云为上一帧的less_flat

            if (frameCount % skipFrameNum == 0)//提取关键帧，间隔四帧提取一次，即每五帧发布一次话题用于建图
            {
                frameCount = 0;

                sensor_msgs::PointCloud2 laserCloudCornerLast2;
                pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
                laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
                laserCloudCornerLast2.header.frame_id = "/camera";
                pubLaserCloudCornerLast.publish(laserCloudCornerLast2);

                sensor_msgs::PointCloud2 laserCloudSurfLast2;
                pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
                laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
                laserCloudSurfLast2.header.frame_id = "/camera";
                pubLaserCloudSurfLast.publish(laserCloudSurfLast2);

                sensor_msgs::PointCloud2 laserCloudFullRes3;
                pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
                laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
                laserCloudFullRes3.header.frame_id = "/camera";
                pubLaserCloudFullRes.publish(laserCloudFullRes3);
            }
            printf("publication time %f ms \n", t_pub.toc());
            printf("whole laserOdometry time %f ms \n \n", t_whole.toc());
            if(t_whole.toc() > 100)
                ROS_WARN("odometry process over 100ms");

            frameCount++;
        }
        rate.sleep();
    }
    return 0;
}

参考：LOAM笔记及A-LOAM源码阅读

A-LOAM源码地址：https://github.com/HKUST-Aerial-Robotics/A-LOAM