专栏首页点云PCLPCL点云配准(2)

PCL点云配准(2)

(1)正态分布变换进行配准(normal Distributions Transform)

介绍关于如何使用正态分布算法来确定两个大型点云之间的刚体变换,正态分布变换算法是一个配准算法,它应用于三维点的统计模型,使用标准最优化技术来确定两个点云间的最优匹配,因为其在配准的过程中不利用对应点的特征计算和匹配,所以时间比其他方法比较快,

对于代码的解析

/*使用正态分布变换进行配准的实验 。其中room_scan1.pcd  room_scan2.pcd这些点云包含同一房间360不同视角的扫描数据*/#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>//NDT(正态分布)配准类头文件
#include <pcl/filters/approximate_voxel_grid.h>   //滤波类头文件  
(使用体素网格过滤器处理的效果比较好)
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
int main (int argc, char** argv)
{  
// 加载房间的第一次扫描点云数据作为目标  pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);  

if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan1.pcd", *target_cloud) == -1)
 {
   PCL_ERROR ("Couldn't read file room_scan1.pcd \n");    
     return (-1);
 }
 std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;  
// 加载从新视角得到的第二次扫描点云数据作为源点云
 pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);  

if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan2.pcd", *input_cloud) == -1)
 {
   PCL_ERROR ("Couldn't read file room_scan2.pcd \n");    return (-1);  }
 std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;  
//以上的代码加载了两个PCD文件得到共享指针,后续配准是完成对源点云到目标点云的参考坐标系的变换矩阵的估计,得到第二组点云变换到第一组点云坐标系下的变换矩阵  
// 将输入的扫描点云数据过滤到原始尺寸的10%以提高匹配的速度,只对源点云进行滤波,减少其数据量,而目标点云不需要滤波处理  
//因为在NDT算法中在目标点云对应的体素网格数据结构的统计计算不使用单个点,而是使用包含在每个体素单元格中的点的统计数据
 pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
 pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
 approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
 approximate_voxel_filter.setInputCloud (input_cloud);

 approximate_voxel_filter.filter (*filtered_cloud);  std::cout << "Filtered cloud contains " << filtered_cloud->size ()  << " data points from room_scan2.pcd" << std::endl;  
 
// 初始化正态分布(NDT)对象
 pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;  
// 根据输入数据的尺度设置NDT相关参数
 ndt.setTransformationEpsilon (0.01);   //为终止条件设置最小转换差异  
 ndt.setStepSize (0.1);    //为more-thuente线搜索设置最大步长
 ndt.setResolution (1.0);   //设置NDT网格网格结构的分辨率(voxelgridcovariance)  //以上参数在使用房间尺寸比例下运算比较好,但是如果需要处理例如一个咖啡杯子的扫描之类更小的物体,需要对参数进行很大程度的缩小  
//设置匹配迭代的最大次数,这个参数控制程序运行的最大迭代次数,一般来说这个限制值之前优化程序会在epsilon变换阀值下终止  
//添加最大迭代次数限制能够增加程序的鲁棒性阻止了它在错误的方向上运行时间过长  
  ndt.setMaximumIterations (35);
 ndt.setInputSource (filtered_cloud);  //源点云 
 ndt.setInputTarget (target_cloud);  //目标点云 
 // 设置使用机器人测距法得到的粗略初始变换矩阵结果
 Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
 Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
 Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();  // 计算需要的刚体变换以便将输入的源点云匹配到目标点云
 pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
 ndt.align (*output_cloud, init_guess);   //这个地方的output_cloud不能作为最终的源点云变换,因为上面对点云进行了滤波处理
 std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()            << " score: " << ndt.getFitnessScore () << std::endl; 
  // 使用创建的变换对为过滤的输入点云进行变换
 pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());  // 保存转换后的源点云作为最终的变换输出  pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);  

// 初始化点云可视化对象
 boost::shared_ptr<pcl::visualization::PCLVisualizer>
 viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
 viewer_final->setBackgroundColor (0, 0, 0);  //设置背景颜色为黑色
 // 对目标点云着色可视化 (red).
 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
 target_color (target_cloud, 255, 0, 0);
 viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
 viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "target cloud"); 
 // 对转换后的源点云着色 (green)可视化.
 pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
 output_color (output_cloud, 0, 255, 0);
 viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
 viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "output cloud"); //启动可视化
 viewer_final->addCoordinateSystem (1.0);  //显示XYZ指示轴
 viewer_final->initCameraParameters ();   //初始化摄像头参数  

// 等待直到可视化窗口关闭
 while (!viewer_final->wasStopped ())
 {
   viewer_final->spinOnce (100);
   boost::this_thread::sleep (boost::posix_time::microseconds (100000));
 }  return (0);
}

编译完成运行的结果:

(2)本实验将学习如何编写一个交互式ICP可视化的程序。该程序将加载点云并对其进行刚性变换。之后,使用ICP算法将变换后的点云与原来的点云对齐。每次用户按下“空格”,进行ICP迭代,刷新可视化界面。

在这里原始例程使用的是PLY格式的文件,可以找一个PLY格式的文件进行实验,也可以使用格式转换文件 把PCD 文件转为PLY文件

#include <iostream>
#include <string>
#include <pcl/io/ply_io.h>    //PLY相关头文件
#include <pcl/point_types.h>  //
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/time.h>  

  typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;   //定义点云的格式
bool next_iteration = false;    voidprint4x4Matrix (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));
}

voidkeyboardEventOccurred (constpcl::visualization::KeyboardEvent& event,                       void* nothing)
{  
//使用空格键来增加迭代次数,并更新显示
 if (event.getKeySym () == "space" && event.keyDown ())
   next_iteration = true;
}intmain (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]);  //传递参数的格式转化为int型
   if (iterations < 1)  //同时不能设置迭代次数为1   
    {
     PCL_ERROR ("Number of initial iterations must be >= 1\n");     
      return (-1);
   }
 }
 pcl::console::TicToc time;     //申明时间记录
 time.tic ();       //time.tic开始  time.toc结束时间
 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;  //定义旋转矩阵和平移向量Matrix4d是为4*4的矩阵
 Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity ();  //初始化  
// 旋转矩阵的定义可以参考 ( https://en.wikipedia.org/wiki/Rotation_matrix)
 double theta = M_PI / 8;  // 旋转的角度用弧度的表示方法
 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 meters)
 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为后期使用  
// 迭代最近点算法
 time.tic ();        //时间
 pcl::IterativeClosestPoint<PointT, PointT> icp;
 icp.setMaximumIterations (iterations);    //设置最大迭代次数iterations=true
 icp.setInputSource (cloud_icp);   //设置输入的点云
 icp.setInputTarget (cloud_in);    //目标点云
 icp.align (*cloud_icp);          //匹配后源点云
 icp.setMaximumIterations (1);  // 设置为1以便下次调用
 std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl;  if (icp.hasConverged ())//icp.hasConverged ()=1(true)输出变换矩阵的适合性评估  {
   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 ().cast<double>();
   print4x4Matrix (transformation_matrix);
 }  else
 {
   PCL_ERROR ("\nICP has not converged.\n");    return (-1);
 }  // 可视化ICP的过程与结果
 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_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::PointCloudColorHandlerCustom<PointT> cloud_tr_color_h (cloud_tr, 20, 180, 20);
 viewer.addPointCloud (cloud_tr, cloud_tr_color_h, "cloud_tr_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);  
// 加入文本的描述在各自的视口界面 //在指定视口viewport=v1添加字符串“white 。。。”其中"icp_info_1"是添加字符串的ID标志,(10,15)为坐标16为字符大小 后面分别是RGB值
 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)
   {      // 最近点迭代算法      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_in" << std::endl;
       transformation_matrix *= icp.getFinalTransformation ().cast<double>();  // WARNING /!\ This is not accurate!        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);
}

生成可执行文件后结果

窗口输出的基本信息

刚开始迭代第一次的结果

按空格键不断迭代的结果

完毕,如有错误请与我联系交流,谢谢,大牛请忽略

本文分享自微信公众号 - 点云PCL(dianyunPCL)

原文出处及转载信息见文内详细说明,如有侵权,请联系 yunjia_community@tencent.com 删除。

原始发表时间:2019-05-28

本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

我来说两句

0 条评论
登录 后参与评论

相关文章

  • 那些会阻碍程序员成长的细节[4]

    不愿意跟领导走的近。 是不是有这样的体会:凡事有领导在的场合,气氛都比较凝重?整个人都放不开?其实这还是一个雇佣关系在作怪。员工与领导并不是处于一个合作互利互惠...

    歪脖贰点零
  • Arrays.asList()使用指南

    最近使用Arrays.asList()遇到了一些坑,然后在网上看到这篇文章:http://javadevnotes.com/java-array-to-list...

    乔戈里
  • 增强式学习:如何使用Q-Learning算法训练围棋机器人

    本节我们看看如何使用该网络训练围棋机器人。我们在标题中提到Q-Learning,它实际上是一种使用上面网络进行训练的算法流程。首先我们先定义执行Q-Learn...

    望月从良
  • 面试官:简历上说精通垃圾收集器?来吧,挨个给我说一遍

    上文已经讲解垃圾收集的各种算法,算法可以理解为方法,如果说收集算法是内存回收的方法论,那么垃圾收集器就是内存回收的具体实现。

    胖虎
  • 数据结构与算法之美 - 时间和空间复杂度

    1.数据结构和算法解决是 “如何让计算机更快时间、更省空间的解决问题”。2.因此需从执行时间和占用空间两个维度来评估数据结构和算法的性能。3.分别用时间复杂度和...

    灵魂画师牧码
  • 那些会阻碍程序员成长的细节[3]

    不能主动推动事物前进。 主动做一件事跟被动接受去做事,心情都是不一样的,做事效率更是千差万别。主动的人有更多的成长机会,反之在被动中不断的响应别人的任务,这与处...

    歪脖贰点零
  • VBA代码库06:实现健壮的“另存为”功能

    下面的自定义函数GetSaveAsFilenamePlus函数的代码能够更好地实现GetSaveAsFilename方法的“另存为”功能。该函数接受两个参数,分...

    fanjy
  • 那些会阻碍程序员成长的细节[6]

    威胁性的处理问题。 比较常见的一种就是要挟式加薪,自恃岗位重要或者人员流动太大导致在岗人员变少等等,如果以此种理由去要求加薪有两种后果,一是领导暂时妥协,一旦找...

    歪脖贰点零
  • 重磅!《深度学习 500 问》已更新,GitHub 标星 2.6W(附完整下载)

    几个月前,红色石头发文介绍过一份在 GitHub 上非常火爆的项目,名为:DeepLearning-500-questions,中文译名:深度学习 500 问。...

    Datawhale
  • 那些会阻碍程序员成长的细节[5]

    断断续续的写成一个系列,也出乎我的意料,越展开内容越多,有点收不住,Let‘s 继续吧,进入主题。

    歪脖贰点零

扫码关注云+社区

领取腾讯云代金券