点云迭代最近邻点配准法_点云配准(一 两两配准)

原文首发于微信公众号「3D视觉工坊」：点云配准（一 两两配准）

这篇文章简单与大家聊一聊点云的配准技术,本文部分内容主要参考郭浩主编的「点云库PCL从入门到精通」.

由于一直对双目视觉较为感兴趣,无论是传统的双目立体视觉,还是面阵的结构光3D相机,亦或是日渐流行的VSLAM中流行的RGB-D相机,都会涉及到点云数据.

由于点云的不完整,旋转到完整点云就需要对局部点云进行配准.

那么这便带来了如下几个问题

1 )什么是点云的配准呢?

为了得到被测物体的完整数据模型,需要确定一个合适的坐标变换,将从各个视角得到的点集合并到一个统一的坐标系下,形成一个完整的数据点云,然后就可以方便地进行可视化等操作,这便是点云数据的配准.

2 ) 配准有哪些方式呢?

常见的点云手段有①手动配准,②依赖仪器的配准和③自动配准.通常意义上的配准技术,即是指自动配准技术.

3)点云自动配准技术的方法是什么呢?

这主要是通过一定的算法或者统计学规律,利用计算机计算两块点云之间的错位,从而达到把两片点云自动配准的效果.

其实质是把在不同的坐标系中测量得到的数据点云进行坐标变换，以得到整体的数据模型。

4)点云配准的关键步骤是什么呢？

问题的关键是如何求得坐标变换参数Ｒ（旋转矩阵）和Ｔ(平移向量)，使得两视角下测得的三维数据经坐标变换后的距离最小。

目前，配准算法按照实现过程可以分为整体配准和局部配准。PCL中有单独的配准模块，实现了配准相关的基础数据结构与经典配准算法如ICP等，以及配准过程中的对应点估计、错误对应点去除等流程。

为了简化问题,我们先从一对点云配准方法讲起.

具体的配准流程图大致总结如下:

我们称一对点云数据集的配准问题为两两配准问题,通常通过应用一个估计得到的表示平移和旋转的4x4刚体变换矩阵来使一个点云数据集精确地与另一个点云数据集（目标数据集）进行完美配准。

具体实现步骤如下：

（1）首先从两个数据集中按照同样的关键点选取标准，提取关键点。（注意：此处两个数据集的关键点提取方法需要相同）。

（2）对选择的所有关键点分别计算其特征描述子。

（3）结合特征描述子在两个数据集中的坐标的位置，以两者之间特征和位置的相似度为基础，来估算它们的对应关系，初步估计对应点对。

（4）假定数据是有噪声的，除去对配准有影响的错误的对应点对。

（5）利用剩余的正确对应关系来估算刚体变换，完成配准。

从上述整个流程可以看出，在整个配准过程中，关键点的提取与关键点的特征描述，影响着整个配准的准确性与效率。

当然,对于整个配准过程中,还会涉及到对应估计,对应关系的去除,变换矩阵算法等一系列问题,包括采样一致性进行初始配准等,限于篇幅的问题,我们后续有机会再作详细讨论.

接下来,我们以一个简单的demo来演示一下PCL中对于迭代最近邻算法是如何操作与实现的.

demo如下:

#include <iostream> //标准输入输出头文件
#include <pcl/io/pcd_io.h> //io操作头文件
#include <pcl/point_types.h> //点类型定义头文件
#include <pcl/registration/icp.h> //ICP配准类所有相关的头文件 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 =6; //设置点云宽度
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<< "input cloudPoints"<<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; //将所有点沿Z方向平移,等价于将输出点云的x坐标都加上一个数,比如0.6f. for (size_t i = 0; i < cloud_in->points.size (); ++i)
{
cloud_out->points[i].z = cloud_in->points[i].z + 0.6f;
} //打印平移后的点
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; //以上,实现了一个简单的点云刚体变换,以构造目标点云,并再次打印处数据集.
pcl::IterativeClosestPoint<pcl::PointXYZ,pcl::PointXYZ> icp; //创建一个IterativeClosestPoint的对象
icp.setInputCloud(cloud_in); //设置源点云
icp.setInputTarget(cloud_out); //设置目标点云
pcl::PointCloud<pcl::PointXYZ> Final; //存储经过配准变换源点云后的点云
icp.align(Final); //执行配准存储变换后的源点云到Final //打印配准相关输入信息
std::cout<< "has converged: "<<icp.hasConverged()<< " "<< "score: "<<icp.getFitnessScore()<<std::endl; //打印输出最终估计的变换矩阵.
std::cout<<icp.getFinalTransformation()<<std::endl; return 0;
}

运行效果如下:

input cloudPoints6data 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
0.183749 0.968809 0.512055
size: 6
Transformed 6data points:
0.352222 -0.151883 0.493605
-0.397406 -0.473106 0.892602
-0.731898 0.667105 1.0413
-0.734766 0.854581 0.563827
-0.4607 -0.277468 -0.316762
0.183749 0.968809 1.11206
has converged: 1 score: 7.03141e-14
1 2.10013e-07 -1.67638e-07 -3.241e-08
2.57511e-07 1 -1.86265e-07 1.65589e-07
-2.6077e-08 -1.86264e-07 1 0.6
0 0 0 1

对于两两点云之间的配准在PCL中的实现大抵如此,请大家细看其中的注释,对于多福点云之间的匹配,后续文章会进行分享.

主要参考:郭浩主编的<点云库PCL从入门到精通>

上述内容，如有侵犯版权，请联系作者，会自行删文。

星球成员，可免费提问，并邀进讨论群