PCL 概述1

由于工作需要，需学习并掌握 PCL 的基本子模块，现记录学习官网的文档。示例代码均参考官方文档

目录

0.PCL overview

PCL(Point Cloud Library)是业界广泛应用的点云处理库，其包含众多子模块化，列举如下：

  1. Common
  2. Filters
  3. Features
  4. Keypoints
  5. Registration
  6. Kd-tree
  7. Octree
  8. Segmentation
  9. Sample Consensus
  10. Surface
  11. I/O
  12. Visualization
  13. Search
  14. Binaries

1.PCD files

I/O 库包含用于读取和写入点云数据(Point Cloud Data，PCD)文件的类和函数，以及从各传感器设备捕获点云数据。

为对 PCD 文件有初步认识，现以 case1.pcd 文件为例，介绍如下：

# .PCD v.7 - Point Cloud Data file format
VERSION .7
FIELDS x y z rgb
SIZE 4 4 4 4
TYPE F F F F
COUNT 1 1 1 1
WIDTH 6
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 6
DATA ascii
0.93773 0.33763 0 4.2108e+06
0.90805 0.35641 0 4.2108e+06
0.81915 0.32 0 4.2108e+06
0.97192 0.278 0 4.2108e+06
0.944 0.29474 0 4.2108e+06
0.98111 0.24247 0 4.2108e+06

File foramt header 内容如下：

  • VERSION：指定 pcd 文件版本

  • FIELDS：指定 单点 的维度变量

    • FIELDS x y z # XYZ
    • FIELDS x y z rgb # XYZ + colors

  • SIZE：指定 每个维度变量的字节数

    • unsigned char has 1 byte
    • unsigned short has 2 bytes
    • unsigned int/float has 4 bytes
    • double has 8 bytes

  • Type：指定 每个维度变量的类型

    • I：represents signed types int8 (char), int16 (short), and int32 (int)
    • U：represents unsigned types uint8 (unsigned char), uint16 (unsigned short), uint32 (unsigned int)
    • F：represents float types

  • COUNT：指定 每个维度变量有多少个元素

  • WIDTH：指定 点云集 水平方向 有多少个点

    • 对于非组织有序的点云数据集，表明整个点云数据的总点数，与 POINTS 含义相同；
    • 对于组织有序的点云数据集，表明水平方向上扫描点集数

  • HEIGHT：指定 点云集 垂直方向 有多少个点

    • 对于非组织有序的点云数据集，一般置为1
    • 对于组织有序的点云数据集，指定垂直方向上扫描点集数；
      补充：这里的组织有序,一般指矩阵结构，能被清晰分成行和列。

  • VIEWPOINT：指定 点云数据集的采集视点。该属性可用于不同坐标系统之间的转换，或者需计算一致方向特征（如表面法向量）的场景。采集视点信息通常是以平移向量（tx ty tz）和四元数（qw qx qy qz）的形式指定。

    • 平移向量（tx, ty, tz）表示点云数据集 相对于 某个固定参照系原点的三维位移。
    • 四元数（qw, qx, qy, qz）表示点云采集时的旋转信息，是一种用于表达三维旋转的数学表示方式。
    • 默认值通常为空或者设定为标准值，例如原点（0, 0, 0）和平移四元数（1, 0, 0, 0），这表示没有平移和旋转，即视点与参考坐标系重合。

  • POINTS：指定 点云数据集中包含的点数

  • DATA：指定 点云数据的存储类型

    • ascii：可读性强，占用空间大，不适用于大数据量
    • binary
    • binrary_compressed

header 汇总如下：

VERSION
FIELDS
SIZE
TYPE
COUNT
WIDTH
HEIGHT
VIEWPOINT
POINTS
DATA

参考链接：

  1. https://pcl-docs.readthedocs.io/en/latest/pcl/doc/tutorials/content/pcd_file_format.html#pcd-file-format
  2. https://funnywii.com/archives/点云数据pointclouddata

2. Reading data from PCD files

从pcd 文件中读取 点云数据，示例代码如下：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

int main (){
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  if (pcl::io::loadPCDFile<pcl::PointXYZ> ("test_pcd.pcd", *cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
    return (-1);
  }
  std::cout << "Loaded "
            << cloud->width * cloud->height		# 取结构体数据
            << " data points from test_pcd.pcd with the following fields: "
            << std::endl;
  for (const auto& point: *cloud)
    std::cout << "    " << point.x
              << " "    << point.y
              << " "    << point.z << std::endl;

  return (0);
}

3. Writing data to PCD files

示例代码如下：


#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

int main (){
  pcl::PointCloud<pcl::PointXYZ> cloud;

  // Fill in the cloud data
  cloud.width    = 5;
  cloud.height   = 1;
  cloud.is_dense = false;
  cloud.resize (cloud.width * cloud.height);

  for (auto& point: cloud)
  {
    point.x = 1024 * rand () / (RAND_MAX + 1.0f);
    point.y = 1024 * rand () / (RAND_MAX + 1.0f);
    point.z = 1024 * rand () / (RAND_MAX + 1.0f);
  }

  pcl::io::savePCDFileASCII ("test_pcd.pcd", cloud);
  std::cerr << "Saved " << cloud.size () << " data points to test_pcd.pcd." << std::endl;

  for (const auto& point: cloud)
    std::cerr << "    " << point.x << " " << point.y << " " << point.z << std::endl;

  return (0);
}

4. Concatenate the points of two Point Clouds

示例代码如下：


#include <iostream>
#include <pcl/point_types.h>
#include <pcl/common/io.h> // for concatenateFields

int
  main (int argc, char** argv)
{
  if (argc != 2)
  {
    std::cerr << "please specify command line arg '-f' or '-p'" << std::endl;
    exit(0);
  }
  pcl::PointCloud<pcl::PointXYZ> cloud_a, cloud_b, cloud_c;
  pcl::PointCloud<pcl::Normal> n_cloud_b;	# 法向量
  pcl::PointCloud<pcl::PointNormal> p_n_cloud_c; 	#包含XYZ坐标信息，还包含相应的法线信息

  // Fill in the cloud data
  cloud_a.width  = 5;
  cloud_a.height = cloud_b.height = n_cloud_b.height = 1;
    cloud_a.resize (cloud_a.width * cloud_a.height);
  if (strcmp(argv[1], "-p") == 0)
  {
    cloud_b.width  = 3;
    cloud_b.resize (cloud_b.width * cloud_b.height);
  }
  else{
    n_cloud_b.width = 5;
    n_cloud_b.resize (n_cloud_b.width * n_cloud_b.height);
  }

  for (std::size_t i = 0; i < cloud_a.size (); ++i)
  {
    cloud_a[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
    cloud_a[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
     cloud_a[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
  }
  if (strcmp(argv[1], "-p") == 0)
    for (std::size_t i = 0; i < cloud_b.size (); ++i)
    {
      cloud_b[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
      cloud_b[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
      cloud_b[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
    }
  else
    for (std::size_t i = 0; i < n_cloud_b.size (); ++i)
    {
      n_cloud_b[i].normal[0] = 1024 * rand () / (RAND_MAX + 1.0f);
      n_cloud_b[i].normal[1] = 1024 * rand () / (RAND_MAX + 1.0f);
      n_cloud_b[i].normal[2] = 1024 * rand () / (RAND_MAX + 1.0f);
    }
  std::cerr << "Cloud A: " << std::endl;
  for (std::size_t i = 0; i < cloud_a.size (); ++i)
    std::cerr << "    " << cloud_a[i].x << " " << cloud_a[i].y << " " << cloud_a[i].z << std::endl;

  std::cerr << "Cloud B: " << std::endl;
  if (strcmp(argv[1], "-p") == 0)
    for (std::size_t i = 0; i < cloud_b.size (); ++i)
      std::cerr << "    " << cloud_b[i].x << " " << cloud_b[i].y << " " << cloud_b[i].z << std::endl;
  else
    for (std::size_t i = 0; i < n_cloud_b.size (); ++i)
      std::cerr << "    " << n_cloud_b[i].normal[0] << " " << n_cloud_b[i].normal[1] << " " << n_cloud_b[i].normal[2] << std::endl;

  // Copy the point cloud data
  if (strcmp(argv[1], "-p") == 0)
  {
    cloud_c  = cloud_a;
    cloud_c += cloud_b;
    std::cerr << "Cloud C: " << std::endl;
    for (std::size_t i = 0; i < cloud_c.size (); ++i)
      std::cerr << "    " << cloud_c[i].x << " " << cloud_c[i].y << " " << cloud_c[i].z << " " << std::endl;
  }
  else
  {
    pcl::concatenateFields (cloud_a, n_cloud_b, p_n_cloud_c);
    std::cerr << "Cloud C: " << std::endl;
    for (std::size_t i = 0; i < p_n_cloud_c.size (); ++i)
      std::cerr << "    " <<
        p_n_cloud_c[i].x << " " << p_n_cloud_c[i].y << " " << p_n_cloud_c[i].z << " " <<
        p_n_cloud_c[i].normal[0] << " " << p_n_cloud_c[i].normal[1] << " " << p_n_cloud_c[i].normal[2] << std::endl;
  }
  return (0);
}

5. PointCloudViewer

本节用于可视化点云数据，示例代码如下：


#include <pcl/visualization/cloud_viewer.h>

 void foo ()
 {
   pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
   //... populate cloud
   pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
   viewer.showCloud (cloud);
   while (!viewer.wasStopped ())
   {
   }
 }

int main(){
    foo()
    return 0;
}

复杂示例如下：


#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/common/io.h>
#include <pcl/io/pcd_io.h>
    
int user_data;
    
void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
{
    viewer.setBackgroundColor (1.0, 0.5, 1.0);
    pcl::PointXYZ o;
    o.x = 1.0;
    o.y = 0;
    o.z = 0;
    viewer.addSphere (o, 0.25, "sphere", 0);
    std::cout << "i only run once" << std::endl;
    
}
    
void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
{
    static unsigned count = 0;
    std::stringstream ss;
    ss << "Once per viewer loop: " << count++;
    viewer.removeShape ("text", 0);
    viewer.addText (ss.str(), 200, 300, "text", 0);
    
    //FIXME: possible race condition here:
    user_data++;
}
    
int main ()
{
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
    pcl::io::loadPCDFile ("my_point_cloud.pcd", *cloud);
    
    pcl::visualization::CloudViewer viewer("Cloud Viewer");
    
    //blocks until the cloud is actually rendered
    viewer.showCloud(cloud);
    
    //use the following functions to get access to the underlying more advanced/powerful
    //PCLVisualizer
    
    //This will only get called once
    viewer.runOnVisualizationThreadOnce (viewerOneOff);
    
    //This will get called once per visualization iteration
    viewer.runOnVisualizationThread (viewerPsycho);
    while (!viewer.wasStopped ())
    {
    //you can also do cool processing here
    //FIXME: Note that this is running in a separate thread from viewerPsycho
    //and you should guard against race conditions yourself...
    user_data++;
    }
    return 0;
}

6. plane model segmentation

RANSAC（Random Sample Consensus）方法 是平面分割应用中常见的方法，示例代码如下：


#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

int main ()
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

  // Fill in the cloud data
  cloud->width  = 15;
  cloud->height = 1;
  cloud->points.resize (cloud->width * cloud->height);

  // Generate the data
  for (auto& point: *cloud)
  {
    point.x = 1024 * rand () / (RAND_MAX + 1.0f);
    point.y = 1024 * rand () / (RAND_MAX + 1.0f);
    point.z = 1.0;
  }

  // Set a few outliers
  (*cloud)[0].z = 2.0;
  (*cloud)[3].z = -2.0;
  (*cloud)[6].z = 4.0;

  std::cerr << "Point cloud data: " << cloud->size () << " points" << std::endl;
  for (const auto& point: *cloud)
    std::cerr << "    " << point.x << " "
                        << point.y << " "
                        << point.z << std::endl;

  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);	// 初始化 指向 pcl::ModelCoefficients 类型对象的智能指针
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);	// 初始化一个指向 pcl::PointIndices 类型对象的智能指针
    
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients (true);	// 设定 seg 对象
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);	// 设定 拟合模型
  seg.setMethodType (pcl::SAC_RANSAC);	// 设定 拟合方法
  seg.setDistanceThreshold (0.01);	

  seg.setInputCloud (cloud);
  seg.segment (*inliers, *coefficients);

  if (inliers->indices.size () == 0)
  {
    PCL_ERROR ("Could not estimate a planar model for the given dataset.\n");
    return (-1);
  }

  std::cerr << "Model coefficients: " << coefficients->values[0] << " " 
                                      << coefficients->values[1] << " "
                                      << coefficients->values[2] << " " 
                                      << coefficients->values[3] << std::endl;

  std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
  for (const auto& idx: inliers->indices)
    std::cerr << idx << "    " << cloud->points[idx].x << " "
                               << cloud->points[idx].y << " "
                               << cloud->points[idx].z << std::endl;

  return (0);
}

7. Removing outliers using a StatisticalOutlierRemoval filter

本节学习使用 统计分析的方法从点云数据集删除噪声测量值。

为什么需要删除噪声测量值？在点云配准时，因为这些噪声测量值的存在，会使得局部点云特征估计复杂，有可能导致点云配准失败。

基本思路：

  • 在输入数据集中，针对每个点，计算其所有近邻点的平均值 DIS；
  • 假设该值 符合高斯分布：均值ｍ，标准差std；
  • 如果 DIS 超出一定区间：（m-k*std, m+k*std），则认为是异常值进行剔除

示例代码，测试数据


#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>

int main ()
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

  // Fill in the cloud data
  pcl::PCDReader reader;
  // Replace the path below with the path where you saved your file
  reader.read<pcl::PointXYZ> ("table_scene_lms400.pcd", *cloud);

  std::cerr << "Cloud before filtering: " << std::endl;
  std::cerr << *cloud << std::endl;

  // Create the filtering object
  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
  sor.setInputCloud (cloud);
  sor.setMeanK (50);	// 设定每个点的近邻点数量有 50 个
  sor.setStddevMulThresh (1.0);		// 设定标准差乘积为 k=1
  sor.filter (*cloud_filtered);

  std::cerr << "Cloud after filtering: " << std::endl;
  std::cerr << *cloud_filtered << std::endl;

  pcl::PCDWriter writer;
  writer.write<pcl::PointXYZ> ("table_scene_lms400_inliers.pcd", *cloud_filtered, false);

  sor.setNegative (true);
  sor.filter (*cloud_filtered);
  writer.write<pcl::PointXYZ> ("table_scene_lms400_outliers.pcd", *cloud_filtered, false);

  return (0);
}

8. ICP

ICP(Interactive Closest Point)，迭代近邻点。更多资料参考：链接

示例代码如下：


#include <iostream>
#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.h>   // TicToc

typedef pcl::PointXYZ PointT;	// 为pcl::PointXYZ 声明 PointT的别名，表示一个三维空间内的点，包含三个float类型的成员变量，(x,y,z)
typedef pcl::PointCloud<PointT> PointCloudT; // 为 pcl::PointCloud<pcl::PointXYZ> 声明一个别名，表示一个 包含三维点的 点云对象

bool next_iteration = false;

void print4x4Matrix (const Eigen::Matrix4d & matrix)	// 参数 表示接收1个Eigen::Matrix4d类型的常量引用， 其中 Eigen::Matrix4d 表示 4*4 的双精度浮点数矩阵类型，& 表示引用传递，相比于传值传递，可以在不影响函数功能的前提下减少内存复制开销，尤其是大型矩阵数据结构
{
  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[]) // argc: arg count 表示命令行参数个数，包括程序本身的名称；argv 表示一个指向 命令行参数字符串 的指针。长度是 argc; argc[0] 通常指向程序名字；argc[1] 是第一个命令行参数；
{
  // The point clouds we will be using
  PointCloudT::Ptr cloud_in (new PointCloudT);  // Original point cloud， 其中 PointCloudT::Ptr 是 PointCloudT 类型的智能指针类型，名为 cloud_in, 指向一个 new PointCloudT 类型
  PointCloudT::Ptr cloud_tr (new PointCloudT);  // Transformed point cloud
  PointCloudT::Ptr cloud_icp (new PointCloudT);  // ICP output point cloud

  // Checking program arguments
  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;  // Default number of ICP iterations
  if (argc > 2)
  {
    // If the user passed the number of iteration as an argument
    iterations = atoi (argv[2]); // atoi 是标准库函数，ascii to integer, 将字符串转换成整数
    if (iterations < 1)
    {
      PCL_ERROR ("Number of initial iterations must be >= 1\n");
      return (-1);
    }
  }

  pcl::console::TicToc time; // 声明一个 TicToc 类型的对象 time, 用于计时
  time.tic();	// 开始计时，记录当前时刻的时间戳； 获取中间耗时： double duration = time.toc(), ，默认单位为 second
  
  if (pcl::io::loadPLYFile (argv[1], *cloud_in) < 0) // 使用 pcl::io::loadPLYFile 方法，从 文件argv[1] 中加载数据到 *cloud_in 对象。之所以用 *， 是因为 cloud_in 为指针变量，需要 *cloud_in 才取到实际的变量对象
  {
    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;

  // Defining a rotation matrix and translation vector
  Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity ();  // 创建4*4的单位矩阵，通常用于表示3D空间中的 旋转 和 平移矩阵； 哪些元素表示 旋转 和 平移向量？

  // A rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
  double theta = M_PI / 8;  // The angle of rotation in radians
  transformation_matrix (0, 0) = std::cos (theta);	// 其中 theta 单位为弧度制
  transformation_matrix (0, 1) = -sin (theta);
  transformation_matrix (1, 0) = sin (theta);
  transformation_matrix (1, 1) = std::cos (theta);

  // A translation on Z axis (0.4 meters)
  transformation_matrix (2, 3) = 0.4;

  // Display in terminal the transformation matrix
  std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;
  print4x4Matrix (transformation_matrix);

  // Executing the transformation
  pcl::transformPointCloud (*cloud_in, *cloud_icp, transformation_matrix); // *cloud_in 表示原始点云，* 表示引用操作，用于访问指针指向的对象;
  *cloud_tr = *cloud_icp;  // We backup cloud_icp into cloud_tr for later use, 你可以这样理解：funcAB(A, B, '+'); c= B;

  // The Iterative Closest Point algorithm
  time.tic ();	
  pcl::IterativeClosestPoint<PointT, PointT> icp;	// 实例化 pcl::IterativeClosestPoint<PointT, PointT> 类的对象 icp; 处理类型为 pointT 的点云数据
  icp.setMaximumIterations (iterations);
  icp.setInputSource (cloud_icp);	// 其中 cloud_icp 为指向点云数据结构的指针
  icp.setInputTarget (cloud_in);
  icp.align (*cloud_icp); 	// 对源数据执行 ICP 操作
  
    icp.setMaximumIterations (1);  // We set this variable to 1 for the next time we will call .align () function
  std::cout << "Applied " << iterations << " ICP iteration(s) in " << time.toc () << " ms" << std::endl;

  if (icp.hasConverged ())	// icp.hasConverged () 查询 ICP 算法是否收敛； true 表示满足条件，但不能保证是最优解
  {
    std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl; //  lower score means better
    std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
    transformation_matrix = icp.getFinalTransformation ().cast<double>(); // icp.getFinalTransformation (), 该方法用于获取 执行完 ICP 算法后的最佳刚体变换矩阵，通常为4*4，包含旋转 和 平移，其中左上角3*3为旋转矩阵，最后一列为平移量 ； .cast<double> 则是将矩阵中的数据类型转换为 double 类型；
    print4x4Matrix (transformation_matrix);
  }
  else
  {
    PCL_ERROR ("\nICP has not converged.\n");
    return (-1);
  }

  // Visualization
  pcl::visualization::PCLVisualizer viewer ("ICP demo");
  // Create two vertically separated viewports
  int v1 (0);
  int v2 (1);
  viewer.createViewPort (0.0, 0.0, 0.5, 1.0, v1);	//设置 v1 视口，左下角(0,0); 右上角为(0.5, 1.0), 占屏幕竖向一半
  viewer.createViewPort (0.5, 0.0, 1.0, 1.0, v2);

  // The color we will be using
  float bckgr_gray_level = 0.0;  // Black
  float txt_gray_lvl = 1.0 - bckgr_gray_level;

  // Original point cloud is white
  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); // 给 cloud_in 指定一个颜色，参数 (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl, (int) 255 * txt_gray_lvl 指定 RGB 颜色；
  viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v1", v1);	// 将自定义颜色cloud_in_color_h 的 cloud_in 添加到视口 v1; 
  viewer.addPointCloud (cloud_in, cloud_in_color_h, "cloud_in_v2", v2);

  // Transformed point cloud is green
  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 aligned point cloud is red
  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);

  // Adding text descriptions in each viewport
  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;		// 将整数变量写入到 ss 流中
  std::string iterations_cnt = "ICP iterations = " + ss.str ();  // ss.str 获取写入的整数变量，但以字符串变量显示
  viewer.addText (iterations_cnt, 10, 60, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "iterations_cnt", v2);

  // Set background color
  viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);
  viewer.setBackgroundColor (bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);

  // Set camera position and orientation
  viewer.setCameraPosition (-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);
  viewer.setSize (1280, 1024);  // Visualiser window size

  // Register keyboard callback :
  viewer.registerKeyboardCallback (&keyboardEventOccurred, (void*) NULL);

  // Display the visualiser
  while (!viewer.wasStopped ())
  {
    viewer.spinOnce ();

    // The user pressed "space" :
    if (next_iteration)
    {
      // 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_in" << std::endl;
        transformation_matrix *= icp.getFinalTransformation ().cast<double>();  // WARNING /!\ This is not accurate! For "educational" purpose only!
        print4x4Matrix (transformation_matrix);  // Print the transformation between original pose and current pose

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

编译：

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)

project(pcl-interactive_icp)

find_package(PCL 1.5 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (interactive_icp interactive_icp.cpp)
target_link_libraries (interactive_icp ${PCL_LIBRARIES})

运行：

$ ./interactive_icp monkey.ply 1

9.NDT

NDT(Normal Distributions Transform，正态分布变换)点云配准算法, 是通过构建点云的统计模型来匹配两组点云数据，从而估计它们之间的相对变换。

示例代码如下：

#include <iostream>
#include <thread>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <pcl/registration/ndt.h>
#include <pcl/filters/approximate_voxel_grid.h>

#include <pcl/visualization/pcl_visualizer.h>

using namespace std::chrono_literals;

int main ()
{
  // Loading first scan of room.
  pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>); // 声明指针变量 target_cloud
  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;

  // Loading second scan of room from new perspective.
  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;

  // Filtering input scan to roughly 10% of original size to increase speed of registration.
  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); // 设置 体素网格的大小边长为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;

  // Initializing Normal Distributions Transform (NDT).
  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt; // NDT 模板类声明， 输入为 pcl::PointXYZ, 输出为 pcl::PointXYZ

  // Setting scale dependent NDT parameters
  // Setting minimum transformation difference for termination condition.
  ndt.setTransformationEpsilon (0.01); // 判定 迭代算法的收敛条件, 影响配准精度。 将两次迭代之间的变换矩阵的F范数小于该阈值，便认为满足条件
  // Setting maximum step size for More-Thuente line search.
  ndt.setStepSize (0.1);	// 设定搜索步长 
  //Setting Resolution of NDT grid structure (VoxelGridCovariance).
  ndt.setResolution (1.0);	// 设定体素网格的大小

  // Setting max number of registration iterations.
  ndt.setMaximumIterations (35); // 最大迭代次数

  // Setting point cloud to be aligned.
  ndt.setInputSource (filtered_cloud);
  // Setting point cloud to be aligned to.
  ndt.setInputTarget (target_cloud);

  // Set initial alignment estimate found using robot odometry.
  Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());	// 定位围绕Z轴旋转 0.6931 弧度
  Eigen::Translation3f init_translation (1.79387, 0.720047, 0); // 定义初始平移量
  Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix (); // 设定初始的刚体变换矩阵

  // Calculating required rigid transform to align the input cloud to the target cloud.
  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  ndt.align (*output_cloud, init_guess); 

  std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
            << " score: " << ndt.getFitnessScore () << std::endl;

  // Transforming unfiltered, input cloud using found transform.
  pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());

  // Saving transformed input cloud.
  pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);

  // Initializing point cloud visualizer
  pcl::visualization::PCLVisualizer::Ptr
  viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer_final->setBackgroundColor (0, 0, 0);

  // Coloring and visualizing target cloud (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");

  // Coloring and visualizing transformed input 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");

  // Starting visualizer
  viewer_final->addCoordinateSystem (1.0, "global");
  viewer_final->initCameraParameters ();

  // Wait until visualizer window is closed.
  while (!viewer_final->wasStopped ())
  {
    viewer_final->spinOnce (100);
    std::this_thread::sleep_for(100ms);
  }

  return (0);
}
posted @ 2024-05-07 22:58  小明同学写博客  阅读(258)  评论(1)    收藏  举报