【PCL】教程 interactive_icp.cpp 使用ICP算法对3D点云进行准确的对齐，并通过可视化展示算法的迭代过程...

最新推荐文章于 2024-08-30 11:32:28 发布

十年一梦实验室

最新推荐文章于 2024-08-30 11:32:28 发布

阅读量621

点赞数 9

文章标签：算法 linux 运维服务器

本文链接：https://blog.csdn.net/cxyhjl/article/details/138105832

版权

本文详细介绍了使用PCL库实现的迭代最近点（ICP）算法，通过示例展示了如何对3D点云进行精确对齐，以及如何通过键盘事件控制算法迭代。代码中包含加载PLY文件、定义变换、执行ICP对齐和实时可视化的过程。

摘要由CSDN通过智能技术生成

测试程序

./interactive_icp ../monkey.ply 5

终端输出：

[pcl::PLYReader::listPropertyDefinitionCallback] no fitting callbacks. element_name=face, property_name=vertex_indices
[pcl::PLYReader] monkey.ply:14: property 'list uint8 uint32 vertex_indices' of element 'face' is not handled


Loaded file monkey.ply (31488 points) in 235.44 ms


Applying this rigid transformation to: cloud_in -> cloud_icp
Rotation matrix :
    |  0.924 -0.383  0.000 |
R = |  0.383  0.924  0.000 |
    |  0.000  0.000  1.000 |
Translation vector :
t = <  0.000,  0.000,  0.400 >


Applied 5 ICP iteration(s) in 255.85 ms


ICP has converged, score is 0.0149681
ICP has converged, score is +4e-03
ICP has converged, score is +3e-03> cloud_in
ICP has converged, score is +3e-03> cloud_in
ICP has converged, score is +2e-03> cloud_in
ICP has converged, score is +2e-03> cloud_in
ICP has converged, score is +1e-03> cloud_in
ICP has converged, score is +1e-03-> cloud_in
ICP has converged, score is +9e-04-> cloud_in
ICP has converged, score is +7e-04-> cloud_in
ICP has converged, score is +5e-04-> cloud_in
ICP has converged, score is +3e-04-> cloud_in
ICP has converged, score is +2e-04-> cloud_in
ICP has converged, score is +1e-04-> cloud_in
ICP has converged, score is +5e-05-> cloud_in
ICP has converged, score is +1e-06-> cloud_in
ICP has converged, score is +2e-05-> cloud_in
ICP has converged, score is +2e-06-> cloud_in
ICP has converged, score is +1e-10-> cloud_in
ICP has converged, score is +1e-11-> cloud_in
ICP has converged, score is +4e-12-> cloud_in
ICP has converged, score is +3e-12-> cloud_in
ICP has converged, score is +3e-12-> cloud_in
ICP has converged, score is +3e-12-> cloud_in
ICP has converged, score is +3e-12-> cloud_in
ICP has converged, score is +3e-12-> cloud_in
ICP has converged, score is +3e-12-> cloud_in
ICP has converged, score is +4e-12-> cloud_in
ICP has converged, score is +3e-12-> cloud_in
ICP has converged, score is +3e-12-> cloud_in
ICP has converged, score is +3e-12-> cloud_in
ICP has converged, score is +3e-12-> cloud_in
ICP has converged, score is +3e-12-> cloud_in
ICP transformation 36 : cloud_icp -> cloud_in
ICP transformation 37 : cloud_icp -> cloud_in
Rotation matrix :82 -0.023 | >0433 ms
    |  0.924  0.382 -0.023 | >9019 ms
R = | -0.382  0.924  0.019 | >2053 ms
    |  0.029 -0.008  1.000 | >2671 ms
Translation vector :, -0.400 >7246 ms
t = < -0.005, -0.004, -0.400 >8765 ms
Applied 1 ICP iteration in 20.0796 ms

源码解析

这段代码是一个使用点云库（PCL）实现的迭代最近点（Iterative Closest Point, ICP）算法的示例。它展示了如何使用ICP算法来对齐（注册）两个点云。以下是代码的功能和方法的总结：

导入必要的头文件：引入了C++标准库、PCL库中的点云IO、点云类型、ICP注册、以及可视化等模块。
定义点云类型别名：为pcl::PointXYZ和pcl::PointCloud<pcl::PointXYZ>定义了别名PointT和PointCloudT，以便简化代码。
**布尔变量next_iteration**：用于控制是否进行下一次ICP迭代。
**函数print4x4Matrix**：打印4x4的变换矩阵，包括旋转部分和平移向量。
**函数keyboardEventOccurred**：通过按键事件控制（当按下空格键时）next_iteration的值，以触发ICP的下一次迭代。
主函数：

首先，检查命令行参数，确保提供了足够的输入（至少一个PLY文件和迭代次数）。
加载输入的PLY文件到cloud_in点云。
定义旋转矩阵和平移向量，通过应用这个变换矩阵，生成变换后的点云cloud_icp。
初始化ICP对象，设置最大迭代次数，并设置源点云和目标点云。
进行ICP对齐，每次用户按下空格键，都会执行一次额外的ICP迭代，并更新变换矩阵和视图。
使用pcl::visualization::PCLVisualizer对象创建视图，原始点云显示为白色，变换后的点云显示为绿色，ICP对齐后的点云显示为红色。
循环直到关闭视图窗口。

此示例演示了如何使用ICP算法对3D点云进行准确的对齐，并通过可视化展示算法的迭代过程。还通过键盘事件来控制ICP迭代的进行，增加了用户交互的可能性。该程序对于理解和应用点云对齐技术有很好的示范作用。

#include <iostream> // 引入iostream库，用于输入输出流操作
#include <string> // 引入string库，用于使用字符串类型


#include <pcl/io/ply_io.h> // 引入PCL库的PLY文件输入输出操作
#include <pcl/point_types.h> // 引入PCL库的点类型定义
#include <pcl/registration/icp.h> // 引入PCL库的迭代最近点（ICP）算法
#include <pcl/visualization/pcl_visualizer.h> // 引入PCL库的可视化工具
#include <pcl/console/time.h> // 引入PCL库的计时工具 TicToc


typedef pcl::PointXYZ PointT; // 定义点类型PointT为pcl::PointXYZ，表示三维坐标的点
typedef pcl::PointCloud<PointT> PointCloudT; // 定义点云类型PointCloudT为pcl::PointCloud<PointT>


bool next_iteration = false; // 定义一个全局变量，决定是否进行下一次ICP迭代


// 打印4x4变换矩阵
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));
}


// 键盘事件处理函数
void
keyboardEventOccurred (const pcl::visualization::KeyboardEvent& event,
                       void*)
{
  if (event.getKeySym () == "space" && event.keyDown ())
    next_iteration = true; // 如果按下空格键，设置下一次迭代为真
}


int
main (int argc,
      char* argv[])
{
  // 定义将要使用的点云
  PointCloudT::Ptr cloud_in (new PointCloudT);  // 原始点云
  PointCloudT::Ptr cloud_tr (new PointCloudT);  // 变换后的点云
  PointCloudT::Ptr cloud_icp (new PointCloudT);  // ICP输出点云


  // 检查程序参数
  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); // 如果用户指定的迭代次数小于1，则输出错误信息并退出程序
    }
  }


  pcl::console::TicToc time; // 创建一个计时器
  time.tic (); // 开始计时
  if (pcl::io::loadPLYFile (argv[1], *cloud_in) < 0) // 读取PLY文件，并判断是否成功
  {
    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; // 如果成功，输出读取耗时信息


  // 定义一个旋转矩阵和平移向量
  Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity (); // 创建一个单位矩阵


  // 定义旋转矩阵
  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_tr中，供后用


  // 迭代最近点（ICP）算法
  time.tic (); // 重新开始计时
  pcl::IterativeClosestPoint<PointT, PointT> icp; // 创建一个ICP对象
  icp.setMaximumIterations (iterations); // 设置最大迭代次数
  icp.setInputSource (cloud_icp); // 设置输入点云（即需要变换的点云）
  icp.setInputTarget (cloud_in); // 设置目标点云（即要对齐的点云）
  icp.align (*cloud_icp); // 执行ICP对齐操作
  icp.setMaximumIterations (1);  // 设置后续调用.align()函数的迭代次数为1
  std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl; // 输出ICP迭代次数和耗时


  if (icp.hasConverged ())
  {
    std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl; // 如果ICP收敛，则输出得分
    std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
    transformation_matrix = icp.getFinalTransformation ().cast<double>(); // 获取ICP变换结果
    print4x4Matrix (transformation_matrix); // 打印变换矩阵
  }
  else
  {
    PCL_ERROR ("\nICP has not converged.\n"); // 如果ICP没有收敛，则输出错误信息并退出程序
    return (-1);
  }


  // 可视化
  // 创建一个PCLVisualizer对象并命名为"ICP demo"
  pcl::visualization::PCLVisualizer viewer ("ICP demo");
  // 创建两个垂直分割的视口
  int v1 (0);
  int v2 (1);
  viewer.createViewPort (0.0, 0.0, 0.5, 1.0, v1); // 创建视口1
  viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2); // 创建视口2


  // 设置颜色
  float bckgr_gray_level = 0.0;  // 背景颜色为黑色
  float txt_gray_lvl = 1.0 - bckgr_gray_level; // 文本颜色为白色


  // 将原始点云设置为白色并显示在两个视口中
  pcl::visualization::PointCloudColorHandlerCustom<PointT> 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); // 视口1中添加原点云
  viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v2", v2); // 视口2中添加原点云


  // 设置变换后的点云为绿色并显示在视口1中
  pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h (cloud_tr, 20, 180, 20);
  viewer.addPointCloud (cloud_tr, cloud_tr_color_h, "cloud_tr_v1", v1);


  // 将ICP对齐后的点云设置为红色并显示在视口2
  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: 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; // 使用stringstream格式化字符串
  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); // 视口1的背景颜色
  viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2); // 视口2的背景颜色


  // 设置摄像头位置和方向
  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 (); // 处理GUI事件并更新窗口


    // 用户如果按下空格键
    if (next_iteration)
    {
      // 再次进行ICP算法
      time.tic (); // 开始计时
      icp.align (*cloud_icp); // 进行一次ICP迭代
      std::cout << "Applied 1 ICP iteration in " << time.toc () << " ms" << std::endl; // 输出迭代信息


      if (icp.hasConverged ())
      {
        printf ("\033[11A");  // 在终端输出上移动11行。
        printf ("\nICP has converged, score is %+.0e\n", icp.getFitnessScore ());  // 打印出ICP收敛和适应度评分
        std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl; //打印出ICP变换和迭代次数
        transformation_matrix *= icp.getFinalTransformation ().cast<double>();  // 这是错误的！仅仅是为了教学演示！因此在每次迭代之后，继续更新转换矩阵，而不是重新计算。
        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");   // 如果ICP算法没有收敛，打印错误，并返回-1
        return (-1);
      }
    }
    next_iteration = false;  // 设置下一次迭代为假，直到用户再次按下空格键触发下一次迭代
  }
  return (0);  // 如果所有的代码都正确执行，返回0，表示程序正常结束.
}

笔记

viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);

代码中为什么要两次设置：
icp.setMaximumIterations (iterations);   icp.setMaximumIterations (1);

pcl::IterativeClosestPoint<PointT, PointT>

viewer.addText

viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v1", v1);

pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h (cloud_tr, 20, 180, 20);

viewer.setCameraPosition (-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);

viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);

viewer.updateText   viewer.updatePointCloud

作者陈晓永：智能装备专业高级工程师，软件工程师。机器人自动化产线仿真动画制作

The End

十年一梦实验室

关注

9
点赞
踩
8

收藏

觉得还不错? 一键收藏
0
评论
复制链接

分享到 QQ

分享到新浪微博

扫一扫