前言

A-LOAM的cpp有四个,其中 kittiHelper.cpp 的作用是将kitti数据集转为rosbag
剩下的三个是作为 slam 的 部分,分别是:

• laserMappin.cpp ++++ 当前帧到地图的优化
• laserOdometry.cpp ++++ 帧间里程计
• scanRegistration.cpp ++++ 前端lidar点预处理及特征提取

本篇主要解读 帧间里程计部分的代码
对于前端的lidar点预处理及特征提取 在前面分析过了,链接:A-LOAM :前端lidar点特征提取部分代码解读
这部分在最后发布了5种topic.

• 所有的点云
• 角点
• 弱角点
• 面点
• 弱面点

帧间里程计则订阅这5种topic,并根据相邻两帧的特征点,优化出两帧间的位姿.

帧间里程计的代码在laserOdometry.cpp中

帧间里程计在计算相邻帧位姿的时候是通过ceres优化的,关于这部分优化的内容在这篇博客中:ALOAM:Ceres 优化部分及代码解析

激光雷达里程计原理

tk是第k帧lidar的开始时间
点云在这一帧结束的过程中,逐渐接收点 形成点云Pk

tk+1 是 第k帧雷达结束的时间 ,将k帧里面的所有点都投影到tk+1时刻的点上,形成点云 ^Pk

在下一帧点云(k+1)帧形成的时候,^Pk用来和新接受的点云(第k+1帧点云 Pk+1)一起,估计lidar的运动

假设 ^Pk 和 Pk+1 都是可用的了,然后开始找出特征点的匹配对.

对于Pk+1,找出边缘点和面点,用上一节曲率的方法. 用Ek+1和Hk+1来代表 边缘点集合和面点集合
我们将会找出^Pk 中的与之对应的边缘点和面点

在第k+1帧开始的时候,Pk+1还是空的,点在之后逐渐接收
lidar里程计在k+1帧开始接收的时候,递归的估计6自由度的运动,

在每一次迭代,用当前估计的变换,将Ek+1和Hk+1投影到tk+1时刻的坐标系中,用 ^Ek+1 和 ^Hk+1 表示投影后的点集

对于在^Ek+1 和 ^Hk+1中的每个点,需要找到在 ^Pk 中的距离最近的点 ,通过3d KD-tree的方法 . ^Pk 存在一个KD-tree中

代码解读

帧间里程计是一个单独的节点,所以整个代码可以从main函数开始

int main(int argc, char **argv)
{
    //初始化节点 laserOdometry
    ros::init(argc, argv, "laserOdometry");
    //声明句柄
    ros::NodeHandle nh;

  

 

  
1
  
2
  
3
  
4
  
5
  
6
 

ros的基本操作
初始化节点
声明句柄
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    //从服务器读取 参数 mapping_skip_frame 下采样的频率, 向后端发送数据的频率  launch文件中设置 设置为1
    //     如果为 1, do mapping 10 Hz, if 2, do mapping 5 Hz. Suggest to use 1, it will adjust frequence automaticlly -
    nh.param<int>("mapping_skip_frame", skipFrameNum, 2);

    //打印 建图的频率
    printf("Mapping %d Hz \n", 10 / skipFrameNum);


  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
 

从服务器读取 参数 mapping_skip_frame 下采样的频率 launch文件中设置 设置为1

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    // 订阅一些点云的topic   100是队列
    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);//所有的点


  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
 

订阅一些点云的topic 100是队列
角点
弱角点
面点
弱面点
所有的点

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

    //这个节点要发布的topic
    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);

  

 

  
1
  
2
  
3
  
4
  
5
  
6
 

这个节点要发布的topic

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

while (ros::ok())//整个while非常大 
    {
        ros::spinOnce();//触发一次回调    spin是很个消息都及时处理 这样会丢帧

  

 

  
1
  
2
  
3
 

开始整个while,非常大
首先调用spinOnce 触发一次回调 spin是很个消息都及时处理 这样会丢帧
下面看下各topic的回调函数

//下面的回调函数  都是将接收的点云存入 各自的队列当中
void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2)
{
    mBuf.lock();
    cornerSharpBuf.push(cornerPointsSharp2);
    mBuf.unlock();
}
void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsLessSharp2)
{
    mBuf.lock();
    cornerLessSharpBuf.push(cornerPointsLessSharp2);
    mBuf.unlock();
}

void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsFlat2)
{
    mBuf.lock();
    surfFlatBuf.push(surfPointsFlat2);
    mBuf.unlock();
}
void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsLessFlat2)
{
    mBuf.lock();
    surfLessFlatBuf.push(surfPointsLessFlat2);
    mBuf.unlock();
}
//receive all point cloud
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
{
    mBuf.lock();
    fullPointsBuf.push(laserCloudFullRes2);
    mBuf.unlock();
}

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
  
22
  
23
  
24
  
25
  
26
  
27
  
28
  
29
  
30
  
31
  
32
  
33
 

都是将接收的点云存入 各自的队列当中

std::queue<sensor_msgs::PointCloud2ConstPtr> cornerSharpBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> cornerLessSharpBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> surfFlatBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> surfLessFlatBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> fullPointsBuf;

  

 

  
1
  
2
  
3
  
4
  
5
 

存入的队列都是标准的 std::queue
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

        //判断五个消息的队列是否为空  为空则不进行整个功能的运行  进行按while的频率运行spinonce
        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();
            }

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
 

进行了两个判断
一个是队列里是否有消息,有消息才能处理
一个是队列里第一个的时间戳是否一致 同一帧的时间戳肯定是一样的如果不一样 那么 则出现错误
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

            //分别将五个点云取出来,同时转成pcl的点云格式
            mBuf.lock();
            cornerPointsSharp->clear();
            pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp);//转成pcl的点云格式
            cornerSharpBuf.pop();//去掉队列里面的第一个

            cornerPointsLessSharp->clear();
            pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp);//转成pcl的点云格式
            cornerLessSharpBuf.pop();//去掉队列里面的第一个

            surfPointsFlat->clear();
            pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat);//转成pcl的点云格式
            surfFlatBuf.pop();//去掉队列里面的第一个

            surfPointsLessFlat->clear();
            pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat);//转成pcl的点云格式
            surfLessFlatBuf.pop();//去掉队列里面的第一个

            laserCloudFullRes->clear();
            pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes);//转成pcl的点云格式
            fullPointsBuf.pop();//去掉队列里面的第一个
            mBuf.unlock();

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
  
22
 

分别将五个点云取出来,同时转成pcl的点云格式

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

            // initializing  进行初始化  等有两帧了才行
            if (!systemInited)
            {
                systemInited = true;
                std::cout << "Initialization finished \n";
            }
            else
            {//初始化完成 继续做真正的工作

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
 

进行初始化 等有两帧了才行
初始化完成 继续做真正的工作
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
在上面完成了将从上一个节点订阅的信息取出来,转成pcl形式,并完初始化工作,初始化工作就是等有两帧数据
然后继续看else里面的内容

                //取出角点和面点的 特征点 数量   相当于约束的大小,一个特征点一个约束   ceres一共6个约束
                int cornerPointsSharpNum = cornerPointsSharp->points.size();//一般为2
                int surfPointsFlatNum = surfPointsFlat->points.size();//一般为4

  

 

  
1
  
2
  
3
 

然后一个for循环进行2次的迭代求解

                //进行两次迭代求解  
                for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)
                {
                  

  

 

  
1
  
2
  
3
  
4
 

                    corner_correspondence = 0;//初始化 角点匹配的点对的数量
                    plane_correspondence = 0;//初始化 面点匹配的点对的数量

  

 

  
1
  
2
 

初始化 角点\面点匹配的点对的数量

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

                    //ceres::LossFunction *loss_function = NULL;
                    //定义 ceres 的 损失函数 0.1代表 残差大于0.1的点 ,则权重降低  小于0.1 则认为正常,不做特殊的处理
                    ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
                    //由于旋转不满足一般的加法,所以要用 ceres自带的 local Param  
                    ceres::LocalParameterization *q_parameterization =
                        new ceres::EigenQuaternionParameterization();

                    //声明ceres 的 Problem::Options
                    ceres::Problem::Options problem_options;
                    //声明ceres 的problem
                    ceres::Problem problem(problem_options);

                    //待优化的变量是帧间位姿,平移和旋转 ,这里旋转使用四元数来表示  
                    //para_q是一个数组的指针  后面跟参数的的长度  不符合普通加法则再设置 local Param 
                    problem.AddParameterBlock(para_q, 4, q_parameterization);// 添加四元数的参数块
                    problem.AddParameterBlock(para_t, 3);//添加平移的参数块

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
 

定义 ceres 的 损失函数
由于旋转不满足一般的加法,所以要用 ceres自带的 local Param
声明ceres 的 Problem::Options
声明ceres 的problem
添加四元数/平移的参数块

                    pcl::PointXYZI pointSel;//畸变校正后的点云
                    std::vector<int> pointSearchInd;//kdtree找到的最近邻点的id
                    std::vector<float> pointSearchSqDis;//kdtree找到的最近邻点的距离

  

 

  
1
  
2
  
3
 

声明这三个变量
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
然后for寻循环,遍历每个角点,在里面完成角点的约束

                    for (int i = 0; i < cornerPointsSharpNum; ++i)
                    {

  

 

  
1
  
2
 

                        //运动补偿
                        TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);
                        //在上一帧所有角点(弱角点)构成的kdtree中寻找距离当前帧最近的一个点   因为前面有初始化的判断 所有 第二帧肯定有上一帧
                        kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);

  

 

  
1
  
2
  
3
  
4
 

将该帧的点进行运动补偿
然后在上一帧的弱角点中寻找最近邻点

                        //只有小于给定门限才认为是有效约束  DISTANCE_SQ_THRESHOLD 是25
                        if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)
                        {

  

 

  
1
  
2
  
3
 

判断下找到的最近距离,距离满足要求才去找那两个点

                            //找到的最近邻点的 id 索引取出来
                            closestPointInd = pointSearchInd[0];

                            //取出这个点的 scan id  存在intensity的整数部分   因为 下一个点 的 scanid 不能和 这个一样
                            int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);

                            //
                            double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
 

找到的最近邻点的 id 索引取出来
取出这个点的 scan id 存在intensity的整数部分 因为 下一个点 的 scanid 不能和 这个一样

                            //寻找下一个角点,在刚刚角点id上下分别寻找,目的是找到最近的角点,由于其按照线束进行排序,所以就是向上找
                            for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++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)
                                {
                                    // find nearer point
                                    minPointSqDis2 = pointSqDis;//更新找到的最小的距离
                                    minPointInd2 = j;//记录点的索引id
                                }
                            }

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
  
22
  
23
  
24
  
25
  
26
  
27
  
28
  
29
 

寻找下一个角点,在刚刚角点id上下分别寻找,目的是找到最近的角点,由于其按照线束进行排序,所以就是向上找
要求找到的线束与当前线束不能太远
计算找到的点和 当前帧角点的距离
距离满足要求 那么就更新需要的最小距离, 最后得到的就是满足要求的 距离最小的点

                            //同样另一个方向寻找第2近的点
                            // 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)
                                {
                                    // find nearer point
                                    minPointSqDis2 = pointSqDis;
                                    minPointInd2 = j;
                                }
                            }

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
  
22
  
23
  
24
  
25
  
26
 

同样另一个方向寻找第2近的点

如果找到了有效的两个点,则进行ceres的角点约束的添加

                        //如果找到了有效的两个点
                        if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid
                        {
                            //取出当前角度和上一帧的两个角点
                            //当前帧的角点
                            Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x,
                                                       cornerPointsSharp->points[i].y,
                                                       cornerPointsSharp->points[i].z);
                            //上一帧的a点
                            Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x,
                                                         laserCloudCornerLast->points[closestPointInd].y,
                                                         laserCloudCornerLast->points[closestPointInd].z);
                            //上一帧的b点
                            Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x,
                                                         laserCloudCornerLast->points[minPointInd2].y,
                                                         laserCloudCornerLast->points[minPointInd2].z);

                            double s;
                            if (DISTORTION)
                                s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;//当前点的时间戳占比
                            else
                                s = 1.0;

                            //添加ceres的约束项 就是定义 CostFuction 代价函数
                            ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
                            //给problem 添加 残差项 
                            problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
                            //角点的约束次数 加 1 
                            corner_correspondence++;
                        }

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
  
22
  
23
  
24
  
25
  
26
  
27
  
28
  
29
  
30
 

把当前帧的角点和上一帧角点里面找到点a和点b取出来
构建Ceres的代价函数,其中代价函数是核心内容,这部分拿出来单说

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

                   // find correspondence for plane features
                    // 进行面点的约束
                    for (int i = 0; i < surfPointsFlatNum; ++i)
                    {
                        //去运动畸变,统一到起始点
                        TransformToStart(&(surfPointsFlat->points[i]), &pointSel);
                        //找到本帧面点的在上一帧面点里的最近邻点
                        kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);

                        int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
                        
                        //寻找另外的两个点,并添加面点的约束
                        if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)//最近邻点距离满足要求
                        {
                            closestPointInd = pointSearchInd[0];

                            // get closest point's scan ID
                            int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);
                            double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;

                            // 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;
                                    minPointInd2 = j;
                                }
                                // if in the higher scan line
                                else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
                                {
                                    minPointSqDis3 = pointSqDis;
                                    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)
                            {
                                /*取出当前面点和上一帧的上面找到的三个面点*/
                                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,
                                                                laserCloudSurfLast->points[closestPointInd].y,
                                                                laserCloudSurfLast->points[closestPointInd].z);
                                Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x,
                                                                laserCloudSurfLast->points[minPointInd2].y,
                                                                laserCloudSurfLast->points[minPointInd2].z);
                                Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x,
                                                                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;
                                //构建代价函数
                                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++;
                            }
                        }
                    }

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
  
22
  
23
  
24
  
25
  
26
  
27
  
28
  
29
  
30
  
31
  
32
  
33
  
34
  
35
  
36
  
37
  
38
  
39
  
40
  
41
  
42
  
43
  
44
  
45
  
46
  
47
  
48
  
49
  
50
  
51
  
52
  
53
  
54
  
55
  
56
  
57
  
58
  
59
  
60
  
61
  
62
  
63
  
64
  
65
  
66
  
67
  
68
  
69
  
70
  
71
  
72
  
73
  
74
  
75
  
76
  
77
  
78
  
79
  
80
  
81
  
82
  
83
  
84
  
85
  
86
  
87
  
88
  
89
  
90
  
91
  
92
  
93
  
94
  
95
  
96
  
97
  
98
  
99
  
100
  
101
  
102
  
103
  
104
  
105
 

面点的约束添加和角点差不多,不展开细说了

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

添加完角点和面点的约束后,则进行ceres的相关配置,然后进行优化了

                    //上面完成了ceres的约束添加
                    ceres::Solver::Options options;
                    //优化的相关配置
                    options.linear_solver_type = ceres::DENSE_QR;
                    options.max_num_iterations = 4;
                    options.minimizer_progress_to_stdout = false;
                    ceres::Solver::Summary summary;
                    //进行优化
                    ceres::Solve(options, &problem, &summary);

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
 

在进行了两次的优化后
进行位姿的累加

                //把计算的当前帧和上一帧的变换累加,形成相对第一帧的位姿变换,也就是世界坐标系下的位姿
                t_w_curr = t_w_curr + q_w_curr * t_last_curr;
                q_w_curr = q_w_curr * q_last_curr;

  

 

  
1
  
2
  
3
 

把计算的当前帧和上一帧的变换累加,形成相对第一帧的位姿变换,也就是世界坐标系下的位姿
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
之后发布里程计信息

            // publish odometry
            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);

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
 

用世界坐标系下的位姿发布里程计

            /*发布path*/
            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);

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
 

用里程计的信息发布path

+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

下面要做的就是为下一帧来做准备

            pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
            cornerPointsLessSharp = laserCloudCornerLast;
            laserCloudCornerLast = laserCloudTemp;//把当前帧弱角点保存为上一帧弱角点,为下一帧做准备

            laserCloudTemp = surfPointsLessFlat;
            surfPointsLessFlat = laserCloudSurfLast;
            laserCloudSurfLast = laserCloudTemp;//把当前帧弱面点保存为上一帧弱面点,为下一帧做准备

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
 

把当前帧弱角点保存为上一帧弱角点,为下一帧做准备
把当前帧弱面点保存为上一帧弱面点,为下一帧做准备

            //统计 角点和面点数量 
            laserCloudCornerLastNum = laserCloudCornerLast->points.size();
            laserCloudSurfLastNum = laserCloudSurfLast->points.size();

  

 

  
1
  
2
  
3
  
4
 

统计 角点和面点数量

            kdtreeCornerLast->setInputCloud(laserCloudCornerLast);
            kdtreeSurfLast->setInputCloud(laserCloudSurfLast);

  

 

  
1
  
2
 

设置 kdtree 为下次搜索做准备
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
下面要做的就是 发布 ROS的点云,给后面的点图优化订阅.
通过skipFrameNum控制发布的频率

            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);
            }

  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
  
22
 

这就是ALOAM的帧间里程计的完整内容.

文章来源: blog.csdn.net，作者：月照银海似蛟龙，版权归原作者所有，如需转载，请联系作者。

原文链接：blog.csdn.net/qq_32761549/article/details/120825032