Ubuntu通过PCL实现多幅点云逐步配准

VijaySue

已于 2023-12-28 22:10:18 修改

阅读量531

点赞数 13

文章标签： ubuntu linux 运维 c++ qt gnu

于 2023-12-28 22:10:03 首次发布

本文链接：https://blog.csdn.net/weixin_67559204/article/details/135278088

版权

一.获取所有文件名

获取包含pcd的指定路径文件夹中所有文件名，写入数组并将其进行排序后返回

std::vector<std::string> getFilesName(std::string folderPath)
{
    std::vector<std::string> filesName;
    DIR *dir;
    struct dirent *ent;

    if ((dir = opendir(folderPath.c_str())) != NULL)
    {
        while ((ent = readdir(dir)) != NULL)
        {
            if (ent->d_name[0] != '.')
                filesName.emplace_back(ent->d_name);
        }
        closedir(dir);
    }
    else
    {
        perror("Error opening directory");
    }
    std::sort(filesName.begin(), filesName.end(), std::less<std::string>());
    return filesName;
}

二.两幅点云进行配准

SAC粗配准+ICP精配准实现重复式点云数据配准

PointCloud::Ptr pointCloudRegistration(PointCloud::Ptr cloud_src_o, PointCloud::Ptr cloud_tgt_o)
{
    std::vector<int> indices_src;
    pcl::removeNaNFromPointCloud(*cloud_src_o, *cloud_src_o, indices_src);

    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
    voxel_grid.setLeafSize(0.012, 0.012, 0.012);
    voxel_grid.setInputCloud(cloud_src_o);
    PointCloud::Ptr cloud_src(new PointCloud);
    voxel_grid.filter(*cloud_src);

    pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> ne_src;
    ne_src.setInputCloud(cloud_src);
    pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree< pcl::PointXYZ>());
    ne_src.setSearchMethod(tree_src);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_src_normals(new pcl::PointCloud< pcl::Normal>);
    ne_src.setRadiusSearch(0.02);
    ne_src.compute(*cloud_src_normals);

    std::vector<int> indices_tgt;
    pcl::removeNaNFromPointCloud(*cloud_tgt_o, *cloud_tgt_o, indices_tgt);

    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_2;
    voxel_grid_2.setLeafSize(0.01, 0.01, 0.01);
    voxel_grid_2.setInputCloud(cloud_tgt_o);
    PointCloud::Ptr cloud_tgt(new PointCloud);
    voxel_grid_2.filter(*cloud_tgt);

    pcl::NormalEstimation<pcl::PointXYZ,pcl::Normal> ne_tgt;
    ne_tgt.setInputCloud(cloud_tgt);
    pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_tgt(new pcl::search::KdTree< pcl::PointXYZ>());
    ne_tgt.setSearchMethod(tree_tgt);
    pcl::PointCloud<pcl::Normal>::Ptr cloud_tgt_normals(new pcl::PointCloud< pcl::Normal>);
    ne_tgt.setRadiusSearch(0.02);
    ne_tgt.compute(*cloud_tgt_normals);

    pcl::FPFHEstimation<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> fpfh_src;
    fpfh_src.setInputCloud(cloud_src);
    fpfh_src.setInputNormals(cloud_src_normals);
    pcl::search::KdTree<PointT>::Ptr tree_src_fpfh(new pcl::search::KdTree<PointT>);
    fpfh_src.setSearchMethod(tree_src_fpfh);
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_src(new pcl::PointCloud<pcl::FPFHSignature33>());
    fpfh_src.setRadiusSearch(0.05);
    fpfh_src.compute(*fpfhs_src);

    pcl::FPFHEstimation<pcl::PointXYZ,pcl::Normal,pcl::FPFHSignature33> fpfh_tgt;
    fpfh_tgt.setInputCloud(cloud_tgt);
    fpfh_tgt.setInputNormals(cloud_tgt_normals);
    pcl::search::KdTree<PointT>::Ptr tree_tgt_fpfh(new pcl::search::KdTree<PointT>);
    fpfh_tgt.setSearchMethod(tree_tgt_fpfh);
    pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_tgt(new pcl::PointCloud<pcl::FPFHSignature33>());
    fpfh_tgt.setRadiusSearch(0.05);
    fpfh_tgt.compute(*fpfhs_tgt);

    pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> scia;
    scia.setInputSource(cloud_src);
    scia.setInputTarget(cloud_tgt);
    scia.setSourceFeatures(fpfhs_src);
    scia.setTargetFeatures(fpfhs_tgt);
    PointCloud::Ptr sac_result(new PointCloud);
    scia.align(*sac_result);
    Eigen::Matrix4f sac_trans;
    sac_trans=scia.getFinalTransformation();

    PointCloud::Ptr icp_result(new PointCloud);
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputSource(cloud_src);
    icp.setInputTarget(cloud_tgt_o);
    icp.setMaxCorrespondenceDistance(0.04);
    icp.setMaximumIterations(50);
    icp.setTransformationEpsilon(1e-10);
    icp.setEuclideanFitnessEpsilon(0.2);
    icp.align(*icp_result,sac_trans);


    Eigen::Matrix4f icp_trans;
    icp_trans=icp.getFinalTransformation();
    pcl::transformPointCloud(*cloud_src_o, *icp_result, icp_trans);

    return icp_result;
}

三.逐步配准并保存

首先定义两个点云类变量，变量a读取第一幅pcd的数据，在循环中每次变量b读取下一幅点云数据，并将a和b的点云数据进行配准，之后将结果赋值给a，从而实现逐步配准

void pcdsResistration(std::string pcdsFolderName, std::vector<std::string> pcdsName)
{
    PointCloud::Ptr cloud_a (new PointCloud);
    pcl::io::loadPCDFile ("./" + pcdsFolderName + "/" + pcdsName[0], *cloud_a);
    PointCloud::Ptr cloud_b (new PointCloud);

    for (size_t i = 1; i < pcdsName.size(); ++i) {
        pcl::io::loadPCDFile ("./" + pcdsFolderName + "/" + pcdsName[i], *cloud_b);
        cloud_a = pointCloudRegistration(cloud_a, cloud_b);
        qDebug() << i << '\n';
    }

    pcl::io::savePCDFileASCII("finally.pcd", *cloud_a);
}

四.主函数

int main()
{
    std::string pcdsFolderName = ("pcds")
    std::vector<std::string> pcdsName = getFilesName("./" + pcdsFolderName);
    pcdsResistration(pcdFolderName, pcdsName);
}