使用PCL库中PPF+ICP进行点云目标识别

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

using namespace pcl;
using namespace std::chrono_literals;

const Eigen::Vector4f subsampling_leaf_size(1.5f,1.5f, 1.5f, 0.0f);//下采样立方体大小
constexpr float normal_estimation_search_radius = 5.0f;//法线计算搜索半径


int
main(int argc, char** argv)
{
    /// 读取点云文件
    PointCloud::Ptr cloud_scene(new PointCloud());
    if (pcl::io::loadPCDFile("screws_M8_40_ronghe.pcd", *cloud_scene) < 0)
    {
        std::cout << "Error loading scene cloud." << std::endl;
        return (-1);
    }
    else
    {
        cout << "cloud_scene 读取成功" << endl;
    }
   
    PointCloud::Ptr cloud_model(new PointCloud());
    if (pcl::io::loadPCDFile("screws_M8_40.pcd", *cloud_model) < 0)
    {
        std::cout << "Error loading model cloud." << std::endl;
        return (-1);
    }
    else
    {
        cout << "cloud_model 读取成功" << endl;
    }

    //背景部分处理
    PointCloud::Ptr cloud_scene_input(new PointCloud());
    PointCloud::Ptr cloud_scene_subsampled(new PointCloud());
    //下采样滤波
    VoxelGrid subsampling_filter;
    subsampling_filter.setInputCloud(cloud_scene);
    subsampling_filter.setLeafSize(subsampling_leaf_size);
    subsampling_filter.filter(*cloud_scene_subsampled);
    //计算背景法线
    PointCloud::Ptr cloud_scene_normals(new PointCloud());
    NormalEstimation normal_estimation_filter;
    normal_estimation_filter.setInputCloud(cloud_scene_subsampled);
    search::KdTree::Ptr search_tree(new search::KdTree);
    normal_estimation_filter.setSearchMethod(search_tree);
    normal_estimation_filter.setRadiusSearch(normal_estimation_search_radius);
    normal_estimation_filter.compute(*cloud_scene_normals);
    pcl::concatenateFields(*cloud_scene_subsampled, *cloud_scene_normals, *cloud_scene_input);
    cout << cloud_scene->size() << " down to" << cloud_scene_subsampled->size() << endl;
    //模型部分处理
    PointCloud::Ptr cloud_model_input(new PointCloud());
    PointCloud::Ptr cloud_model_subsampled(new PointCloud());
    //下采样滤波
    VoxelGrid subsampling_filter2;
    subsampling_filter2.setInputCloud(cloud_model);
    subsampling_filter2.setLeafSize(subsampling_leaf_size);
    subsampling_filter2.filter(*cloud_model_subsampled);
    //计算背景法线
    PointCloud::Ptr cloud_model_normals(new PointCloud());
    NormalEstimation normal_estimation_filter2;
    normal_estimation_filter2.setInputCloud(cloud_model_subsampled);
    search::KdTree::Ptr search_tree2(new search::KdTree);
    normal_estimation_filter2.setSearchMethod(search_tree2);
    normal_estimation_filter2.setRadiusSearch(normal_estimation_search_radius);
    normal_estimation_filter2.compute(*cloud_model_normals);
    pcl::concatenateFields(*cloud_model_subsampled, *cloud_model_normals, *cloud_model_input);

    cout << cloud_model->size() << " down to" << cloud_model_subsampled->size() << endl;

   // pcl::PointCloud::Ptr cloud_model_ppf = pcl::PointCloud::Ptr(new pcl::PointCloud());
    PointCloud::Ptr cloud_model_ppf(new PointCloud());
    PPFEstimation ppf_estimator;
    ppf_estimator.setInputCloud(cloud_model_input);
    ppf_estimator.setInputNormals(cloud_model_input);
    ppf_estimator.compute(*cloud_model_ppf);//之前一直出现指针报错？？？，加多维向量AGX后解决
    PPFHashMapSearch::Ptr hashmap_search(new PPFHashMapSearch( 2 * float(M_PI)/20 ,0.1f));
    hashmap_search->setInputFeatureCloud(cloud_model_ppf);
    

    visualization::PCLVisualizer viewer("PPF Object Recognition - Results");
    viewer.setBackgroundColor(0, 0, 0);
    viewer.addPointCloud(cloud_scene);
    viewer.spinOnce(10);
    PCL_INFO("Registering models to scene ...\n");

    将源点云和目标点云都转化为无序点云
    //cloud_model_input->height = 1;
    //cloud_model_input->is_dense = false;
    //cloud_scene_input->height = 1;
    //cloud_scene_input->is_dense = false;
    PPFRegistration ppf_registration;
    // set parameters for the PPF registration procedure
    ppf_registration.setSceneReferencePointSamplingRate(10);
    ppf_registration.setPositionClusteringThreshold(2.0f);
    ppf_registration.setRotationClusteringThreshold(12.0f / 180.0f * float(M_PI));
    ppf_registration.setSearchMethod(hashmap_search);
    ppf_registration.setInputSource(cloud_model_input);
    ppf_registration.setInputTarget(cloud_scene_input);
    无序点云
    PointCloud::Ptr cloud_output_subsampled(new PointCloud());

    ppf_registration.align(*cloud_output_subsampled);
    //出现数组越界访问，无序点云OR有序点云，  //有疑问的地方？？？？？？？？？？？？？？？？？？？？？？？？？？？？？
    //修改ppf_registration.hpp中的const auto aux_size = static_cast(
        //   std::floor(2 * M_PI / search_method_->getAngleDiscretizationStep() + 1));

    //转换点云XYZ格式
    PointCloud::Ptr cloud_output_subsampled_xyz(new PointCloud());
    for (const auto& point : (* cloud_output_subsampled).points)
        cloud_output_subsampled_xyz->points.emplace_back(point.x, point.y, point.z);

    Eigen::Matrix4f mat = ppf_registration.getFinalTransformation();
    std::cout << "PPF 变换矩阵：" << endl<::Ptr icp_result(new PointCloud());
    pcl::IterativeClosestPoint icp;
    //输入待配准点云和目标点云
    icp.setInputSource(cloud_model_subsampled);
    icp.setInputTarget(cloud_output_subsampled_xyz);
    //Set the max correspondence distance to 4cm (e.g., correspondences with higher distances will be ignored)
    icp.setMaxCorrespondenceDistance(10);//默认单位是米?
    //最大迭代次数
    icp.setMaximumIterations(1000);
    //两次变化矩阵之间的差值
    icp.setTransformationEpsilon(1e-10);
    // 均方误差
    icp.setEuclideanFitnessEpsilon(0.002);
    icp.align(*icp_result, mat);
    Eigen::Matrix4f icp_trans;
    icp_trans = icp.getFinalTransformation();
    std::cout << "icp变换矩阵：" << endl<::Ptr cloud_output(new PointCloud());
    pcl::transformPointCloud(
        *cloud_model, *cloud_output, icp_trans);

    pcl::visualization::PointCloudColorHandlerCustom output(cloud_output_subsampled_xyz, 255, 0, 0);
    pcl::visualization::PointCloudColorHandlerRandom random_color(cloud_output->makeShared());
    viewer.addPointCloud(cloud_output, random_color, "mode_name");
    //viewer.addPointCloud(cloud_output_subsampled_xyz, output, "dsd");
    while (!viewer.wasStopped()) {
        viewer.spinOnce(100);
        std::this_thread::sleep_for(100ms);
    }
    return 0;
}

在使用过程中踩到的坑：   

（1）：PointCloud::Ptr cloud_model_ppf(new PointCloud());
    PPFEstimation ppf_estimator;
    ppf_estimator.setInputCloud(cloud_model_input);
    ppf_estimator.setInputNormals(cloud_model_input);
    ppf_estimator.compute(*cloud_model_ppf);

运行到ppf_estimator.compute(*cloud_model_ppf);一直报错，是指针问题，打开属性面板，选择高级矢量扩展即可

（2）ppf_registration.align(*cloud_output_subsampled);运行到这里时一致报错，弹出警告是vector越界访问。解决办法：

 修改这个库文件，在floor函数后加一即可解决，问题出在floor函数向下取整，aux_size应该等于20，floor括号中算出来的是19.999.。。。，向下取整就变成19了，所以会出现越界访问！！

附上最后运行效果：

 

 

 绿色点云是找到的目标，加ICP是为了使位姿更准确