0. 简介

最近在看PCL滤波配准等操作,之前在自动驾驶-激光雷达预处理/特征提取和提到了一些滤除点云等操作,但是最近作者发现里面还有一些配准的方法还没有提到,所以这里重新开个章节来给大家列举一些常用的滤波方式,方便大家查阅和使用

1. 滤波&聚类

1.1 直通滤波器

void pass_through_filter(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &input_cloud) //直通滤波器
    {
        std::cout << "start pass_through_filter" << std::endl;
        calc_sight_center(); //计算视点中心,视点中心为滤波器的输入参数
        // void ex_segmentor::calc_sight_center()
        // {
        // double roll, pitch, yaw;
        // tf::Quaternion quat_tmp;

        // tf::quaternionMsgToTF(latest_camera_pos_.pose.pose.orientation, quat_tmp);
        // tf::Matrix3x3(quat_tmp).getRPY(roll, pitch, yaw);

        // centerX_ = latest_camera_pos_.pose.pose.position.x + gaze_length_ * cos(yaw);
        // centerY_ = latest_camera_pos_.pose.pose.position.y + gaze_length_ * sin(yaw);
        // centerZ_ = latest_camera_pos_.pose.pose.position.z - gaze_length_ * sin(pitch);
        // }
        // build the condition
        pcl::ConditionAnd<pcl::PointXYZRGB>::Ptr range_limit(new pcl::ConditionAnd<pcl::PointXYZRGB>);                                                                         //构建范围限制条件
        range_limit->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB>("x", pcl::ComparisonOps::GT, centerX_ - 1.5))); // x坐标大于视点中心x坐标-1.5
        range_limit->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB>("x", pcl::ComparisonOps::LT, centerX_ + 1.5))); // x坐标小于视点中心x坐标+1.5
        range_limit->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB>("y", pcl::ComparisonOps::GT, centerY_ - 1.5))); // y坐标大于视点中心y坐标-1.5
        range_limit->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB>("y", pcl::ComparisonOps::LT, centerY_ + 1.5))); // y坐标小于视点中心y坐标+1.5
        range_limit->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB>("z", pcl::ComparisonOps::GT, centerZ_ - 1.5))); // z坐标大于视点中心z坐标-1.5
        range_limit->addComparison(pcl::FieldComparison<pcl::PointXYZRGB>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZRGB>("z", pcl::ComparisonOps::LT, centerZ_ + 1.5))); // z坐标小于视点中心z坐标+1.5

        //构建滤波器
        pcl::ConditionalRemoval<pcl::PointXYZRGB> condrem; //构建滤波器
        condrem.setCondition(range_limit);                 //设置滤波条件
        condrem.setInputCloud(input_cloud);                //设置输入点云

        //滤波操作
        condrem.filter(*input_cloud);
    }

1.2 离群点滤波器

    void statical_outlier_filter(const pcl::PointCloud<PointXYZRGB>::Ptr &input_cloud, int nr_k, double stddev_mult) //滤波器移除离群点
    {
        pcl::StatisticalOutlierRemoval<PointXYZRGB> sorfilter(true); //构建滤波器
        sorfilter.setInputCloud(input_cloud);
        sorfilter.setMeanK(nr_k);                  //设置在进行统计时考虑的临近点个数
        sorfilter.setStddevMulThresh(stddev_mult); //设置判断是否为离群点的阀值,用来倍乘标准差,也就是上面的stddev_mult
        sorfilter.filter(*input_cloud);            //滤波结果存储到cloud_filtered
    }

1.3 体素化滤波器

    void voxel_filter(const pcl::PointCloud<PointXYZRGB>::Ptr &input_cloud, float resolution) //体素化滤波器
    {
        pcl::VoxelGrid<PointXYZRGB> voxel_grid; //构建体素化滤波器
        voxel_grid.setInputCloud(input_cloud);   //设置输入点云
        voxel_grid.setLeafSize(resolution, resolution, resolution); //设置体素的大小
        voxel_grid.filter(*input_cloud); //滤波结果存储到cloud_filtered
    }

1.4 平面点滤除

    bool remove_plane(const pcl::PointCloud<PointXYZRGB>::Ptr &input_cloud, const Eigen::Vector3f &axis, double plane_thickness) //移除平面
    {
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); //平面参数矩阵
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices);                //平面内点索引
        // Create the segmentation object
        pcl::SACSegmentation<pcl::PointXYZRGB> seg; //构建分割对象

        seg.setOptimizeCoefficients(true);                   //设置是否优化系数
        seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); //设置模型类型为平面
        seg.setMethodType(pcl::SAC_RANSAC);                  //设置分割方法为RANSAC
        seg.setMaxIterations(500);                           //设置最大迭代次数
        seg.setAxis(axis);                                   //设置分割轴
        seg.setEpsAngle(0.25);                               //设置角度阈值
        seg.setDistanceThreshold(plane_thickness);           //设置距离阈值  0.025 0.018
        seg.setInputCloud(input_cloud);                      //设置输入点云
        seg.segment(*inliers, *coefficients);                //分割平面

        if (inliers->indices.size() < 500)
        {
            // ROS_INFO("plane size is not enough large to remove.");
            return false;
        }
        pcl::ExtractIndices<pcl::PointXYZRGB> extract;
        extract.setInputCloud(input_cloud); //设置输入点云
        extract.setIndices(inliers);        //设置索引,用来滤除
        extract.setNegative(true);          //设置是否滤除索引内的点
        extract.filter(*input_cloud);

        return true;
    }

1.5 RGBD颜色特征聚类

    void clustoring_with_color(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &input_cloud, std::vector<pcl::PointCloud<PointXYZRGB>::Ptr> &clusters, int min_cluster_size, float distance_th, float color_th, float region_color_th, unsigned int num_nbr) //根据点云的颜色完成聚类
    {
        std::vector<pcl::PointIndices> clusters_indices;                                              //聚类索引
        pcl::search::KdTree<pcl::PointXYZRGB>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZRGB>); //构建kd树
        kdtree->setInputCloud(input_cloud);                                                           //设置输入点云
        // 基于颜色的区域生长聚类对象
        pcl::RegionGrowingRGB<pcl::PointXYZRGB> clustering;
        clustering.setInputCloud(input_cloud);
        clustering.setSearchMethod(kdtree); //设置搜索方法
        // 这里,最小簇大小也会影响后处理步骤: 小于这个值的clusters_indices将与邻点合并。
        clustering.setMinClusterSize(min_cluster_size); //设置最小簇大小
        // 设置距离阈值,以知道哪些点将被视为,邻点
        clustering.setDistanceThreshold(distance_th); // 1
        // 颜色阈值,用于比较两个点的RGB颜色
        clustering.setPointColorThreshold(color_th); // 9 6.5 25.0f 18.0f
        // 后处理步骤的区域颜色阈值:颜色在阈值内的clusters_indices将合并为一个。
        clustering.setRegionColorThreshold(region_color_th); // 2
        //区域耦合时检查的附近的数量。 默认为100, 在不影响结果的范围内适度设定小范围。
        clustering.setNumberOfRegionNeighbours(num_nbr); //设置近邻数量

        // clustering.setSmoothModeFlag(true);
        // clustering.setSmoothnessThreshold(0.95);

        clustering.extract(clusters_indices); //提取聚类索引

        for (std::vector<pcl::PointIndices>::const_iterator i = clusters_indices.begin(); i != clusters_indices.end(); ++i)//遍历聚类索引
        {
            pcl::PointCloud<pcl::PointXYZRGB>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZRGB>); //构建聚类点云
            for (std::vector<int>::const_iterator pit = i->indices.begin(); pit != i->indices.end(); ++pit) //遍历聚类索引中的点索引
            {
                cluster->points.push_back(input_cloud->points[*pit]); //将点添加到聚类点云
            }
            cluster->width = cluster->points.size();
            cluster->height = 1;
            cluster->is_dense = true;
            clusters.push_back(cluster); //将聚类点云添加到聚类点云集合中
        }
    }

2. 匹配器

2.1 ICP点云精配准

    template <typename PointCloudPtr>
    bool ex_segmentor::icp_registration(PointCloudPtr &input_obj, PointCloudPtr &input_scene, PointCloudPtr &output_obj, Eigen::Matrix4f &result_transform, float &result_error, uint max_iteration, float max_distance, float ransac_th) // icp匹配
    {
        pcl::IterativeClosestPoint<PointXYZRGB, PointXYZRGB> icp; // icp对象
        icp.setInputSource(input_obj);                            //设置输入源点云
        icp.setInputTarget(input_scene);                          //设置输入目标点云
        // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
        icp.setMaxCorrespondenceDistance(max_distance); //设置最大匹配距离 0.05
        // Set the maximum number of iterations (criterion 1)
        icp.setMaximumIterations(max_iteration); //设置最大迭代次数 50
        // Set the transformation epsilon (criterion 2)
        // icp.setTransformationEpsilon (1e-5);  //1e-8
        // Set the euclidean distance difference epsilon (criterion 3)
        // icp.setEuclideanFitnessEpsilon (0.000001);  //1
        icp.setRANSACOutlierRejectionThreshold(ransac_th); //设置RANSAC阈值
        icp.align(*output_obj);                            // icp匹配
        if (icp.hasConverged())                            //如果icp匹配成功
        {
            result_transform = icp.getFinalTransformation(); //获取最终变换矩阵
            result_error = icp.getFitnessScore();            //获取最终匹配误差
            return true;
        }
        else
        {
            result_transform = Eigen::Matrix4f::Identity(4, 4);
            result_error = 1.0;
            return false;
        }
    }
/**
    icp_registration(object_aligned, cluster, Final, icp_result_transform, icp_error);
**/

2.2 FPFH点云粗配准

在这里插入图片描述

    void FPFH_generation(pcl::PointCloud<PointXYZRGB>::Ptr &input, FPFHCloud::Ptr &output) // FPFH特征提取
    {
        // 首先,生成法线
        pcl::NormalEstimationOMP<PointNormal, PointNormal> nest;                  // OMP线程数
        pcl::PointCloud<PointNormal>::Ptr temp(new pcl::PointCloud<PointNormal>); //构建暂时点云
        pcl::copyPointCloud(*input, *temp);                                       //拷贝点云
        nest.setRadiusSearch(0.01);                                               //设置半径搜索范围
        nest.setInputCloud(temp);                                                 //设置输入点云
        nest.compute(*temp);                                                      //计算暂时点云

        // 然后生成FPFH点云
        pcl::FPFHEstimationOMP<PointNormal, PointNormal, FPFH> fest; // OMP线程数
        fest.setRadiusSearch(0.01);                                  // 0.025
        fest.setInputCloud(temp);
        fest.setInputNormals(temp);
        fest.compute(*output);
    }

    template <typename PointType, typename PointCloudPtr>
    bool FPFH_matching(PointCloudPtr &object, FPFHCloud::Ptr &object_feature, PointCloudPtr &scene, FPFHCloud::Ptr &scene_feature, PointCloudPtr &result_cloud, Eigen::Matrix4f &result_transformation) // FPFH粗配准方法
    {
        pcl::SampleConsensusPrerejective<PointType, PointType, FPFH> align; //采样一致性预排除算法
        align.setInputSource(object);                                       //设置输入源点云
        align.setSourceFeatures(object_feature);                            //设置输入源特征点云
        align.setInputTarget(scene);                                        //设置输入目标点云
        align.setTargetFeatures(scene_feature);                             //设置输入目标特征点云
        align.setMaximumIterations(5000);                                   // 设置最大迭代次数
        align.setNumberOfSamples(7);                                        // 设置采样点数
        align.setCorrespondenceRandomness(10);                              // 设置随机匹配点数
        align.setSimilarityThreshold(0.5f);                                 // 设置相似度阈值
        align.setMaxCorrespondenceDistance(0.01f);                          // 设置最大匹配距离
        align.setInlierFraction(0.05f);                                     // 设置内点比例
        align.align(*result_cloud);
        if (align.hasConverged())
        {
            result_transformation = align.getFinalTransformation(); //获取最终变换矩阵
            // pcl::console::print_info("Inliers: %i/%i , %i\n", align.getInliers().size(), scene->size(), object->size());
            // return (float(align.getInliers().size()) / float(object->size()));
            return true;
        }
        // return 0.0f;
        return false;
    }

/**
    FPFHCloud::Ptr cluster_feature(new FPFHCloud);
    FPFH_generation(cluster, cluster_feature);
    ROS_INFO("cluster_size : %d, feature size : %d", cluster->size(), cluster_feature->size());
    bool FPFH_match_success = FPFH_matching<PointXYZRGB>(object_, object_feature_, cluster, cluster_feature, object_aligned, align_result_transform);
**/

2.3 PCA点云粗配准

void ex_segmentor::PCA_registration(pcl::PointCloud<PointXYZRGB>::Ptr &input_obj, pcl::PointCloud<PointXYZRGB>::Ptr &input_scene, pcl::PointCloud<PointXYZRGB>::Ptr &projected_obj, Eigen::Matrix4f &result_transform) // PCA粗配准
    {
        using PointType = PointXYZRGB; //点类型
        pcl::PCA<PointType> pca;       // PCA算法

        pcl::PointCloud<PointType> objProj;
        pca.setInputCloud(input_obj);                          //设置输入点云
        pca.project(*input_obj, objProj);                      //投影点云
        Eigen::Matrix3f EigenSpaceObj = pca.getEigenVectors(); //获取特征向量
        // std::cout << pca.getMean() << std::endl;
        Eigen::Vector3f PcaTransObj(pca.getMean()(0), pca.getMean()(1), pca.getMean()(2)); //获取平均值
        Eigen::Matrix4f transform_obj;                                                     //变换矩阵
        transform_obj << EigenSpaceObj(0, 0), EigenSpaceObj(0, 1), EigenSpaceObj(0, 2), PcaTransObj(0),
            EigenSpaceObj(1, 0), EigenSpaceObj(1, 1), EigenSpaceObj(1, 2), PcaTransObj(1),
            EigenSpaceObj(2, 0), EigenSpaceObj(2, 1), EigenSpaceObj(2, 2), PcaTransObj(2),
            0, 0, 0, 1; //设置变换矩阵

        std::cout << transform_obj << std::endl;

        Eigen::Matrix3f EigenSpaceObjT = EigenSpaceObj.transpose();      //获取特征向量的转置矩阵
        Eigen::Vector3f PcaTransObj_inv = -EigenSpaceObjT * PcaTransObj; //获取平均值的逆矩阵
        Eigen::Matrix4f transform_obj_inv;
        transform_obj_inv << EigenSpaceObjT(0, 0), EigenSpaceObjT(0, 1), EigenSpaceObjT(0, 2), PcaTransObj_inv(0),
            EigenSpaceObjT(1, 0), EigenSpaceObjT(1, 1), EigenSpaceObjT(1, 2), PcaTransObj_inv(1),
            EigenSpaceObjT(2, 0), EigenSpaceObjT(2, 1), EigenSpaceObjT(2, 2), PcaTransObj_inv(2),
            0, 0, 0, 1;

        std::cout << transform_obj_inv << std::endl;

        pcl::PointCloud<PointType> sceneProj;                    //投影点云
        pca.setInputCloud(input_scene);                          //设置输入点云
        pca.project(*input_scene, sceneProj);                    //投影点云
        Eigen::Matrix3f EigenSpaceScene = pca.getEigenVectors(); //获取特征向量
        Eigen::Vector4f PcaTransScene = pca.getMean();           //获取平均值
        Eigen::Matrix4f transform_scene;
        // std::cout << pca.getMean() << std::endl;

        transform_scene << EigenSpaceScene(0, 0), EigenSpaceScene(0, 1), EigenSpaceScene(0, 2), PcaTransScene(0),
            EigenSpaceScene(1, 0), EigenSpaceScene(1, 1), EigenSpaceScene(1, 2), PcaTransScene(1),
            EigenSpaceScene(2, 0), EigenSpaceScene(2, 1), EigenSpaceScene(2, 2), PcaTransScene(2),
            0, 0, 0, 1;

        std::cout << transform_scene << std::endl;

        result_transform = transform_scene * transform_obj_inv;                 //计算最终变换矩阵
        pcl::transformPointCloud(*input_obj, *projected_obj, result_transform); //变换点云

        return;
    };
    /**
         PCA_registration(object_, cluster, object_aligned, align_result_transform);
    **/

参考链接

https://blog.csdn.net/fei_12138/article/details/118677416
https://github.com/TsuruMasato/OnlineObjectDetector/blob/master/src/ex_segmentor.cpp
https://blog.csdn.net/aliexken/article/details/123892377