一种基于次邻帧约束位姿图优化的点云三维重建方法

本发明涉及到三维点云处理领域，尤其是一种基于次邻帧约束位姿图优化的点云三维重建方法。该方法基于截断最小二乘点云配准方法和位姿图优化三维重建方法，对主动式传感器获取的多视角点云进行处理，实现目标的高精度三维重建。

背景技术：

1、三维重建技术作为一种点云处理技术，以其非接触式测量、受环境光照影响小、能重建目标三维模型等优点被广泛应用在城市规划、文物保护、工业制造、空间交会对接等领域。三维重建通过主动式传感器获取目标多个视角下的点云，并采用点云配准技术计算点云之间的刚体变换矩阵，将不同视角下的点云变换到统一坐标系中，获取目标三维模型。由于点云配准存在误差，导致重建时整体误差逐渐累积。这种累积误差可能会导致重建的点云模型出现畸变、形变或错位等问题。为抑制累积误差的影响，采用位姿图优化方法将误差重新分配到各帧，避免出现较大的累积误差，从而提高三维重建精度。位姿图是由节点和边构成的图结构，其中节点表示点云位姿，是待优化的变量，边表示点云位姿之间的变换关系，是优化问题的约束条件。通过位姿图优化，提升点云位姿整体精度和三维重建精度。受传感器噪声、远距离点云稀疏化、遮挡、低重叠率等问题的影响，点云配准中可能存在误差较大的配准结果，影响位姿图优化三维重建精度。因此，如何设计位姿图结构，降低配准误差对位姿图优化精度的影响，是一个亟待解决的问题。

2、为克服所述配准误差对位姿图优化精度的影响，目前常用的处理方式包括两种，一种是基于完全图重加权的位姿图构建，即对多视角点云进行穷举配准，使得位姿图的每两个节点之间都有边连接，再对所有边进行迭代重加权，逐步降低大误差配准结果所对应的边的权重。其中，所述迭代重加权主要通过点云重叠率和点云配准结果的内点数量迭代确定边的权重，减少异常边对全局位姿估计的影响。所述完全图重加权方法通过穷举配准和迭代降低异常边权重，可以有效提升三维重建精度，但同时大大增加了计算量。由于穷举配准的算法时间复杂度为o(n2)，且每次迭代重加权时都要重新计算所有配准结果重叠率和内点数量，增大了计算成本。另一种是基于稀疏位姿图构建，即通过选择性配准和优化来减少计算复杂度，同时保持较高的三维重建精度。所述稀疏位姿图构建，通常基于特征匹配或点云重叠率选择点云对进行配准，并在位姿图中添加相应的边。所述基于特征匹配选择，通过检测和匹配点云中的局部特征点，基于特征点匹配的数量或质量选择配准的帧对，构建稀疏位姿图。所述特征匹配方法对噪声和遮挡较为敏感，当点云中存在噪声和稀疏时，特征点的提取和匹配的可靠性会显著下降。在纹理较少或重复模式多的区域，提取的特征点可能较为稀疏或不具备唯一性，导致匹配的困难和错误。所述点云重叠率用于衡量两个点云之间共同部分的程度，判断两个点云之间的重叠区域大小。通常两帧点云之间重叠率越高，点云配准精度越高。所述重叠率计算相对简单，计算效率较高，对局部变化不敏感，能够有效处理噪声和部分遮挡问题，保证重叠区域的可靠性。所述基于点云重叠率选择，通过选择点云重叠率高的帧对，提高点云配准精度和位姿图优化精度。

3、所述基于重叠率的位姿图构建方法，分为无序点云的重叠率和有序点云的重叠率。所述无序点云，点云之间的时间顺序和位置顺序未知，需要穷举计算所有点云之间的重叠率；所述有序点云，点云之间的时间顺序和位置顺序已知，通常邻帧和次邻帧就是重叠率最高的点云对。本发明所述三维重建应用场景，令传感器绕目标运动一周，获取目标多个视角的点云，最终传感器回到初始位置附近，因此可以构建回环约束。所获取的多视角点云，属于有序点云重叠率的三维重建技术，其时间顺序和位置顺序已知，因此基于重叠率的方法主要选择邻帧点云和次邻帧进行配准，在位姿图中添加邻帧约束边、次邻帧约束边和回环约束边。

4、根据现有专利局提供的专利查阅，现存专利申请是根据点云重叠率选择所述邻帧点云构建位姿图，通过邻帧点云配准结果得到位姿图节点初始值，并根据图优化后的位姿获取目标三维重建模型。上述专利包括在中国专利cn117911617a中公开的“一种三维重建方法及系统”。上述专利未考虑所述邻帧配准误差较大的问题，当所述邻帧配准结果误差过大，导致所述位姿图精度降低。本技术专利与上述专利不同包括：第一，点云配准方法不同，上述专利通过从惯性测量单元采集的角速度和加速度信息进行积分等操作，计算点云的粗略位姿，根据所述点云的粗略位姿，对相邻点云使用迭代最邻近点(icp)、正态分布变换(ndt)等点云精配准算法计算相邻点云位姿，本专利通过截断最小二乘与半定松弛(truncated least squares estimation and semidefinite relaxation，teaser)算法进行粗配准，通过截断最小二乘函数抑制误差较大匹配对的影响，通过点面icp算法进行精配准；第二，位姿图约束边类型不同，上述专利仅基于邻帧配准结果，在位姿图中添加邻帧约束边，本专利除了邻帧约束边，还基于次邻帧配准结果，在位姿图中添加次邻帧约束边；第三，约束边代价函数不同，上述专利基于邻帧特征匹配对计算其均方根误差或重投影误差等作为代价函数，本专利计算邻帧相对位姿与点云配准结果之间的误差，并以其李代数形式作为代价函数。

5、另一类型三维重建的专利申请基于完全图重加权的位姿图构建，对任意两个扫描站数据执行特征匹配，基于特征匹配数量确定权重，构建全连通的加权图，并生成该图的最小生成树，得到任意两站之间的配准路径，利用特征匹配对，计算任意两个连通的站之间的变换矩阵，然后再通过树的路径，得到每站相对于固定站的变换矩阵，完成所有站的全局配准，利用icp配准原理，对路径内每个配准站进行精配准，得到精化的变换矩阵。上述专利包括在中国专利cn201510833425中公开的“一种适用于多视角自动化配准多站地面激光点云数据的方法”。上述专利没有考虑两帧点云之间可能存在错误匹配对，当两帧点云之间存在错误匹配对时，点云配准精度降低，且容易给误差较大的配准结果分配较大的权重，导致位姿图三维重建精度降低。本技术专利与上述专利不同包括：第一，特征匹配方法不同，上述专利通过点云聚类方法获取直线、圆柱等几何特征，并为特征点分配语义信息，通过构建三角几何约束条件与语义约束条件来匹配语义特征点，本技术专利通过计算特征描述子，描述点云局部几何信息，并在两帧点云之间相互查找特征描述子欧式距离最小的点，作为匹配对；第二，点云配准方法不同，上述专利在获取匹配对后通过随机采样一致性算法剔除错误匹配对，对筛选后的匹配对利用奇异值分解计算变换矩阵，本技术专利利用截断最小二乘函数抑制错误匹配对的影响，实现高精度粗配准，并将粗配准结果输入点面icp算法进一步提高点云配准精度；第三，位姿图构建不同，上述专利需要对多视角点云两两配准，构建全连通的完全图，如果点云帧数为n，则位姿图构建的算法时间复杂度为o(n2)，本文采用邻帧和次邻帧构建位姿图，算法时间复杂度为o(n)。

6、为提升激光点云三维重建的精度，降低三维重建算法的计算成本和复杂性，实现基于点云配准和位姿图优化从多视角激光点云重建目标高精度三维模型，本发明公开一种基于次邻帧约束位姿图优化的激光点云三维重建方法。所述激光点云三维重建方法，基于主动式传感器采集目标多视角点云，通过teaser粗配准和点面icp精配准计算变换矩阵，基于邻帧约束边、次邻帧约束边、回环约束边构建位姿图，执行位姿图优化，完成对目标的三维重建。本发明在激光点云三维重建领域具有广阔的应用前景。

技术实现思路

1、本发明公开一种基于次邻帧约束位姿图优化的点云三维重建方法，其特征在于，通过次邻帧约束增强位姿估计精度，实现高精度三维重建；所述次邻帧约束位姿图优化的点云三维重建方法，包括点云配准和位姿图优化；所述点云配准通过计算两帧点云之间刚体变换位姿矩阵，为位姿图建立提供基础；所述点云配准步骤包括特征描述子计算、匹配对计算、变换矩阵估计；所述位姿图包含图节点和约束边，所述图节点为单帧点云位姿矩阵，所述约束边为两帧点云之间的刚体变换矩阵，所述约束边包括邻帧约束边、次邻帧约束边、回环约束边；所述邻帧约束边为位姿图中相邻两帧点云之间的变换矩阵约束；所述次邻帧约束边为位姿图中间隔一帧的两帧点云之间的变换矩阵约束，用于提高位姿图鲁棒性；所述回环约束边为最后一帧点云与第一帧点云闭环构建的变换矩阵约束，用于提高连续多帧点云位姿估计精度；通过所述位姿图优化方法，降低邻帧点云配准的累积误差，减小整体点云位姿误差，利用优化后点云位姿，将所有点云变换到统一坐标系中，得到重建目标模型；

2、所述点云位姿矩阵t为4×4矩阵，由点云相对基准坐标系的旋转矩阵r和平移向量t组成；所述位姿误差包括旋转误差erot和平移误差etra，其定义为：

3、

4、其中，t为位姿矩阵估计值，tg为位姿矩阵真值，δr为旋转矩阵r残差，δt为平移向量t残差，tr(·)代表矩阵的迹，||·||代表向量的二范数；所述累积误差指点云邻帧配准时存在微小误差，导致重建时误差逐渐累积，位姿误差逐渐增大；

5、所述点云三维重建方法包括以下步骤：

6、(1)多视角目标点云获取：采用主动式三维成像传感器，围绕目标获取多视角目标点云；主动式传感器围绕目标做圆周运动，每隔m°扫描一帧点云，传感器运动至少大于一周，共获取n帧点云；

7、(2)特征描述子计算：对多视角点云进行体素降采样，压缩数据量，提高计算速度；对降采样点云中的各点计算特征描述子；所述特征描述子通过在特征点处建立局部坐标系，将邻域点的空间位置信息或几何特征统计信息结合起来描述特征点，其基本思想是：基于邻域点建立局部坐标系，然后将每个点的几何特征统计到直方图进行编码，得到三维描述子，所述点云特征描述子本质为一个向量；

8、(3)匹配对确定：基于所述点云特征描述子，利用相互查找方式计算匹配对；两帧待配准点云分别称为源点云和目标点云；所述源点云中的一点，在所述目标点云中查找与该点特征描述子欧式距离最小的点；对于查找到所述目标点云中的对应点，再次在所述源点云中进行相同查找；两次查找的所述源点云和所述目标点云一对空间点相同，则这一对空间点视为一个匹配对；

9、(4)邻帧及次邻帧变换矩阵估计：基于匹配对计算结果，采用截断最小二乘代价函数和半定松弛结合方法进行变换矩阵估计；所述截断最小二乘代价函数，增强对错误匹配对的鲁棒性，使用半定松弛将旋转估计这一非凸问题改写为凸优化问题，提升三维点云配准精度；所述非凸问题是指在优化过程中，目标函数或约束条件存在多个局部极值点，导致难以找到全局最优解，容易陷入局部极值点；所述凸优化问题是指优化过程中，目标函数和约束条件具有唯一的全局最优解，试图找到全局最优解；

10、(5)位姿图构建：基于邻帧变换矩阵估计结果，计算各帧点云的位姿矩阵，根据所述位姿矩阵初始化位姿图节点；基于邻帧变换矩阵，添加邻帧约束边；基于次邻帧变换矩阵，添加次邻帧约束边；对最后一帧点云和第一帧点云，添加回环约束边，即最后一帧点云与第一帧点云之间的变换矩阵为单位矩阵，代表传感器经过圆周运动后回到初始位置；

11、(6)位姿图优化：对位姿图计算代价函数，假设位姿图有n个节点，定义邻帧相对位姿李群表达形式如下：

12、

13、其中，ti和ti+1分别为第i帧和第i+1帧的位姿，ti,i+1为第i帧和第i+1帧之间的相对位姿，在所述图优化中，邻帧相对位姿ti,i+1已通过所述点云配准算法获得，所以误差可以通过下式计算：

14、

15、类似地，次邻帧误差可以通过下式计算：

16、

17、其中，ti,i+2为次邻帧相对位姿；回环误差为：

18、

19、其中，tn,1为最后一帧与第一帧相对位姿；所述代价函数f为这三项误差之和；利用levenberg-marquardt算法优化，迭代求解直到所述代价函数收敛，得到优化后点云位姿tio,i＝1,2,…,n；

20、(7)三维重建：根据所述图优化后点云位姿，将所有点云通过刚体变换对齐到第一帧点云的坐标系下，得到重建目标模型。

21、所述的基于次邻帧约束位姿图优化的点云三维重建方法，其特征在于，所述主动式传感器可选择激光雷达，或tof相机；所述多视角目标点云获取，令主动式传感器围绕目标做圆周运动，最后回到初始位置附近，使最后一帧点云与第一帧点云能够通过点云配准构成闭环；本部分输出为n帧不同视角下的点云。

22、所述的基于次邻帧约束位姿图优化的点云三维重建方法，其特征在于，所述特征描述子计算可选择方向直方图签名(signature of histogram of orientation,shot)描述子，或点特征直方图(point feature histogram,fph)描述子，或快速点特征直方图(fast point feature histogram,fpfh)描述子；所述特征描述子计算步骤如下：

23、1)称点云中要计算特征描述子的点为查询点，对于点云中的每个查询点p，构建点邻域的协方差矩阵m：

24、

25、其中，r表示邻域半径，p为查询点，pi为邻域内的点，di为pi与p之间的距离；

26、2)通过对协方差矩阵进行奇异值分解，求得其特征值及对应的特征向量，将特征值按递减的顺序排列，对应的特征向量即为局部坐标系的x、y、z轴；

27、3)计算邻域点法线、查询点法线夹角的余弦值等几何信息，将邻域点的几何特征统计到直方图进行编码，形成特征描述子，其本质为k维向量；假设点云中查询点的集合为p＝{pi,i＝1,2,…,m}，则计算结果为特征描述子集合本部分输出为特征描述子计算结果d，将作为下一步匹配对确定的输入。

28、所述的基于次邻帧约束位姿图优化的点云三维重建方法，其特征在于，所述匹配对确定，基于所述特征描述子计算结果，假设源点云为p＝{pi,i＝1,2,…,m}，对应的特征描述子为目标点云为q＝{qi,i＝1,2,…,m}，对应的特征描述子为对于源点云中一点pi，其对应的特征描述子为在所述目标点云特征描述子dq中查询与欧式距离最小的特征描述子对应的点qj为pi的初步对应点；为提升匹配对精度，采用相互查找方式，即对于所述初步对应点qj，在所述源点云特征描述子dp中进行相同的查找，如果查找到的对应点也是pi，则pi和qj视为一个匹配对；遍历所述特征描述子dp，最终得到n个匹配对；为避免遍历查找带来的高耗时，对所述源点云和目标点云特征描述子建立kd树，加速所述相互查找；本部分输出为点云之间的n个匹配对，将作为下一步粗配准的输入。

29、所述的基于次邻帧约束位姿图优化的点云三维重建方法，其特征在于，假设源点云p＝{pi,i＝1,2,…,n}和目标点云q＝{qi,i＝1,2,…,n}之间存在尺度、平移、旋转变换，所述截断最小二乘代价函数为：

30、

31、其中，s为尺度变换因子，r为所述旋转矩阵、t为所述平移矩阵，n为所述匹配对数，βi为第i次测量中所能忍受的测量残差最大值，为常数；通过所述截断最小二乘代价函数，给具有较大残差的测量值分配常数代价，使所述变换矩阵估计对具有较大残差的测量值不敏感，本部分输出为粗配准结果，即所述旋转矩阵r、所述平移矩阵t，将作为下一步精配准的输入。

32、所述的基于次邻帧约束位姿图优化的点云三维重建方法，其特征在于，所述截断最小二乘与半定松弛配准结果作为所述变换矩阵初始估计，输入精配准算法进行进一步精配准；所述精配准算法可选择迭代最近点(iterative closest point,icp)算法，或点-面icp(point-to-plane icp)算法；所述icp算法代价函数为：

33、

34、其中，t为所述变换矩阵，n为所述匹配对数量，pi为所述源点云中一点，qclosest为所述目标点云中与pi欧式距离最小的点，||·||表示向量的二范数；

35、所述点-面icp算法代价函数为：

36、

37、其中，vi为所述目标点切平面的法向量；，本部分输出为所述邻帧刚体变换矩阵ti,i+1,i＝1,2,…,n和所述次邻帧刚体变换矩阵ti,i+2,i＝1,2,…,n，将作为下一步位姿图建立的输入。

38、所述的基于次邻帧约束位姿图优化的点云三维重建方法，其特征在于，所述位姿图构建，基于所述邻帧配准结果确定所述位姿节点的初始值；所述邻帧配准结果为邻帧刚体变换矩阵ti,i+1,i＝1,2,…,n，所述位姿节点为位姿矩阵ti,i＝1,2,…,n，二者之间计算关系为：

39、t1＝i4 (11)

40、ti＝ti-1·ti-1,i,i＝2,3,…,n       (12)

41、其中i4为4阶单位矩阵，式(11)表示将第一个位姿节点设为单位矩阵，以此为基准；式(12)表示所述位姿节点初始值为上一个位姿节点初始值乘以上一个位姿节点到该位姿节点的刚体变换矩阵；本部分输出为位姿图节点ti,i＝1,2,…,n。

42、所述的基于次邻帧约束位姿图优化的点云三维重建方法，其特征在于，所述位姿图构建，基于所述邻帧刚体变换矩阵ti,i+1,i＝1,2,…,n在所述位姿图中添加邻帧约束边；基于所述次邻帧刚体变换矩阵ti,i+2,i＝1,2,…,n添加次邻帧约束边；在最后一个位姿节点与第一个位姿节点之间添加回环约束边；

43、邻帧约束边本质为式(4)所示的误差项，其李代数形式为：

44、

45、其中，ξi和ξi+1为ti和ti+1对应的李代数，是后续优化中的优化变量，且有ti＝exp(ξi)^，ti+1＝exp(ξi+1)^，表示ti-1对应的李代数，且有ti-1＝exp((-ξi)^)；其中·^符号表示将向量转换为反对称矩阵，·∨符号表示将反对称矩阵转换为向量；在所述图优化中，相对位姿ti,i+1已通过所述点云配准算法获得，其对应的李代数记为ξi,i+1，所以误差表达式为：

46、ei,i+1＝ln(exp((-ξi,i+1)∧)·exp((-ξi)∧)exp(ξi+1)∧)∨     (14)

47、类似地，次邻帧误差表达式为：

48、ei,i+2＝ln(exp((-ξi,i+2)∧)·exp((-ξi)∧)exp(ξi+2)∧)∨        (15)

49、回环约束误差表达式为：

50、en,1＝ln(exp((-ξn,1)∧)·exp((-ξn)∧)exp(ξ1)∧)∨       (16)

51、本部分输出为位姿图所述三种约束边误差表达式ei,i+1、ei,i+2和en,1，便于下一步计算位姿图代价函数。

52、所述的基于次邻帧约束位姿图优化的点云三维重建方法，其特征在于，所述位姿图的代价函数定义为：

53、

54、所述代价函数f为6维向量，其中前三维表示平移误差，后三维表示旋转误差，ωi为预设的信息矩阵，代表对e中每个元素对应的误差项相关性的预计，为误差的各个维度赋予不同的权重；所述信息矩阵ωi为6×6对角矩阵，主对角线元素代表优化变量(tx,ty,tz,rx,ry,rz)的权重，对角线元素值越大，代表对应的优化变量权重越大；对所述代价函数迭代求解直到其收敛；本部分输出为图优化后点云位姿tio,i＝1,2,…,n，将作为下一步三维重建的输入。

55、所述的基于次邻帧约束位姿图优化的点云三维重建方法，其特征在于，第一帧点云位姿固定为单位矩阵i，作为基准；所述位姿图优化后得到的位姿为tio,i＝2,3,…,n，实质为各点云相对第一帧点云的变换矩阵t1,i,i＝2,3,…,n；所述三维重建将所有点云变换到第一帧点云坐标系下，即对各帧点云pi,i＝2,3,…,n做如下变换：

56、

57、其中，pit为变换后点云；本部分输出为变换后点云的集合pit,i＝2,3,…,n，即三维重建模型。