利用PCL实现点云配准

一、介绍

This document demonstrates using the Iterative Closest Point algorithm in your code which can determine if one PointCloud is just a rigid transformation of another by minimizing the distances between the points of two pointclouds and rigidly transforming them.

使用ICP算法实现点云的配准，刚性变换4*4矩阵。

How to use the Normal Distributions Transform (NDT) algorithm to determine a rigid transformation between two large point clouds, both over 100,000 points.

使用NDT算法实现大数据的点云配准，刚性变换4*4矩阵。

二、代码

1、ICP算法

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>

int main()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>(5, 1));
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);

	for (auto& point : *cloud_in)
	{
		point.x = 1024 * rand() / (RAND_MAX + 1.0f);
		point.y = 1024 * rand() / (RAND_MAX + 1.0f);
		point.z = 1024 * rand() / (RAND_MAX + 1.0f);
	}
	std::cout << "cloud_in num = " << cloud_in->size() << " ################## " << std::endl;
	for (auto& point : *cloud_in)
		std::cout << point << std::endl;

	*cloud_out = *cloud_in;
	for (auto& point : *cloud_out)
		point.x += 0.7f;
	std::cout << "cloud_out num = " << cloud_out->size() << " ################## " << std::endl;
	for (auto& point : *cloud_out)
		std::cout << point << std::endl;

	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputSource(cloud_in);
	icp.setInputTarget(cloud_out);

	pcl::PointCloud<pcl::PointXYZ> Final;
	icp.align(Final);
	std::cout << "Final num = " << Final.size() << " ################## " << std::endl;
	for (auto& point : Final)
		std::cout << point << std::endl;

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

	return (0);
}

2、NDT算法

    数据链接：链接：https://pan.baidu.com/s/1jR7k5iI-acVyyehKcYbetA?pwd=mr16
    提取码：mr16

#include <iostream>
#include <thread>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std::chrono_literals;
using namespace std;

int main()
{
	// Loading first scan of room.
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("room_scan1.pcd", *target_cloud) == -1)
	{
		PCL_ERROR("Couldn't read file room_scan1.pcd \n");
		return (-1);
	}
	cout << "Loaded " << target_cloud->size() << " data points from room_scan1.pcd" << endl;

	// Loading second scan of room from new perspective.
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("room_scan2.pcd", *input_cloud) == -1)
	{
		PCL_ERROR("Couldn't read file room_scan2.pcd \n");
		return (-1);
	}
	cout << "Loaded " << input_cloud->size() << " data points from room_scan2.pcd" << endl;

	// Filtering input scan to roughly 10% of original size to increase speed of registration.
	pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
	approximate_voxel_filter.setLeafSize(0.2, 0.2, 0.2);
	approximate_voxel_filter.setInputCloud(input_cloud);
	approximate_voxel_filter.filter(*filtered_cloud);
	cout << "Filtered cloud contains " << filtered_cloud->size() << " data points from room_scan2.pcd" << endl;

	// Initializing Normal Distributions Transform (NDT).
	pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

	// Setting scale dependent NDT parameters
	// Setting minimum transformation difference for termination condition.
	ndt.setTransformationEpsilon(0.01);
	// Setting maximum step size for More-Thuente line search.
	ndt.setStepSize(0.1);
	// Setting Resolution of NDT grid structure (VoxelGridCovariance).
	ndt.setResolution(1.0);
	// Setting max number of registration iterations.
	ndt.setMaximumIterations(35);
	// Setting point cloud to be aligned.
	ndt.setInputSource(filtered_cloud);
	// Setting point cloud to be aligned to.
	ndt.setInputTarget(target_cloud);

	// Set initial alignment estimate found using robot odometry.
	Eigen::AngleAxisf init_rotation(0.6931, Eigen::Vector3f::UnitZ());
	Eigen::Translation3f init_translation(1.79387, 0.720047, 0);
	Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix();
	cout << init_guess << endl;

	// Calculating required rigid transform to align the input cloud to the target cloud.
	pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	ndt.align(*output_cloud, init_guess);
	cout << ndt.getFinalTransformation() << endl;
	cout << "NDT has converged:" << ndt.hasConverged() << " score: " << ndt.getFitnessScore() << endl;

	// Transforming unfiltered input cloud using found transform.
	pcl::transformPointCloud(*input_cloud, *output_cloud, ndt.getFinalTransformation());
	cout << "transformResult " << output_cloud->size() << " data points from room_scan2.pcd" << endl;

	// Saving transformed input cloud.
	pcl::io::savePCDFileASCII("room_scan2_transformed.pcd", *output_cloud);

	// Initializing point cloud visualizer
	pcl::visualization::PCLVisualizer::Ptr
	viewer_final(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer_final->setBackgroundColor(0, 0, 0);

	// Coloring and visualizing target cloud (red).
	pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZ>
	target_color(target_cloud, 255, 0, 0);
	viewer_final->addPointCloud<pcl::PointXYZ>(target_cloud, target_color, "target cloud");
	viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");

	// Coloring and visualizing transformed input cloud (green).
	pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZ>
	output_color(output_cloud, 0, 255, 0);
	viewer_final->addPointCloud<pcl::PointXYZ>(output_cloud, output_color, "output cloud");
	viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "output cloud");

	// Starting visualizer
	viewer_final->addCoordinateSystem(1.0, "global");
	viewer_final->initCameraParameters();

	// Wait until visualizer window is closed.
	while (!viewer_final->wasStopped())
	{
		viewer_final->spinOnce(100);
		this_thread::sleep_for(100ms);
	}

	return (0);
}

三、结论

1、ICP

参考：How to use iterative closest point — Point Cloud Library 0.0 documentation 

2、NDT

参考：How to use Normal Distributions Transform — Point Cloud Library 0.0 documentation

四、ICP配准

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/registration/icp.h>
#include <thread>

using namespace std::chrono_literals;

int main()
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>);
	if (-1 == pcl::io::loadPLYFile("bun000.ply", *cloud1))
	{
		std::cout << "read ply file error!" << std::endl;
	}
	std::cout << "cloud1 size = " << cloud1->points.size() << std::endl;  // 204800

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZ>);
	if (-1 == pcl::io::loadPLYFile("bun045.ply", *cloud2))
	{
		std::cout << "read ply file error!" << std::endl;
	}
	std::cout << "cloud2 size = " << cloud2->points.size() << std::endl;  // 204800

	pcl::PointCloud<pcl::PointXYZ> source;
	for (auto point : *cloud1)
	{
		if (isfinite(point.x) && isfinite(point.y) && isfinite(point.z))
		{
			source.push_back(point);
		}
	}
	source.width = source.size();
	source.height = 1;
	source.is_dense = false;
	std::cout << "source size = " << source.points.size() << std::endl;

	pcl::PointCloud<pcl::PointXYZ> target;
	for (auto point : *cloud2)
	{
		if (isfinite(point.x) && isfinite(point.y) && isfinite(point.z))
		{
			target.push_back(point);
		}
	}
	target.width = target.size();
	target.height = 1;
	target.is_dense = false;
	std::cout << "target size = " << target.points.size() << std::endl;

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudSource = source.makeShared();
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTarget = target.makeShared();

	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setInputSource(cloudSource);
	icp.setInputTarget(cloudTarget);
	pcl::PointCloud<pcl::PointXYZ> Final;
	icp.align(Final);
	std::cout << "Final num = " << Final.size() << std::endl;
	std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
	std::cout << icp.getFinalTransformation() << std::endl;

	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*cloudSource, *output, icp.getFinalTransformation());

	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(0, 0, 0);

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> target_color(cloudTarget, 255, 0, 0);
	viewer->addPointCloud<pcl::PointXYZ>(cloudTarget, target_color, "target cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud");

	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> output_color(cloudSource, 0, 255, 0);
	viewer->addPointCloud<pcl::PointXYZ>(cloudSource, output_color, "output cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "output cloud");

	// viewer->addCoordinateSystem(0.1, "global");
	viewer->initCameraParameters();
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		std::this_thread::sleep_for(100ms);
	}

	return 0;
}