-
三维点云配准的实质是将两个不同视角下获得的输入点云,通过求解其刚体变换得到旋转矩阵R和平移矢量T,整合到同一个坐标变换系下。
设目标点云为
$V = \left\{ {{v_n}^\prime \left| {n = 1,2, \cdots ,K} \right.} \right\}$ ,待配准点云为$U = \left\{ {{u_n}^\prime \left| {n = 1,2, \cdots ,K} \right.} \right\}$ 。在配准过程中,需要找到两个点云间对应的旋转矩阵R和平移矩阵T,使得U可以映射到V,由此可以转化为一个有约束的旋转矩阵R和平移矩阵T的最小二乘问题:$$F({\boldsymbol{R}},{\boldsymbol{T}}) = \frac{1}{K}\sum\limits_{n = 1}^K {{{\left\| {{v_n}^\prime - {\boldsymbol{R}}{u_n}^\prime - {\boldsymbol{T}}} \right\|}^2}} $$ (1) 设
$\overline v$ 和$\overline u$ 为两个点云的质心,则点云中相对于质心的坐标由${v_n} = {v_n}^\prime - \overline v,{u_n} = {u_n}^\prime - \overline u$ 得到,将${\boldsymbol T} = \overline v -{\boldsymbol R}\overline u$ 代入公式(1)(将源点云质心平移至目标点云),所以公式(1)可化简为:$$F({\boldsymbol{R}}) = \frac{1}{K}\sum\limits_{n = 1}^K {{{\left\| {{v_n} - {\boldsymbol{R}}{u_n}} \right\|}^2}} $$ (2) 传统点云配准算法根据公式(2)进行SVD分解,计算复杂、计算量巨大,而将问题引入几何代数空间可以解决这类缺点。文中所有推导都基于公式(2)进行。
-
几何代数空间中最重要的计算符为几何积,包含了几何代数中最基本的两种用于子空间构建的运算符:外积与内积,即:
$$ \left\{\begin{array}{l}{\text{几何积:}}{\boldsymbol{ab}}={\boldsymbol{a}}\cdot {\boldsymbol{b}}+{\boldsymbol{a}}\wedge {\boldsymbol{b}}\\ {\text{外积:}}{\boldsymbol{a}}\wedge {\boldsymbol{b}}=(\left|{\boldsymbol{a}}\right|\times \left|{\boldsymbol{b}}\right|{\rm sin}\theta ){\boldsymbol{i}}\\ {\text{内积:}}{\boldsymbol{a}}\cdot {\boldsymbol{b}}=(\left|{\boldsymbol{a}}\right|\times \left|{\boldsymbol{b}}\right|{\rm cos}\theta ){\boldsymbol{i}}\end{array} \right.$$ (3) 式中:a和
${\boldsymbol{b}}$ 为几何代数空间中两个元素;θ为a和${\boldsymbol{b}}$ 的夹角;i表示运算空间。外积运算对子空间进行升维,内积运算对子空间进行降维,几何积将两者相互融合,从而计算出维度混合的多重向量[12]。几何积中同时包含的内积和外积,类似于复数中的实部与虚部,符号“+”仅对不同维度的几何对象进行连接,并不进行任何其他操作。
n维几何代数空间Gn是欧氏空间Rn的几何拓展。一组正交基
${{\boldsymbol{e}}_1},{{\boldsymbol{e}}_2},{{\boldsymbol{e}}_3},\cdots\cdots,{{\boldsymbol{e}}_n}$ 可以构建一个n维几何代数空间Gn。例如,对R3中三个单位正交基向量${{\boldsymbol{e}}_1},{{\boldsymbol{e}}_2},{{\boldsymbol{e}}_3}$ ,由公式(3)有:$$\begin{array}{l} {{\boldsymbol{e}}_i}{{\boldsymbol{e}}_j} = {{\boldsymbol{e}}_i} \cdot {{\boldsymbol{e}}_j} + {{\boldsymbol{e}}_i} \wedge {{\boldsymbol{e}}_j} = {{\boldsymbol{e}}_i} \wedge {{\boldsymbol{e}}_j} \triangleq {{\boldsymbol{e}}_{ij}} ,\\ i = 1,2,3,j = 1,2,3 \\ {{\boldsymbol{e}}_1}{{\boldsymbol{e}}_2}{{\boldsymbol{e}}_3} = {{\boldsymbol{e}}_1} \wedge {{\boldsymbol{e}}_2} \wedge {{\boldsymbol{e}}_3} \triangleq {{\boldsymbol{e}}_{123}} \\ \end{array} $$ (4) 而eij=−eij,故由欧氏空间的单位向量基可扩张为一个23=8维的线性空间:
$${G^3} = \left\{ {1,{{\boldsymbol{e}}_1},{{\boldsymbol{e}}_2},{{\boldsymbol{e}}_3},{{\boldsymbol{e}}_{12}},{{\boldsymbol{e}}_{23}},{{\boldsymbol{e}}_{13}},{{\boldsymbol{e}}_{123}}} \right\}$$ (5) G3中最基本的元素为多重矢量M,形式为:
$$ \begin{split} {\boldsymbol{M}} =& {m_0} + {m_1}{{\boldsymbol{e}}_1} + {m_2}{{\boldsymbol{e}}_2} + {m_3}{{\boldsymbol{e}}_3} +\\ & {m_{12}}{{\boldsymbol{e}}_{12}} + {m_{23}}{{\boldsymbol{e}}_{23}} + {m_{13}}{{\boldsymbol{e}}_{13}} + {m_{123}}{{\boldsymbol{e}}_{123}} =\\ & {\left\langle {\boldsymbol{M}} \right\rangle _0} + {\left\langle {\boldsymbol{M}} \right\rangle _1} + {\left\langle {\boldsymbol{M}} \right\rangle _2} + {\left\langle {\boldsymbol{M}} \right\rangle _3} =\\ & \sum\limits_k {{{\left\langle {\boldsymbol{M}} \right\rangle }_k}} \\ \end{split} $$ (6) 式中:
${\left\langle {\boldsymbol{M}} \right\rangle _0} = {m_0}$ ,${\left\langle {\boldsymbol{M}} \right\rangle _1} = {m_1}{{\boldsymbol{e}}_1} + {m_2}{{\boldsymbol{e}}_2} + {m_3}{{\boldsymbol{e}}_3}$ ,${\left\langle {\boldsymbol{M}} \right\rangle _2} = {m_{12}}{{\boldsymbol{e}}_{12}} + $ $ {m_{23}}{{\boldsymbol{e}}_{23}} + {m_{13}}{{\boldsymbol{e}}_{13}}$ ,${\left\langle {\boldsymbol{M}} \right\rangle _3} = {m_{123}}{{\boldsymbol{e}}_{123}}$ 。${\left\langle {\boldsymbol{M}} \right\rangle _k}$ 为k阶片积,其逆为:$${\tilde {\boldsymbol{M}}_k} = {( - 1)^{\frac{1}{2}k(k - 1)}}{\left\langle {\boldsymbol{M}} \right\rangle _k}$$ (7) 来自公式(6)一个多重向量M可以被表示为不同等级的片积组合,因而可计算其逆为
$\tilde {\boldsymbol{M}} = {\left\langle {\boldsymbol{M}} \right\rangle _0} + {\left\langle {\boldsymbol{M}} \right\rangle _1} - $ $ {\left\langle {\boldsymbol{M}} \right\rangle _2} - {\left\langle {\boldsymbol{M}} \right\rangle _3}$ 。在几何代数空间中两个多重向量A和B的标量积
$ * $ 为:$${\boldsymbol{A}} * {\boldsymbol{B}} = {\left\langle {{\boldsymbol{AB}}} \right\rangle _0}$$ (8) 因此一个多重向量M的标量积为:
$${\left| {\boldsymbol{M}} \right|^2} = {\boldsymbol{M}} * \tilde {\boldsymbol{M}} = \sum\limits_k {\left| {\boldsymbol{M}} \right|_k^2} $$ (9) -
为解决参考文献[8]中GA-LMS算法在点云配准过程中收敛速度较慢这个问题,文中将用于多维信号处理的GA-NLMS用于点云配准,以加快点云配准的收敛速度。
欧氏空间中NLMS是最小均方算法的一种变体,意在加快收敛速度[13],迭代公式为:
$$h(n + 1) = h(n) + \left( {\frac{{\mu e(n)x(n)}}{{{x^H}(n)x(n)}}} \right) = h(n) + \mu \frac{{e(n)x(n)}}{{{{\left\| {x(n)} \right\|}^2}}}$$ (10) 式中:
$e(n) = d(n) - {h^H}(n)x(n)$ ,是期望响应$d(n)$ 和滤波输出信号${h^H}(n)x(n)$ 之间的误差函数。NLMS依据输入信号$x(n)$ 在迭代过程中估计梯度矢量,并更新权系数$h(n)$ ,以达到最优的自适应迭代。为防止
$x(n)$ 过小而导致步长过大,可以加入稳定因子$\;\beta $ 来调控,公式(10)可修改为:$$h(n + 1) = h(n) + \mu \frac{{e(n)x(n)}}{{\left( {\beta + {{\left\| {x(n)} \right\|}^2}} \right)}}$$ (11) $\beta $ 通常被设定为0或1,文中对两种取值都进行仿真实验,以讨论不同取值对于点云配准问题的影响。文中将上述分析引入G3,将目标点云及待配准点云中的三维点vn、un化为几何代数形式:
$$\begin{array}{l} {v_n} = {x_{{v_n}}} * {{\boldsymbol{e}}_1} + {y_{{v_n}}} * {{\boldsymbol{e}}_2} + {{\textit{z}}_{{v_n}}} * {{\boldsymbol{e}}_3} \\ {u_n} = {x_{{u_n}}} * {{\boldsymbol{e}}_1} + {y_{{u_n}}} * {{\boldsymbol{e}}_2} + {{\textit{z}}_{{u_n}}} * {{\boldsymbol{e}}_3} \\ \end{array} $$ (12) 则公式(2)中旋转操作
${\boldsymbol{R}}{u_n}$ 在几何代数空间中可利用rotor转子r表示为:${\boldsymbol{R}}{u_n} = {\boldsymbol{r}}{u_n}\tilde {\boldsymbol{r}}$ ,其中${\boldsymbol{r}}\tilde {\boldsymbol{r}} = \tilde {\boldsymbol{rr}} = $ $ {\left| {\boldsymbol{r}} \right|^2} = 1$ 。将vn看作$d(n)$ ,${\boldsymbol{r}}{u_n}\tilde {\boldsymbol{r}}$ 看作${h^H}(n)x(n)$ ,则:$$e(n) = {v_n} - {\boldsymbol{r}}{u_n}\tilde {\boldsymbol{r}}$$ (13) 因此公式(2)中的函数
$F\left( {\boldsymbol{R}} \right)$ 可以变形为新的代价函数:$$J({\boldsymbol{r}}) = \frac{1}{K}\sum\limits_{n = 1}^K {e(n) * \tilde e(n)} {\rm{ = }}\frac{1}{K}\sum\limits_{n = 1}^K {\left\langle {e(n)\tilde e(n)} \right\rangle } {\rm{ = }}\frac{1}{K}\sum\limits_{n = 1}^K {{{\left| {e(n)} \right|}^2}} $$ (14) 从而将三维点云配准问题模拟为信号滤波问题,构造新的迭代公式来解决点云配准问题。设rn为第n次迭代计算出的转子,将公式(13)代入公式(14)可得:
$$ \begin{split} J({{\boldsymbol{r}}_n}) =& \dfrac{1}{K}\sum\limits_{n = 1}^K {{{({v_n} - {{\boldsymbol{r}}_n}{u_n}{{\tilde {\boldsymbol{r}}}_n})}^2}} =\\ & \dfrac{1}{K}\sum\limits_{n = 1}^K {\left( {{{\left| {{v_n}} \right|}^2} + {{\left| {{u_n}} \right|}^2} + 2{{\left\langle {{v_n}{{\boldsymbol{r}}_n}{u_n}{{\tilde {\boldsymbol{r}}}_n}} \right\rangle }_0}} \right)} \\ \end{split} $$ (15) 为保证每次迭代代价函数
$J({{\boldsymbol{r}}_n})$ 都在减小,由梯度下降法及常规NLMS递归公式(11)可以得到如下迭代公式对转子进行估计:$${{\boldsymbol{r}}_{n + 1}} = {{\boldsymbol{r}}_n} + \mu \frac{{{\partial _r}J({{\boldsymbol{r}}_n})}}{{\beta + {{\left\| {{u_n}} \right\|}^2}}}$$ (16) 式中:
${\partial _r}J({{\boldsymbol{r}}_n}) = e(n)x(n)$ ;${\partial _r}$ 是几何代数中关于r求偏导的微分算子,可以发现这是对标量积的求导。利用其对称性和重新排序的属性[14]进行计算:$$ \begin{split} {\partial _r}J({{\boldsymbol{r}}_n}) =& \dfrac{2}{K}{\sum\limits_{i = 1}^K {{\partial _r}\left\langle {{v_n}{{\boldsymbol{r}}_n}{u_n}{{\tilde {\boldsymbol{r}}}_n}} \right\rangle } _0} = \\ & \dfrac{2}{K}\sum\limits_{n = 1}^K {\left( {\partial _r}\left[ {{{\dot {\boldsymbol{r}}}_n} * \left( {{u_n}{{\tilde {\boldsymbol{r}}}_n}{v_n}} \right)} \right] +{\partial _r}\left[ {{{\dot {\tilde {\boldsymbol{r}}}}_n} * \left( {{v_n}{{\boldsymbol{r}}_n}{u_n}} \right)} \right] \right)} =\\ & \dfrac{2}{K}\sum\limits_{n = 1}^K {\left( {{u_n}{{\tilde {\boldsymbol{r}}}_n}{v_n} - {{\tilde {\boldsymbol{r}}}_n}\left( {{v_n}{{\boldsymbol{r}}_n}{u_n}} \right){{\tilde {\boldsymbol{r}}}_n}} \right)} =\\ & \dfrac{2}{K}{{\tilde {\boldsymbol{r}}}_n}\sum\limits_{n = 1}^K {\left( {({{\boldsymbol{r}}_n}{u_n}{{\tilde {\boldsymbol{r}}}_n}){v_n} - {v_n}({{\boldsymbol{r}}_n}{u_n}{{\tilde {\boldsymbol{r}}}_n})} \right)} \\ \end{split} $$ (17) 又因为关于两个向量的外积存在公式:
${\boldsymbol{x}} \wedge {\boldsymbol{y}} = $ $ \dfrac{1}{2}\left( {{\boldsymbol{xy}} - {\boldsymbol{yx}}} \right)$ ,则公式(17)可化简为:$${\partial _r}J({{\boldsymbol{r}}_n}) = \frac{4}{K}{\tilde {\boldsymbol{r}}_n}\sum\limits_{n = 1}^K {{v_n} \wedge ({{\boldsymbol{r}}_n}{u_n}{{\tilde {\boldsymbol{r}}}_n})} $$ (18) 公式(18)作为几何代数框架下的公式,是一个完全的基于几何代数的梯度公式,使得点云配准问题可以在几何代数空间解决。由公式(17)可发现
${\partial _r}J({{\boldsymbol{r}}_n})$ 中自变量变为rn,故将归一化因子统一为rn,公式(16)变为:$${{\boldsymbol{r}}_{n + 1}} = {{\boldsymbol{r}}_n} + \mu \frac{{{\partial _r}J({{\boldsymbol{r}}_n})}}{{\beta + {{\left\| {{{\boldsymbol{r}}_n}} \right\|}^2}}}$$ (19) 将公式(18)代入迭代公式(19)可得:
$${{\boldsymbol{r}}_n} = {{\boldsymbol{r}}_{n - 1}} + \mu \dfrac{{\dfrac{4}{k}\left[ {\displaystyle\sum\limits_{n = 1}^k {{v_n} \wedge ({{\boldsymbol{r}}_{n - 1}}{u_n}{{\tilde {\boldsymbol{r}}}_{n - 1}})} } \right]{{\boldsymbol{r}}_{n - 1}}}}{{\beta + {{\left\| {{{\boldsymbol{r}}_{n - 1}}} \right\|}^2}}}$$ (20) 式中:
$k$ 代表了算法中每次迭代用于计算的点云中的对应点对数,k∈[1,K]。当k=K时,每次迭代计算使用了所有的K对特征对应点,k=1时,每次迭代只使用一对对应特征点,这时公式(20)可化简为:$${{\boldsymbol{r}}_n} = {{\boldsymbol{r}}_{n - 1}} + \mu \frac{{\left[ {{v_n} \wedge ({{\boldsymbol{r}}_{n - 1}}{u_n}{{\tilde {\boldsymbol{r}}}_{n - 1}})} \right]{{\boldsymbol{r}}_{n - 1}}}}{{\beta + {{\left\| {{{\boldsymbol{r}}_{n - 1}}} \right\|}^2}}}$$ (21) 由公式(21),采用几何代数解决点云配准问题,每次迭代只采用一个点对构造代价函数,相比传统采用所有点对进行计算的ICP方法,计算量更小,无须对点云数据进行预处理,用最少的计算成本可以得到更优的结果。
-
通过对GA-NLMS进行仿真实验可以发现,当调节因子
$\;\beta $ 为0时,GA-NLMS相较于GA-LMS在解决点云配准问题时有更快的收敛速度,但收敛后波形仍然有波动,这说明虽然收敛速度变快但稳态误差变大。当调节因子$\;\beta $ 为1时,虽然收敛后波形波动有所调节,稳态误差变小,但是收敛速度相较于$\;\beta {\rm{ = 0}}$ 时变慢。分析原因可以发现,NLMS本质上是一个变步长的归一化均方算法,根据原LMS算法中误差信号与远端输入信号的乘积,对远端输入信号的欧氏范数进行归一化处理,将固定步长因子的LMS算法变为根据输入信号时变的变步长算法。所以当加入稳定因子$\;\beta $ 后,每次迭代的瞬时误差变小,导致步长值变小,从而收敛速度变慢。当稳定因子$\;\beta {\rm{ = 0}}$ 时,每次迭代瞬时误差变大,步长值变大,在初期能通过较大的步长快速达到收敛,但在收敛后由于步长值过大,导致稳态误差变大,容易产生误配准状态。为协调收敛速度与稳态误差之间的冲突,研究者们设计了许多改进算法,覃景繁等[15]发现 Sigmoid 函数对于步长因子具有良好的调节效果,并证明了迭代步长是代价函数的 Sigmoid 函数,将这种算法命名为Sigmoid函数变步长LMS算法(SVSLMS)。但在误差向量接近零时,迭代步长不具有缓慢变化的特性,变化剧烈。因此文中结合NLMS与SVS-LMS两者的优势,提出基于几何代数的Sigmoid函数变步长NLMS算法(GA-SVSNLMS)解决点云配准问题,以改善GA-NLMS收敛速度快但稳态误差过大的问题。
基于Sigmoid函数,公式(21)中步长
$\mu $ 的迭代表达式为:$$\mu (n) = \gamma \left[ {\frac{1}{{1 + \exp ( - \alpha \left| {e(n)} \right|)}} - 0.5} \right]$$ (22) 式中:
$\alpha ,\gamma > 0$ 。通过当前的误差情况调整式(21)中的步长,能够做到在初期迭代时使用较大步长加快收敛速度,在接近收敛时使用较小的步长减小稳态误差。因此将Sigmoid函数作为步长迭代公式加入GA-NLMS,可以解决收敛过后稳态误差过大的问题。在几何代数空间中误差估计
${e_n} = {v_n} - r{u_n}\tilde r$ 无法被表达为标量,为此引入内积的另一个几何意义,即距离度量。在几何代数中,为定义两个点之间的度量性质,引入了内积运算,对于矢量a和b的内积,线性代数与欧氏空间是等价的,即为一个标量。在三维空间中,两个点的内积可以视为两点的距离,则公式(22)可写为:$$\mu (n) = \gamma \left[ {\frac{1}{{1 + \exp ( - \alpha \left| {{v_n} \cdot {{\boldsymbol{r}}_{n - 1}}{u_n}{{\tilde {\boldsymbol{r}}}_{n - 1}}} \right|)}} - 0.5} \right]$$ (23) 而GA-SVSNLMS的rotor转子迭代公式可以写为:
$${{\boldsymbol{r}}_n} = {{\boldsymbol{r}}_{n - 1}} + \mu (n)\frac{{\left[ {{v_n} \wedge ({{\boldsymbol{r}}_{n - 1}}{u_n}{{\tilde {\boldsymbol{r}}}_{n - 1}})} \right]{{\boldsymbol{r}}_{n - 1}}}}{{{{\left\| {{{\boldsymbol{r}}_{n - 1}}} \right\|}^2}}}$$ (24) 2.2节与2.3节所提算法的流程相近,只是迭代公式不同,故两种算法的流程图可归纳为图1。
-
为充分证明基于几何代数的NLMS与SVS-NLMS点云配准算法对三维点云模型配准尺度和精度的有效性,验证算法的收敛速度,使算法具有普遍性,采用了两个不同的三维点云数据进行实验。数据一来自于参考文献[8],为两个同等大小的立方体,逐点匹配;数据二来自斯坦福大学提供的Bunny三维点云数据。该仿真实验在python环境下,因特尔六核处理器E5-2620和英伟达K2200硬件条件下完成。
GA-NLMS、GA-SVSNLMS中的几何代数部分运算是使用python中的Clifford模块实现,该模块可以将欧氏空间的点转换为几何代数空间形式,并进行内积、外积和几何积等基本运算。对于所有仿真,rotor转子的初始值均设定为:
$${\boldsymbol{r}} = 0.5 + 0.5{{\boldsymbol{e}}_{12}} + 0.5{{\boldsymbol{e}}_{23}} + 0.5{{\boldsymbol{e}}_{13}}$$ (25) -
数据集一为人造数据集立方体:边长0.5 m,点云数目为1728个,目标点云与操作点云之间分别绕x, y, z轴旋转120°,90°,45°的角度,如图2(a)所示,此数据的两个点云特征点逐一匹配,仿真分别采用ICP、SAC-IA+ICP、GA-LMS、GA-NLMS、GA-SVSNLMS对数据集进行配准,图2(b)为SAC-IA+ICP配准结果,图2(c)为GA-SVSNLMS配准结果(由于几何代数空间下精度几乎相同,只是收敛速度与稳态误差有区别,只展示GA-SVSNLMS结果),其中GA-NLMS分为调节因子
$\;\beta $ 为0或1两种情况。从图2(b)可以看出:传统基于SVD分解的ICP算法配准虽然有一定效果,但由于该算法对于点云数据的初始值要求相当高,收敛性过度依赖初始位姿,所以无法对立方体达到一个精度较高的配准效果。从图2(c)可以看出两个点云几乎完全配准,精度很高。表1为五个配准算法的精度对比,评价标准为对应点之间的均方根误差(Root Mean Square Error, RMSE)。由表1可看出,当点云初始位姿较差时,ICP算法精度为10−2数量级,经过SAC-IA粗配准调整位姿后的ICP算法虽然精度有所提升,但效果一般,依然陷入局部最优。而基于几何代数空间的算法都可以达到10−8数量级,精度较高。图 2 立方体数据集仿真实验结果。(a)原始数据;(b) SAC-IA+ICP算法配准结果;(c) GA-SVSNLMS算法配准结果
Figure 2. Experiment results of cube data set simulation. (a) Raw data; (b) Result of SAC-IA+ICP algorithm registration; (c) Result of GA-SVSNLMS algorithm registration
表 1 各算法的运行精度与收敛速度
Table 1. Running accuracy and convergence speed of different algorithms
Algorithm RMSE/mm Convergence speed/times ICP ${\rm{2}}.{\rm{5079}} \times {10^{ - 2}}$ 1000 SAC-IA+ICP ${\rm{2}}.2{\rm{483}} \times {10^{ - 2}}$ 1000 GA-LMS $1.2617 \times {10^{ - 8}}$ 750 GA-NLMS($\;\beta = 0$) $1.2684 \times {10^{ - 8}}$ 140 GA-NLMS($\;\beta = {\rm{1}}$) $1.2679 \times {10^{ - 8}}$ 175 GA-SVSNLMS $1.8852 \times {10^{ - 8}}$ 70 图3(a)为几何代数空间每次迭代误差函数的曲线,为使曲线更为直观,将RMSE单位由mm变为dB,即
$10\log 10({\rm{RMSE}})$ 。可以看出,随着迭代次数的增加,误差函数逐渐减小直至收敛,说明该算法可以将点云逐步配准,而不是整体计算。每次迭代只使用一对匹配点,计算代价更小。而传统ICP算法使用所有的特征点对进行迭代,算法更偏向于对点云整体进行计算,计算量增大,算法复杂度增加,导致迭代时间变长。图 3 立方体数据集几何代数空间各算法收敛曲线。(a)误差函数收敛曲线;(b)代价函数收敛曲线
Figure 3. Convergence curves of each algorithm in geometric algebraic space of cube dataset. (a) Convergence curves of error function; (b) Convergence curves of cost function
图3(b)为代价函数曲线图,即每次旋转后操作点云与目标点云之间的RMSE,单位为dB,通过图3(b)与表1可以看出,GA-LMS收敛速度最慢,在迭代到750次的时候达到收敛。其次为GA-NLMS算法,调节因子
$\;\beta = 1$ 时,在迭代到175次左右收敛,而调节因子$\;\beta = 0$ 时,在140次左右收敛。但从图中可以看出,当调节因子为0时,收敛后稳态误差较大,波形有较大起伏,调节因子为1时,稳态误差较小,波形几乎没有起伏,造成这种情况的原因是,当调节因子为0时,由于归一化因子较小,导致步长较大,开始迭代时能够加速收敛,但当收敛后,由于步长较大所以容易产生过度旋转的情况,导致稳态误差较大。当调节因子为1时,解决了算法分母部分变大导致步长变小,虽然收敛速度变慢,但当收敛后由于步长较小,可以保持较小的稳态误差。收敛速度最快的是GA-SVSNLMS,在迭代70次达到收敛,且收敛后波形抖动相较于GA-NLMS有所缓解,有较小的稳态误差,由于引入sigmoid函数,可以在初期开始迭代时步长较大,达到较快的收敛速度,在进入收敛后调整步长变小,以达到较小的稳态误差。由表1和图4可知,在同样配准误差精度的情况下,GA-SVSNLMS在保持较快的迭代收敛速度的同时还能保持较好的稳态误差,而GA-NLMS虽然收敛速度优于GA-LMS,但是,当调节因子为0时,稳态误差较差,当调节因子为1时,虽然保持了良好的稳态误差,但收敛速度又不够快。
-
样本2 为斯坦福大学bunny点云:点云数目为778个,目标点云与操作点云之间绕z轴相对旋转45度角度,两个点云初始位姿如图4(a)所示。通过参考文献[16]的方法可得到目标点云与操作点云之间共191对匹配点对,利用匹配的点进行误差函数构建,逐一迭代191次。与立方体数据一样,仿真分别采用ICP、SAC-IA+ICP、GA-LMS、GA-NLMS、GA-SVSNLMS对数据集进行配准,图4(b)为SAC-IA+ICP配准结果,图4(c)为GA-NLMS配准结果。可以看出,bunny点云数据通过传统ICP方法与几何代数方法都达到了一定的配准效果,只是配准误差精度有所不同,表2为六个配准算法的精度对比,评价标准为对应点之间的均方根误差。
表 2 各算法的运行精度与收敛速度
Table 2. Running accuracy and convergence speed of different algorithms
Algorithm RMSE/mm Convergence speed/times ICP ${\rm{5}}.{\rm{4046}} \times {10^{ - {\rm{3}}}}$ 200 SAC-IA+ICP ${\rm{4}}.{\rm{4687}} \times {10^{ - {\rm{3}}}}$ 200 GA-LMS ${\rm{2}}{\rm{.9595}} \times {10^{ - {\rm{3}}}}$ 210 GA-NLMS($\;\beta = 0$) ${\rm{2}}{\rm{.9443}} \times {10^{ - {\rm{3}}}}$ 55 GA-NLMS($\;\beta = {\rm{1}}$) ${\rm{2}}{\rm{.7620}} \times {10^{ - {\rm{3}}}}$ 85 GA-SVSNLMS ${\rm{2}}{\rm{.6658}} \times {10^{ - {\rm{3}}}}$ 40 由表2可以看出,六种算法对于bunny数据集的算法精度都为10−3数量级,但基于几何代数空间的算法相较于SAC-IA+ICP算法精度均提升了25%以上,其中GA-SVSNLMS算法提升精度最高,达到35%。
图5(a)为对bunny点云数据使用GA-SVSNLMS算法的误差函数曲线图与代价函数曲线图,可以看出,该数据集利用几何代数空间算法进行逐点旋转配准,通过代价函数曲线可以看出,随着逐渐旋转,两个点云逐渐配准。由图5(b)与表2可以看出,GA-LMS算法迭代速度最慢,直至匹配点对遍历完成时才达到收敛,分析原因为步长固定导致的,变步长的GA-NLMS算法比GA-LMS算法收敛速度有所提升,但和立方体数据集所显示的结果相同,调节因子为1时,由于步长问题,收敛速度要小于调节因子为0的GA-NLMS算法,在85次迭代时才开始收敛,后者只使用了55对匹配点对就达到了收敛,但稳态误差前者优于后者。收敛速度最快的为GA-SVSNLMS,只使用了40对匹配点对就达到了收敛,同时保持了较好的稳态误差。
图 5 bunny数据集几何代数空间各算法收敛曲线。(a) GA-SVSNLMS误差函数曲线与代价函数曲线;(b) 几何空间各算法代价函数收敛曲线
Figure 5. Convergence curves of each algorithm in geometric algebraic space of bunny dataset. (a) Error function curve and cost function curve of GA-SVSNLMS; (b) Cost function convergence curve of each algorithm in geometric algebraic space
结合表2与图5可发现,同样的配准精度下,GA-NLMS收敛速度优于GA-LMS接近一倍,仅使用了一半左右的匹配点对即可达到收敛,而GA-SVSNLMS在保证收敛速度的同时也达到了最大的精度。
-
为验证所提算法的鲁棒性,分别向cube,bunny点云数据添加σ=0.003的概率密度函数为
$N(0,{\sigma ^2})$ 的高斯噪声,得到高斯噪声点云数据。图6为加入噪声后两种数据集目标点云与待配准点云对比以及欧氏空间算法与几何代数空间算法对比。由表3可得,在加入高斯噪声后,各算法运行精度都有所下降,其中cube数据集精度下降最多,分析原因为cube数据集的特征点为逐一对应,加入噪声后所有匹配点均有偏离,导致精度下降。与传统ICP算法和SAC-IA+ICP算法对比,几何代数空间下的点云配准算法,在保持原有的收敛速度下,仍达到了比传统欧式空间点云算法更高的配准精度。实验结果表明,在加入噪声的情况下,GA-NLMS与GA-SVSNLMS仍可以用较少的匹配点对达到比ICP算法更好的配准效果。图 6 高斯噪声下各数据集仿真实验结果。(a) cube原始数据;(b) cube SAC-IA+ICP算法配准结果;(c) cube GA-SVSNLMS算法配准结果;(d) bunny原始数据;(e) bunny SAC-IA+ICP算法配准结果;(f) bunny GA-SVSNLMS算法配准结果
Figure 6. Simulation experiment results of each data set under Gaussian noise. (a) Raw data of cube; (b) Result of SAC-IA+ICP algorithm registration of cube; (c) Result of GA-SVSNLMS algorithm registration of cube; (d) Raw data of bunny; (e) Result of SAC-IA+ICP algorithm registration of bunny; (f) Result of GA-SVSNLMS algorithm registration of bunny
表 3 高斯噪声下各算法的运行精度与收敛速度
Table 3. Running accuracy and convergence speed of different algorithms under Gaussian noise
Algorithm cube bunny RMSE/mm Convergence speed/times RMSE/mm Convergence speed/times ICP ${\rm{2}}.{\rm{284\;7}} \times {10^{ - {\rm{1}}}}$ 1000 ${\rm{1}}{\rm{.286\;4}} \times {10^{ - 2}}$ 200 SAC-IA+ICP ${\rm{3}}{\rm{.328\;6}} \times {10^{ - 2}}$ 1000 ${\rm{6}}{\rm{.398\;4}} \times {10^{ - {\rm{3}}}}$ 200 GA-LMS ${\rm{5}}{\rm{.227\;6}} \times {10^{ - {\rm{3}}}}$ 750 ${\rm{6}}{\rm{.015\;9}} \times {10^{ - {\rm{3}}}}$ 210 GA-NLMS($\;\beta = 0$) ${\rm{5}}{\rm{.603\;7}} \times {10^{ - {\rm{3}}}}$ 140 ${\rm{6}}{\rm{.019\;4}} \times {10^{ - {\rm{3}}}}$ 55 GA-NLMS($\;\beta = {\rm{1}}$) ${\rm{5}}{\rm{.576\;8}} \times {10^{ - {\rm{3}}}}$ 175 ${\rm{6}}{\rm{.005\;6}} \times {10^{ - {\rm{3}}}}$ 85 GA-SVSNLMS ${\rm{5}}{\rm{.237\;7}} \times {10^{ - {\rm{3}}}}$ 70 ${\rm{5}}{\rm{.841\;5}} \times {10^{ - {\rm{3}}}}$ 40 由上述三个实验可发现,立方体点云数据集一共1728个点,所有点均为匹配点,匹配点占比为100%,该点云配准精度可达10−8数量级,精度较高。而bunny点云数据集一共778个点,匹配点数为191个,占比25%,配准精度为10−3数量级。分析原因可知,虽然基于几何代数的点云配准算法可以利用较少的计算代价达到较好的配准效果,但对于匹配点的质量有所要求。由于立方体数据集形状规则所限,所以匹配点中的局外点(outliers)较少,错误匹配较少。而bunny数据集形状不规则,数据中局外点较多,匹配点对中存在错误匹配,故匹配精度相较于立方体数据集较差。但随着匹配点对的增多可发现算法收敛速度随之下降,立方体数据集的收敛速度相较于bunny数据集各算法均慢50%以上。
文中实验所采用的数据为小样本数据集,实现了在几何代数空间下结合sigmoid函数对点云实现配准功能,在使用真实数据集进行验证时,由参考文献[17]可知,由于激光雷达扫描后,空间点云数量过大且存在误差,需要进行降采样滤波,滤波后点云从十万数量级变为几千个点,文中所使用bunny数据集即为降采样处理后从106数量级变为102数量级。之后可通过SHOT描述子对两个点云进行特征匹配,继而进行几何代数空间下的点云配准。真实数据与文中使用数据不同点为需要提前进行数据预处理,处理后,点云数量级和配准流程与文中实验相同。真实数据中容易遇见的最大问题为数据噪声过高无法进行高精度配准,故文中对算法抗噪性进行了验证。GA-SVSNLMS算法具有较好的鲁棒性。
SVS-NLMS point cloud registration algorithm based on geometric algebra
-
摘要: 针对欧氏空间点云配准方法匹配精度低、计算成本大、收敛速度慢等问题,利用几何代数对于高维空间的表达能力,提出一种基于几何代数的点云配准算法。首先,将点云数据转化为几何代数形式,基于几何代数的rotor转子,给出了几何代数空间点云配准的代价函数。其次,结合归一化最小均方算法,将求解rotor转子模拟为信号滤波问题,在几何代数空间基于最速下降法构建rotor转子迭代公式,使每次迭代计算仅使用一对匹配点对而不是全部点对。迭代计算得到的转子可用于任意维度的旋转估计问题,从而将三维点云逐步旋转配准。最后,为进一步解决收敛速度与稳态误差之间的冲突,利用Sigmoid函数给出了一种变步长的rotor转子迭代公式,在加快收敛速度的同时降低稳态误差。采用模型数据集与公共数据集验证所提算法的配准性能,与经典迭代最近点算法相比,模型数据集的配准精度由10−2提升至10−8数量级,公共数据集的配准精度提升35%,所提算法收敛速度更快,配准精度更高,且具有较低的稳态误差。Abstract: To address the problems of low matching accuracy, high computational cost and slow convergence speed of point cloud registration methods in Euclidean space, a point cloud registration algorithm based on geometric algebra was proposed by using geometric algebra’s expressive power for high dimensional space. Firstly, the point cloud data was transformed into geometric algebraic form, and based on the rotor of geometric algebra, the cost function of point cloud registration in geometric algebra space was given. Secondly, combined with the normalized least mean square algorithm, the solution of the rotor was simulated as a signal filtering problem, and the rotor iteration formula was constructed based on the steepest descent method in the geometric algebraic space, so that only one points pair instead of all point pairs was used for each calculation. The rotor obtained by iterative calculation could be used for any dimensional rotation estimation problem, so that the three-dimensional point cloud was gradually rotated and registered. Finally, in order to further optimize the conflict between the convergence speed and the steady-state error, a variable-step rotor iteration formula was given by using the Sigmoid function, which can speed up the convergence speed while reducing the steady-state error. The registration performance of the proposed algorithm was verified by using the model data set and the public data set. Compared with the classical iterative closest point algorithm, the registration accuracy of the model data set is increased from 10−2 to 10−8 orders of magnitude, and the registration accuracy of the public data set is increased by 35%. The proposed algorithm has faster convergence speed, higher registration accuracy and lower steady-state error.
-
Key words:
- geometric algebra /
- point cloud registration /
- rotor /
- normalized least mean square
-
图 5 bunny数据集几何代数空间各算法收敛曲线。(a) GA-SVSNLMS误差函数曲线与代价函数曲线;(b) 几何空间各算法代价函数收敛曲线
Figure 5. Convergence curves of each algorithm in geometric algebraic space of bunny dataset. (a) Error function curve and cost function curve of GA-SVSNLMS; (b) Cost function convergence curve of each algorithm in geometric algebraic space
图 6 高斯噪声下各数据集仿真实验结果。(a) cube原始数据;(b) cube SAC-IA+ICP算法配准结果;(c) cube GA-SVSNLMS算法配准结果;(d) bunny原始数据;(e) bunny SAC-IA+ICP算法配准结果;(f) bunny GA-SVSNLMS算法配准结果
Figure 6. Simulation experiment results of each data set under Gaussian noise. (a) Raw data of cube; (b) Result of SAC-IA+ICP algorithm registration of cube; (c) Result of GA-SVSNLMS algorithm registration of cube; (d) Raw data of bunny; (e) Result of SAC-IA+ICP algorithm registration of bunny; (f) Result of GA-SVSNLMS algorithm registration of bunny
表 1 各算法的运行精度与收敛速度
Table 1. Running accuracy and convergence speed of different algorithms
Algorithm RMSE/mm Convergence speed/times ICP ${\rm{2}}.{\rm{5079}} \times {10^{ - 2}}$ 1000 SAC-IA+ICP ${\rm{2}}.2{\rm{483}} \times {10^{ - 2}}$ 1000 GA-LMS $1.2617 \times {10^{ - 8}}$ 750 GA-NLMS( $\;\beta = 0$ )$1.2684 \times {10^{ - 8}}$ 140 GA-NLMS( $\;\beta = {\rm{1}}$ )$1.2679 \times {10^{ - 8}}$ 175 GA-SVSNLMS $1.8852 \times {10^{ - 8}}$ 70 表 2 各算法的运行精度与收敛速度
Table 2. Running accuracy and convergence speed of different algorithms
Algorithm RMSE/mm Convergence speed/times ICP ${\rm{5}}.{\rm{4046}} \times {10^{ - {\rm{3}}}}$ 200 SAC-IA+ICP ${\rm{4}}.{\rm{4687}} \times {10^{ - {\rm{3}}}}$ 200 GA-LMS ${\rm{2}}{\rm{.9595}} \times {10^{ - {\rm{3}}}}$ 210 GA-NLMS( $\;\beta = 0$ )${\rm{2}}{\rm{.9443}} \times {10^{ - {\rm{3}}}}$ 55 GA-NLMS( $\;\beta = {\rm{1}}$ )${\rm{2}}{\rm{.7620}} \times {10^{ - {\rm{3}}}}$ 85 GA-SVSNLMS ${\rm{2}}{\rm{.6658}} \times {10^{ - {\rm{3}}}}$ 40 表 3 高斯噪声下各算法的运行精度与收敛速度
Table 3. Running accuracy and convergence speed of different algorithms under Gaussian noise
Algorithm cube bunny RMSE/mm Convergence speed/times RMSE/mm Convergence speed/times ICP ${\rm{2}}.{\rm{284\;7}} \times {10^{ - {\rm{1}}}}$ 1000 ${\rm{1}}{\rm{.286\;4}} \times {10^{ - 2}}$ 200 SAC-IA+ICP ${\rm{3}}{\rm{.328\;6}} \times {10^{ - 2}}$ 1000 ${\rm{6}}{\rm{.398\;4}} \times {10^{ - {\rm{3}}}}$ 200 GA-LMS ${\rm{5}}{\rm{.227\;6}} \times {10^{ - {\rm{3}}}}$ 750 ${\rm{6}}{\rm{.015\;9}} \times {10^{ - {\rm{3}}}}$ 210 GA-NLMS( $\;\beta = 0$ )${\rm{5}}{\rm{.603\;7}} \times {10^{ - {\rm{3}}}}$ 140 ${\rm{6}}{\rm{.019\;4}} \times {10^{ - {\rm{3}}}}$ 55 GA-NLMS( $\;\beta = {\rm{1}}$ )${\rm{5}}{\rm{.576\;8}} \times {10^{ - {\rm{3}}}}$ 175 ${\rm{6}}{\rm{.005\;6}} \times {10^{ - {\rm{3}}}}$ 85 GA-SVSNLMS ${\rm{5}}{\rm{.237\;7}} \times {10^{ - {\rm{3}}}}$ 70 ${\rm{5}}{\rm{.841\;5}} \times {10^{ - {\rm{3}}}}$ 40 -
[1] Besl P J, Mckay N D. A method for registration of 3-D Shapes [J]. Transactions on Pattern Analysis and Machine Intelligence, 1992, 14(2): 239-256. doi: 10.1109/34.121791 [2] Vlaminck M, Luong H, Philips W. Multi-resolution ICP for the efficient registration of point clouds based on octrees[C]//2017 Fifteenth IAPR International Conference on Machine Vision Applications (MVA). New York: IEEE, 2017: 17047517. [3] Wang Z F, Yan B, Tong L, et al. Normal estimate method of point clouds based on adaptive neighbor size [J]. Infrared and Laser Engineering, 2014, 43(4): 1322-1326. (in Chinese) doi: 10.3969/j.issn.1007-2276.2014.04.054 [4] Wang S, Sun H Y, Guo H C, et al. Overlapping region extraction method for laser point clouds registration [J]. Infrared and Laser Engineering, 2017, 46(S1): S126002. (in Chinese) doi: 10.3788/IRLA201746.S126002 [5] Li H B. Conformal geometric algebra-A new framework for computational geometry [J]. Journal of Computer-Aided Design & Computer Graphics, 2005, 17(11): 3-13. (in Chinese) [6] 刘丙槐. 基于共形几何代数的机器人运动仿真开发[D]. 北京: 北方工业大学, 2018. Liu B H. The computer simulation of robot mechanisms based on the conformal geometric algebra[D]. Beijing: North China University of Technology, 2018. (in Chinese) [7] Su H J, Zhao B. Hyperspectral band selection using conformal geometric algebra [J]. Remote Sensing Technology and Application, 2017, 32(3): 539-545. (in Chinese) [8] Lopes W B, Al-Nuaimi A, Lopes C G. Geometric-algebra LMS adaptive filter and its application to rotation estimation [J]. IEEE Signal Processing Letters, 2016, 23(6): 858-862. doi: 10.1109/LSP.2016.2558461 [9] Al-Nuaimi A, Lopes W B, Steinbach E, et al. 6DOF point cloud alignment using geometric algebra-based adaptive filtering[C]//IEEE Winter Conference on Applications of Computer Vision. New York: IEEE, 2016: 16035869. [10] Wang R, Liang M, He Y, et al. A normalized adaptive filtering algorithm based on geometric algebra [J]. IEEE Access, 2020, 8: 92861-92874. doi: 10.1109/ACCESS.2020.2994230 [11] Wang R, He Y, Huang C, et al. A novel least-mean kurtosis adaptive filtering algorithm based on geometric algebra [J]. IEEE Access, 2020, 7: 78298-78310. doi: 10.1109/ACCESS.2019.2922343. [12] Eckhard Hitzer. Introduction to Clifford’s geometric [J]. Journal of the Society of Instrument and Control Engineers, 2012, 51(4): 338-350. [13] 郭华. 自适应滤波算法及应用研究[D]. 兰州: 西北师范大学, 2007. Guo H. The study on algorithms and application of adaptive filter[D]. Lanzhou: Northwest Normal University, 2007. (in Chinese) [14] Bayro-Corrochano E. Geometric algebra applications Vol. I: Computer Vision, Graphics and Neurocomputing[M]. Cham, Switzerland: Springer, 2018. [15] Qin J F, Ouyang J Z. A new variable step size lms adaptive filtering algorithm [J]. Journal of Data Acquisition & Processing, 1997, 12(3): 171-174. (in Chinese) [16] Nuaimi A, Piccolrovazzi M, Gedikli S, et al. Indoor location retrieval using shape matching of kinectfusion scans to large-scale indoor point clouds[C]//Proceedings of the 2015 Eurographics Workshop on 3D Object Retrieval(3DOR’15). Goslar: Eurographics Association, 2015: 31-38. [17] 王建军, 卢云鹏, 张荠匀等. 实现激光点云高效配准的ICP优化及性能验证[J/OL]. 红外与激光工程: 1-10[2021-04-16]. https://kns-cnki-net.webvpn.cauc.edu.cn/kcms/detail/12.1261.tn.20210309.1605.013.html. Wang J J, Lu Y P, Zhang J Y, et al. ICP optimization and performance verification of laser point cloud efficient registration[J/OL]. [2021-04-16]. http://www.irla.cn/cn/article/doi/10.3788/IRLA20200483.