PCL 特征匹配

Question

首先，我是 PCL 的新手，我正在寻找有关使用检测器和描述符进行点云配准的特征匹配主题的帮助。

我的管道工作如下：

加载源云
检测 ISS 关键点
使用 FPFH 描述关键点
加载目标云
检测 ISS 关键点
使用 FPFH 描述关键点
估计对应关系
通信过滤
转换估计最后 --> 使用来自步骤 9 的估计变换矩阵变换源云

问题是：

代码在法线计算中花费了太多时间
结果非常糟糕，如图所示 [结果][1]

代码是：

#include <iostream>
#include <vector>

// PCL
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/keypoints/iss_3d.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/shot.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_rejection_one_to_one.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/registration/icp.h>
#include <pcl/features/fpfh.h>

using namespace pcl;
using namespace pcl::io;

pcl::visualization::PCLVisualizer viewer("Cloud Viewer");
pcl::visualization::PCLVisualizer ICPView("ICP Viewer");

double computeCloudResolution(const pcl::PointCloud<PointXYZ>::ConstPtr &cloud)
{
    double res = 0.0;
    int n_points = 0;
    int nres;
    std::vector<int> indices(2);
    std::vector<float> sqr_distances(2);
    pcl::search::KdTree<PointXYZ> tree;
    tree.setInputCloud(cloud);

    for (size_t i = 0; i < cloud->size(); ++i)
    {
        if (!pcl_isfinite((*cloud)[i].x))
        {
            continue;
        }
        //Considering the second neighbor since the first is the point itself.
        nres = tree.nearestKSearch(i, 2, indices, sqr_distances);
        if (nres == 2)
        {
            res += sqrt(sqr_distances[1]);
            ++n_points;
        }
    }
    if (n_points != 0)
    {
        res /= n_points;
    }
    return res;
}

int main(int, char **argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new PointCloud<PointXYZ>());
    loadPCDFile("cloudASCII000.pcd", *source_cloud);
    std::cout << "File 1 points: " << source_cloud->points.size() << std::endl;
    //file 1

    // Compute model_resolution

    double model_resolution = computeCloudResolution(source_cloud);

    pcl::ISSKeypoint3D<pcl::PointXYZ, pcl::PointXYZ> iss_detector;
    pcl::PointCloud<pcl::PointXYZ>::Ptr source_keypoints(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());

    iss_detector.setSearchMethod(tree);
    iss_detector.setSalientRadius(10 * model_resolution);
    iss_detector.setNonMaxRadius(8 * model_resolution);
    iss_detector.setThreshold21(0.2);
    iss_detector.setThreshold32(0.2);
    iss_detector.setMinNeighbors(10);
    iss_detector.setNumberOfThreads(10);
    iss_detector.setInputCloud(source_cloud);
    iss_detector.compute((*source_keypoints));
    pcl::PointIndicesConstPtr keypoints_indices = iss_detector.getKeypointsIndices();

    std::cout << "No of ISS points in the result are " << (*source_keypoints).points.size() << std::endl;
    std::string Name = "ISSKeypoints1.pcd";
    savePCDFileASCII(Name, (*source_keypoints));

    // Compute the normals
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;
    normalEstimation.setInputCloud(source_cloud);
    normalEstimation.setSearchMethod(tree);

    pcl::PointCloud<pcl::Normal>::Ptr source_normals(new pcl::PointCloud<pcl::Normal>);
    normalEstimation.setRadiusSearch(0.2);
    normalEstimation.compute(*source_normals);

    pcl::PointCloud<pcl::FPFHSignature33>::Ptr source_features(new pcl::PointCloud<pcl::FPFHSignature33>());
    pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;
    fpfh.setInputCloud(source_cloud);
    fpfh.setInputNormals(source_normals);
    fpfh.setIndices(keypoints_indices);
    fpfh.setSearchMethod(tree);
    fpfh.setRadiusSearch(0.2);
    fpfh.compute(*source_features);

    /* SHOT optional Descriptor
        pcl::PointCloud<pcl::SHOT352>::Ptr source_features(new pcl::PointCloud<pcl::SHOT352>());
        pcl::SHOTEstimation< pcl::PointXYZ, pcl::Normal, pcl::SHOT352 > shot;
        shot.setSearchMethod(tree); //kdtree
        shot.setIndices(keypoints_indices); //keypoints
        shot.setInputCloud(source_cloud); //input
        shot.setInputNormals(source_normals); //normals
        shot.setRadiusSearch(0.2); //support
        shot.compute(*source_features); //descriptors
        */

    //Target Point Cloud

    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new PointCloud<PointXYZ>());
    loadPCDFile("cloudASCII003.pcd", *target_cloud);
    std::cout << "File 2 points: " << target_cloud->points.size() << std::endl;

    // Compute model_resolution

    double model_resolution_1 = computeCloudResolution(target_cloud);

    pcl::ISSKeypoint3D<pcl::PointXYZ, pcl::PointXYZ> iss_detector_1;
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_keypoints(new pcl::PointCloud<pcl::PointXYZ>());

    iss_detector_1.setSearchMethod(tree);
    iss_detector_1.setSalientRadius(10 * model_resolution_1);
    iss_detector_1.setNonMaxRadius(8 * model_resolution_1);
    iss_detector_1.setThreshold21(0.2);
    iss_detector_1.setThreshold32(0.2);
    iss_detector_1.setMinNeighbors(10);
    iss_detector_1.setNumberOfThreads(10);
    iss_detector_1.setInputCloud(target_cloud);
    iss_detector_1.compute((*target_keypoints));
    pcl::PointIndicesConstPtr keypoints_indices_1 = iss_detector_1.getKeypointsIndices();

    std::cout << "No of ISS points in the result are " << (*target_keypoints).points.size() << std::endl;
    savePCDFileASCII("ISSKeypoints2.pcd", (*target_keypoints));

    // Compute the normals
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation_1;
    normalEstimation_1.setInputCloud(target_cloud);
    normalEstimation_1.setSearchMethod(tree);

    pcl::PointCloud<pcl::Normal>::Ptr target_normals(new pcl::PointCloud<pcl::Normal>);
    normalEstimation_1.setRadiusSearch(0.2);
    normalEstimation_1.compute(*target_normals);

    pcl::PointCloud<pcl::FPFHSignature33>::Ptr target_features(new pcl::PointCloud<pcl::FPFHSignature33>());
    pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh1;
    fpfh1.setInputCloud(target_cloud);
    fpfh1.setInputNormals(target_normals);
    fpfh1.setIndices(keypoints_indices_1);
    fpfh1.setSearchMethod(tree);
    fpfh1.setRadiusSearch(0.2);
    fpfh1.compute(*target_features);

    /* Shot Optional
        pcl::PointCloud<pcl::SHOT352>::Ptr target_features(new pcl::PointCloud<pcl::SHOT352>());
        pcl::SHOTEstimation< pcl::PointXYZ, pcl::Normal, pcl::SHOT352 > shot_1;
        shot_1.setSearchMethod(tree); //kdtree
        shot_1.setIndices(keypoints_indices_1); //keypoints
        shot_1.setInputCloud(target_cloud); //input
        shot_1.setInputNormals(target_normals); //normals
        shot_1.setRadiusSearch(0.2); //support
        shot_1.compute(*target_features); //descriptors
        */

    // estimate correspondences
    pcl::registration::CorrespondenceEstimation<pcl::FPFHSignature33, pcl::FPFHSignature33> est;
    pcl::CorrespondencesPtr correspondences(new pcl::Correspondences());
    est.setInputSource(source_features);
    est.setInputTarget(target_features);
    est.determineCorrespondences(*correspondences);

    // Duplication rejection Duplicate

    pcl::CorrespondencesPtr correspondences_result_rej_one_to_one(new pcl::Correspondences());
    pcl::registration::CorrespondenceRejectorOneToOne corr_rej_one_to_one;
    corr_rej_one_to_one.setInputCorrespondences(correspondences);
    corr_rej_one_to_one.getCorrespondences(*correspondences_result_rej_one_to_one);

    // Correspondance rejection RANSAC

    Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
    pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZ> rejector_sac;
    pcl::CorrespondencesPtr correspondences_filtered(new pcl::Correspondences());
    rejector_sac.setInputSource(source_keypoints);
    rejector_sac.setInputTarget(target_keypoints);
    rejector_sac.setInlierThreshold(2.5); // distance in m, not the squared distance
    rejector_sac.setMaximumIterations(1000000);
    rejector_sac.setRefineModel(false);
    rejector_sac.setInputCorrespondences(correspondences_result_rej_one_to_one);
    ;
    rejector_sac.getCorrespondences(*correspondences_filtered);
    correspondences.swap(correspondences_filtered);
    std::cout << correspondences->size() << " vs. " << correspondences_filtered->size() << std::endl;
    transform = rejector_sac.getBestTransformation(); // Transformation Estimation method 1

    // Transformation Estimation method 2
    //pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> transformation_estimation;
    //transformation_estimation.estimateRigidTransformation(*source_keypoints, *target_keypoints, *correspondences, transform);
    std::cout << "Estimated Transform:" << std::endl
              << transform << std::endl;

    // / refinement transform source using transformation matrix ///////////////////////////////////////////////////////

    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_source(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr final_output(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::transformPointCloud(*source_cloud, *transformed_source, transform);
    savePCDFileASCII("Transformed.pcd", (*transformed_source));

    // viewer.setBackgroundColor (0, 0, 0);
    viewer.setBackgroundColor(1, 1, 1);
    viewer.resetCamera();
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler_source_cloud(transformed_source, 150, 80, 80);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler_source_keypoints(source_keypoints, 255, 0, 0);

    viewer.addPointCloud<pcl::PointXYZ>(transformed_source, handler_source_cloud, "source_cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "source_cloud");
    viewer.addPointCloud<pcl::PointXYZ>(source_keypoints, handler_source_keypoints, "source_keypoints");

    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler_target_cloud(target_cloud, 80, 150, 80);
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> handler_target_keypoints(target_keypoints, 0, 255, 0);

    viewer.addPointCloud<pcl::PointXYZ>(target_cloud, handler_target_cloud, "target_cloud");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target_cloud");
    viewer.addPointCloud<pcl::PointXYZ>(target_keypoints, handler_target_keypoints, "target_keypoints");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "target_keypoints");
    viewer.addCorrespondences<pcl::PointXYZ>(source_keypoints, target_keypoints, *correspondences, 1, "correspondences");

    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(transformed_source);
    icp.setInputTarget(target_cloud);
    icp.align(*final_output);
    std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
    std::cout << icp.getFinalTransformation() << std::endl;

    ICPView.addPointCloud<pcl::PointXYZ>(final_output, handler_source_cloud, "Final_cloud");
    ICPView.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "source_keypoints");
    while (!viewer.wasStopped())
    {

        viewer.spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
    while (!ICPView.wasStopped())
    {

        ICPView.spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
    /*
        // Setup the SHOT features
        typedef pcl::SHOT352 ShotFeature;
        pcl::SHOTEstimation<pcl::PointXYZ, pcl::Normal, ShotFeature> shotEstimation;
    
        shotEstimation.setInputCloud(model);
        shotEstimation.setInputNormals(normals);
        shotEstimation.setIndices(keypoint_indices);
    
        // Use the same KdTree from the normal estimation
        shotEstimation.setSearchMethod(tree);
        pcl::PointCloud<ShotFeature>::Ptr shotFeatures(new pcl::PointCloud<ShotFeature>);
        //spinImageEstimation.setRadiusSearch (0.2);
        shotEstimation.setKSearch(10);
    
        // Actually compute the spin images
        shotEstimation.compute(*shotFeatures);
        std::cout << "SHOT output points.size (): " << shotFeatures->points.size() << std::endl;
    
        // Display and retrieve the SHOT descriptor for the first point.
        ShotFeature descriptor = shotFeatures->points[0];
        std::cout << descriptor << std::endl;
        */

    return 0;
}

最后提前致谢，非常感谢您的帮助... [1]: https://i.stack.imgur.com/xtvUA.png

Answer 1

为了改善花费的时间，请在加载源云后尝试移除离群值和下采样。

PCL 特征匹配

PCL Feature matching

point-cloud-library