点云平面检测

基于PCL-SAC_RANSAC检测: my_SAC_RANSAC
基于最小二乘法检测: my_plane

#include <iostream>
#include <string>
#include <cmath>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>    // 去除离散点
#include <pcl/visualization/pcl_visualizer.h>
#include <ctime>
#include <pcl/common/transforms.h>
#include <pcl/segmentation/sac_segmentation.h>//基于采样一致性分割的类的头文件
#include <pcl/filters/extract_indices.h>//索引提取

#include <Eigen/Core>
#include<opencv2/opencv.hpp>

using namespace cv;
using namespace std;

void my_view(pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud_in, int time)
{
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewers(new pcl::visualization::PCLVisualizer("3D Viewer"));
    pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor(cloud_in, "z"); // 按照z字段进行渲染
    viewers->addPointCloud<pcl::PointXYZ>(cloud_in, fildColor, "sample cloud");
    viewers->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); // 设置点云大小

    while (!viewers->wasStopped())
    {
        viewers->spinOnce(time);
    }
}

/**
 * @brief my_SAC_RANSAC     提取点云地面及外点
 * @param cloud_src         输入原始点云
 * @param dis               输入地面高度
 * @param cloud_plane       输出地面点云
 * @param cloud_out         输出外点点云
 * @return
 */
bool my_SAC_RANSAC(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_src, float dis, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_plane, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_out)
{
    // 创建平面模型的分割对象，并设置所有参数
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setMaxIterations (500);            // 迭代次数
    seg.setDistanceThreshold (dis);
    seg.setInputCloud (cloud_src);
    seg.segment (*inliers, *coefficients);

    // 从输入云中提取平面模型内点
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud (cloud_src);
    extract.setIndices (inliers);
    extract.setNegative (false);
    // 得到与平面相关的点
    extract.filter (*cloud_plane);

    extract.setNegative (true);
    // 得到与平面外相关的点
    extract.filter (*cloud_out);

    return 0;
}



bool my_plane(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_src, float& A, float& B, float& C, float& D)
{
    pcl::PointXYZ center(0.0, 0.0, 0.0);

    for(int i=0; i<cloud_src->size(); i++)
    {
        center.x += cloud_src->at(i).x;
        center.y += cloud_src->at(i).y;
        center.z += cloud_src->at(i).z;
    }

    center.x /= cloud_src->size(); center.y /= cloud_src->size(); center.z /= cloud_src->size();
    Eigen::MatrixXf points_center(cloud_src->size(), 3);

    for(int i=0; i<cloud_src->size(); i++)
    {
        points_center(i, 0) = cloud_src->at(i).x - center.x;
        points_center(i, 1) = cloud_src->at(i).y - center.y;
        points_center(i, 2) = cloud_src->at(i).z - center.z;
    }

    Eigen::JacobiSVD<Eigen::MatrixXf> svd(points_center, Eigen::ComputeThinV);
    A = svd.matrixV()(0, 2);
    B = svd.matrixV()(1, 2);
    C = svd.matrixV()(2, 2);
    D = -(A * center.x + B * center.y + C * center.z);

    return true;
}

int main()
{
    // FOV视场角范围
    float M_pi = 3.1415926535;
    // 角度限制
    float angle_x = tan(80.0/360.0*M_pi*0.9);        
    float angle_y = tan(50.0/360.0*M_pi*0.9);         
    cout << angle_x << "," << angle_y << endl;

    float plan = 0;

    string src_path = "./data/";
    vector<cv::String> file_vec;
    cv::glob(src_path + "*.pcd", file_vec, false);
    for (std::string file_name : file_vec)
    {
       std::cout << "文件路径：" << file_name << std::endl;
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_ori(new pcl::PointCloud<pcl::PointXYZ>());
//        if (pcl::io::loadPLYFile(file_name, *cloud_ori) < 0)
        if (pcl::io::loadPCDFile(file_name, *cloud_ori) < 0)
        {
            std::cout << "文件打开失败。错误编码：001" << std::endl;
        }

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
        copyPointCloud(*cloud_ori, *cloud);

        cloud_ori->clear();
        vector<cv::Point3f> points;
        for(int i=0; i<cloud->size(); i++)
        {
            pcl::PointXYZ point;
            point.x = cloud->at(i).x * 1000;
            point.y = cloud->at(i).y * 1000;
            point.z = cloud->at(i).z * 1000;

            if(abs(point.x/point.z) < angle_x && abs(point.y/point.z) < angle_y)
            {
                cloud_ori->push_back(point);
            }
        }

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>());
        my_SAC_RANSAC(cloud_ori, 10.0, cloud_plane, cloud_out);

        // 利用最小二乘法拟合平面方程
        float A, B, C, D;
        my_plane(cloud_plane, A, B, C, D);
//        cout << A << "," << B << "," << C << "," << D << endl;
//        cout << atan(A/C)/M_pi*180.0 << "," << atan(B/C)/M_pi*180.0 << endl;

        plan += (float)cloud_plane->size()/(float)cloud_ori->size();
//        cout << "点云占比:" << (float)cloud_plane->size()/(float)cloud_ori->size() << endl;

        float z_min = 90000;
        float z_max = -100;
        for(int i=0; i<cloud_plane->size(); i++)
        {
            if(cloud_plane->at(i).z < z_min)
            {
                z_min = cloud_plane->at(i).z;
            }

            if(cloud_plane->at(i).z > z_max)
            {
                z_max = cloud_plane->at(i).z;
            }
        }

        cout << cloud_plane->size() << "," << D << "," << z_min << "," << z_max << "," << (float)cloud_plane->size()/(float)cloud_ori->size()
             << "," << atan(A/C)/M_pi*180.0 << "," << atan(B/C)/M_pi*180.0 << endl;
//        my_view(cloud_ori, 10);

    }
    plan /= (float)file_vec.size();
    cout << plan << endl;
    return 0;
}