迭代最近点算法（Iterative CLosest Point简称ICP算法）
ICP算法对待拼接的2片点云，首先根据一定的准则确立对应点集P与Q，其中对应点对的个数，然后通过最小二乘法迭代计算最优的坐标变换，即旋转矩阵R和平移矢量t，使得误差函数最小，ICP处理流程分为四个主要的步骤：
1.对原始点云数据进行采样
2.确定初始对应点集
3.去除错误对应点对
4.坐标变换的求解
代码实现
#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:
1.28125 577.094 197.938
828.125 599.031 491.375
358.688 917.438 842.562
764.5 178.281 879.531
727.531 525.844 311.281
size:5
Transformed 5 data points:
1.98125 577.094 197.938
828.825 599.031 491.375
359.388 917.438 842.562
765.2 178.281 879.531
728.231 525.844 311.281
has converged:1 score: 4.9876e-07
1 1.07289e-06 -3.87431e-07 0.698853
1.2815e-06 1 -7.59961e-07 -0.00146397
-6.85454e-07 -6.70553e-07 1 0.000182647
0 0 0 1配准多对点云
使用迭代最近点算法，实现逐步地对一系列点云进行两两匹配，其思想是对所有的点云进行变换，使得都与第一个点云在统一坐标系中，在每个连贯的有重叠的点云之间找出最佳的变换，并积累这些变换到全部的点云，能够进行ICP算法的点云需要粗略的预匹配(比如在一个机器人的量距内或者在地图的框架内)，并且一个点云与另一个点云需要有重叠的部分。
#include <boost/smart_ptr/make_shared_object.hpp> //boost指针相关头文件
#include <pcl/point_types.h> //点类型定义头文件
#include <pcl/point_cloud.h> //点云类定义头文件
#include <pcl/point_representation.h> //点表示相关的头文件
#include <pcl/io/pcd_io.h> //PCD文件打开存储类头文件
#include <pcl/filters/voxel_grid.h> //用于体素网格化的滤波类头文件
#include <pcl/filters/filter.h> //滤波相关头文件
#include <pcl/features/normal_3d.h> //法线特征头文件
#include <pcl/registration/icp.h> //ICP类相关头文件
#include <pcl/registration/icp_nl.h> //非线性ICP 相关头文件
#include <pcl/registration/transforms.h> //变换矩阵类头文件
#include <pcl/visualization/pcl_visualizer.h> //可视化类头文件
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
//定义
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud; //申明pcl::PointXYZ数据
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
//申明一个全局可视化对象变量，定义左右视点分别显示配准前和配准后的结果点云
pcl::visualization::PCLVisualizer* p; //创建可视化对象
int vp_1, vp_2; //定义存储 左 右视点的ID
//申明一个结构体方便对点云以文件名和点云对象进行成对处理和管理点云，处理过程中可以同时接受多个点云文件的输入
struct PCD
{
PointCloud::Ptr cloud; //点云共享指针
std::string f_name; //文件名称
PCD() : cloud(new PointCloud) {};
};
struct PCDComparator //文件比较处理
{
bool operator () (const PCD& p1, const PCD& p2)
{
return (p1.f_name < p2.f_name);
}
};
// 以< x, y, z, curvature >形式定义一个新的点表示
class MyPointRepresentation : public pcl::PointRepresentation <PointNormalT>
{
using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;
public:
MyPointRepresentation()
{
nr_dimensions_ = 4; //定义点的维度
}
// 重载copyToFloatArray方法将点转化为四维数组
virtual void copyToFloatArray(const PointNormalT& p, float* out) const
{
// < x, y, z, curvature >
out[0] = p.x;
out[1] = p.y;
out[2] = p.z;
out[3] = p.curvature;
}
};
/** \左视图用来显示未匹配的源和目标点云*/
void showCloudsLeft(const PointCloud::Ptr cloud_target, const PointCloud::Ptr cloud_source)
{
p->removePointCloud("vp1_target");
p->removePointCloud("vp1_source");
PointCloudColorHandlerCustom<PointT> tgt_h(cloud_target, 0, 255, 0);
PointCloudColorHandlerCustom<PointT> src_h(cloud_source, 255, 0, 0);
p->addPointCloud(cloud_target, tgt_h, "vp1_target", vp_1);
p->addPointCloud(cloud_source, src_h, "vp1_source", vp_1);
PCL_INFO("Press q to begin the registration.\n");
p->spin();
}
/** \右边显示配准后的源和目标点云*/
void showCloudsRight(const PointCloudWithNormals::Ptr cloud_target, const PointCloudWithNormals::Ptr cloud_source)
{
p->removePointCloud("source");
p->removePointCloud("target");
PointCloudColorHandlerGenericField<PointNormalT> tgt_color_handler(cloud_target, "curvature");
if (!tgt_color_handler.isCapable())
PCL_WARN("Cannot create curvature color handler!");
PointCloudColorHandlerGenericField<PointNormalT> src_color_handler(cloud_source, "curvature");
if (!src_color_handler.isCapable())
PCL_WARN("Cannot create curvature color handler!");
p->addPointCloud(cloud_target, tgt_color_handler, "target", vp_2);
p->addPointCloud(cloud_source, src_color_handler, "source", vp_2);
p->spinOnce();
}
/** \简单的加载一组我们要一起注册的PCD文件
* \param argc the number of arguments (pass from main ())
* \param argv the actual command line arguments (pass from main ())
* \param models the resultant vector of point cloud datasets
*/
void loadData(int argc, char** argv, std::vector<PCD, Eigen::aligned_allocator<PCD> >& models)
{
std::string extension(".pcd");
// 第一个参数是命令本身，所以要从第二个参数开始解析
for (int i = 1; i < argc; i++)
{
std::string fname = std::string(argv[i]);
// PCD文件名至少为5个字符大小字符串（因为后缀名.pcd就已经占了四个字符位置）
if (fname.size() <= extension.size())
continue;
std::transform(fname.begin(), fname.end(), fname.begin(), (int(*)(int))tolower);
//检查参数是否为一个pcd后缀的文件
if (fname.compare(fname.size() - extension.size(), extension.size(), extension) == 0)
{
//加载点云并保存在总体的点云列表中
PCD m;
m.f_name = argv[i];
pcl::io::loadPCDFile(argv[i], *m.cloud);
//从点云中移除NAN点也就是无效点
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*m.cloud, *m.cloud, indices);
models.push_back(m);
}
}
}
/** \简单的对齐一对PointCloud数据集并返回结果
* \param cloud_src the source PointCloud
* \param cloud_tgt the target PointCloud
* \param output the resultant aligned source PointCloud
* \param final_transform the resultant transform between source and target
*实现匹配，其中参数有输入一组需要配准的点云，以及是否需要进行下采样，其他参数输出配准后的点云以及变换矩阵
void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false)
{
// 下采样实现一致性，提高速度
// 注意：对于大型数据集启用此功能
PointCloud::Ptr src(new PointCloud); //存储滤波后的源点云
PointCloud::Ptr tgt(new PointCloud); //存储滤波后的目标点云
pcl::VoxelGrid<PointT> grid; /滤波处理对象
if (downsample)
{
grid.setLeafSize(0.05, 0.05, 0.05); //设置滤波时采用的体素大小
grid.setInputCloud(cloud_src);
grid.filter(*src);
grid.setInputCloud(cloud_tgt);
grid.filter(*tgt);
}
else
{
src = cloud_src;
tgt = cloud_tgt;
}
// 计算表面的法向量和曲率
PointCloudWithNormals::Ptr points_with_normals_src(new PointCloudWithNormals);
PointCloudWithNormals::Ptr points_with_normals_tgt(new PointCloudWithNormals);
pcl::NormalEstimation<PointT, PointNormalT> norm_est; //点云法线估计对象
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
norm_est.setSearchMethod(tree);
norm_est.setKSearch(30);
norm_est.setInputCloud(src);
norm_est.compute(*points_with_normals_src);
pcl::copyPointCloud(*src, *points_with_normals_src);
norm_est.setInputCloud(tgt);
norm_est.compute(*points_with_normals_tgt);
pcl::copyPointCloud(*tgt, *points_with_normals_tgt);
// 实例化我们的自定义点表示（如上定义）
MyPointRepresentation point_representation;
// 对“曲率”维度进行加权，使其与x、y和z平衡
float alpha[4] = { 1.0, 1.0, 1.0, 1.0 };
point_representation.setRescaleValues(alpha);
// 配准
pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg; // 配准对象
reg.setTransformationEpsilon(1e-6); //设置收敛判断条件，越小精度越大，收敛也越慢
// 将两个对应关系之间的最大距离（src<->tgt）设置为10cm，大于此值的点对不考虑
// 注意：根据数据集的大小进行调整
reg.setMaxCorrespondenceDistance(0.1);
// 设置点表示
reg.setPointRepresentation(pcl::make_shared<const MyPointRepresentation>(point_representation));
reg.setInputSource(points_with_normals_src); // 设置源点云
reg.setInputTarget(points_with_normals_tgt); // 设置目标点云
// 在循环中运行相同的优化并可视化结果
Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity(), prev, targetToSource;
PointCloudWithNormals::Ptr reg_result = points_with_normals_src;
reg.setMaximumIterations(2); //设置最大的迭代次数，即每迭代两次就认为收敛，停止内部迭代
for (int i = 0; i < 30; ++i) //手动迭代，每手动迭代一次，在配准结果视口对迭代的最新结果进行刷新显示
{
PCL_INFO("Iteration Nr. %d.\n", i);
// 存储点云以便可视化
points_with_normals_src = reg_result;
// 估计
reg.setInputSource(points_with_normals_src);
reg.align(*reg_result);
//在每次迭代之间累积变换
Ti = reg.getFinalTransformation() * Ti;
//如果此转换与上一个转换之间的差异小于阈值，则减少最大对应距离
if (fabs((reg.getLastIncrementalTransformation() - prev).sum()) < reg.getTransformationEpsilon())
reg.setMaxCorrespondenceDistance(reg.getMaxCorrespondenceDistance() - 0.001);
prev = reg.getLastIncrementalTransformation();
// 可视化当前状态
showCloudsRight(points_with_normals_tgt, points_with_normals_src);
}
// 获取从目标到源的转换
targetToSource = Ti.inverse();
// 将目标变换回源帧
pcl::transformPointCloud(*cloud_tgt, *output, targetToSource);
p->removePointCloud("source");
p->removePointCloud("target");
PointCloudColorHandlerCustom<PointT> cloud_tgt_h(output, 0, 255, 0);
PointCloudColorHandlerCustom<PointT> cloud_src_h(cloud_src, 255, 0, 0);
p->addPointCloud(output, cloud_tgt_h, "target", vp_2);
p->addPointCloud(cloud_src, cloud_src_h, "source", vp_2);
PCL_INFO("Press q to continue the registration.\n");
p->spin();
p->removePointCloud("source");
p->removePointCloud("target");
//将源添加到转换后的目标
*output += *cloud_src;
final_transform = targetToSource;
}
int main(int argc, char** argv)
{
// 存储管理所有打开的点云
std::vector<PCD, Eigen::aligned_allocator<PCD> > data;
loadData(argc, argv, data); // 加载所有点云到data
// 检查输入
if (data.empty())
{
PCL_ERROR("Syntax is: %s <source.pcd> <target.pcd> [*]", argv[0]);
PCL_ERROR("[*] - multiple files can be added. The registration results of (i, i+1) will be registered against (i+2), etc");
return (-1);
}
PCL_INFO("Loaded %d datasets.", (int)data.size());
// 创建PCL可视化对象
p = new pcl::visualization::PCLVisualizer(argc, argv, "Pairwise Incremental Registration example");
p->createViewPort(0.0, 0, 0.5, 1.0, vp_1); //用左半窗口创建视口vp_1
p->createViewPort(0.5, 0, 1.0, 1.0, vp_2); //用右半窗口创建视口vp_2
PointCloud::Ptr result(new PointCloud), source, target;
Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity(), pairTransform;
for (size_t i = 1; i < data.size(); ++i) //循环处理所有点云
{
source = data[i - 1].cloud; //连续配准
target = data[i].cloud; // 相邻两组点云
showCloudsLeft(source, target); //可视化为配准的源和目标点云
//调用子函数完成一组点云的配准，temp返回配准后两组点云在第一组点云坐标下的点云
PointCloud::Ptr temp(new PointCloud);
PCL_INFO("Aligning %s (%d) with %s (%d).\n", data[i - 1].f_name.c_str(), source->points.size(), data[i].f_name.c_str(), target->points.size());
// pairTransform返回从目标点云target到source的变换矩阵
pairAlign(source, target, temp, pairTransform, true);
//把当前两两配准后的点云temp转化到全局坐标系下返回result
pcl::transformPointCloud(*temp, *result, GlobalTransform);
//用当前的两组点云之间的变换更新全局变换
GlobalTransform = GlobalTransform * pairTransform;
//保存转换到第一个点云坐标下的当前配准后的两组点云result到文件i.pcd
std::stringstream ss;
ss << i << ".pcd";
pcl::io::savePCDFile(ss.str(), *result, true);
}
}正态分布变换配准(NDT)
用正态分布变换算法来确定两个大型点云（都超过 100000个点）之间的刚体变换。正态分布变换算法是一个配准算法，它应用于三维点的统计模型，使用标准最优化技术来确定两个点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快，相关论文：“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/visualization/pcl_visualizer.h>
//使用【正态分布变换算法】和【用来过滤数据的过滤器】对应的头文件，这个过滤器可以用其他过滤器来替换,
//但是使用体素网格过滤器(approximate voxel filter)处理结果较好
#include <pcl/registration/ndt.h> // ndt配准头文件
#include <pcl/filters/approximate_voxel_grid.h>
using namespace std::chrono_literals;
int main(int argc, char** argv)
{
// 加载首次的房间扫描数据作为目标点云 target_cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>("G:/vsdata/PCLlearn/PCDdata/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;
// 加载从新的视角得到的房间第二次扫描数据作为输入源点云 input_cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>("G:/vsdata/PCLlearn/PCDdata/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的变换矩阵
-------------------------------------------------------------------------------------------------------------------------*/
// 过滤输入点云到原始大小的约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;
// 根据输入数据的尺度设置NDT相关参数
// 设置终止条件的最小转换差
ndt.setTransformationEpsilon(0.01);
// 设置More Thuente行搜索的最大步长
ndt.setStepSize(0.1);
// 设置NDT网格结构的分辨率（VoxelGridCovariance）
ndt.setResolution(1.0);
/*-------------------------------------------------------------------------------------------------------------------------
以上代码设置一些尺度相关的参数,因为 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
中，以备将来使用。
-------------------------------------------------------------------------------------------------------------------------*/
// 初始化点云可视化工具
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);
}输出结果
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.767801效果
刚性物体的鲁棒姿态估计
以下示例展示如何在具有杂波和遮挡的场景中找到刚体的对齐姿势。
代码实现
资源准备：对象模型 和 场景模型
#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>
// 首先定义类型，以免使代码混乱
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;
// 将刚性对象与具有杂波和遮挡的场景对齐
int main(int argc, char** argv)
{
// 实例化必要的数据容器，检查输入参数并加载对象和场景点云
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);
// 获取输入对象和场景
if (argc != 3)
{
pcl::console::print_error("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
return (1);
}
// 加载对象和场景
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);
}
// 为了加快处理速度，使用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);
// 估计场景的法线
pcl::console::print_highlight("Estimating scene normals...\n");
pcl::NormalEstimationOMP<PointNT, PointNT> nest;
nest.setRadiusSearch(0.01);
nest.setInputCloud(scene);
nest.compute(*scene);
// 特征估计
// 对于下采样点云中的每个点，使用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);
// 对齐配准对象创建与配置
// 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); // RANSAC 迭代次数
align.setNumberOfSamples(3); // 样本数-在对象和场景之间进行采样的点对应数。至少需要3个点才能计算姿态
align.setCorrespondenceRandomness(5); // 对应随机性-我们可以在N个最佳匹配之间随机选择，而不是将每个对象FPFH描述符匹配到场景中最接近的匹配特征。这增加了必要的迭代，但也使算法对异常匹配具有鲁棒性
align.setSimilarityThreshold(0.9f); // 多边形相似度阈值-对齐类使用pcl::registration::CorrespondenceRejectorPoly类，根据采样点之间距离的位置不变的几何一致性，尽早消除不良姿态。在物体和场景上， 将该值设置为越接近1，则贪婪程度越高，从而使算法变得更快。但是，这也会增加存在噪音时消除好姿态的风险
align.setMaxCorrespondenceDistance(2.5f * leaf); // 内在阈值-这是欧几里德距离阈值，用于确定变换后的对象点是否正确对齐到最近的场景点。在此示例中，我们使用的启发式值为点云分辨率的2.5倍
align.setInlierFraction(0.25f); // Inlier分数-在许多实际情况下，由于杂波，遮挡或两者兼而有之，场景中观察到的对象的大部分都不可见。在这种情况下，我们需要考虑不会将所有对象点都用于对准场景的姿态假设（猜想）。正确对齐的点的绝对数量是使用inlier阈值确定的，并且如果该数量与对象中总点数的比大于指定的inlier分数，则我们接受姿态假设（猜想）为有效
// 执行配准并输出结果
{
pcl::ScopeTime t("Alignment");
align.align(*object_aligned);
}//对齐的对象存储在点云object_aligned中。如果找到了一个具有足够inliers的姿态（占对象点总数的25％以上），则该算法会收敛，并且我们可以打印并可视化结果
if (align.hasConverged())
{
// 打印结果
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());
// 显示对齐方式
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);
}windows下执行命令运行程序:
.\xxx.exe ...\chef.pcd ...\rs1.pcd结果：
