PCL中ICP配准点云（计算出RT，并且设置不同颜色来显示三个点云）

根据PCL英语官网教程（http://pointclouds.org/documentation/tutorials/）ICP两个例子更改：

PCL中ICP配准点云（计算出RT，并且设置不同颜色来显示三个点云），代码如下：

#include <iostream>

#include <pcl/io/pcd_io.h>

#include <pcl/point_types.h>

#include <pcl/registration/icp.h>

typedef pcl::PointXYZ PointT;

typedef pcl::PointCloud<PointT> PointCloudT;

typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;

include <iostream>

void

print4x4Matrix(const Eigen::Matrix4d & matrix)

{
printf("Rotation matrix :\n");
printf("    | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
printf("    | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
printf("Translation vector :\n");
printf("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));

}

int

main(int argc, char** argv)

{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile("0XYZ.pcd", *cloud_in);//cloud_in：初始PCD点云
pcl::io::loadPCDFile("00XYZ.pcd", *cloud_out); //cloud_out：目标PCD点云

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
//icp.setMaximumIterations(25);
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;

PointCloudT::Ptr cloud_icp(new PointCloudT);  //cloud_icp： ICP生成的结果点云
Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();

transformation_matrix = icp.getFinalTransformation().cast<double>();

pcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);

////////////////////////////以下是三个pcd分别加入颜色并且合并成一个，并显示：

PointCloud::Ptr MergeCloud(new PointCloud);

for (int j = 0; j < cloud_in->points.size(); j += 1)

{
pcl::PointXYZRGB p;
p.x = cloud_in->points[j].x;
p.y = cloud_in->points[j].y;
p.z = cloud_in->points[j].z;
p.r = 255;//红色
p.g = 0;
p.b = 0;
MergeCloud->points.push_back(p);

}

for (int j = 0; j < cloud_out->points.size(); j += 1)

{
pcl::PointXYZRGB p;
p.x = cloud_out->points[j].x;
p.y = cloud_out->points[j].y;
p.z = cloud_out->points[j].z;
p.r = 0;//黑色
p.g = 0;
p.b = 0;
MergeCloud->points.push_back(p);

}

for (int j = 0; j < cloud_icp->points.size(); j += 1)

{
pcl::PointXYZRGB p;
p.x = cloud_icp->points[j].x;
p.y = cloud_icp->points[j].y;
p.z = cloud_icp->points[j].z;
p.r = 150;//灰色
p.g = 150;
p.b = 150;
MergeCloud->points.push_back(p);

}

// 设置并保存点云

MergeCloud->height = 1;

MergeCloud->width = MergeCloud->points.size();

MergeCloud->is_dense = false;

pcl::io::savePCDFile("ICPmerge3-in-out-icp.pcd", *MergeCloud);

// 清除数据并退出

MergeCloud->points.clear();

std::cout << "cloud_in(input1-0XYZ):" << "红色" << std::endl;

std::cout << "cloud_out(input2-00XYZ):" << "黑色" << std::endl;

std::cout << "cloud_icp(icp-result):" << "灰色" << std::endl;
return (0);

}