PCL学习笔记--利用矩阵对点云进行刚体变换(ICP)

利用矩阵对点云进行刚体变换
在这篇教程中，我们会学习如何用一个4*4的矩阵对点云进行变换。我们会加载一个点云，然后对其进行刚体变换（旋转加平移），并可视化结果，最后通过ICP算法对点云进行配准。

**程序**
/*
* 功能： 点云刚体变换
* 头文件： #include <pcl/common/transforms.h>
* 功能函数： pcl::transformPointCloud(*cloud, *cloudOut, transform_1);
*/
#include "stdafx.h"
#include <iostream>
#include <pcl\common\transforms.h>
#include <pcl\io\pcd_io.h>
#include <pcl\visualization\pcl_visualizer.h>
#include <pcl\registration\icp.h>


int _tmain(int argc, _TCHAR* argv[])
{
	//读入点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile("rabbit.pcd", *cloud);

	//可视化初始化
	pcl::visualization::PCLVisualizer viewer;
	viewer.setCameraFieldOfView(0.785398);//fov 45°  视场角
	viewer.setBackgroundColor(0.2, 0.2, 0.2);
	viewer.setCameraPosition(
		0, 0, 0,
		0, 0, -1,
		0, 0, 0);

	//点云旋转
	Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();//单位矩阵
	float theta = M_PI / 20;
	//给矩阵赋值
	transform_1(0, 0) = cos(theta);
	transform_1(0, 1) = -sin(theta);
	transform_1(1, 0) = sin(theta);
	transform_1(1, 1) = cos(theta);

	//沿x轴平移0.025
	transform_1(0, 3) = 0.01;
	printf("Method #1: using a Matrix4f\n");
	std::cout << transform_1 << std::endl;

	//方法2 
	Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
	transform_2.translation() << 0.025, 0.0, 0.0;
	transform_2.rotate(Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
	// 打印变换矩阵
	printf("\nMethod #2: using an Affine3f\n");
	std::cout << transform_2.matrix() << std::endl;
	//执行变换

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOut(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*cloud, *cloudOut, transform_1);

	//点云可视化
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> incloudHandler(cloud, 255, 255, 255);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> outcloudHandler(cloudOut, 225, 30, 30);
	viewer.addPointCloud(cloud, incloudHandler, "In");
	viewer.addPointCloud(cloudOut, outcloudHandler, "Out");
	viewer.addCoordinateSystem(0.1, "cloud", 0);

	int v2 = 1;
	viewer.createViewPort(0.5, 0, 1, 1, v2);
	viewer.createViewPortCamera(v2);
	viewer.setCameraFieldOfView(0.785398,v2);//fov 45°  视场角
	viewer.setBackgroundColor(0.0, 0.2, 1.0,v2);
	viewer.setCameraPosition(
		0, 0, 0,
		0, 0, -1,
		0, 0, 0,v2);
	
	// ICP  
   // pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;  
   // pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1 (new pcl::search::KdTree<pcl::PointXYZ>);  
   // tree1->setInputCloud(cloud_source);  
   // pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ>);  
   // tree2->setInputCloud(cloud_target);  
   // icp.setSearchMethodSource(tree1);  
   // icp.setSearchMethodTarget(tree2);  
   // icp.setInputSource(cloud_source);  
   // icp.setInputTarget(cloud_target);  
   // icp.setMaxCorrespondenceDistance(1500);  
   // icp.setTransformationEpsilon(1e-10);  
   // icp.setEuclideanFitnessEpsilon(0.1);  
   // icp.setMaximumIterations(300);  
   // icp.align(*cloud_source_registration);  
   // Eigen::Matrix4f transformation = icp.getFinalTransformation();
   
	//ICP配准
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputSource(cloud);
	icp.setInputTarget(cloudOut);
	pcl::PointCloud<pcl::PointXYZ>::Ptr Final(new pcl::PointCloud<pcl::PointXYZ>);
	icp.align(*Final);

	std::cout << "has converged:" << icp.hasConverged() << " score: " <<
		icp.getFitnessScore() << std::endl;
	//输出最终的变换矩阵（4x4）
	std::cout << icp.getFinalTransformation() << std::endl;

	//点云可视化
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> incloudHandler1(cloud, 255, 255, 255);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> outcloudHandler1(cloudOut, 225, 30, 30);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> FinalcloudHandler1(Final, 0, 255, 0);
	//viewer.addPointCloud(cloud, incloudHandler1, "In22",v2);
	viewer.addPointCloud(cloudOut, outcloudHandler1, "Out22",v2);
	viewer.addPointCloud(Final, FinalcloudHandler1, "Final22",v2);
	viewer.addCoordinateSystem(0.1, "cloud22", v2);

	while(!viewer.wasStopped())
	{
		viewer.spinOnce();
	}

	system("pause");
	return 0;
}