PCL交互迭代最近点方法

**代码出处：**http://pointclouds.org/documentation/tutorials/interactive_icp.php#interactive-icp

交互迭代最近法允许用户每次按下“空格”键，一次ICP迭代算法执行，并更新视图。
此程序主要步骤：

1、导入点云数据

PCL提供IO模块直接读入点云文件。

pcl::io::loadPCDFile("dragon_source.pcd", *cloud_in)

注：控制台参数输入时，cd到exe路径，执行 "XX arg1 arg2 …"

2、旋转平移获取待配准点云数据

pcl中矩阵的定义及赋值：

//定义旋转矩阵和平移矩阵
	Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();

	double theta = M_PI / 8;//旋转角度
	transformation_matrix(0,0) = cos(theta);
	transformation_matrix(0,1) = -sin(theta);
	transformation_matrix(1,0) = sin(theta);
	transformation_matrix(1,1) = cos(theta);
	
	transformation_matrix(0, 3) = 0;//沿x轴移动0.m
	transformation_matrix(1, 3) = 0;//沿y轴移动0.m
	transformation_matrix(2, 3) = 0.04;//沿z轴移动0.04m

	pcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);//执行变换

3、icp配准

	pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ> icp;//配准对象
	icp.setInputSource(cloud_icp);
	icp.setInputTarget(cloud_in);
	/*icp.setEuclideanFitnessEpsilon(0.1);
	icp.setRANSACIterations(10);
	icp.setTransformationEpsilon(1e-6);*/
	icp.align(*cloud_icp);

	icp.setMaximumIterations(1);//迭代一次

4、点云可视化

//可视化
	pcl::visualization::PCLVisualizer viewer("ICP demo");
	//创建两个视口
	int v1(0);
	int v2(1);
	viewer.createViewPort(0, 0, 0.5, 1, v1);
	viewer.createViewPort(0.5, 0, 1, 1, v2);
	//颜色
	float bckgr_gray_level = 0.0;//黑色
	float txt_gray_lvl = 1.0 - bckgr_gray_level;

	//原始点云 白色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_in_color_h
		(cloud_in, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl);

	viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
	viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v2", v2);
	
	//目标变换点云 绿色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_tr_color_h
		(cloud_tr, 20,180,20);
	viewer.addPointCloud(cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);

	//ICP 配准点云 红色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_icp_color_h
		(cloud_tr, 180, 20, 20);
	viewer.addPointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);

完整代码

/** Filename:  interaction_icp.cpp

** Date: 2018-4-17

**Description:

**/

#include "stdafx.h"
#include <iostream>
#include <string>
#include <vector>

#include <pcl\visualization\pcl_plotter.h>
#include <pcl\visualization\pcl_visualizer.h>
#include <pcl\point_types.h>
#include <pcl\point_cloud.h>
#include <pcl\range_image\range_image.h>
#include <pcl\visualization\range_image_visualizer.h>
#include <pcl\io\ply_io.h>
#include <pcl\io\pcd_io.h>
#include <pcl\registration\icp.h>
#include <pcl\console\time.h>

bool next_iteration = false;

void print4x4Matrix(const Eigen::Matrix4d& matrix)
{
	printf("旋转矩阵R:\n");
	printf("      |%6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
	printf(" R =  |%6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
	printf("      |%6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
	printf("平移矩阵T: \n");
	printf("T = < %6.3f %6.3f %6.3f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));
}


void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void *nothing)
{
	if (event.getKeySym() == "space" && event.keyDown())
		next_iteration = true;
}

int main(int argc, char* argv[])
{
	//定义点云类型
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);//目标点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tr(new pcl::PointCloud<pcl::PointXYZ>);//变换点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_icp(new pcl::PointCloud<pcl::PointXYZ>);//icp配准后点云
	
	//检查程序参数
	if (argc < 2)
	{
		printf("Usage:\n");
		printf("\t\t%s file.ply number_of_ICP_iterations\n",argv[0]);
		PCL_ERROR("Provide one ply file. \n");
		return -1;
	}
	int iterations = 1;//默认迭代次数
	if (argc>2)
	{
		iterations = atoi(argv[1]);
		if (iterations < 1)
		{
			PCL_ERROR("Number of initial iterations must be >= 1\n");
			return -1;
		}
	}
	pcl::console::TicToc time;
	time.tic();
	//加载点云数据
	if (pcl::io::loadPCDFile("dragon_source.pcd", *cloud_in) < 0)
	{
		PCL_ERROR("Error loading cloud %s.\n", argv[1]);
		return -1;
	}

	std::cout << "\n Loaded file " << argv[1] << "(" << cloud_in->points.size() << "points) in " << time.toc() << "ms\n\n";

	//定义旋转矩阵和平移矩阵
	Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
	print4x4Matrix(transformation_matrix);
	std::cout << transformation_matrix << std::endl;

	double theta = M_PI / 8;//旋转角度
	transformation_matrix(0,0) = cos(theta);
	transformation_matrix(0,1) = -sin(theta);
	transformation_matrix(1,0) = sin(theta);
	transformation_matrix(1,1) = cos(theta);

	transformation_matrix(0, 3) = 0;//沿x轴移动0.m
	transformation_matrix(1, 3) = 0;//沿y轴移动0.m
	transformation_matrix(2, 3) = 0.04;//沿z轴移动0.04m
	
	//在控制台显示变换矩阵
	printf("刚性变换矩阵：\n");
	std::cout << transformation_matrix << std::endl;
	pcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);//执行变换
	*cloud_tr = *cloud_icp;//备份目标点云

	//ICP算法
	std::cout << "ICP 配准开始" << std::endl;
	time.tic();
	pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ> icp;
	icp.setInputSource(cloud_icp);
	icp.setInputTarget(cloud_in);
	/*icp.setEuclideanFitnessEpsilon(0.1);
	icp.setRANSACIterations(10);
	icp.setTransformationEpsilon(1e-6);*/
	icp.align(*cloud_icp);

	icp.setMaximumIterations(1);
	std::cout << "Applied" << iterations << "ICP iteration(s) in " << time.toc() << "ms"<<std::endl;
	if (icp.hasConverged())
	{
		std::cout << "\n ICP has converged, score is " << icp.getFitnessScore() << std::endl;
		std::cout << "\n ICP transformation " << iterations << ": cloud_icp -> cloud_in" << std::endl;
		transformation_matrix = icp.getFinalTransformation().cast<double>();;
		print4x4Matrix(transformation_matrix);
	}
	else
	{
		PCL_ERROR(" \n ICP has not converged.\n");
		return -1;
	}

	//可视化
	pcl::visualization::PCLVisualizer viewer("ICP demo");
	//创建两个视口
	int v1(0);
	int v2(1);
	viewer.createViewPort(0, 0, 0.5, 1, v1);
	viewer.createViewPort(0.5, 0, 1, 1, v2);
	//颜色
	float bckgr_gray_level = 0.0;//黑色
	float txt_gray_lvl = 1.0 - bckgr_gray_level;

	//原始点云 白色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_in_color_h
		(cloud_in, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl);

	viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
	viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v2", v2);
	
	//目标变换点云 绿色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_tr_color_h
		(cloud_tr, 20,180,20);
	viewer.addPointCloud(cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);

	//ICP 配准点云 红色
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_icp_color_h
		(cloud_tr, 180, 20, 20);
	viewer.addPointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);
	//添加文本描述
	viewer.addText("White: Original point cloud\n Green :Matix transformed point cloud", 10, 15, 16,
		txt_gray_lvl, txt_gray_lvl, txt_gray_lvl,"icp_info_1",v1);
	viewer.addText("White: Original point cloud\n Red: ICP aligned point cloud", 10, 15, 16, 
		txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);

	std::stringstream ss;
	ss << iterations;
	std::string iterations_cnt = "ICP iterations = " + ss.str();
	viewer.addText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);

	//设置背景颜色
	viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
	viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);

	//设置相机位置和方向
	viewer.setCameraPosition(-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
	//窗口大小
	viewer.setSize(1280, 1024);
	viewer.registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);

	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
		//按下空格键
		if (next_iteration)
		{
			//ICP 算法
			time.tic();
			icp.align(*cloud_icp);
			std::cout << "Applied 1 ICP iteration in " << time.toc() << "ms" << std::endl;
			if (icp.hasConverged())
			{
				printf("\033[11A");
				printf("\n ICP has converged ,score is %+.0e\n", icp.getFitnessScore());
				std::cout << "\n ICP transformation" << ++iterations << " ：cloud_icp -> cloud_in" << std::endl;
				transformation_matrix *= icp.getFinalTransformation().cast<double>();  
				print4x4Matrix(transformation_matrix);  

				ss.str("");
				ss << iterations;
				std::string iterations_cnt = "ICP iterations = " + ss.str();
				viewer.updateText(iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt");
				viewer.updatePointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2");
			}
			else
			{
				PCL_ERROR("\nICP has not converged.\n");
				return (-1);
			}
		}
		next_iteration = false;
	}
	system("pause");
	return 0;
}

可视化结果