如何在PCL中使用GICP

Question

我想在PCL中使用GICP。我编写了GICP代码，但结果却很奇怪。我想知道我的GICP程序是否错误。尽管我对网络文档和PCL文档进行了调查，但找不到任何关键信息。

如果有人知道如何在PCL中使用GICP，您能告诉他们如何使用它吗？我当前的代码如下。

#include <iostream>
#include <fstream>
#include <vector>
#include <numeric>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/impl/point_types.hpp>
#include <pcl/filters/impl/filter.hpp>
#include <pcl/registration/gicp.h>
#include <pcl/registration/icp.h>
#include <pcl/io/pcd_io.h>

using namespace pcl;
using Gicp = GeneralizedIterativeClosestPoint<PointXYZ, PointXYZ>;

#define READ_FAILURE (-1)

int main(void)
{
    Gicp gicp;

    Gicp::PointCloudSourcePtr cloud_src_ptr (new pcl::PointCloud<pcl::PointXYZ>);
    Gicp::PointCloudTargetPtr cloud_dst_ptr (new pcl::PointCloud<pcl::PointXYZ>);

    Eigen::Matrix4f out_matrix = Eigen::Matrix4f::Zero();
    PointCloud<PointXYZ> temp_cloud;
    std::vector<int> indices_src, indices_dst;

    if(READ_FAILURE == io::loadPCDFile<PointXYZ>("cloud_target.pcd", *cloud_dst_ptr))
    {
        PCL_ERROR("Could not read file for dst");
    }

    if(READ_FAILURE == io::loadPCDFile<PointXYZ>("cloud_source.pcd", *cloud_src_ptr))
    {
        PCL_ERROR("Could not read file for src");
    }

    indices_dst.resize(cloud_dst_ptr->size());
    indices_src.resize(cloud_src_ptr->size());
    std::iota(indices_dst.begin(), indices_dst.end(), 0);
    std::iota(indices_src.begin(), indices_src.end(), 0);

    gicp.setInputSource(cloud_src_ptr);
    gicp.setInputTarget(cloud_dst_ptr);
    gicp.setMaximumOptimizerIterations(50);
    gicp.setRotationEpsilon(1e-1);
    gicp.setCorrespondenceRandomness(10);

    // I call align to update private variable gicp.mahalanobis_
    gicp.align(*cloud_src_ptr, out_matrix);
    gicp.estimateRigidTransformationBFGS(*cloud_src_ptr, indices_src, *cloud_dst_ptr, indices_dst, out_matrix);

    return 0;
}

align的结果如下。没有旋转和平移似乎很奇怪。

0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 0

BFGS的结果如下。

1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1

我尝试在此处附加两个pcd文件。但是我找不到解决方法。