【PCL】教程 normal_distributions_transform 将一个新的点云（室内扫描）对齐到先前扫描的点云...

如何使用正态分布变换 

在本教程中，我们将介绍如何使用正态分布变换 (NDT) 算法来确定两个超过 10 万点的大型点云之间的刚性变换。NDT 算法是一种注册算法，它将标准优化技术应用于三维点的统计模型，以确定两个点云之间最可能的注册。有关 NDT 算法内部工作原理的更多信息，请参阅 Martin Magnusson 博士的博士论文："三维正态分布变换--用于注册、表面分析和环路检测的高效表示法"。

Loaded 112586 data points from room_scan1.pcd
Loaded 112624 data points from room_scan2.pcd
Filtered cloud contains 12433 data points from room_scan2.pcd
Normal Distributions Transform has converged:1 score: 0.679887

代 码

首先，下载数据集 room_scan1.pcd 和 room_scan2.pcd，并将其保存到磁盘中。这些点云包含从不同角度对同一房间进行的 360 度扫描。

这段代码的目标是进行3D点云的对齐，主要使用了PCL库中的一种基于正态分布变换（NDT）的方法。

主要流程如下：

1. 首先，它从两个PCD文件("room_scan1.pcd" 和 "room_scan2.pcd")中加载点云数据，这两个文件分别代表了同一个房间从两个不同的视角进行扫描得到的点云数据。

2. 然后，对第二个扫描的点云数据进行滤波处理，大约减小到原始大小的10%，以提高配准的速度。这一步是通过使用近似体素网格滤波器来实现的，设置其叶子的大小为0.2x0.2x0.2。

3. 初始化正态分布变换（NDT），并设置其参数。其中：

• 设置变化的最小差值为0.01。

• 设置More-Thuente线搜索的最大步长为0.1。

• 设置NDT网格结构（VoxelGridCovariance）的分辨率为1.0。

• 设置最大配准迭代次数为35。

• 设定滤波过后的点云作为需要对齐的点云。

• 设定第一个扫描得到的点云作为目标点云。

设置初始估计值。通过机器人测距得到初始估计值，也就是第一个扫描和第二个扫描之间大约的平移和旋转。

对点云进行配准处理，计算出使输入点云对齐到目标点云所需的刚性变换。

使用找到的变换对未过滤的输入点云进行变换，并保存变换后的输入云。

初始化点云可视化器，并设置背景颜色为黑色。

设置点云的颜色，使目标点云为红色，变换后的输入点云为绿色，并添加到可视化器中。

启动可视化器，并等待可视化窗口关闭。

以上就是整个代码的功能及方法，主要通过NDT方法实现两个视角下的点云数据的配准。

#include <iostream>// 导入C++的输入输出库
#include <thread>// 导入C++多线程库


#include <pcl/io/pcd_io.h>// PCL库读取pcd文件的模块
#include <pcl/point_types.h>// PCL库中关于点类型定义的模块


#include <pcl/registration/ndt.h>// PCL库中进行正态分布变换的模块
#include <pcl/filters/approximate_voxel_grid.h>// PCL库中进行近似体素栅格滤波的模块


#include <pcl/visualization/pcl_visualizer.h>// PCL库进行可视化的模块


using namespace std::chrono_literals;


int
main ()
{
  // 加载第一个扫描的房间场景
  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 ("无法读取文件 room_scan1.pcd \n");
    return (-1);
  }
  std::cout << "从 room_scan1.pcd 加载 " << target_cloud->size () << " 个点" << 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 ("无法读取文件 room_scan2.pcd \n");
    return (-1);
  }
  std::cout << "从 room_scan2.pcd 加载 " << input_cloud->size () << " 个点" << std::endl;


  // 过滤输入点云，减少至原始大小的大约10%，以提高配准的速度
  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->size ()<< " 来自 room_scan2.pcd 的点" << std::endl;


  // 初始化正态分布变换
  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;


  // 设置NDT的参数
  ndt.setTransformationEpsilon (0.01); // 设置最小转换差异的终止条件
  ndt.setStepSize (0.1); // 设置More-Thuente线搜索的最大步长
  ndt.setResolution (1.0); // 设置NDT网格结构的分辨率（VoxelGridCovariance）


  // 设置最大迭代次数
  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);


  std::cout << "正态分布变换已收敛：" << ndt.hasConverged ()
            << " 分数：" << ndt.getFitnessScore() << std::endl;


  // 使用找到的变换转换未过滤的输入点云
  pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation());


  // 保存转换的输入点云
  pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);


  // 初始化点云可视化器
  pcl::visualization::PCLVisualizer::Ptr
  viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer_final->setBackgroundColor (0, 0, 0);


  // 对目标点云进行着色和可视化（红色）
  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");


  // 对转换的输入点云进行着色和可视化（绿色）
  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, "global");
  viewer_final->initCameraParameters();


  // 等待直到可视化窗口关闭
  while (!viewer_final->wasStopped ())
  {
    viewer_final->spinOnce (100);
    std::this_thread::sleep_for(100ms);
  }


  return (0);
}

这段代码的主要目标是将一个新的点云（室内扫描）对齐到先前扫描的点云。为了加速对齐过程，我们首先对输入数据进行了过滤。然后，我们使用NDT算法对输入的点云和目标点云之间进行转换。附加的可视化步骤可以帮助我们检查对齐的结果。

笔 记

pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;

pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;

这段代码中主要使用了以下PCL（Point Cloud Library）库的功能与函数：

1. 加载点云：

   1. pcl::io::loadPCDFile<pcl::PointXYZ>：加载PCD（Point Cloud Data）文件到点云对象中。此函数用于加载两个室内扫描得到的点云数据。

2. 过滤点云：

• pcl::ApproximateVoxelGrid<pcl::PointXYZ>：这是一种用于下采样的过滤器，通过创建一个近似体素栅格来减少点云中的点数，从而提高算法的效率。

正态分布变换（NDT）配准：

• pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ>：正态分布变换（NDT）用于计算两个点云之间的刚体变换（旋转和平移），目的是将一个点云对齐到另一个点云。

设置NDT参数：

• setTransformationEpsilon、setStepSize、setResolution、setMaximumIterations：这些函数用于设置NDT算法的参数，包括转换的最小差异、搜索的最大步长、NDT网格的分辨率和最大迭代次数。

执行配准和获取结果：

• ndt.align：根据设置的参数和初始猜测，执行NDT算法进行点云配准。

• ndt.hasConverged 和 ndt.getFitnessScore：检查NDT算法是否收敛，并获取配准的分数（质量）。

变换点云：

• pcl::transformPointCloud：使用NDT算法得到的最终变换矩阵，将输入点云变换到目标点云的坐标系中。

保存点云：

• pcl::io::savePCDFileASCII：将处理后的点云保存到PCD文件中。

可视化：

• pcl::visualization::PCLVisualizer：PCL的一个可视化工具，用于显示点云和变换结果。

• pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>：设置点云的颜色，用于区分不同的点云。

• 各种viewer的函数，如addPointCloud、setPointCloudRenderingProperties、addCoordinateSystem、initCameraParameters等，用于添加点云到可视化器、设置渲染属性、添加坐标系、初始化相机参数等。

通过这些函数的组合使用，代码实现了加载、过滤、配准点云并进行可视化展示的完整流程。

报  错