[自动驾驶-目标检测] C++ PCL 障碍物检测

1 文章引言

由于时间原因，3D检测框部分暂时未作优化，仅部署成基础可视化功能。

在实际实现3D目标检测时，在不依靠深度学习的训练模型时，仅采用传统方法实现目标检测。现阶段由于固态激光雷达的普及，很多地方均可利用小视场的固态激光雷达叠加来进行点云式的目标检测，但由于数据集难以短时间内获取符合项目所需要的数据量并标注，因此，本文暂时采用传统方法实现目标检测功能。通过传统的地面分割与点云聚类来完成目标检测。

本文的感知部分致力于解决多激光雷达(Livox为例)叠加且地面带坡度、凹凸不平等场景的目标实时检测。

2 难点分析

1. 多激光雷达传感器通过标定融合后，会在远距离产生分层现象，尤其是在不平整路面上，很容易出现目标点云重影，从而导致目标的点云分布杂乱无序，这对目标检测来说，会严重影响检测精度。
2. 在车辆进行上下坡时，大多数点云地面分割算法会将带坡度的地面视作是非地面点云，这对后续聚类算法造成严重干扰。
3. 地面分割的阈值选取十分困难，无法广泛应用于各个场景或会产生较大波动。

3 初期思路

1. 首先从局部点云分析，每个目标对象的地面点云分布是不一样的，但根据patchwork所阐述的大部分的地面点云是处在车辆周围的约20-30米内。该文章主要通过不规则圆形的划分来确定距离与角度，从而保证周身地面点云分割。但是在遇到不平整地面与坡度时，依然很容易出现误判。可以考虑栅格二阶段判定法来确定初始地面点云。
2. 由于多传感器的数据融合后，当车辆在非平整路面容易出现震动也会将传感器之间的相对位姿造成抖动误差，进而会出现数据分层现象，从算法角度很难直接避免这个问题，该部分需要从多传感器标定方法以及去抖动方面考虑。
3. 一般近处的地面点云较远处的点云更容易分割，这是因为近处点云较多，而随着距离越远，点云也就越稀疏。因此，可以考虑采用邻近点云的特征值进行判定，提高地面分割准确度。
4. 为了提高检测速度，可以对其进行数据降维处理，比如在去除地面点云之后，将非地面点云部分以栅格投影方式转换成鸟瞰图或者标签图来进行聚类，由二维图像聚类来提高聚类速度。
5. 为了使远处与近处的地面分割效果保持一致，可以采用由近到远的特征传递的方式来进行计算约束阈值。通过统计邻近点云的特征分布(如平整度、法向量等参考值)来进行分割阈值判定。这样可以提高远处稀疏点云的地面分割精度。

…

4 初期展示(Kitti数据集)

5 初步方案

整个输入点云处理可分为三个部分：栅格化、地面分割方法、点云聚类方法。

5.1 栅格化

栅格化的方式通常分为矩阵栅格化、圆环栅格化，可以根据实际场景以及激光雷达传感器进行有效选择。本项目由于在封闭且开阔场景下，为了后续聚类降维方便处理，采用矩阵栅格化的方式，并参考patchwork++的栅格划分特点进行处理，将$X、Y$ 轴根据不同距离划分成不同大小的矩阵栅格。需要明确的是，是否需要栅格对齐，若不需要则可以按照patchwork文章的划分方式，需要对齐则需要重点考虑先后划分的顺序以及尺寸大小。栅格划分原理部分可去读一下patchwork论文。

值得注意的是本文采用以车辆坐标原点视作是栅格图的像素中心坐标。

栅格划分部分代码：

/*
*@brief DeterminePointID Fill the point cloud into the specified grid and get its grid number
*@param point_t[in] Enter the point to be judged
*@param map_axi_size[in] The dimension value of the grid coordinates
*@param num_grid[in] which grid area belongs to
*@param num_each_zone[in] Grid division width 
*@param num_grid_size[in] The size corresponding to the grid division width
*/
int PatchHeightGPF::DeterminePointID(const double point_t,
                                                                                const int map_axi_size,
                                                                                const std::vector<int> num_grid,
                                                                                const std::vector<int> num_each_zone,
                                                                                const std::vector<double> num_grid_size ){
  	int pc_id,pt_id,pc_point_id;
    double point_dis =  abs(point_t);
    if (point_dis< num_each_zone[0]) {
        pc_id = 0;
        pt_id = 0;
    }
    else if (point_dis < num_each_zone[1] ) {
        pc_id = 1;
        pt_id = num_grid[0];
    }
    else if (point_dis < num_each_zone[2] ) {
        pc_id = 2;
        pt_id = num_grid[0] + num_grid[1];
    }
    else if (point_dis < num_each_zone[3] ) {
        pc_id = 3;
        pt_id = num_grid[0] + num_grid[1] +  num_grid[2];
    }

    if(point_t > 0){
        if(pc_id == 0) pc_point_id = 0.5*map_axi_size + (pt_id + std::min(static_cast<int>(floor((point_dis) / 
                                                                 num_grid_size[pc_id])), num_grid[pc_id] - 1));
        else pc_point_id = 0.5*map_axi_size+  (pt_id + std::min(static_cast<int>(floor((point_dis - num_each_zone[pc_id - 1]) /
                                                                 num_grid_size[pc_id])), num_grid[pc_id] - 1));
    }else{
        if(pc_id == 0) pc_point_id = -1 + 0.5*map_axi_size - (pt_id + 
                std::min(static_cast<int>(floor((point_dis) / 
                num_grid_size[pc_id])), num_grid[pc_id] - 1));
        else  pc_point_id = -1 + 0.5*map_axi_size- (pt_id + 
                std::min(static_cast<int>(floor((point_dis - num_each_zone[pc_id - 1]) /
                num_grid_size[pc_id])), num_grid[pc_id] - 1));
    }
    return pc_point_id;
}

/*
*@brief PointCloud2RectModel Convert the input point cloud into
*a grid point cloud and obtain its corresponding height features
*@param src_cloud[in] Enter the point cloud that needs to be rasterized
*/
void PatchHeightGPF::PointCloud2RectModel(const pcl::PointCloud<PointT> &input_cloud) {
    /*Calculate picture channel normalization parameters*/
    noise_cloud_.clear();
    ground_cloud_.clear();
    non_ground_cloud_.clear();
    /*Coordinate system conversion (plane parameters of the previous frame)*/
    //pcl::transformPointCloud(input_cloud,input_cloud,transform_matrix_);

    /*Grid data fill*/
    for (auto const &pt : input_cloud.points) {
        double point_x = pt.x;
        double point_y = pt.y;
        double point_z = pt.z;
        if ((abs(point_x) < lidar_max_x_) &&
            (abs(point_y) < lidar_max_y_) &&
            (point_z >= pc_height_min_    && point_z < pc_height_max_)) {
            /*Determines the grid sequence number*/
            int pc_point_idx = DeterminePointID(point_x,map_len_,num_len_grid_,num_len_each_zone_,num_len_grid_size_);
            int pc_point_idy = DeterminePointID(point_y,map_wid_,num_wid_grid_,num_wid_each_zone_,num_wid_grid_size_);
            /*Sequence diagram and point cloud grid assignment filling*/
            map_param_.grid_pc_model[pc_point_idx][pc_point_idy].points.emplace_back(pt);
        }
        else{
            noise_cloud_.points.emplace_back(pt);
        }
    }
}

栅格化结果展示示意图

5.2 地面分割

在传统方法检测中，地面分割的准确度会直接影响到聚类以及去噪的难度。本文的地面分割部分分为两个阶段，即初步分割以及细化分割两个部分。

1. 首先将循环遍历出带有点云的栅格部分，对每个栅格内的点云做平面估计，其每个栅格均采用的方法为R-GPF分割方法，并将栅格点云的法向量、高度差、高度最大小值等特征存储。其中首先比较栅格的最大高度与传感器高度，再者需确保所获取的高度差小于指定高度差阈值$t{h}_{diff}$且所对应的法向量与向量$Z(0,0,1)$的夹角小于指定夹角阈值$t{h}_{angle}$即可将整个栅格内的点云视作是地面点云。
2. 将首次初步地面分割结果放入标签矩阵中，并记录未加入地面点云的栅格的序列集合$gri{d}_{non-process}$。这一步的主要目的是为了后续直接索引到未处理的栅格区域，而不需要再次遍历判定。
3. 遍历序列集合$gri{d}_{non-process}$ ,并提取邻近标签为地面的栅格特征，统计邻近栅格的特征分布(采用箱线图)，计算出其正常值(剔除异常值)作为阈值，进而进行判定。若周围未能找到则与初始栅格特征对比。
4. 最终将$gri{d}_{non-process}$内符合条件的平面点云加入地面点云，不符合条件的则将整个栅格点云加入非地面点云，实现点云二分类，实现最终的地面分割。

整个地面分割思路从图像对齐的邻近区域出发进行探究，局部平面估计代码可以参考我之前一篇博客[自动驾驶-目标检测] C++ PCL 地面点云分割或者是ERASOR文章中的R-GPF部分。

当然还有一种思路可以进行尝试，由于时间关系，还将代码未编写完成进行测试，有兴趣的朋友可以进行尝试。可以按照常规的圆环划分并对齐，只考虑射线上的栅格区域，并统计其射线上的法向量变化，找到突变严重的栅格区域视作是非地面栅格，进一步由射线上相邻栅格进行阈值判定。

以下是首次的初步地面判定代码。

void PatchHeightGPF::CoarseGroundSeg(){
    Eigen::Vector3f init_normal;
    init_normal<<0,0,1;
    for(int i=0; i<map_len_; i++){
        for(int j=0; j<map_wid_; j++){
            if(map_param_.grid_pc_model[i][j].points.size()>= grid_min_size_){
                /*Extract corresponding point cloud features*/
                pcl::PointCloud<PointT> grid_cloud,grid_ground_cloud,grid_non_ground_cloud;
                pcl::copyPointCloud(map_param_.grid_pc_model[i][j],grid_cloud);
                sort(grid_cloud.points.begin(), grid_cloud.points.end(), SortPointZ<PointT>);
                CloudFeature grid_feature;
                grid_feature.height_diff = ExtractPiecewiseground(grid_cloud,grid_ground_cloud,grid_non_ground_cloud);
                grid_feature.height_zmean = plane_mean_(2);
                grid_feature.normal = plane_normal_.head<3>();
                pcl::copyPointCloud(grid_ground_cloud,grid_feature.plane_pc);
                pcl::copyPointCloud(grid_non_ground_cloud,grid_feature.non_plane_pc);
                map_param_.grid_pc_feature[i][j] = grid_feature;
                double cos_angle = grid_feature.normal.dot(init_normal) / 
                                                        (grid_feature.normal.norm()*init_normal.norm()); //cos value
                double  angle = acos(cos_angle) * 180 / M_PI;
                Eigen::Vector4f pc_min,pc_max;
                pcl::getMinMax3D(grid_cloud,pc_min,pc_max);
                grid_feature.height_zmax = pc_max(2);
                grid_feature.height_zmin  = pc_min(2);
                if(grid_feature.height_diff < pc_height_diff_thre_ && angle < th_init_angle_ && 
                    grid_feature.height_zmax < - sensor_height_ + 0.5)JudgedAsGround(i,j);
                else JudgedAsNonGround(i,j);
            }
            else{
                /*Determine whether there is noise point cloud*/
                noise_cloud_+= map_param_.grid_pc_model[i][j];
                map_param_.grid_pc_model[i][j].clear();
            }
        }
    }
    /*Estimate initial point cloud ground*/
    if (ground_cloud_.size()>100) {
        EstimatePlane(ground_cloud_); 
        init_ground_height_ = plane_mean_(2);
        init_plane_param_.normal = plane_normal_;
        init_plane_param_.d = plane_d_;
    }
    else {
        init_ground_height_ =  -1 * sensor_height_;
        init_plane_param_.normal.resize(3);
        init_plane_param_.normal << 0,0,1;
        init_plane_param_.d = 0;
    }
    return;
}

以及二阶段的地面栅格判定代码

/*
*@brief GetNeighborData  Find point clouds whose neighboring raster belongs to the ground raster
*@param neighbor_num[out]  Output the number of adjacent ground point clouds
*@param neighbor_normal[out]  Output the average normal vector of adjacent grids
*@param len_i[in]  Enter the serial number (length) corresponding to the grid
*@param wid_j[in] Enter the serial number (width) corresponding to the grid
*/    
bool PatchHeightGPF::GetNeighborData(int & neighbor_num, Eigen::Vector3f & neighbor_normal,
                                                                                    const int len_i,const int wid_j){
    double neighbor_ground_mean = 0.;
    neighbor_normal.setZero();
    neighbor_num = 0;
    for (int i = -2; i <= 2; i++){
        if(len_i + i > map_len_ || len_i + i < 0) continue;      // Prevent search exceptions
        for (int j = -1; j <= 1; j++){
            if(wid_j+j > map_wid_  ||  wid_j+j < 0) continue; // Prevent search exceptions
            if(map_param_.map_height_lable(len_i+i,wid_j+j)==1){
                neighbor_ground_mean += map_param_.grid_pc_feature[len_i+i][wid_j+j].height_zmean;
                neighbor_normal += map_param_.grid_pc_feature[len_i+i][wid_j+j].normal;
                neighbor_num ++;
            }
        }
    }
    if (!neighbor_num) return false;
    neighbor_ground_mean /= neighbor_num;
    neighbor_normal /= neighbor_num;
    neighbor_normal /= neighbor_normal.sum();
    if (map_param_.grid_pc_feature[len_i][wid_j].height_zmean <= neighbor_ground_mean + pc_height_diff_thre_ &&
        map_param_.grid_pc_feature[len_i][wid_j].height_zmax < - sensor_height_ + 0.5)
        return true;
    else return false;
}

void PatchHeightGPF::PreciseGroundSeg(){
    /*<1> find all non-processed grid areas*/
    for (int seq = 0;  seq < non_ground_sequence.size(); seq++){
        /*non-processed grid areas*/        
        int i = non_ground_sequence[seq](0);
        int j = non_ground_sequence[seq](1);
        /*Search whether there is a ground grid around, and determine the ground points within this grid*/
        int neighbor_num = 0;
        Eigen::Vector3f  neighbor_normal;
        if(GetNeighborData(neighbor_num,neighbor_normal,i,j)) JudgedAsGround(i,j);
        else{
            PlaneParam grid_plane_param,neighbor_plane_param;
             /*Need to do plane parameter determination (based on the initial ground plane determination)*/
            neighbor_plane_param = init_plane_param_;
            grid_plane_param.normal = map_param_.grid_pc_feature[i][j].normal;
            if (!neighbor_num){
                /*To find the adjacent ground point cloud, the judgment parameters need to be updated*/
                neighbor_plane_param.normal = neighbor_normal;
            }
            /*Judging whether the conditions are met (angle judgment)*/
            double cos_angle = grid_plane_param.normal.dot(neighbor_plane_param.normal) / 
                                                    (grid_plane_param.normal.norm()*neighbor_plane_param.normal.norm()); //cos value
            double  angle = acos(cos_angle) * 180 / M_PI;
            if(angle < th_angle_) ground_cloud_+= map_param_.grid_pc_feature[i][j].plane_pc;
            else non_ground_cloud_ += map_param_.grid_pc_feature[i][j].plane_pc;
            non_ground_cloud_ += map_param_.grid_pc_feature[i][j].non_plane_pc;;
        }
    }
    return;
}

5.2 点云聚类

点云聚类是一个比较经典的研究领域，较为简单的在PCL库内有很多聚类方法，也包括之前写了一篇较为简单的点云聚类方法—[自动驾驶-目标检测] C++ PCL 连通域点云聚类，不过基于八叉树的聚类方法虽然可以提高分割精度，但其计算量较大，可以依然转为二维的连通域聚类方式，提高聚类速度。

处理思路很简单，只是将点云转换成二维图像即可，同样是利用不同尺寸的矩形栅格划分来建立二维鸟瞰图，不过此时划分方式是根据指定分割阈值进行划分，一般设定10cm为一个栅格，通过线性函数来划分远处栅格。进一步利用图像连通域算法进行聚类。最后将不同标签的栅格归类即可。

而3D检测框可以采用PCA方式进行绘制，将聚类好的点云分别提取出来，投影至估计出来的地平面上，通过PCA方法计算出所在的法向量作为地面上的$X$轴方向即可绘制出3D检测框。

…
未完待续

6 参考文献

1. [自动驾驶-目标检测] C++ PCL 连通域点云聚类
2. [自动驾驶-目标检测] C++ PCL 地面点云分割
3. Lim H , Hwang S , Myung H . ERASOR: Egocentric Ratio of Pseudo Occupancy-based Dynamic Object Removal for Static 3D Point Cloud Map Building[J]. IEEE Robotics and Automation Letters, 2021.
4. Lim H , Oh M , Myung H . Patchwork: Concentric Zone-based Region-wise Ground Segmentation with Ground Likelihood Estimation Using a 3D LiDAR Sensor[J]. IEEE Robotics and Automation Letters, 2021.
5. Seungjae Lee, Hyungtae Lim, Hyun Myung. Patchwork++: Fast and Robust Ground Segmentation Solving Partial Under-Segmentation Using 3D Point Cloud. Computer Vision and Pattern Recognition (cs.CV)
6. Zermas D, Izzat I, Papanikolopoulos N. Fast segmentation of 3d point clouds: A paradigm on lidar data for autonomous vehicle applications[C]2017 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2017: 5067-5073.
7. …