A_loam源码解析三

laserMapping.cpp实现自动驾驶后端算法

原创已于 2022-07-06 00:33:51 修改 · 707 阅读

3 ·

CC 4.0 BY-SA版权

文章标签：

#自动驾驶 #c++ #算法

于 2022-07-06 00:33:39 首次发布

本文围绕laserMapping.cpp展开，介绍其在自动驾驶后端算法中的应用。包括设置节点、降采样、订阅话题等步骤，阐述了scan_to_map算法，将当前帧与地图匹配以获取更准确位姿和构建地图，还提及栅格地图的维护及优化求解位姿等内容。

2.3laserMapping.cpp

1、设置节点

ros::init(argc, argv ,"laserMapping");
ros::NodeHandle nh;

2、降采样

float lineRes = 0;     //线特征的点云分辨率
  float planeRes = 0;
  nh.param<float>("mapping_line_resolution", lineRes, 0.4);  //下采样 ，体素滤波的参数
  nh.param<float>("mapping_plane_resolution",planeRes,0.8);

  downSizeFilterCorner.setLeafSize(lineRes,lineRes,lineRes); //体素滤波器的边长0.4*0.4*0.4
  downSizeFilterSurf.setLeafSize(planeRes,planeRes,planeRes);

3、订阅点云以及起始位姿

ros::Subscriber subLaserCloudCornerLast = nh.subscriber<sensor_msgs::PointCloud2>("/laser_cloud_corner_last",100,laserCloudCornerLastHandler);//

  ros::Subscriber subLaserCloudSurfLast = nh.subscriber<sensor_msgs::PointCloud2>("/laser_cloud_surf_last",100,laserCloudSurfLastHandle);

  ros::Subscriber subLaserCloudFullRes = nh.subscriber<sensor_msgs::PointCloud2>("/velodyne_cloud_3",100,laserCloudFullResHandle);

  ros::Subscriber subLaserOdometry = nh.subscriber<nav_msgs::Odometry>("/laser_odom_to_init",100,laserOdometryHandle);

1）使用了四个回调函数，接收上一节点的话题

std::queue<sensor_msgs::PointCloud2ConstPtr> cornerLastBuf;   //队列
std::queue<sensor_msgs::PointCloud2ConstPtr> surfLastBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> fullResBuf;
std::queue<nav_msgs::Odometry::ConstPtr> odometryBuf;

//回调函数中将消息送入各自队列
void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudCornerLast2)
{
  mBuf.lock();
  cornerLastBuf.push(laserCloudCornerLast2);
  mBuf.unlock();
}

4、定义需要发布的话题

5、以栅格的格式，存储后端地图 21x21x11=4851个栅格

 for(int i=0; i< laserCloudNum;i++)
  {
    laserCloudCornerArray[i].reset(new pcl::PointCloud<PointType>());    //存储后端地图的数组，栅格的格式
    laserCloudSurArray[i].reset(new pcl::PointCloud<PointType>());
  }

6、进入处理主线程 process

直接进入while循环

1）确保每个队列都有值

2）以cornerLastBuf位基准，把时间戳小于其的全部POP出去清空残留的数据实时性，内存考虑时间同步

 while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
                odometryBuf.pop();
            if (odometryBuf.empty())
            {
                mBuf.unlock();
                break;
            }

3）判断每个话题的时间戳是否一致

原则上取出来的时间戳都是一样的，如果不一样说明有问题

4）将ROS消息转换为pcl点云，将队列中的值推出

laserCloudSurfLast->clear();
pcl::fromROSMsg(*surfLastBuf.front(), *laserCloudSurfLast);
surfLastBuf.pop();

5）lidar odom的结果转换成eigen形式

q_wodom_curr.x() = odometryBuf.front()->pose.pose.orientation.x;
q_wodom_curr.y() = odometryBuf.front()->pose.pose.orientation.y;
q_wodom_curr.z() = odometryBuf.front()->pose.pose.orientation.z;
q_wodom_curr.w() = odometryBuf.front()->pose.pose.orientation.w;
t_wodom_curr.x() = odometryBuf.front()->pose.pose.position.x;
t_wodom_curr.y() = odometryBuf.front()->pose.pose.position.y;
t_wodom_curr.z() = odometryBuf.front()->pose.pose.position.z;
odometryBuf.pop();
//考虑到实时性，就把队列里其他信息都pop出去，不然可能出现延时的情况

6）根据前端的结果，得到后端的一个初始估计值

不同于前端的scan_to_scan的过程，LOAM的后端是scan_to_map的算法，就是把当前帧和地图进行匹配，得到更准确的位姿的同时构建更好的地图；scan_to_scan精度高但是计算量大。

前端里程计定期向后端发送位姿 Todom_curr，但是在mapping中，需要的是Tmap_curr，因此，mapping模块就是需要估计出odom坐标系和map坐标系之间的相对位姿变化Tmap_odom

$$
T_{map\_curr}=T_{map\_odom}\times T_{odom\_curr}
$$

transformAssociateToMap();

从上一节点可以获得当前里程计的位姿 q_wodom_curr

q_wmap_wodom 为里程计位姿到地图坐标系的变换

void transformAssociateToMap()
{
  //T_w_curr = T_w_last * T_last_curr(from lidar odom)
  q_w_curr = q_wmap_wodom * q_wodom_curr;    //当前帧配准至地图坐标系下
  t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
}

transformUpdate() odom到map之间的位姿变换

void transformUpdate()
{
  q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();
  t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;
}

7）栅格地图的维护

地图通常是当前帧通过匹配得到地图坐标系下的准确位姿之后拼接而成，如果保留所有的拼接点云，随着时间的运行，内存会吃不消；因此，考虑存储离当前帧比较近的部分地图，同时，为了地图的更新和调整，在原始LOAM中，使用的是基于栅格的地图存储方式。

具体来说，就是将整个地图分成21x21x11个栅格，每个栅格是一个边长为50cm的正方体，当地图逐渐增加，栅格之外的部分就被舍弃；但是如果当前位姿远离栅格的覆盖范围，则地图也就没有意义了；因此，栅格也需要随着当前位姿动态调整，从而保证可以从栅格地图中取出离当前位姿比较近的点云来进行sacn_to_map算法，从而获得最有位姿估计；

//根据初始估计值计算寻找当前位姿在地图中的索引，一个各自边长是50m 下
      //后端的地图本质是以一个当前点为中心，一个栅格地图
      int centerCubeI = int((t_w_curr.x() + 25.0) /50.0 ) + laserCloudCenWidth;    //估计当前帧落在哪个栅格 laserCloudCenWidth  平移量
      int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;
      int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;

      //由于c语言的取整是向零取整，如-1.66就取成了-1，但实际行该是-2，因此这里自减1
      if (t_w_curr.x() + 25.0 < 0)
                centerCubeI--;
      if (t_w_curr.y() + 25.0 < 0)
                centerCubeJ--;
      if (t_w_curr.z() + 25.0 < 0)
                centerCubeK--;
      //如果当前的栅格索引小于3，就说明当前点快接近地图边界了，需要进行调整，相当于地图整体向x正方向移动

同理其他方向；

以上操作相当于维护了一个局部地图，保证当前帧不在这个局部地图的边缘，这样才可以从地图中获取足够的约束

8）scan_to_map1 5x5x3

在栅格地图在取一个局部地图 250x*250 x150m

//局部地图的栅格大小
const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth; //4851
//局部地图下的局部地图索引
int laserCloudValidInd[125];
int laserCloudSurroundInd[125];

int laserCloudValidNum = 0;
int laserCloudSurroundNum = 0;
//从当前格子为中心，选中地图中一定范围内的点云   开始scan_to_map
for(int i = centerCubeI-2; i<=centerCubeI +2; i++)
{
    for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++)
    {
        for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++)
        {
            //保证索引不越界
            if (i >= 0 && i < laserCloudWidth &&j >= 0 && j < laserCloudHeight &&
                k >= 0 && k < laserCloudDepth)
            { 
              //把各自的索引记录下来   在栅格地图在取一个局部地图 250*250*150m
            laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
            laserCloudValidNum++;
            laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
            laserCloudSurroundNum++;
        }
    }
}
}

9) scan_to_map2

地图分为线地图和面地图

      //局部地图
      laserCloudCornerFromMap ->clear();  //线地图
      laserCloudSurfFromMap -> clear();   //面地图

      //开始构建用来这一帧优化的小的局部地图
      for (int i = 0; i < laserCloudValidNum; i++)
      {
            *laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
            *laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
      }
     int laserCloudCornerFromMapNum = laserCloudCornerFromMap->points.size();
     int laserCloudSurfFromMapNum = laserCloudSurfFromMap->points.size();

      //为了减少运算量，对点云进行下采样
      pcl::PointCloud<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());
      downSizeFilterCorner.setInputCloud(laserCloudCornerLast);   //下采样的点云，从前端来的当真帧的角点
      downSizeFilterCorner.filter(*laserCloudCornerStack);        //下采样的结果
      int laserCloudCornerStackNum = laserCloudCornerStack->points.size();

      pcl::PointCloud<PointType>::Ptr laserCloudSurfStack(new pcl::PointCloud<PointType>());
      downSizeFilterSurf.setInputCloud(laserCloudSurfLast);
      downSizeFilterSurf.filter(*laserCloudSurfStack);
      int laserCloudSurfStackNum = laserCloudSurfStack->points.size();    //下采样后面点的个数

10）scan_to_map3

1局部地图中的角点至少需要10个，面点50个

if (laserCloudCornerFromMapNum > 10 && laserCloudSurfFromMapNum > 50)
      {
        TicToc t_opt;
        TicToc t_tree;
        //送进kdtree便于最近搜索
        kdtreeCornerFromMap ->setInputCloud(laserCloudCornerFromMap);
        kdtreeSurfFromMap ->setInputCloud(laserCloudSurfFromMap);

2建立对应关系，迭代次数不超过两次使用ceres_solver

在前端里程计部分，通过当前帧的线面特征和上一帧的先面特征进行匹配，构建约束，进行优化求解；

在后端的当前帧和地图匹配时，需要从地图里寻找线面特征的约束对，由于没有线速信息，需要额外的信息来判断；

for(int iterCount = 0; iterCount < 2; iterCount++)
        {
            ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
            ceres::LocalParameterization *q_parameterization =
                        new ceres::EigenQuaternionParameterization();
            ceres::Problem::Options problem_options;

            ceres::Problem problem(problem_options);
            problem.AddParameterBlock(parameters, 4, q_parameterization);
            problem.AddParameterBlock(parameters + 4, 3);

            int corner_num = 0;

3构建角点相关的约束

地图中寻找离该点最近的五个点，求特征值和特征向量，构建方向上的ab两点

通过kdtree在地图中找到5个最近的线特征，为了判断其是否符合线特征的特性，需要进行特征值分解；如果5个点都在一条直线上，他们只有一个主方向，特征值是一个大特征值以及两个小特征值，最大特征值对应的特征向量就是直线的方向向量；

for(int i=0;i<laserCloudCornerStackNum ; i++)
            {
                point0ri = laserCloudCornerStack -> points[i];
                //把当前点根据初值投到地图坐标系下
                pointAssociateToMap (&pointOri, &pointSel);
                //地图中寻找该点最近的五个点
                kdtreeCornerFromMap->nearestKSearch(pointSel,5,pointSearchInd,pointSearchSqDis);
                //按照距离排序，pointSearchSqDis[4]是最远的点，pointSearchInd是点的索引
                
                //判断最远的点距离不能超过1m,否则就是无效约束
                if(pointSearchSqDis[4] < 1.0)
                {
                    std::vector<Eigen::Vector3d> nearCorners;
                    Eigen::Vector3d center(0, 0, 0);
                    for (int j = 0; j < 5; j++)
                    {
                        Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,laserCloudCornerFromMap->points[pointSearchInd[j]].y,laserCloudCornerFromMap->points[pointSearchInd[j]].z);
                        center = center + tmp;
                        nearCorners.push_back(tmp);
                    }
                    //计算这五个点的均值
                    center = center / 5.0;
                }
                Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();   //3*3
                //构建协方差矩阵?
                for (int j=0; j<5; j++)
                {
                    Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;  //保证数值稳定性
                    covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();  //3*1  * 1*3
                }
                //进行特征值分解
                Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);
                //根据特征值分解情况看看是不是真正的线特征
                //特征向量就是线特征的方向
                Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);  //取出最大的特诊向量
                Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
                //最大特征值是次大的3倍，认为是线特征
                if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])
                { 
                    Eigen::Vector3d point_on_line = center;
                    //构建中心点，在特征方向上的两个点 a b
                    Eigen::Vector3d point_a, point_b;
                    point_a = 0.1 * unit_direction + point_on_line;
                    point_b = -0.1 * unit_direction + point_on_line;
                    // 选了五个点，确定中心点和特征方向，再构建方向上的两个虚拟点ab  
                    // 将当前点，虚拟点AB送入求解器
                    //构建约束，和lidar odom约束一致
                    ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
                    problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
                    corner_num++;   
                }
            }

4构建面点的相关约束

通过kdtree在地图中找到最近的5个面特征，代码选用的是平面拟合的方式，特征分解：选出最小特征值对应的特征向量及平面点的法向量；平面方程如下：

$$
Ax+By+Cz+1=0
$$

分别求出5个点到求出平面的距离

int surf_num = 0；
            for (int i=0; i<laserCloudSurfStackNum ; i++)
            {
                point0ri = laserCloudSurfStack ->points[i];
                pointAssociateToMap(&pointOri, &pointSel);  //投影
                kdtreeSurfFromMap ->nearestKSearch(pointSel,5,pointSearchInd,pointSearchSqDis);

                Eigen::Matrix<double,5,3> matA0;
                Eigen::Matrix<double,5,1> matB0 = -1*Eigen::Matrix<double,5,1>::ones();
                //构建平面方程Ax+By+Cz+1=0
                //通过构建一个超定方程来求解这个平面方程
                if(pointSearchSqDis[4] < 1.0)
                {
                    for(int j=0; j<5; j++ )
                    {
                        matA0(j,0) = laserCloudSurfStack->points[pointSearchInd[j]].x;
                        matA0(j,1) = laserCloudSurfStack->points[pointSearchInd[j]].y;
                        matA0(j,2) = laserCloudSurfStack->points[pointSearchInd[j]].z;
                    }
                    //调用eigen接口求解该方程
                    Eigen::Vector3d norm = matA0.colPivHouseholder().solve(matB0); //norm为3*1的向量，解ABC
                    double negative_OA_dot_norm = 1/norm.norm(); //向量模的倒数
                    //法向量归一化
                    norm.normValid();

                    bool planeValid = true;
                    //根据求出的平面方程进行校验，看看是不是符合平面约束
                    for(int j=0; j<5;j++)
                    {
                        //求点到平面的距离，阈值0.2
                        if(fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
                                         norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
                                         norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2)
                        {
                            planeValid = false;//如果点到平面的距离太远，则认为这是个拟合不好的平面
                            break;
                        }
                        Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
                        if (planeValid)
                        {
                            ceres::CostFunction *cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);
                            problem.AddResidualBlock(cost_function, loss_function, parameters, param eters + 4);  //残差块
                            surf_num++;
                        }
                    }
                }
            }
        }

11）构建优化问题求解位姿

ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.max_num_iterations = 4;
options.minimizer_progress_to_stdout = false;
options.check_gradients = false;
options.gradient_check_relative_precision = 1e-4;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

12）地图位姿更新

//将优化后的当前帧角点加到局部地图中去
      for(int i=0; i < laserCloudCornerStackNum; i++)
      {
        //该点根据位姿投影到地图坐标系
        pointAssociateToMap(&laserCloudCornerStack ->points[i],&pointSel);
        //计算出这个点所在格子的索引
        int CubeI = int((pointSel.x+25.0)/50.0) + laserCloudCenWidth;
        int CubeJ = int((pointSel.y+25.0)/50.0) + laserCloudCenHeight;
        int CubeK = int((pointSel.z+25.0)/50.0) + laserCloudCenDepth;
        //同样负数做相应的操作
        if(pointSel.x +25 <0)
            CubeI--;
        if(pointSel.y +25 <0)
            CubeJ--;
        if(pointSel.z +25 <0)
            CubeK--;
        //如果超过边界就算了
        if (cubeI >= 0 && cubeI < laserCloudWidth &&
                    cubeJ >= 0 && cubeJ < laserCloudHeight &&
                    cubeK >= 0 && cubeK < laserCloudDepth)
        
        {
            //根据三维xyz索引，计算在一维坐标的索引
            int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
            laserCloudCornerArray[cubeInd]->push_back(pointSel); //把点放进相应的格子
        }
      }
      //面点也做同样的处理
      for(int i=0; i < laserCloudSurfStackNum; i++)
      {
        //该点根据位姿投影到地图坐标系
        pointAssociateToMap(&laserCloudSurfStack ->points[i],&pointSel);
        //计算出这个点所在格子的索引
        int CubeI = int((pointSel.x+25.0)/50.0) + laserCloudCenWidth;
        int CubeJ = int((pointSel.y+25.0)/50.0) + laserCloudCenHeight;
        int CubeK = int((pointSel.z+25.0)/50.0) + laserCloudCenDepth;
        //同样负数做相应的操作
        if(pointSel.x +25 <0)
            CubeI--;
        if(pointSel.y +25 <0)
            CubeJ--;
        if(pointSel.z +25 <0)
            CubeK--;
        //如果超过边界就算了
        if (cubeI >= 0 && cubeI < laserCloudWidth &&
                    cubeJ >= 0 && cubeJ < laserCloudHeight &&
                    cubeK >= 0 && cubeK < laserCloudDepth)
        
        {
            //根据三维xyz索引，计算在一维坐标的索引
            int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
            laserCloudSurfArray[cubeInd]->push_back(pointSel); //把点放进相应的格子
        }
      }