ros实现地面过滤+欧式聚类(boundingbox)

代码解析

注:完整代码请点击这篇博客

地面分割

根据论文和开源代码进行了修改，主要实现过程是这样的

1. 首先是对点云根据z值进行排序

    // 1. 复制一份点云
    pcl::PointCloud<VPoint> cloud_org(cloud_in);  // 复制一份点云
    // cout << "点云复制完成" << endl;

    // 2.Sort on Z-axis value
    sort(cloud_in.points.begin(), cloud_in.end(), point_cmp);  // 排序

// 用户排序的参数
bool point_cmp(VPoint a, VPoint b){
    return a.z<b.z;
}

2. 对特别低的点进行滤除

    pcl::PointCloud<VPoint>::iterator it = cloud_in.points.begin();
    for (size_t i = 0; i < cloud_in.points.size(); i++) {    // 统计小于-1.5*sensor_height_的点数目
        if (cloud_in.points[i].z < -1.5 * sensor_height_) {
            it++;
        } else {
            break;
        }
    }
    cloud_in.points.erase(cloud_in.points.begin(), it);  // 清除 在地面下的点 ,因为之前是根据z排序了 所以很方便
    // std::cout<<"清除地面下的点后的点数: " <<cloud_in.points.size()<<std::endl;

3. 提取大致的地面点

// 4. Extract init ground seeds.
    extract_initial_seeds_(cloud_in);  // 获得待拟合的地面点
    g_ground_pc = g_seeds_pc;   // 待拟合的地面点

// 获得待拟合的地面点
void
GroundFit::extract_initial_seeds_(const pcl::PointCloud<VPoint>& p_sorted)
{
    // LPR is the mean of low point representative
    double sum = 0;
    int cnt = 0;
    // Calculate the mean height value.
    for(size_t i=0;i<p_sorted.points.size() && cnt<num_lpr_;i++){  // 提取20个最低的点作为种子点
        sum += p_sorted.points[i].z;
        cnt++;
    }
    double lpr_height = cnt!=0?sum/cnt:0;// in case divide by 0   // 最低的20个种子点的平均值
    g_seeds_pc->clear();
    // iterate pointcloud, filter those height is less than lpr.height+th_seeds_   // 小于lpr_height + th_seeds_ 的点作为待拟合的地面点
    for(size_t i=0;i<p_sorted.points.size();i++){
        if(p_sorted.points[i].z < lpr_height + th_seeds_){
            g_seeds_pc->points.push_back(p_sorted.points[i]);
        } else {
            break;  // 因为是按顺序遍历的 所以可以直接退出循环
        }
    }
    // return seeds points  g_seeds_pc
}

4. 循环拟合大致的地面点，

    // 5. Ground plane fitter mainloop
    for (int i = 0; i < num_iter_; i++) {    // 迭代3次

        // cout<<" -----------iter"<<"["<<i+1<<"]"<<"------------" <<endl;
        estimate_plane_();     // g_ground_pc 进行平面拟合 得到法向量normal_ 和 th_dist_d_
        g_ground_pc->clear();
        g_not_ground_pc->clear();

        //pointcloud to matrix
        MatrixXf points(cloud_org.points.size(), 3);
        int j = 0;
        for (auto p:cloud_org.points) {
            points.row(j++) << p.x, p.y, p.z;
        }

        // 得到所有点到平面的距离相关的 result
        // ground plane model
        VectorXf result = points * normal_;  // result=Ax+By+Cz
        // threshold filter
        for (int r = 0; r < result.rows(); r++) {
            if (result[r] < th_dist_d_) {      // 到拟合的平面的距离小于th_dist_的点 作为最后的地面点
                g_ground_pc->points.push_back(cloud_org[r]);
            } else {                          // 非地面点
                g_not_ground_pc->points.push_back(cloud_org[r]);
            }
        }

        // 每次迭代输出结果
        // cout<<"["<<i+1<<"]"<<" "<<"地面点: "<<g_ground_pc->points.size()<<", "<<"非地面点: "<<g_not_ground_pc->points.size()<<"\n\n\n"<<endl;
    }

将确定的地面点放到g_ground_pc中，然后再使用GroundFit::estimate_plane_()再计算得到新的大致的地面点，不断拟合下去。

// 更新拟合平面的A B C D
void
GroundFit::estimate_plane_()
{
    // Create covarian matrix in single pass.
    // TODO: compare the efficiency.
    Eigen::Matrix3f cov;
    Eigen::Vector4f pc_mean;   // 归一化坐标值
    pcl::computeMeanAndCovarianceMatrix(*g_ground_pc, cov, pc_mean); // 对地面点(最小的n个点)进行计算协方差和平均值
    // Singular Value Decomposition: SVD
    JacobiSVD<MatrixXf> svd(cov,Eigen::DecompositionOptions::ComputeFullU);
    // use the least singular vector as normal
    normal_ = (svd.matrixU().col(2));   // 取最小的特征值对应的特征向量作为法向量
    // cout<<"normal_ \n"<<normal_<<endl;
    // mean ground seeds value
    Eigen::Vector3f seeds_mean = pc_mean.head<3>();   // seeds_mean 地面点的平均值
    // cout<<"seeds_mean \n"<<seeds_mean<<endl;

    // according to normal.T*[x,y,z] = -d
    d_ = -(normal_.transpose()*seeds_mean)(0,0);  // 计算d   D=d
//    std::cout<<"d_: "<<d_<<std::endl;
    // set distance threhold to `th_dist - d`
    th_dist_d_ = th_dist_ - d_;   // ------------------------------? // 这里只考虑在拟合的平面上方的点 小于这个范围的点当做地面
//    std::cout<<"th_dist_d_=th_dist_ - d_ : "<<th_dist_d_<<std::endl;

    // return the equation parameters

}

欧式聚类

这一块比较简单，可以参考原博主，主要是为了解决激光点的稀疏性不同的问题，将原始点云划分为5个范围，分别根据不同的阈值进行欧式聚类

结果可视化

地面滤除

• 代码实现:https://github.com/VincentCheungM/Run_based_segmentation

• 论文：Fast Segmentation of 3D Point Clouds: A Paradigm on LiDAR Data for Autonomous Vehicle Applications
  

欧式聚类

遇到的问题

调用PCL库时出现segmentation fault(core dumped)错误

当我在欧式聚类中设置KdTree时,

        tree->setInputCloud(cloud_2d);

这句话会导致segmentation fault(core dumped)这个错误.
最终我把pcl使用的1.7的版本,就没报错了.

参考:
C+11编译调用PCL库时出现segmentation fault(core dumped)错误
ROS初学笔记 - C++11与PCL库冲突问题
用了pcl的地方， 程序直接崩溃 挂掉
PCL：1.7.2使用时的一个问题（core dumped与-std=c++11)
Segfault on app start using Trusty Beta with packaged PCL

参考

点云地面检测算法

无人驾驶汽车系统入门（二十五）——基于欧几里德聚类的激光雷达点云分割及ROS实现