【点云处理】点云法线联合随机采样一致性(RANSAC)分割地面点

        在之前的代码中尝试直接使用随机一致性采样来分割地面点，但是在我的实验环境下效果很差，始终没有找到地表这个平面。这里尝试一种新思路，计算点云的法线。因为地面点云的法线z值较高，通过阈值先过滤出非地面点云，再对剩下的点云

进行随机一致性采样得到地面点云。

【代码】

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 

template
class GroundSeg:public pcl::PCLBase
{
public:
    GroundSeg(){
    }

    void seg(pcl::PointIndices::Ptr& ground_indices,pcl::PointIndices::Ptr&  no_ground_indices) {
        bool init_ret = this->initCompute();
        if(init_ret == false) {
            return;
        }
        pcl::console::TicToc tt;
        typename pcl::search::KdTree::Ptr kdtree_ptr(new pcl::search::KdTree);
        pcl::PointCloud::Ptr normal_ptr(new pcl::PointCloud);
        pcl::PointIndices::Ptr indices(new pcl::PointIndices());
        indices->indices.reserve(this->getInputCloud()->size());
        {
            tt.tic();
            pcl::NormalEstimationOMP normal_est_omp;
            normal_est_omp.setInputCloud(this->getInputCloud());
            //normal_est_omp.setIndices(this->getIndices());
            normal_est_omp.setSearchMethod(kdtree_ptr);
            normal_est_omp.setViewPoint(1e9,1e9,1e9);
            normal_est_omp.setKSearch(35); //neighbour size
            normal_est_omp.compute(*normal_ptr);
            {
                for(int i = 0; i < normal_ptr->points.size();++i) {
                    const auto normal = normal_ptr->at(i);
                    const double abs_normal_z = std::abs(normal.normal_z);
                    if(std::isnormal(abs_normal_z) && abs_normal_z > 0.5) {
                        indices->indices.push_back(i);
                    }
                }
            }
            std::cout <<__FUNCTION__ << ":" <<__LINE__<<",calc normal cost " << tt.toc() << " ms" << std::endl;
        }
#if 1
        {
            tt.tic();
            pcl::SACSegmentation seg;
            pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
            //pcl::PointIndices::Ptr ground_inliers(new pcl::PointIndices);

            seg.setOptimizeCoefficients(true);
            seg.setModelType(pcl::SACMODEL_PLANE);
            seg.setDistanceThreshold(0.3);
            seg.setInputCloud(this->getInputCloud());
            seg.setIndices(indices);
            //std::cout << __FUNCTION__ << __LINE__ << std::endl;
            seg.segment(*ground_indices,*coefficients);
            {
                pcl::ExtractIndices extract;   //点提取对象
                extract.setInputCloud(this->getInputCloud());
                extract.setIndices(ground_indices);
                extract.setNegative(true);//设置成true是保存滤波后剩余的点，false是保存在区域内的点
                extract.filter(no_ground_indices->indices);
            }
            std::cout <<__FUNCTION__ << ":" <<__LINE__<<",ransac seg cost " << tt.toc() << " ms" << std::endl;
        }
#endif
        this->deinitCompute();
        //std::cout << __FUNCTION__ << __LINE__ <<"end"<< std::endl;
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "n_ground_seg");

    ros::NodeHandle node;
    ros::Publisher pc_pub = node.advertise("segment_pointcloud",1);
    pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
    pcl::PointIndices::Ptr  ground_indices(new pcl::PointIndices);
    pcl::PointIndices::Ptr  no_ground_indices(new pcl::PointIndices);
    GroundSeg ground_seg;
    sensor_msgs::PointCloud2 msg_seg_pc;
    const boost::function&)> callback =[&](sensor_msgs::PointCloud2::ConstPtr msg_pc_ptr) {
        pcl::fromROSMsg(*msg_pc_ptr, *cloud);
        ground_seg.setInputCloud(cloud);
        ground_seg.seg(ground_indices, no_ground_indices);
        std::cerr << "ground points: " << ground_indices->indices.size() << std::endl;
        std::cerr << "no_ground points: " << no_ground_indices->indices.size() << std::endl;
        for (const auto& idx : ground_indices->indices) {
            auto& point = cloud->at(idx);
            point.r = 0;
            point.g = 255;
            point.b = 0;
        }
        for (const auto& idx : no_ground_indices->indices) {
            auto& point = cloud->at(idx);
            point.r = 255;
            point.g = 0;
            point.b = 0;
        }
        pcl::toROSMsg(*cloud,msg_seg_pc);
        msg_seg_pc.header.frame_id = "livox_frame";
        pc_pub.publish(msg_seg_pc);
    };
    ros::Subscriber pc_sub = node.subscribe("/livox/lidar", 1, callback);
    ros::spin();
    return 0;
}

【实验场景1-说明】

雷达：Livox Horizon

安装方式：路侧，非水平安装，向地面倾斜约30度。

【实验场景1-测试】

 rosrun n_lidar_learn n_lidar_learn_node
seg:49,calc normal cost 175 ms
seg:72,ransac seg cost 2 ms
ground points: 9442
no_ground points: 14558
seg:49,calc normal cost 183 ms
seg:72,ransac seg cost 2 ms
ground points: 9479
no_ground points: 14521
.....

绿色的为地面点云，红色为非地面点云，可见效果是不错的，空旷地面上的人和车都能较好的分离出来。

【参考文献】

K-D树数据结构介绍及C++实现_蘇丶的博客-CSDN博客