PCL教程-点云配准之ICP算法的使用

原文链接：How to use iterative closest point

 在本篇教程中，将在代码中使用迭代最近点（Iterative Closest Point ，ICP）算法，通过最小化两个点云之间的距离并严格转换它们，可以确定一个点云是否只是另一个点云的严格转换。这里的转换是平移+旋转的空间转换。

算法步骤：

Iterative Closest Point

1. Search for correspondences.（搜索对应关系）
2. Reject bad correspondences.（剔除差的对应关系）
3. Estimate a transformation using the good correspondences.（使用好的对应关系来估算出转换矩阵）
4. Iterate.（迭代）

程序代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
int
main(int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
	// 填入点云数据
	cloud_in->width = 5;
	cloud_in->height = 1;
	cloud_in->is_dense = false;
	cloud_in->points.resize(cloud_in->width * cloud_in->height);

	//使用随机数填充点云
	for (size_t i = 0; i < cloud_in->points.size(); ++i)
	{
		cloud_in->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
		cloud_in->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
		cloud_in->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
	}

	//打印创建的点云数据
	std::cout << "Saved " << cloud_in->points.size() << " data points to input:"
		<< std::endl;
	for (size_t i = 0; i < cloud_in->points.size(); ++i) std::cout << "    " <<
		cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
		cloud_in->points[i].z << std::endl;

	//创建第二个点云
	*cloud_out = *cloud_in;
	std::cout << "size:" << cloud_out->points.size() << std::endl;
	for (size_t i = 0; i < cloud_in->points.size(); ++i)
		//这里只做了平移变换，所有点的x坐标平移了0.7的距离
		cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;


	std::cout << "Transformed " << cloud_in->points.size() << " data points:"
		<< std::endl;
	for (size_t i = 0; i < cloud_out->points.size(); ++i)
		std::cout << "    " << cloud_out->points[i].x << " " <<
		cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;

	//创建ICP对象
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	//为ICP算法设置输入点云
	icp.setInputCloud(cloud_in);
	//设置目标点云
	icp.setInputTarget(cloud_out);
	//Final为验证的结果
	pcl::PointCloud<pcl::PointXYZ> Final;
	icp.align(Final);

	//打印出验证的结果
	std::cout << "Final " << cloud_in->points.size() << " data points:"
		<< std::endl;
	for (size_t i = 0; i < Final.points.size(); ++i)
		std::cout << "    " << Final.points[i].x << " " <<
		Final.points[i].y << " " << Final.points[i].z << std::endl;

	//如果hasConverged() == 1,说明输入点云和目标点云之间的是刚性变换，并返回ICP的评估分数
	std::cout << "has converged:" << icp.hasConverged() << " score: " <<
		icp.getFitnessScore() << std::endl;

	//打印出输入点云和目标点云之间的转换矩阵
	std::cout << icp.getFinalTransformation() << std::endl;
	return (0);
}

关键代码解析

 pcl::PointCloud<pcl::PointXYZ> Final;
  icp.align(Final);

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

• 创建一个pcl::PointCloud<pcl::PointXYZ> Final对象， ICP可以在应用算法后将结果点云保存到该对象。
• 如果输入点云和目标点云对齐正确(意味着它们都是同一个点云，只是对其中一个应用了某种严格的转换（刚性变换）)，那么icp.hasConverged() = 1 (true)。
• 然后输出最终变换的适应度评分和一些相关信息。
• pcl中的score定义是两份点云之间的误差（error），score越小表示匹配的越准；

实验结果

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
Final 5 data points:
    1.28113 577.094 197.938
    828.125 599.031 491.375
    358.687 917.438 842.562
    764.5 178.281 879.531
    727.531 525.844 311.281
has converged:1 score: 4.41085
           1  4.47035e-07 -4.17233e-07      2.79968
           2.68221e-07            1 -3.12925e-07 -0.000610081
           -2.38419e-07 -3.27826e-07            1 -0.000122237
           0            0            0            1