代码展示:
#include <string>
#include <iostream>
#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;
using PointT = pcl::PointXYZ;
using PointCloudT = pcl::PointCloud<PointT>;
bool next_iteration = false;
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void *nothing) {
if (event.getKeySym() == "space" && event.keyDown())
next_iteration = true;
}
int main() {
// 声明需要用到的点云(读入的,转换的,ICP调整的三个点云)
PointCloudT::Ptr cloud_in(new PointCloudT(10, 1)); // 原始点云
PointCloudT::Ptr cloud_tr(new PointCloudT); // 转换后的点云
PointCloudT::Ptr cloud_icp(new PointCloudT); // ICP输出后的点云
int iterations = 0; // 缺省的迭代次数
// 随机填充点云
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);
}
// 定义旋转平移的转换矩阵
Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
double theta = M_PI / 4; // 旋转角度(弧度)
transformation_matrix(0, 0) = cos(theta);
transformation_matrix(0, 1) = -sin(theta);
transformation_matrix(1, 0) = sin(theta);
transformation_matrix(1, 1) = cos(theta);
// Z轴平移0.4米
transformation_matrix(2, 3) = 0.4;
//打印出旋转矩阵R和平移T
cout << "cloud_in到cloud_icp的旋转矩阵为: " << endl << transformation_matrix << endl;
//转移后
pcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);
*cloud_tr = *cloud_icp; //
// ICP算法调用
pcl::IterativeClosestPoint<PointT, PointT> icp;
icp.setMaximumIterations(iterations);
icp.setInputSource(cloud_icp);
icp.setInputTarget(cloud_in);
icp.align(*cloud_icp);
icp.setMaximumIterations(1);
if (icp.hasConverged()) {
cout << "ICP已经收敛, 分数为: " << icp.getFitnessScore() << endl;
transformation_matrix = icp.getFinalTransformation().cast<double>();
cout << "cloud_in到cloud_icp的旋转矩阵为: " << endl << transformation_matrix << endl;
} else {
PCL_ERROR("ICP没有收敛.");
system("pause");
return (-1);
}
// 可视化
pcl::visualization::PCLVisualizer viewer("viewer");
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);
// 原始点云为白色
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h(cloud_in, 255, 255, 255);
viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v1", v1);
viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v2", v2);
// 转换后的点云为绿色
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h(cloud_tr, 0, 255, 0);
viewer.addPointCloud(cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);
// ICP得到的点云为红色
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icp_color_h(cloud_icp, 255, 0, 0);
viewer.addPointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2", v2);
// 在每个视口中添加说明文字
viewer.addText("white: cloud_raw \n green: cloud_tr", 10, 10, "icp_info_1", v1);
viewer.addText("white: cloud_raw \n red: cloud_ICP", 10, 10, "icp_info_2", v2);
// 设置点云的显示大小
int pointSize(10);
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, "cloud_in_v1");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, "cloud_in_v2");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, "cloud_tr_v1");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, "cloud_icp_v2");
stringstream ss;
ss << iterations;
string iterations_cnt = "ICP iterations = " + ss.str();
viewer.addText(iterations_cnt, 10, 60, "iterations_cnt", v2);
// 设置背景颜色
viewer.setBackgroundColor(0.1, 0.1, 0.1, v1);
viewer.setBackgroundColor(0.1, 0.1, 0.1, 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();
// 按空格建进入下一次配准 :
if (next_iteration) {
icp.align(*cloud_icp);
if (icp.hasConverged()) {
cout << "ICP已经收敛, 分数为: " << icp.getFitnessScore() << endl;
cout << "\nICP从cloud_icp到cloud_in迭代次数: " << ++iterations << endl;
transformation_matrix *= icp.getFinalTransformation().cast<double>();
cout << "cloud_in到cloud_icp的旋转矩阵为: " << endl << transformation_matrix << endl;
ss.str("");
ss << iterations;
iterations_cnt = "ICP iterations = " + ss.str();
viewer.updateText(iterations_cnt, 10, 60, "iterations_cnt");
viewer.updatePointCloud(cloud_icp, cloud_icp_color_h, "cloud_icp_v2");
} else {
PCL_ERROR("ICP没有收敛.");
system("pause");
return (-1);
}
}
next_iteration = false;
}
system("pause");
return (0);
}
效果展示: