PCL的ICP配准示例

2019/07/18，转载请注明。

使用的PCL 1.8.0，VS2013。

我用的3D模型，是网上下载的obj模型，自己用代码转成ply文件，一开始读取失败，检查发现是ply的header不符合PCL的，

按照PCL的格式修改后就可以了。

一、PCL中ply格式

PCL库读取PLY文件，要注意文件的header是否符合要求的，在ply._io.h中有注明：

/** \brief Point Cloud Data (PLY) file format reader.
   52     *
   53     * The PLY data format is organized in the following way:
   54     * lines beginning with "comment" are treated as comments
   55     *   - ply
   56     *   - format [ascii|binary_little_endian|binary_big_endian] 1.0
   57     *   - element vertex COUNT
   58     *   - property float x 
   59     *   - property float y 
   60     *   - [property float z] 
   61     *   - [property float normal_x] 
   62     *   - [property float normal_y] 
   63     *   - [property float normal_z] 
   64     *   - [property uchar red] 
   65     *   - [property uchar green] 
   66     *   - [property uchar blue] ...
   67     *   - ascii/binary point coordinates
   68     *   - [element camera 1]
   69     *   - [property float view_px] ...
   70     *   - [element range_grid COUNT]
   71     *   - [property list uchar int vertex_indices]
   72     *   - end header
   73     *
   74     * \author Nizar Sallem
   75     * \ingroup io
   76     */

二、ICP代码

#include <iostream>
#include <string>
#include <conio.h>
#include <vector>
#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>//可视化头文件
#include <pcl/console/time.h>   // TicToc
using namespace std;
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
 
bool next_iteration = false;
bool end_iteration = false;
 
void print4x4Matrix(const Eigen::Matrix4d & matrix) //打印旋转矩阵和平移矩阵
{
	printf("Rotation matrix :\r\n");
	printf("    | %6.6f %6.6f %6.6f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
	printf("R = | %6.6f %6.6f %6.6f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
	printf("    | %6.6f %6.6f %6.6f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
	printf("Translation vector :\n");
	printf("t = < %6.6f, %6.6f, %6.6f >\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;
	if (event.getKeySym() == "c" && event.keyDown())//使用c键来结束迭代过程，并退出循环
		end_iteration = true;
}
 
 
 
int main(int argc, char* argv[])
{
	//创建点云指针
	PointCloudT::Ptr cloud_in(new PointCloudT);  // Original point cloud//输入点云指针
	PointCloudT::Ptr cloud_tr(new PointCloudT);  // Transformed point cloud//目标点云指针
	PointCloudT::Ptr cloud_icp(new PointCloudT);  // ICP output point cloud//ICP点云指针
	pcl::PLYWriter plywriter;
	//创建计时器
	pcl::console::TicToc time;
	time.tic();
	//载入输入点云
	if (pcl::io::loadPLYFile<PointT>("02_sfm.ply", *cloud_in) == -1)
	{
		PCL_ERROR("Couldn't read file1 \n");
		return (-1);
	}
	std::cout << "Loaded " << cloud_in->size() << " data points from file1" << std::endl;
	//载入目标点云
	if (pcl::io::loadPLYFile<PointT>("02_gray.ply", *cloud_tr) == -1)
	{
		PCL_ERROR("Couldn't read file2 \n");
		return (-1);
	}
 
	int iterations = 1;  // Default number of ICP iterations//默认的ICP迭代次数
	//定义旋转矩阵和平移矩阵向量Matrix4d是为4*4的矩阵
	Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
	/* Reminder: how transformation matrices work :
           |-------> This column is the translation
    | 1 0 0 x |  \
    | 0 1 0 y |   }-> The identity 3x3 matrix (no rotation) on the left
    | 0 0 1 z |  /
    | 0 0 0 1 |    -> We do not use this line (and it has to stay 0,0,0,1)
 
    METHOD #1: Using a Matrix4f
    This is the "manual" method, perfect to understand but error prone !
  */ 
	//初始化一个接近的位置
	double theta = M_PI;  // The angle of rotation in radians
	transformation_matrix(0, 0) = cos(theta);
	transformation_matrix(0, 2) = -sin(theta);
	transformation_matrix(2, 0) = sin(theta);
	transformation_matrix(2, 2) = cos(theta);
	pcl::transformPointCloud(*cloud_in, *cloud_in, transformation_matrix);
	
	//点太多 降采样加速
	pcl::VoxelGrid<PointT> sor2;// 创建采样对象
	PointCloudT::Ptr down_cloud_in(new PointCloudT);  // 降采样输入点云指针
	PointCloudT::Ptr down_cloud_tr(new PointCloudT);  // 降采样目标点云指针
	sor2.setLeafSize(1.2f, 1.2f, 1.2f);//设置滤波时创建的体素体积为1.2的立方体
	sor2.setInputCloud(cloud_in);//设置需要过滤的点云
	sor2.filter(*down_cloud_in);//执行滤波处理 
	std::cout << "down_cloud_in point num " << down_cloud_in->size()<< std::endl;
	sor2.setInputCloud(cloud_tr);//设置需要过滤的点云
	sor2.filter(*down_cloud_tr);//执行滤波处理 
	std::cout << "down_cloud_tr point num " << down_cloud_tr->size() << std::endl;
	*cloud_icp = *down_cloud_in;
	
	//计时器开始
	time.tic();
	
	//创建ICP迭代器
	pcl::IterativeClosestPoint<PointT, PointT> icp;
	icp.setMaximumIterations(iterations);//每次align最大迭代次数
	icp.setInputSource(cloud_icp);//这里的icp源数据必定是在不停迭代的变量cloud_icp，与align()中的输出一致
	icp.setInputTarget(down_cloud_tr);//配准目标
	icp.align(*cloud_icp);//一定要与setInputSource中的输入一致，确保下次在本次结果上迭代
	std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc() << " ms" << std::endl;
 
	if (icp.hasConverged())
	{
		std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
		std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_tr" << std::endl;
		transformation_matrix = icp.getFinalTransformation().cast<double>();
		print4x4Matrix(transformation_matrix);
	}
	else
	{
		PCL_ERROR("\nICP has not converged.\n");
		return (-1);
	}
 
	//创建可视化窗口
	pcl::visualization::PCLVisualizer viewer("ICP demo");
	//创建两个水平的可视化组件
	int v1(0);
	int v2(1);
	viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);
	//背景色和字体色
	float bckgr_gray_level = 0.0;  // Black
	float txt_gray_lvl = 1.0 - bckgr_gray_level;
 
	// 目标点云是白色
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h(down_cloud_tr, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl,	(int)255 * txt_gray_lvl);
	viewer.addPointCloud(down_cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);
	viewer.addPointCloud(down_cloud_tr, cloud_tr_color_h, "cloud_tr_v2", v2);
 
	// 源点云是绿色
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h(down_cloud_in, 20, 180, 20);
	viewer.addPointCloud(down_cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
 
	// ICP点云是红色
	pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h(cloud_icp, 180, 20, 20);
	viewer.addPointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);
 
	// 添加文字
	viewer.addText("White: pcd no colour\nGreen: Ori colour pcd ", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);
	viewer.addText("White: pcd no colour\nRed: ICP colour pcd", 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();
		// 按下空格，next_iteration会被赋值true，执行一次ICP
		if (next_iteration)
		//if (c!=27)
		{
			// The Iterative Closest Point algorithm
			time.tic();
			icp.align(*cloud_icp);
			std::cout << "Applied 1 ICP iteration in " << time.toc() << " ms" << std::endl;
			if (icp.hasConverged())
			{
				printf("\033[11A");  // Go up 11 lines in terminal output.
				printf("\nICP has converged, score is %+.0e\n", icp.getFitnessScore());
				std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_tr" << std::endl;
				transformation_matrix = icp.getFinalTransformation().cast<double>()*transformation_matrix;  
				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;
		//键盘按c，则ICP配准结束，退出循环
		if (end_iteration == true)
		{
			viewer.close();
			break;
		}
	}
	printf("ICP over!\n");
 
	// plywriter.write("add_colour.ply", *down_cloud_tr);
	
	system("pause");
	return (0);
}