三维点云语义分割 三维点云匹配

【三维点云数据处理】PCL三维点云配准 SACIA

• 算法原理
• 代码实现
• 实验结果

算法原理

该算法的总体思路如下：

将需要配准的目标点云P中选择n个采样点。为了保证所选取的采样点尽可能有不同的FPFH特征，采样点的距离一般要选择的恰当，尽可能分散，一般要大于预定的最小距离d；

在模板点云Q中查找与目标点云P中具有相似FPFH特征的对应点，这些点可能是一个或多个，但是从这些点中选取一个作为最终的对应点。

根据以上步骤得到对应点集，计算目标点云和模板点云之间的刚性变换矩阵，同时计算出配准后的误差，以及点云配准后的质量。

重复以上三个步骤，找到误差和最小的变换，然后用于点云的粗配准。

文献通过查找大量不同的对应点集，快速找到良好的转换关系。采用Huber惩罚函数来确定最小误差和。

其中l_e为设定的阈值，e_i为两组点云对应点之间的距离差。

代码实现

//PCL SAC-IA 初始配准算法-->求变换矩阵
#include <pcl/registration/ia_ransac.h>//sac_ia算法
#include <pcl/filters/voxel_grid.h>//体素下采样滤波
Eigen::Matrix4f SAC_IA(pcl::PointCloud<pcl::PointXYZ>::Ptr& target_cloud,
    pcl::PointCloud<pcl::PointXYZ>::Ptr& source_cloud)
{
    clock_t start, end, time;
    start = clock();
    //---------------------------去除源点云的NAN点------------------------
    vector<int> indices_src; //保存去除的点的索引
    pcl::removeNaNFromPointCloud(*source_cloud, *source_cloud, indices_src);
    //-------------------------源点云下采样滤波-------------------------
    pcl::VoxelGrid<pcl::PointXYZ> vs;
    vs.setLeafSize(1.5, 1.5, 1.5);
    vs.setInputCloud(source_cloud);
    pointcloud::Ptr source(new pointcloud);
    vs.filter(*source);
    cout << "down size *source_cloud from " << source_cloud->size() << " to " << source->size() << endl;
    //--------------------------去除目标点云的NAN点--------------------
    vector<int> indices_tgt; //保存去除的点的索引
    pcl::removeNaNFromPointCloud(*target_cloud, *target_cloud, indices_tgt);
    //----------------------目标点云下采样滤波-------------------------
    pcl::VoxelGrid<pcl::PointXYZ> vt;
    vt.setLeafSize(1.5, 1.5, 1.5);
    vt.setInputCloud(target_cloud);
    pointcloud::Ptr target(new pointcloud);
    vt.filter(*target);
    cout << "down size *target_cloud from " << target_cloud->size() << " to " << target->size() << endl;
    //---------------计算源点云和目标点云的FPFH------------------------
    fpfhFeature::Ptr source_fpfh = compute_fpfh_feature(source);
    fpfhFeature::Ptr target_fpfh = compute_fpfh_feature(target);
    //--------------采样一致性SAC_IA初始配准----------------------------
    pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia;
    sac_ia.setInputSource(source);
    sac_ia.setSourceFeatures(source_fpfh);
    sac_ia.setInputTarget(target);
    sac_ia.setTargetFeatures(target_fpfh);
    sac_ia.setMinSampleDistance(0.05);//设置样本之间的最小距离
    sac_ia.setCorrespondenceRandomness(10); //在选择随机特征对应时，设置要使用的邻居的数量;
                                           //也就是计算协方差时选择的近邻点个数，该值越大，协防差越精确，但是计算效率越低.(可省)
    pointcloud::Ptr align(new pointcloud);
    sac_ia.align(*align);
    end = clock();
    Eigen::Matrix4f sac_trans = sac_ia.getFinalTransformation();
    pcl::transformPointCloud(*source_cloud, *align, sac_ia.getFinalTransformation());
    cout << "calculate time is: " << float(end - start) / CLOCKS_PER_SEC << "s" << endl;
    cout << "\nSAC_IA has converged, score is " << sac_ia.getFitnessScore() << endl;
    cout << "变换矩阵：\n" << sac_ia.getFinalTransformation() << endl;
    //  将改变的点云输出到文件
    pcl::io::savePCDFile<pcl::PointXYZ>("./result.pcd", *align);
    //-------------------可视化------------------------------------
    visualizeCloud(source_cloud, target_cloud, align);
    return sac_trans ;
}

实验结果