PCL ICP点云配准

一、概述

  PCL中迭代最近点算法的简单使用案例

二、代码

iterative_closest_point.cpp

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>

int
 main ()
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>(5,1));
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);

  // Fill in the CloudIn data
  for (auto& point : *cloud_in)
  {
    point.x = 1024 * rand() / (RAND_MAX + 1.0f);
    point.y = 1024 * rand() / (RAND_MAX + 1.0f);
    point.z = 1024 * rand() / (RAND_MAX + 1.0f);
  }
  
  std::cout << "Saved " << cloud_in->size () << " data points to input:" << std::endl;
      
  for (auto& point : *cloud_in)
    std::cout << point << std::endl;
      
  *cloud_out = *cloud_in;
  
  std::cout << "size:" << cloud_out->size() << std::endl;
  for (auto& point : *cloud_out)
    point.x += 0.7f;

  std::cout << "Transformed " << cloud_in->size () << " data points:" << std::endl;
      
  for (auto& point : *cloud_out)
    std::cout << point << std::endl;

  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  icp.setInputSource(cloud_in);
  icp.setInputTarget(cloud_out);
  
  pcl::PointCloud<pcl::PointXYZ> Final;
  icp.align(Final);

  std::cout << "has converged:" << icp.hasConverged() << " score: " <<
  icp.getFitnessScore() << std::endl;
  std::cout << icp.getFinalTransformation() << std::endl;

 return (0);
}

三、结果

Saved 5 data points to input:
(1.28125,577.094,197.938)
(828.125,599.031,491.375)
(358.688,917.438,842.562)
(764.5,178.281,879.531)
(727.531,525.844,311.281)
size:5
Transformed 5 data points:
(1.98125,577.094,197.938)
(828.825,599.031,491.375)
(359.388,917.438,842.562)
(765.2,178.281,879.531)
(728.231,525.844,311.281)
has converged:1 score: 7.32622e-07
           1  1.16229e-06 -7.15257e-07      0.69861
  1.2517e-06            1 -7.74862e-07  -0.00146401
-6.85455e-07 -6.55652e-07            1  6.06187e-05
           0            0            0            1