（编程）点云地面分割——点云格网化

1.介绍

将点云格网化

读取每个格网内点云高程方向的统计特征，极值、均值、方差

判断与邻域内其他格网的相关性

例如均值之差<2cm等

2.代码

#include <string>
#include<iostream>
#include <cstdlib>
#include<fstream>
#include <sstream>	
#include <Eigen/Core>
#include <opencv2/opencv.hpp>
#include <pcl/io/ply_io.h>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/console/parse.h>
#include <pcl/registration/icp.h>
#include <pcl/common/common.h> 
#include <pcl/filters/passthrough.h> 
#include <boost/thread/thread.hpp>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/progressive_morphological_filter.h>

int main()
{
    /*读取点云文件*/
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("../data/subway.pcd", *cloud) == -1) 
    {
        PCL_ERROR("Couldn't read file\n");
        return(-1);
    }
    /*读取点云范围*/
    double x_min = 10000; double y_min = 10000;
    double x_max = 0; double y_max = 0;
    for (size_t i = 0; i < cloud->points.size(); ++i) 
    {
        if(cloud->points[i].x <= x_min) x_min = cloud->points[i].x;
        if(cloud->points[i].y <= y_min) y_min = cloud->points[i].y;
        if(cloud->points[i].x >= x_max) x_max = cloud->points[i].x;
        if(cloud->points[i].y >= y_max) y_max = cloud->points[i].y;
    }
    // cout << (int)x_min << "  ,  " << (int)y_min << endl;
    // cout << (int)x_max + 1 << "  ,  " << (int)y_max + 1 << endl;
    /*设置格网参数*/
    double pt0_x = (int)x_min; double pt0_y = (int)y_min;
    double pt1_x = (int)x_max + 1; double pt1_y = (int)y_max + 1;
    double grid_width = 0.05;
    int grid_num = (int) (pt1_x - pt0_x) / grid_width * (pt1_y - pt0_y) / grid_width;
    /*生成格网*/
    double grid[grid_num][5];
    for(int i = 0; i < grid_num; ++i)
    {
        int grid_num_y = (int) (pt1_y - pt0_y) / grid_width;
        int grid_num_x_i = (int) (i / grid_num_y);
        int grid_num_y_i = (int) (i - grid_num_x_i * grid_num_y);
        double i_y_start = grid_num_y_i * grid_width + pt0_y;
        double i_x_start = grid_num_x_i * grid_width + pt0_x;
        std::vector<double> grid_pt_z;
        int num_grid_pt_z = 0;
        for (size_t j = 0; j < cloud->points.size(); ++j) 
        {
           if(cloud->points[j].x <= i_x_start + grid_width && cloud->points[j].x >= i_x_start && cloud->points[j].y <= i_y_start + grid_width && cloud->points[j].y >= i_y_start)
           {
               grid_pt_z.push_back(cloud->points[j].z);
               num_grid_pt_z ++;
           }
        }
        if(num_grid_pt_z <= 1) continue;
        double z_sum = 0; double z_min = 10000; double z_max = 0;
        for(int k = 0; k < num_grid_pt_z; ++k)
        {
            z_sum = z_sum + grid_pt_z[k];
            if(grid_pt_z[k] > z_max) z_max = grid_pt_z[k];
            if(grid_pt_z[k] < z_min) z_min = grid_pt_z[k];
        }
        double z_avg = z_sum / num_grid_pt_z;
        double z_sig_sum = 0;
        for(int k2 = 0; k2 < num_grid_pt_z; ++k2) z_sig_sum = z_sig_sum + (grid_pt_z[k2] - z_avg) * (grid_pt_z[k2] - z_avg);
        double z_sig = z_sig_sum / num_grid_pt_z;
        grid[i][0] = num_grid_pt_z;
        grid[i][1] = z_max;
        grid[i][2] = z_min;
        grid[i][3] = z_avg;
        grid[i][4] = z_sig;
        grid_pt_z.clear();
    }
    /*判断地面*/
    pcl::PointCloud<pcl::PointXYZ> ground_cloud;
    for(int i = 0; i < grid_num; ++i)
    {
        int grid_num_y = (int) (pt1_y - pt0_y) / grid_width;
        if(i < grid_num_y) continue;
        if(i > grid_num - grid_num_y) continue;
        int mod_x = i - ((int) (i / grid_num_y)) * grid_num_y;
        if(mod_x == 0 || mod_x == grid_num_y - 1) continue;
        double z_avg_1 = grid[i][3];
        double z_avg_2 = grid[i - 1][3];
        double z_avg_3 = grid[i + 1][3];
        double z_avg_4 = grid[i - grid_num_y][3];
        double z_avg_5 = grid[i + grid_num_y][3];
        if(abs(z_avg_1 - z_avg_2) <= 0.02 && abs(z_avg_1 - z_avg_3) <= 0.02 && abs(z_avg_1 - z_avg_4) <= 0.02 && abs(z_avg_1 - z_avg_5) <= 0.02)
        {
            int x_num_i = (int) i / grid_num_y;
            int y_num_i = i -  (int) i / grid_num_y * grid_num_y;
            double x_start = x_num_i * grid_width + pt0_x;
            double y_start = y_num_i * grid_width + pt0_y;
            for (size_t j = 0; j < cloud->points.size(); ++j) 
            {
                if(cloud->points[j].x <= x_start + grid_width && cloud->points[j].x >= x_start && cloud->points[j].y <= y_start + grid_width && cloud->points[j].y >= y_start) ground_cloud.push_back(cloud->points[j]);
            }
        }
    }
    /*存储与显示*/
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("cloud show"));
    int v1 = 0;
    viewer->createViewPort(0, 0, 1.0, 1.0, v1);
    viewer->setBackgroundColor(255,255, 255, v1);
    viewer->setBackgroundColor(255,255, 255);
    pcl::PointCloud<pcl::PointXYZ>::Ptr g_cloud = ground_cloud.makeShared();
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> raw(cloud, 0, 0, 0);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> g_pt(g_cloud, 255,0, 0);
    viewer->addPointCloud(cloud, raw, "source cloud", v1);
    viewer->addPointCloud(g_cloud, g_pt, "select ground point", v1);
    // std::string path = "../ground.pcd";
    // pcl::io::savePCDFile(path, *g_cloud);
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(10000));
    }
    system("pause");
    return 0;
}

3.示例

（图中红色为地面点云）