PCL中ICP迭代的过程展示

下面的代码实现了ICP配准按一次空格进行配准一次的功能。

#define _USE_MATH_DEFINES
#include <math.h>//pi为M_PI
#include <iostream>
#include <string>
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/visualization/cloud_viewer.h>//可视化
#include <pcl/filters/voxel_grid.h>  //体素滤波相关 下采样
#include <pcl/console/time.h>//计时器
//pcl segmentation
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/segmentation/extract_clusters.h>
//按照索引提取点云
#include <pcl/filters/extract_indices.h>
//点云移动
#include <pcl/common/transforms.h>
//点云配准
#define BOOST_TYPEOF_EMULATION
#include <pcl/registration/icp.h>

using namespace pcl;
using namespace pcl::io;
using namespace std;

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

int main() {
	pcl::PointCloud<pcl::PointXYZ>::Ptr iphonetest7_sample_filter(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("iphonetest7_sample_filter.pcd", *iphonetest7_sample_filter) == -1) {
		PCL_ERROR("Cloudn't read file!\n");
		return(-1);
	}
	else { cout << "CAD模型的点云数量为\t" << iphonetest7_sample_filter->points.size() << endl; }

	pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud_backplane(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("transformed_cloud_backplane.pcd", *transformed_cloud_backplane) == -1) {
		PCL_ERROR("Cloudn't read file!\n");
		return(-1);
	}
	else { cout << "背盖的点云数量为\t" << transformed_cloud_backplane->points.size() << endl; }


	int i = 0;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_tr(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("transformed_cloud_backplane.pcd", *cloud_tr) == -1) {
		PCL_ERROR("Cloudn't read file!\n");
		return(-1);
	}

	int iterations = 1;  // Default number of ICP iterations
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setMaximumIterations(iterations);
	icp.setInputSource(transformed_cloud_backplane);//source经过变换到target、
    icp.setInputTarget(iphonetest7_sample_filter); //target不变
	//pcl::PointCloud<pcl::PointXYZ> final;//配准后的点云
	timer.tic();
	icp.align(*transformed_cloud_backplane);
	std::cout << "Applied 1 ICP iteration in " << timer.toc() << " ms" << std::endl;
	i = i + 1;
	std::cout << "\n" << "迭代了" << i << "次" << std::endl;
	icp.setMaximumIterations(1);  // 我们将该变量设置为1，以便下次调用.align()函数
	if (icp.hasConverged())//看收敛状态
	{
		std::cout << "ICP has converged, score is " << icp.getFitnessScore() << std::endl;
		std::cout << "ICP transformation " << i << " : 背壳平面 -> 采样后的CAD模型" << std::endl;
		//std::cout << icp.getFinalTransformation().cast<double>();
		std::cout << icp.getFinalTransformation()<< std::endl;
		SUMtransform = icp.getFinalTransformation();//这次的外参

	}
	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);

	// The color we will be using
	float bckgr_gray_level = 0.0;  // Black
	float txt_gray_lvl = 1.0 - bckgr_gray_level;

	//原始的点云是白色的 source
	pcl::visualization::PointCloudColorHandlerCustom<PointXYZ> cloud_in_color_h(iphonetest7_sample_filter, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl,
		(int)255 * txt_gray_lvl);
	//在两个窗口中显示平面点云
	viewer.addPointCloud(iphonetest7_sample_filter, cloud_in_color_h, "cloud_in_v1", v1);
	viewer.addPointCloud(iphonetest7_sample_filter, cloud_in_color_h, "cloud_in_v2", v2);

	// 等待配准的点云是绿色的
	pcl::visualization::PointCloudColorHandlerCustom<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<PointXYZ> cloud_icp_color_h(transformed_cloud_backplane, 180, 20, 20);
	viewer.addPointCloud(transformed_cloud_backplane, cloud_icp_color_h, "cloud_icp_v2", v2);

	// Adding text descriptions in each viewport
	viewer.addText("White: Original point cloud\nGreen: Matrix 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\nRed: 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);
	
	// Set background color
	viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
	viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);
	
	// Set camera position and orientation
	viewer.setCameraPosition(0, 0, -500, 0, 0, 0, 0);
	viewer.setSize(1280, 1024);  //可视化窗口的大小

	// Register keyboard callback :
	viewer.registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);
	
	while (!viewer.wasStopped())
	{
		viewer.spinOnce(); //void spinOnce(int time = 1)调用内部渲染函数一次，重新渲染输出时间最大不超过time, 单位ms

		// The user pressed "space" :
		if (next_iteration)
		{
			// The Iterative Closest Point algorithm
			timer.tic();
			icp.align(*transformed_cloud_backplane);
			i = i + 1;
			std::cout << "\n\n" << "迭代了" << i << "次" << std::endl;
			std::cout << "Applied 1 ICP iteration in " << timer.toc() << " ms" << std::endl;

			if (icp.hasConverged())
			{
				//printf("\033[11A");  //在终端输出中向上移动11行。
				printf("ICP has converged, score is %+.0e\n", icp.getFitnessScore());
				std::cout << "ICP transformation " << i << " : 背壳平面 -> 采样后的CAD模型" << std::endl;
				//std::cout << icp.getFinalTransformation().cast<double>() << std::endl;  // WARNING /!\ This is not accurate! For "educational" purpose only!
				SUMtransform = SUMtransform * icp.getFinalTransformation();
				std::cout << SUMtransform.matrix() << std::endl;
				ss.str("");
				iterations++;
				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(transformed_cloud_backplane, cloud_icp_color_h, "cloud_icp_v2");
			}
			else
			{
				PCL_ERROR("\nICP has not converged.\n");
				return (-1);
			}
		}
		next_iteration = false;
	}
	return(0);
}