本发明涉及点云匹配算法，尤其涉及的是一种基于3dsc和改进icp算法加速点云配准的方法，提高点云匹配精度，加快点云匹配的处理速度。
背景技术：
1、随着3d传感器(如激光雷达、结构光相机等)的发展和普及，获取大规模点云数据集变得容易。这些点云数据集可以包含从不同视角、不同时间或使用不同传感器采集的点云。在将这些点云数据集用于应用之前，通常需要将它们对齐到同一个坐标系统中，点云配准过程，就是求一个两个点云之间的旋转平移矩阵(刚性变换或欧式变换)，将源点云(source cloud)变换到目标点云(target cloud)相同的坐标系下。
2、但在点云数据的实际获取过程中，由于受到采集角度、采集距离、物体遮挡等因素的影响，很难直接获得一个恰当的旋转矩阵和平移矩阵。因此需要点云配准算法的研究与改进。
3、点云配准的本质就是寻找一个最优的变换矩阵，使得两组不同坐标系的点云经过矩阵变换后，重合度能尽可能的高。点云配准的步骤一般有四个步骤，首先是数据预处理，例如数据降噪，数据降采样等；之后是进行粗配准，粗配准是指在目标点云与参考点云的相对位姿完全未知的情况下，通过快速搜索找到一个近似变换阵；其次进行精配准(如icp算法)在粗配准变换的基础上，得到更加精确的矩阵，最后是结果评估，通过算法的评价体系，评估配准的效果和质量。
4、点云配准需要选择合适的粗配准方法和精配准方法，结合一些点云处理技术，才能达到精确和快速的配准方法。目前的粗配准方法，有基于局部特征描述子(pfh、fpfh)，基于概率分布(ndt)等粗配准，这些方法存在计算量大或者配准精度低，对后续的精配准产生较大影响。此外，icp迭代算法，该算法简单、易于实现，求解精度较高。但icp对原始点云和目标点云的初始位姿较为敏感，容易陷入局部最优解，且也计算量大、计算速度较慢等缺点。
技术实现思路
1、本发明提出了一种基于3dsc和改进icp算法加速点云配准的方法，用于提高点云配准的精度，此外利用高斯滤波点云降采样，且保证点云结构的完整性，提高点云配准速度。
2、为了实现上述技术效果，本发明通过下列技术方案实现：
3、一种基于3dsc和改进icp算法加速点云配准的方法，包括如下步骤：
4、s1、通过体素滤波和高斯采样结合，降低点云数据量，同时保证点云结构的完整性，提高点云处理速度。
5、s2、利用3dsc算法进行特征描述子提取和特征描述子匹配。
6、s3、根据点云特点对近邻搜索半径和点云密度等参数进行优化，得到较好的矩阵估计。
7、s4、将粗配准矩阵估计传递给nicp算法，初始化nicp算法的初始齐次变换矩阵，提高配准精度。
8、s5、在nicp算法中，利用kd-tree加速最近邻搜索和点云匹配过程，减少计算时间。
9、s6、结合icp改进算法即nicp精配准算法进行矩阵优化。
10、进一步地，所述s1步骤具体为：
11、s11、将点云数据划分为规则的三维体素网格。每个体素格子覆盖一个固定的空间范围，范围为[v1 v2 v3]。v1,v2,v3分别为沿x,y,z的体素长度。
12、s12、对每个体素格子，选择其中一个点作为代表性点。常见的选择方式是选择格子中的中心点作为代表性点。
13、s13、基于得到的代表性点集合，使用高斯采样方法生成一组采样点。采样点的数量和密度可以根据高斯分布的方差参数进行控制。
14、s14、对于每个采样点，选择与之最近的原始点作为最终的下采样结果。
15、所述步骤s2具体为：
16、s21、针对点云数s，s是其中任意一点，n为s点的法向量l。以s为球心，以l方向为球的正北方向，建立起一个局部的球形空间。按照半径r、方位角α和仰角θ方向分别取j+1，k+1和l+1个分段。获得j*k*l个点云的直方图数据表示。半径分段方程如下：
17、
18、其中rmin和rmax是球形空间的边界。此外，需要对每一个球形空间赋值一个初始权重：
19、
20、其中v(j,k,l)是对应空间的体积，si是对应空间的密度。基于点的数目和半径来估计得到。合并w(si)值组成特征向量该向量包含查询点周围的形状上下文特征。
21、
22、其中，k＝{1,2,…,n},n＝j×k×l。
23、s22、源点云与目标点云利用局部空间描述子进行匹配，采用最近邻匹配策略，计算源点云和特征描述子目标点云特征描述子的欧式距离进行比较，距离最小的为特征描述子匹配对象，重复执行，完成特征描述子匹配。
24、所述步骤s4具体为：
25、s41、先根据粗配准变换替换nicp初始齐次矩阵，设源点云s中任意一点m(xm,ym,zm)，待配准点云o中任意一点n(xn,yn,zn)，且m、n属于点云的集合，通过nicp算法采用其次变换矩阵参数进一步求解两个点云的矩阵变换。初始矩阵变换如下：
26、
27、其中，tx,ty,tz是沿三个坐标轴的平移变量，w是缩放系数，默认是1，α,β,λ分别绕x,y,z轴的旋转角度。
28、s42、将3dsc粗配准得到的矩阵参数传递给nicp算法，作为初始齐次变换矩阵的更新值。
29、s43、设si为点云s的任意一个点，计算以其为中心附近vi的点的协方差公式：
30、
31、其中，μi是点云平均值。由于协方差矩阵具有对称性，进行svd分解得：
32、
33、特征根为λ1,λ2,λ3从小到大进行排序，最小的特征根为法向量方向。曲率为σi＝λ1/(λ1+λ2+λ3)。如果点云平整，协方差矩阵接近奇异，此时最小特征值记作ε。
34、nicp对两组点云的坐标和法向量进行变换，为p'＝(p,n)，变换规则如下：
35、
36、对点进行旋转和平移，对法向量进行旋转。误差函数是点之间的距离和法向量角度的误差和。
37、
38、其中用q表示参考点云的点，最终的误差函数为：
39、
40、其中包含了信息矩阵，平面的和法向量的平面矩阵采用协方差的逆，法向量矩阵在非奇异是采用单位阵，否则如下，
41、
42、优化求解采用l-m-f算法。
43、所述步骤s5具体为：
44、点云具有x,y,z三个维度，利用kd-tree可以进行多维度搜索，本算法采用的是半径搜索，先构建kd-tree，对x,y,z轴进行空间分隔，找到排序顺序的中间位置节点，小于中间位置的为左子节点，大于中间位置的为右子节点。循环执行，直到子节点中所包含的点数少于预设的阈值(leafsize)，向下搜索到查询点所在的叶子节点，算叶子节点中所有点与查询点的距离，将距离小于查询半径r的点记录；计算查询点与当前分隔轴之间的距离，向上回溯，如果r＞查询点到搜索平面的距离，则对子空间进行搜索，将距离小于r的点记录；否则不进行搜索。
45、所述步骤s6具体为：
46、s61、求解法向量，提取目标点周围半径为r的球体范围内的所有点的高斯分布的协方差矩阵。如果目标点领域内的点云表面是良定义的话，它们就可以近似为一个平面，并且协方差的特征值中仅有一个特征值远小于另外两个。那么这个最小的特征值对应的特征向量则被认为是这个点领域点云平面的法向量。
47、数学公式表达如下：设si为点云s的任意一个点，计算以其为中心附近vi的点的协方差公式：
48、
49、其中，μi是点云平均值。
50、s62.寻找匹配点对，匹配点对规则如下：
51、a)点对中的两个点至少一个没有良定义的法向量。
52、b)匹配点之间距离的阈值：
53、c)匹配点之间的曲率之差：|logσi-logσj|＞εd。
54、d)两点之间的法向量之间的角度若超过阈值，则采用向量差乘，根据投影判断夹角
55、s63.根据匹配对，寻找坐标变换。nicp对两组点云的坐标和法向量进行变换，为p'＝(p,n)，变换规则如下：
56、
57、对点进行旋转和平移，对法向量进行旋转。误差函数是点之间的距离和法向量角度的误差和。
58、
59、其中用q表示参考点云的点，最终的误差函数为：
60、
61、其中包含了信息矩阵，平面的和法向量的平面矩阵采用协方差的逆，法向量矩阵在非奇异是采用单位阵，否则如下：
62、
63、优化求解采用l-m-f算法，精配准初步完成。根据点云配准结果，调整参数和误差阈值进行优化。至此点云配准实例完成。本发明相比以往算法所带来的好处为：
64、1.在体素滤波阶段，划分体素格子之后，采用高斯采样，在降采样的同时，进一步强化了点云结构的完整性。
65、2.在3dsc阶段，采用3dsc算法进行特征描述子创建，相比pfh、fpfh等效果更好准确性更高。
66、3.在精配准阶段，采用了icp算法的改进版nicp，加入法向量的评判标准，实测情况下，计算时间没有较大变化的情况下，配准效果有所提升。
67、4.点云精配准的过程采用kd-tree近邻搜索，提高点云数据的处理速度，加快点云配准，提高计算能力。
1.一种基于3dsc和改进icp算法加速点云配准的方法，其特征在于，包括如下步骤：
2.根据权利要求1所述的方法，其特征在于，所述s1步骤具体为：
3.根据权利要求1所述的方法，其特征在于，所述步骤s2具体为：
4.根据权利要求1所述的方法，其特征在于，所述步骤s4具体为：
5.根据权利要求1所述的方法，其特征在于，所述步骤s5具体为：
6.根据权利要求1所述的方法，其特征在于，所述步骤s6具体为：
