一、ICP(迭代最邻近点)的使用
1.1 什么是ICP
本章演示了在代码中使用迭代最近点算法(ICP)，ICP即通过最小化两个点云的点之间的距离并对它们进行严格的转换，来确定一个点云是否只是另一个点云的严格转换的方法，最终输出为源点云(动态点云)到目标点云(静态点云)的RT转换关系。这种方法被广泛应用到点云配准问题上。
需要注意的是ICP往往用于点云配准的最后一步，即精配准过程，这要求源和目标两片点云的姿态相差不能太大，不然容易引起匹配错误。典型的ICP算法包括点-点，点-面，面-面的三种类型，各有优缺点。其共同的缺点是收敛速度慢，对离群值、缺失数据和部分重叠的敏感性差。
在此篇文章中以最简单的点到点ICP在PCL中的使用做出介绍，在文章最后给出了ICP领域最近的学术成果论文，由清华大学著作，以供学习探讨。
1.2 点到点ICP示例代码分析
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
int main ()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>(5,1));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
// 随机生成源点云
for (auto& point : *cloud_in)
{
point.x = 1024 * rand() / (RAND_MAX + 1.0f);
point.y = 1024 * rand() / (RAND_MAX + 1.0f);
point.z = 1024 * rand() / (RAND_MAX + 1.0f);
}
std::cout << "Saved " << cloud_in->size () << " data points to input:" << std::endl;
// 输出随机生成的源点云位置
for (auto& point : *cloud_in)
std::cout << point << std::endl;
// 创建目标点云，每个点在源点云x+0.7的位置
*cloud_out = *cloud_in;
std::cout << "size:" << cloud_out->size() << std::endl;
for (auto& point : *cloud_out)
point.x += 0.7f;
std::cout << "Transformed " << cloud_in->size () << " data points:" << std::endl;
// 输出目标点云位置
for (auto& point : *cloud_out)
std::cout << point << std::endl;
// 声明PCL的ICP类，设置源点云和目标点云
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
// 声明输出迭代结束后的点云
pcl::PointCloud<pcl::PointXYZ> Final;
// 开始迭代对齐计算
icp.align(Final);
//如果两个pointcloud正确对齐(意味着它们都是相同的云，只是对其中一个应用了某种严格的转换)
//那么hasconverged () = 1 (true)。然后输出最终转换的拟合度得分和一些相关信息。
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
return (0);
}
二、增量ICP的方法
2.1 什么是增量ICP
本节介绍如何使用迭代最近点算法来增量地配准一系列的点云，即在第一个点云的坐标系中转换所有的点云。这是通过在每个连续的云之间找到最好的转换，并在整个云集上积累这些转换来实现的。
首先数据集由点云组成，这些点云已经大致预先对齐在一个共同的坐标系中(例如在一个机器人的里程计或地图坐标系中)，并且彼此有重叠区域。示例点云可以在PCL官方github上下载，点击【示例点云】可以下载。以下五个点云全都需要下载下来，示例代码会将5片点云配准成同一整体。
2.2 示例代码
注意: 代码较多，可参考注释理解。
*/
/* \author Radu Bogdan Rusu
* adaptation Raphael Favier*/
#include <pcl/memory.h> // for pcl::make_shared
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/features/normal_3d.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#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;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
// 使用全局变量
// 显示窗口的全局变量指针
pcl::visualization::PCLVisualizer *p;
// 左右视点
int vp_1, vp_2;
// 点云结构体：点云指针、文件名称
struct PCD
{
PointCloud::Ptr cloud;
std::string f_name;
PCD() : cloud (new PointCloud) {};
};
// PCD点云名称比较器
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;
}
// Override the copyToFloatArray method to define our feature vector
// 重写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;
}
};
/** \brief Display source and target on the first viewport of the visualizer
显示窗口的第一个视点显示源点云和目标点云
**/
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();
}
/** \brief Display source and target on the second viewport of the visualizer
*显示窗口的第二个视点显示源点云和目标点云
*/
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!\n");
PointCloudColorHandlerGenericField<PointNormalT> src_color_handler (cloud_source, "curvature");
if (!src_color_handler.isCapable ())
PCL_WARN ("Cannot create curvature color handler!\n");
p->addPointCloud (cloud_target, tgt_color_handler, "target", vp_2);
p->addPointCloud (cloud_source, src_color_handler, "source", vp_2);
p->spinOnce();
}
/** \简介: 加载一组PCD文件
* \参数 argc 是pcd文件的数量 (pass from main ())
* \参数 argv 是pcd文件的名字容器 (pass from main ())
* \参数 models 是加载后的点集结果
*/
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]);
// Needs to be at least 5: .plot
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)
{
// 读点云并保存在model中
PCD m;
m.f_name = argv[i];
pcl::io::loadPCDFile (argv[i], *m.cloud);
//remove NAN points from the cloud
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*m.cloud,*m.cloud, indices);
models.push_back (m);
}
}
}
/** \简介： 对齐一对PointCloud数据集并返回结果
* \参数 cloud_src 是源点云
* \参数 cloud_tgt 是目标点云
* \参数 output 对齐后的源点云(动态点云)
* \参数 final_transform 源点云与目标点云的转换关系
*/
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); // 设置搜索方法为kdtree
norm_est.setKSearch (30); // 设置k参数为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 (std::abs ((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);
// 检查用户输入是否正确
if (data.empty ())
{
PCL_ERROR ("Syntax is: %s <source.pcd> <target.pcd> [*]\n", argv[0]);
PCL_ERROR ("[*] - multiple files can be added. The registration results of (i, i+1) will be registered against (i+2), etc\n");
return (-1);
}
PCL_INFO ("Loaded %d datasets.\n", (int)data.size ());
// 创建可视化对象
p = new pcl::visualization::PCLVisualizer (argc, argv, "Pairwise Incremental Registration example");
p->createViewPort (0.0, 0, 0.5, 1.0, vp_1);
p->createViewPort (0.5, 0, 1.0, 1.0, vp_2);
PointCloud::Ptr result (new PointCloud), source, target;
Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform;
for (std::size_t i = 1; i < data.size (); ++i)
{
source = data[i-1].cloud;
target = data[i].cloud;
// 添加可视化数据
showCloudsLeft(source, target);
PointCloud::Ptr temp (new PointCloud);
PCL_INFO ("Aligning %s (%zu) with %s (%zu).\n", data[i-1].f_name.c_str (), static_cast<std::size_t>(source->size ()), data[i].f_name.c_str (), static_cast<std::size_t>(target->size ()));
pairAlign (source, target, temp, pairTransform, true);
//将当前点云对转换为全局转换
pcl::transformPointCloud (*temp, *result, GlobalTransform);
// 更新全局转换
GlobalTransform *= pairTransform;
// 保存对齐的点对，转换到第一个点云的坐标下
std::stringstream ss;
ss << i << ".pcd";
pcl::io::savePCDFile (ss.str (), *result, true);
}
}
运行以上程序可以看到如下变化。
除了PCL中自带的ICP方法，当前较为前沿的快速鲁棒ICP方法可以参考以下这篇论文，其主要贡献在于使用了安德森加速将收敛速度加快。该文章在github上有开源代码以供学习参考。
[1] Zhang J , Y Yao, Deng B . Fast and Robust Iterative Closet Point[J]. 2020.
【博主简介】
斯坦福的兔子，男，天津大学机械工程工学硕士。毕业至今从事光学三维成像及点云处理相关工作。因工作中使用的三维处理库为公司内部库，不具有普遍适用性，遂自学开源PCL库及其相关数学知识以备使用。谨此将自学过程与君共享。
博主才疏学浅，尚不具有指导能力，如有问题还请各位在评论处留言供大家共同讨论。
若前辈们有工作机会介绍欢迎私信，最好是北京。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
