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