Using PCL to achieve point cloud registration

1. Introduction

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.

Use the ICP algorithm to achieve point cloud registration and rigidly transform the 4*4 matrix.

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

Use the NDT algorithm to achieve point cloud registration of big data and rigidly transform the 4*4 matrix.

2. Code

1. ICP algorithm

#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 algorithm

    Data link: Link: https://pan.baidu.com/s/1jR7k5iI-acVyyehKcYbetA?pwd=mr16
    Extraction code: 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);
}

3. Conclusion

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

4. ICP registration

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