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

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

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;

// 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
}

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

// 更新拟合平面的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

}

tree->setInputCloud(cloud_2d);