建立一个购物网站,个人网站 审批,教育机构网站建设方案书,网站站群管理系统以下程序演示如何编写交互式ICP查看器。该程序将加载点云并对其进行刚性变换。之后#xff0c;使用ICP算法将变换后的点云与原来的点云对齐。每次用户按下“空格”#xff0c;进行ICP迭代#xff0c;刷新可视化界面。
代码实现
资源准备 monkey.ply
#include string使用ICP算法将变换后的点云与原来的点云对齐。每次用户按下“空格”进行ICP迭代刷新可视化界面。
代码实现
资源准备 monkey.ply
#include string
#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.htypedef pcl::PointXYZ PointT;
typedef pcl::PointCloudPointT PointCloudT;bool next_iteration false;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));
}
/*** 此函数是查看器的回调。当查看器窗口位于顶部时只要按任意键就会调用此函数。如果碰到“空格”将布尔值设置为true。*/
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent event,void* nothing)
{if (event.getKeySym() space event.keyDown())next_iteration true;
}int main(int argc,char* argv[])
{// 准备将使用的点云PointCloudT::Ptr cloud_in(new PointCloudT); // Original point cloudPointCloudT::Ptr cloud_tr(new PointCloudT); // Transformed point cloudPointCloudT::Ptr cloud_icp(new PointCloudT); // ICP output point cloud// 检查程序的参数设置初始ICP迭代的次数然后尝试加载PLY文件if (argc 2){printf(Usage :\n);printf(\t\t%s file.ply number_of_ICP_iterations\n, argv[0]);PCL_ERROR(Provide one ply file.\n);return (-1);}int iterations 1; // 默认的ICP迭代次数if (argc 2){// 如果用户将迭代次数作为参数传递iterations atoi(argv[2]);if (iterations 1){PCL_ERROR(Number of initial iterations must be 1\n);return (-1);}}pcl::console::TicToc time;time.tic();if (pcl::io::loadPLYFile(argv[1], *cloud_in) 0){PCL_ERROR(Error loading cloud %s.\n, argv[1]);return (-1);}std::cout \nLoaded file argv[1] ( cloud_in-size() points) in time.toc() ms\n std::endl;// 我们使用刚性矩阵变换来变换原始点云。// cloud_in包含原始点云。// cloud_tr和cloud_icp包含平移/旋转的点云。// cloud_tr是我们将用于显示的备份绿点云。// 定义旋转矩阵和平移向量Eigen::Matrix4d transformation_matrix Eigen::Matrix4d::Identity();// 一个旋转矩阵 (see https://en.wikipedia.org/wiki/Rotation_matrix)double theta M_PI / 8; // 以弧度为单位的旋转角度transformation_matrix(0, 0) std::cos(theta);transformation_matrix(0, 1) -sin(theta);transformation_matrix(1, 0) sin(theta);transformation_matrix(1, 1) std::cos(theta);// Z轴上的平移0.4米transformation_matrix(2, 3) 0.4;// 在终端中显示变换矩阵std::cout Applying this rigid transformation to: cloud_in - cloud_icp std::endl;print4x4Matrix(transformation_matrix);// 执行转换pcl::transformPointCloud(*cloud_in, *cloud_icp, transformation_matrix);*cloud_tr *cloud_icp; // 我们将cloud_icp备份到cloud_tr中以备将来使用// 这是ICP对象的创建。我们设置ICP算法的参数。// setMaximumIterationsiterations设置要执行的初始迭代次数默认值为1。// 然后我们将点云转换为cloud_icp。 第一次对齐后我们将在下一次使用该ICP对象时当用户按下“空格”时将ICP最大迭代次数设置为1。// ICP算法time.tic();pcl::IterativeClosestPointPointT, PointT icp;icp.setMaximumIterations(iterations);icp.setInputSource(cloud_icp);icp.setInputTarget(cloud_in);icp.align(*cloud_icp);icp.setMaximumIterations(1); // 下次调用.align函数时我们将此变量设置为1std::cout Applied iterations ICP iteration(s) in time.toc() ms std::endl;// 检查ICP算法是否收敛否则退出程序。如果返回true我们将转换矩阵存储在4x4矩阵中然后打印刚性矩阵转换。if (icp.hasConverged()){std::cout \nICP has converged, score is icp.getFitnessScore() std::endl;std::cout \nICP transformation iterations : cloud_icp - cloud_in std::endl;transformation_matrix icp.getFinalTransformation().castdouble();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; // 黑色float txt_gray_lvl 1.0 - bckgr_gray_level;// 原始点云为白色pcl::visualization::PointCloudColorHandlerCustomPointT cloud_in_color_h(cloud_in, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl,(int)255 * txt_gray_lvl);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::PointCloudColorHandlerCustomPointT cloud_tr_color_h(cloud_tr, 20, 180, 20);viewer.addPointCloud(cloud_tr, cloud_tr_color_h, cloud_tr_v1, v1);// ICP对齐点云为红色pcl::visualization::PointCloudColorHandlerCustomPointT cloud_icp_color_h(cloud_icp, 180, 20, 20);viewer.addPointCloud(cloud_icp, cloud_icp_color_h, cloud_icp_v2, v2);// 在每个视口中添加文本描述viewer.addText(White: Original point cloud\nGreen: Matrix transformed point cloud, 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, icp_info_1, v1);viewer.addText(White: Original point cloud\nRed: ICP aligned point cloud, 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();// 如果用户按下空格:if (next_iteration){// ICP算法time.tic();// 如果用户按下键盘上的任意键则会调用keyboardEventOccurred函数。 此功能检查键是否为“空格”。// 如果是则全局布尔值next_iteration设置为true从而允许查看器循环输入代码的下一部分调用ICP对象以进行对齐。// 记住我们已经配置了该对象输入/输出云并且之前通过setMaximumIterations将最大迭代次数设置为1。icp.align(*cloud_icp);std::cout Applied 1 ICP iteration in time.toc() ms std::endl;// 和以前一样我们检查ICP是否收敛如果不收敛则退出程序。if (icp.hasConverged()){// printf“ 033 [11A”; 在终端增加11行以覆盖显示的最后一个矩阵是一个小技巧。// 简而言之它允许替换文本而不是编写新行 使输出更具可读性。 我们增加迭代次数以更新可视化器中的文本值。printf(\033[11A); // Go up 11 lines in terminal output.printf(\nICP has converged, score is %.0e\n, icp.getFitnessScore());// 这意味着如果您已经完成了10次迭代则此函数返回矩阵以将点云从迭代10转换为11。std::cout \nICP transformation iterations : cloud_icp - cloud_in std::endl;// 函数getFinalTransformation返回在迭代过程中完成的刚性矩阵转换此处为1次迭代。transformation_matrix * icp.getFinalTransformation().castdouble(); // WARNING /!\ This is not accurate! For educational purpose only!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;}return (0);
}
使用方式
windows下执行以下命令
.\xxx.exe ...\monkey.ply 1
随着迭代次数增加效果如下
迭代次数1 迭代次数19
迭代次数41 官方文档https://pcl-tutorials.readthedocs.io/en/latest/interactive_icp.html
