点云配准流程示例¶
使用迭代最近点算法(ICP)¶
迭代最近点算法（Iterative Closest Point，简称ICP算法）
代码实现¶
创建文件：iterative_closest_point.cpp
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
int
main(int argc, char **argv) {
// 定义输入和输出点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
// 随机填充无序点云
cloud_in->width = 5;
cloud_in->height = 1;
cloud_in->is_dense = false;
cloud_in->points.resize(cloud_in->width * cloud_in->height);
for (size_t i = 0; i < cloud_in->points.size(); ++i) {
cloud_in->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
cloud_in->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
cloud_in->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
}
std::cout << "Saved " << cloud_in->points.size() << " data points to input:"
<< std::endl;
for (size_t i = 0; i < cloud_in->points.size(); ++i)
std::cout << " " <<
cloud_in->points[i].x << " " << cloud_in->points[i].y << " " <<
cloud_in->points[i].z << std::endl;
*cloud_out = *cloud_in;
std::cout << "size:" << cloud_out->points.size() << std::endl;
// 在点云上执行简单的刚性变换，将cloud_out中的x平移0.7f米，然后再次输出数据值。
for (size_t i = 0; i < cloud_in->points.size(); ++i)
cloud_out->points[i].x = cloud_in->points[i].x + 0.7f;
// 打印这些点
std::cout << "Transformed " << cloud_in->points.size() << " data points:"
<< std::endl;
for (size_t i = 0; i < cloud_out->points.size(); ++i)
std::cout << " " << cloud_out->points[i].x << " " <<
cloud_out->points[i].y << " " << cloud_out->points[i].z << std::endl;
// 创建IterativeClosestPoint的实例
// setInputSource将cloud_in作为输入点云
// setInputTarget将平移后的cloud_out作为目标点云
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
// 创建一个 pcl::PointCloud<pcl::PointXYZ>实例 Final 对象,存储配准变换后的源点云,
// 应用 ICP 算法后, IterativeClosestPoint 能够保存结果点云集,如果这两个点云匹配正确的话
// （即仅对其中一个应用某种刚体变换，就可以得到两个在同一坐标系下相同的点云）,那么 icp. hasConverged()= 1 (true),
// 然后会输出最终变换矩阵的匹配分数和变换矩阵等信息。
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
const pcl::Registration<pcl::PointXYZ, pcl::PointXYZ, float>::Matrix4 &matrix = icp.getFinalTransformation();
std::cout << matrix << std::endl;
return (0);
}
输出结果¶
Saved 5 data points to input:
0.352222 -0.151883 -0.106395
-0.397406 -0.473106 0.292602
-0.731898 0.667105 0.441304
-0.734766 0.854581 -0.0361733
-0.4607 -0.277468 -0.916762
size:5
Transformed 5 data points:
1.05222 -0.151883 -0.106395
0.302594 -0.473106 0.292602
-0.0318983 0.667105 0.441304
-0.0347655 0.854581 -0.0361733
0.2393 -0.277468 -0.916762
has converged:1 score: 2.44826e-13
1 -3.25963e-07 -2.98023e-08 0.7
-1.45286e-07 1 1.30385e-07 -1.3113e-07
-2.23517e-07 1.11759e-08 1 -4.61936e-08
0 0 0 1
配准多对点云¶
参加代码示例
正态分布变换配准(NDT)¶
正态分布变换( Normal Distributions Transform )进行配准
用正态分布变换算法来确定两个大型点云（都超过 100 000个点）之间的刚体变换。正态分布变换算法是一个配准算法 , 它应用于 三维点的统计模型，使用标准最优化技术来确定两个点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快，更多关于正态分布变换算法的详细的信息，请看 Martin Magnusson 博士的博士毕业论文“The Three-Dimensional Normal Distributions Transform – an Efficient Representation for Registration, Surface Analysis, and Loop Detection.”。
代码实现¶
#include <iostream>
#include <thread>
#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>
using namespace std::chrono_literals;
int
main(int argc, char **argv) {
// Loading first scan of room.
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>("./data/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>("./data/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);
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;
// Setting scale dependent NDT parameters
// Setting minimum transformation difference for termination condition.
ndt.setTransformationEpsilon(0.01);
// 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());
Eigen::Translation3f init_translation(1.79387, 0, 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);
}
输出结果¶
/home/ty/Lesson/PCL/Code/PCLDemo/build/Debug/bin/normal_distributions_transform
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.713633
实现效果¶
代码详解¶
#include <pcl/registration/ndt.h> // ndt配准文件头
#include <pcl/filters/approximate_voxel_grid.h>// 滤波文件头
使用【正态分布变换算法】和【用来过滤数据的过滤器】对应的头文件，这个过滤器可以用其他过滤器来替换 , 但是使用体素网格过滤器( approximate voxel filter)处理结果较好。
// Loading first scan of room.
// 加载首次的房间扫描数据作为目标点云 target_cloud
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;
// Loading second scan of room from new perspective.
// 加载从新的视角得到的房间第二次扫描数据作为输入源点云 input_cloud
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 文件到共享指针，配准操作是完成 【源点云input_cloud】到【目标点云target_cloud】坐标系变换矩阵的估算，即求出input_cloud变换到target_cloud的变换矩阵
// Filtering input scan to roughly 10% of original size to increase speed of registration.
// 过滤输入点云到约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 contains " << filtered_cloud->size ()
<< " data points from room_scan2.pcd" << std::endl;
以上代码将过滤输入点云到约10%的原始大小，以提高配准速度。这里用任何其他均匀过滤器都可以，目标点云target_cloud不需要进行滤波处理，因为NDT算法在目标点云对应的体素Voxel网格数据结构计算时，不使用单个点，而是使用体素的点。即已做了降采样处理。
// 初始化正态分布变化NDT对象
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
Here we create the NDT algorithm with the default values. The internal data structures are not initialized until later.
//根据输入数据的尺度设置NDT相关参数
ndt.setTransformationEpsilon(0.01); // 为终止条件设置最小转换差异
// Setting maximum step size for More-Thuente line search.
ndt.setStepSize(0.1); // 为More-Thuente线搜索设置最大步长
//Setting Resolution of NDT grid structure (VoxelGridCovariance).
ndt.setResolution(1.0); // 设置NDT网格结构的分辨率 VoxelGridCovariance
这里设置一些尺度相关的参数,因为 NDT 算法使用一个体素化数据结构和More-Thuente 线搜索，因此需要缩放一些参数来适应数据集。以上参数看起来在我们使用的房间尺寸比例下运行地很好，但是它们如果需要处理例如一个咖啡杯的扫描之类更小物体，需要对参数进行很大程度的缩小。在变换中 Epsilon 参数分别从长度和弧度，定义了变换矢量[ x, y, z,roll,pitch, yaw]的最小许可的递增量，一旦递增量减小到这个临界值以下 ，那么配准算法就将终止。步长StepSize参数定义了 More-Thuente 线搜索允许的最大步长，这个线搜索算法确定了最大值以下的最佳步长，当靠近最优解时该算法会缩短迭代步长，在更大的最大步长将会在较少的迭代次数下遍历较大的距离，但是却有过度迭代和在不符合要求的局部最小值处结束的风险。
//设置匹配迭代的最大次数
ndt.setMaximumIterations (35);
这个MaximumIterations参数控制了优化程序运行的最大迭代次数，一 般来说，在达到这个限制值之前优化程序就会在 epsilon 变换阈值下终止。添加此最大迭代次数限制能够增加程序鲁棒性,阻止了它在错误的方向运行太久。
// 设置过滤后的输入源点云（第二次扫描数据）
ndt.setInputSource (filtered_cloud);
// 设置目标点云（第一次扫描数据），作为对其的目标。
ndt.setInputTarget (target_cloud);
这里，我们把点云赋给 NDT 配准对象，目标点云的坐标系是被匹配的输入点云的参考坐标系，匹配完成后输入点云将被变换到与目标点云统一坐标系下，当加载目标点云后，NDT 算法的内部数据结构被初始化。
///设置使用机器人测距法得到的粗略初始变换矩阵
Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ());
Eigen::Translation3f init_translation (1.79387, 0, 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 << "Normal Distributions Transform has converged:" << ndt.hasConverged()
<< " score: " << ndt.getFitnessScore() << std::endl;
最后，我们准备对齐点云。生成的转换后的输入云存储在输出云中。然后，我们显示对齐的结果以及欧几里得适合度得分FitnessScore，该分数计算为从输出云到目标云中最近点的距离的平方。
// 使用找到的变换矩阵，来对未过滤的输入云进行变换。
pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation());
// 保存变换后的输入点云
pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);
在对齐之后，输出云output_cloud将立即包含过滤后的输入云的转换版本，因为我们向算法传递了过滤后的点云，而不是原始输入云。为了获得原始云的对齐版本，我们从NDT算法中提取最终转换矩阵并转换原始输入云。现在，我们可以将该云保存到文件room_scan2_transformed.pcd中，以备将来使用。
配准收敛过程轨迹 RegistrationVisualizer ，pcl_registration_visualizer
刚性物体的鲁棒姿态估计¶
Robust pose estimation of rigid objects
在本教程中，我们将展示如何在具有杂波和遮挡的场景中找到刚体的对齐姿势。
代码实现¶
创建文件：alignment_prerejective.cpp
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
// 首先从定义类型开始，以免使代码混乱
// Types
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointCloudT;
typedef pcl::FPFHSignature33 FeatureT;
typedef pcl::FPFHEstimationOMP<PointNT,PointNT,FeatureT> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;
typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> ColorHandlerT;
// Align a rigid object to a scene with clutter and occlusions
int
main (int argc, char **argv)
{
// 然后，我们实例化必要的数据容器，检查输入参数并加载对象和场景点云。
// Point clouds
PointCloudT::Ptr object (new PointCloudT);
PointCloudT::Ptr object_aligned (new PointCloudT);
PointCloudT::Ptr scene (new PointCloudT);
FeatureCloudT::Ptr object_features (new FeatureCloudT);
FeatureCloudT::Ptr scene_features (new FeatureCloudT);
// Get input object and scene
if (argc != 3)
{
pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
return (1);
}
// Load object and scene
pcl::console::print_highlight ("Loading point clouds...\n");
if (pcl::io::loadPCDFile<PointNT> (argv[1], *object) < 0 ||
pcl::io::loadPCDFile<PointNT> (argv[2], *scene) < 0)
{
pcl::console::print_error ("Error loading object/scene file!\n");
return (1);
}
// Downsample
// 为了加快处理速度，我们使用PCL的：pcl::VoxelGrid类将对象和场景点云的采样率下采样至5 mm。
pcl::console::print_highlight ("Downsampling...\n");
pcl::VoxelGrid<PointNT> grid;
const float leaf = 0.005f;
grid.setLeafSize (leaf, leaf, leaf);
grid.setInputCloud (object);
grid.filter (*object);
grid.setInputCloud (scene);
grid.filter (*scene);
// Estimate normals for scene
pcl::console::print_highlight ("Estimating scene normals...\n");
pcl::NormalEstimationOMP<PointNT,PointNT> nest;
nest.setRadiusSearch (0.01);
nest.setInputCloud (scene);
nest.compute (*scene);
// Estimate features
// 对于下采样点云中的每个点，我们现在使用PCL的pcl::FPFHEstimationOMP<>类来计算用于对齐过程中用于匹配的快速点特征直方图（FPFH）描述符。
pcl::console::print_highlight ("Estimating features...\n");
FeatureEstimationT fest;
fest.setRadiusSearch (0.025);
fest.setInputCloud (object);
fest.setInputNormals (object);
fest.compute (*object_features);
fest.setInputCloud (scene);
fest.setInputNormals (scene);
fest.compute (*scene_features);
// Perform alignment
// SampleConsensusPrerejective 实现了有效的RANSAC姿势估计循环
pcl::console::print_highlight ("Starting alignment...\n");
pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align;
align.setInputSource (object);
align.setSourceFeatures (object_features);
align.setInputTarget (scene);
align.setTargetFeatures (scene_features);
align.setMaximumIterations (50000); // Number of RANSAC iterations
align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose
align.setCorrespondenceRandomness (5); // Number of nearest features to use
align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold
align.setMaxCorrespondenceDistance (2.5f * leaf); // Inlier threshold
align.setInlierFraction (0.25f); // Required inlier fraction for accepting a pose hypothesis
{
pcl::ScopeTime t("Alignment");
align.align (*object_aligned);
}
if (align.hasConverged ())
{
// Print results
printf ("\n");
Eigen::Matrix4f transformation = align.getFinalTransformation ();
pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
pcl::console::print_info ("\n");
pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
pcl::console::print_info ("\n");
pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());
// Show alignment
pcl::visualization::PCLVisualizer visu("Alignment");
visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned");
visu.spin ();
}
else
{
pcl::console::print_error ("Alignment failed!\n");
return (1);
}
return (0);
}
- 执行命令运行程序
./alignment-prerejective ./data/chef.pcd ./data/rs1.pcd
代码详解¶
对齐配准对象创建与配置
// Perform alignment
pcl::console::print_highlight ("Starting alignment...\n");
pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align;
align.setInputSource (object);
align.setSourceFeatures (object_features);
align.setInputTarget (scene);
align.setTargetFeatures (scene_features);
align.setMaximumIterations (50000); // Number of RANSAC iterations
align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose
align.setCorrespondenceRandomness (5); // Number of nearest features to use
align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold
align.setMaxCorrespondenceDistance (2.5f * leaf); // Inlier threshold
align.setInlierFraction (0.25f); // Required inlier fraction for accepting a pose hypothesis
除了常用的输入点云和功能以外，此类还包含一些其他运行时参数，这些参数对对齐算法的性能有很大影响。 前两个与对齐类pcl::SampleConsensusInitialAlignment作用相同
-
样本数-setNumberOfSamples()
在对象和场景之间进行采样的点对应数。 至少需要3个点才能计算姿势。
-
对应随机性-setCorrespondenceRandomness()
我们可以在N个最佳匹配之间随机选择，而不是将每个对象FPFH描述符匹配到场景中最接近的匹配特征。 这增加了必要的迭代，但也使算法对异常匹配具有鲁棒性。
-
多边形相似度阈值-setSimilarityThreshold()
对齐类使用
pcl::registration::CorrespondenceRejectorPoly类，根据采样点之间距离的位置不变的几何一致性，尽早消除不良姿势。在物体和场景上， 将该值设置为越接近1，则贪婪程度越高，从而使算法变得更快。 但是，这也会增加存在噪音时消除好姿势的风险。
-
内在阈值-setMaxCorrespondenceDistance()
这是欧几里德距离阈值，用于确定变换后的对象点是否正确对齐到最近的场景点。
在此示例中，我们使用的启发式值为点云分辨率的1.5倍。
-
Inlier分数-setInlierFraction()
在许多实际情况下，由于混乱，遮挡或两者兼而有之，场景中观察到的对象的大部分都不可见。
在这种情况下，我们需要考虑不会将所有对象点都用于对准场景的姿势假设（猜想）。
正确对齐的点的绝对数量是使用inlier阈值确定的，并且如果该数量与对象中总点数的比大于指定的inlier分数，则我们接受姿势假设（猜想）为有效。
执行配准并输出结果
最后，我们执行对齐配准过程：
{
pcl::ScopeTime t("Alignment");
align.align (*object_aligned);
}
对齐的对象存储在点云object_aligned中。 如果找到了一个具有足够inliers的姿势（占对象点总数的25％以上），则该算法会收敛，并且我们可以打印并可视化结果。
// Print results
printf ("\n");
Eigen::Matrix4f transformation = align.getFinalTransformation ();
pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
pcl::console::print_info (" | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
pcl::console::print_info ("\n");
pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
pcl::console::print_info ("\n");
pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());
// Show alignment
pcl::visualization::PCLVisualizer visu("Alignment");
visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned");
visu.spin ();
输出结果¶
可视化窗口应该如下图所示。场景显示为绿色，对齐的对象模型显示为蓝色。注意大量不可见的对象点。
- 前视图
- 侧视图
