之前为了剔除地面点，计算了全部点云的法向量。然后，先通过点云的法向量做一个粗过滤，剔除明显的非地面点，再通过随机采样一致性算法估算出地面方程。计算全部点云的法向量相对是比较耗时的操作，大部分的点一开是就对我们最后估算地平面方程没有什么用，所以先通过CropBox做一个立方体过滤，只保留对我们有意义的点。

【代码】

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/passthrough.h>
#include <boost/function.hpp>
#include <pcl/filters/crop_box.h>
#include <Eigen/Eigen>

int main (int argc, char** argv) {
    ros::init(argc, argv, "pcl_passthrough");
#if 0
    float min_z = 0;
    if (argc < 2) {
        std::cerr << "Usage: " << argv[0] << " [min_z]";
        return 1;
    } else {
        min_z = std::atof(argv[1]);
    }
#endif
    ros::NodeHandle nd;
    ros::Publisher pc_pub = nd.advertise<sensor_msgs::PointCloud2>("pcl_cropbox",1);
    const boost::function<void (const boost::shared_ptr<sensor_msgs::PointCloud2 const>&)> callback =
            [&](sensor_msgs::PointCloud2::ConstPtr msg_pc_ptr) {
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::fromROSMsg(*msg_pc_ptr,*cloud);

        pcl::PointIndices::Ptr cloud_filter_indices(new pcl::PointIndices);
        pcl::PointIndices::Ptr cloud_filter_rest_indices(new pcl::PointIndices);
        
        //定义立体范围
        pcl::CropBox<pcl::PointXYZRGB> box_filter;
        float x_min=0,y_min=-6,z_min=-6;
        box_filter.setMin(Eigen::Vector4f(x_min,y_min,z_min,1.));
        float x_max=30,y_max=6,z_max=6;
        box_filter.setMax(Eigen::Vector4f(x_max,y_max,z_max,1.));
        box_filter.setInputCloud(cloud);
        box_filter.setNegative(false);
        box_filter.filter(cloud_filter_indices->indices);
#if 1
        for (size_t i=0; i<cloud_filter_indices->indices.size(); ++i) {
            auto& point = cloud->at(cloud_filter_indices->indices.at(i));
            point.g = 0;
            point.r = 255;
            point.b = 0;
        }

        box_filter.setNegative(true);
        box_filter.filter(cloud_filter_rest_indices->indices);
        for (size_t i=0; i<cloud_filter_rest_indices->indices.size(); ++i) {
            auto& point = cloud->at(cloud_filter_rest_indices->indices.at(i));
            point.g = 255;
            point.r = 0;
            point.b = 0;
        }
#endif
        sensor_msgs::PointCloud2 msg_pc_filter;
        pcl::toROSMsg(*cloud, msg_pc_filter);
        msg_pc_filter.header.frame_id = "livox_frame";
        pc_pub.publish(msg_pc_filter);
        std::cout << "OrgPoints: " << cloud->points.size() << ",FilterPoints: " << cloud_filter_indices->indices.size() << std::endl;
    };
    ros::Subscriber pc_sub = nd.subscribe<sensor_msgs::PointCloud2>("/livox/lidar",1,callback);
    ros::spin();
    return 0;
}

【实验场景1-说明】

雷达：Livox Horizon

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

【实验场景1-测试】

rosrun n_lidar_learn pcl_cropbox_node

OrgPoints: 24000,FilterPoints: 12751
OrgPoints: 24000,FilterPoints: 12806
OrgPoints: 24000,FilterPoints: 12801

.......