CPD\Super 4PCS点云配准

一、CPD

        CPD（Coherent Point Drift）配准算法在C++中并没有直接的开源实现。但是有一些社区成员已经自行实现了CPD算法，比如libpointmatcher库。

使用libpointmatcher库实现CPD配准的基本步骤如下：

1. 安装libpointmatcher库。你可以在GitHub - norlab-ulaval/libpointmatcher: An Iterative Closest Point (ICP) library for 2D and 3D mapping in Robotics上找到源代码和安装指南。

2. 包含必要的头文件，创建点云数据，设置配准参数，然后调用配准函数。

#include "pointmatcher/PointMatcher.h"
#include 

typedef PointMatcher PM;
typedef PM::DataPoints DP;

int main()
{
    PM::ICP icp;

    // Load point clouds
    const DP ref(DP::load("ref.vtk"));
    const DP data(DP::load("data.vtk"));

    // Create the default ICP algorithm
    icp.setDefault();

    // See the implementation of setDefault() to create a custom ICP algorithm
    // icp.setDefault();
    // icp.reader.open(ifs);
    // icp.loadFromYaml(ifs);

    // Compute the transformation to express data in ref
    PM::TransformationParameters T = icp(data, ref);
    std::cout << "Transformation matrix:" << std::endl << T << std::endl;

    // Transform data to express it in ref
    DP data_out(data);
    icp.transformations.apply(data_out, T);

    // Save to file
    data_out.save("data_out.vtk");

    // Inspect results
    std::cout << "Ref mean:" << ref.features.rowwise().mean() << std::endl;
    std::cout << "Data mean:" << data.features.rowwise().mean() << std::endl;
    std::cout << "Data_out mean:" << data_out.features.rowwise().mean() << std::endl;
    std::cout << "Translation (from parameters):" << T.rightCols(1) << std::endl;
    std::cout << "Translation (ref-data_out):" << ref.features.rowwise().mean() - data_out.features.rowwise().mean() << std::endl;

    // TO validate with icp_simple example results
    assert(data.features.cols() == 2334);
    assert(ref.features.cols() == 2334);
    assert((T - PM::TransformationParameters::Identity(4,4)).norm() < 1e-6);
    assert((data.features.rowwise().mean() - ref.features.rowwise().mean()).norm() < 1e-6);
    return 0;
}

二、Super 4PCS 

为了在 C++ 中使用 Super 4PCS 点云配准算法，你可以使用开源库 libpointmatcher 或者 CGAL 库。

以下是使用 CGAL 库实现 Super 4PCS 算法的一段基本示例代码：

#include 
#include 
#include 
#include 
#include 

typedef CGAL::Simple_cartesian Kernel;
typedef Kernel::Point_3 Point_3;
typedef CGAL::Identity_property_map Pmap;

int main(int argc, char* argv[])
{
  std::vector P,Q;
  const double overlap = 0.8;
  const int number_of_points = 1000;

  if(argc < 3)
  {
    std::cerr << "Error: need file names as input" << std::endl;
    return EXIT_FAILURE;
  }

  // Reading
  if(!CGAL::IO::read_points(argv[1], std::back_inserter(P), Pmap()))
  {
    std::cerr << "Error: cannot read file " << argv[1] << std::endl;
    return EXIT_FAILURE;
  }

  if(!CGAL::IO::read_points(argv[2], std::back_inserter(Q), Pmap()))
  {
    std::cerr << "Error: cannot read file " << argv[2] << std::endl;
    return EXIT_FAILURE;
  }

  // Matching
  CGAL::Superimposition_traits_3 t;
  std::vector transformations;
  CGAL::Four_point_set_matching matcher(P, Q, t, overlap);
  matcher.extract_transformations(std::back_inserter(transformations));

  // Output
  for(std::size_t i = 0; i < transformations.size(); ++i)
  {
    std::cout << transformations[i] << std::endl;
  }

  return EXIT_SUCCESS;
}