pcl学习笔记--ransac地面去除

#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <unordered_set>

using namespace std;
int main()
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PCDReader reader;
    reader.read("/data/1.pcd", *cloud);
    int num_points = cloud->points.size();
    std::unordered_set<int> inliersResult;    //定义一个哈希结构的数组
    int maxIterations = 40;                   //迭代次数
    while (maxIterations--)  //
    {
        std::unordered_set<int> inliers;      //存放平面的内点，平面上的点也属于平面的内点
        //因为一开始定义inliers，内点并没有存在，所以随机在原始点云中随机选取了三个点作为点云的求取平面系数的三个点
        while (inliers.size() < 3)  //当内点小于3 就随机选取一个点 放入内点中 也就是需要利用到三个内点
        {
            inliers.insert(rand() % num_points);   //产生 0~num_points 中的一个随机数
        }
        // 需要至少三个点 才能找到地面
        float x1, y1, z1, x2, y2, z2, x3, y3, z3;
        auto itr = inliers.begin();  //auto 自动类型，从内点中选三个点
        x1 = cloud->points[*itr].x;
        y1 = cloud->points[*itr].y;
        z1 = cloud->points[*itr].z;
        itr++;
        x2 = cloud->points[*itr].x;
        y2 = cloud->points[*itr].y;
        z2 = cloud->points[*itr].z;
        itr++;
        x3 = cloud->points[*itr].x;
        y3 = cloud->points[*itr].y;
        z3 = cloud->points[*itr].z;

        //计算平面系数
        float a, b, c, d, sqrt_abc;
        a = (y2 - y1) * (z3 - z1) - (z2 - z1) * (y3 - y1);
        b = (z2 - z1) * (x3 - x1) - (x2 - x1) * (z3 - z1);
        c = (x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1);
        d = -(a * x1 + b * y1 + c * z1);
        sqrt_abc = sqrt(a * a + b * b + c * c);

        std::cerr << a << " "<< b << " "<<  c << " "<< d << endl;
        //分别计算这些点到平面的距离
        for (int i = 0; i < num_points; i++)
        {
            if (inliers.count(i) > 0) //判断一下有没有内点
                { // that means: if the inlier in already exist, we dont need do anymore
                continue;
                }
            pcl::PointXYZI point = cloud->points[i];
            float x = point.x;
            float y = point.y;
            float z = point.z;
            float dist = fabs(a * x + b * y + c * z + d) / sqrt_abc; // calculate the distance between other points and plane
            float distanceTol = 0.35;
            if (dist < distanceTol)
            {
                inliers.insert(i); //如果点云中的点 距离平面距离的点比较远 那么该点则视为内点
            }
            if (inliers.size() > inliersResult.size())// 保存内点最多的平面
            {
                inliersResult = inliers;
            }
        }
    }
    //迭代结束后，所有属于平面上的内点都存放在inliersResult中
    //std::unordered_set<int> inliersResult;
    cout << inliersResult.size() << endl;  //1633

    //创建两片点云，一片存放地面，一片存放其他点
    pcl::PointCloud<pcl::PointXYZI>::Ptr out_plane(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr in_plane(new pcl::PointCloud<pcl::PointXYZ>);

    for (int i = 0; i < num_points; i++)
    {
        pcl::PointXYZI pt = cloud->points[i];
        pcl::PointXYZ temp;

        temp.x = pt.x;
        temp.y = pt.y;
        temp.z = pt.z;
        if (inliersResult.count(i))
        {
            out_plane->points.push_back(pt);
        }
        else
        {
            in_plane->points.push_back(temp);
        }
    }

    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("显示窗口"));  //窗口显示点云
    viewer->addPointCloud(in_plane, "*cloud");
    viewer->resetCamera();		//相机点重置
    viewer->spin();
    system("pause");
    return (0);
}

CMakeLists.txt 

cmake_minimum_required(VERSION 3.20)
project(day3)
set(CMAKE_CXX_STANDARD 14)
find_package(PCL 1.7 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
FIND_PACKAGE( PCL REQUIRED COMPONENTS common io visualization )
add_definitions(${PCL_DEFINITIONS})
add_executable(day3 ransac.cpp)
target_link_libraries(day3 ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES} ${PCL_LIBRARIES})