CN106570823A - 基于平面特征匹配的点云粗拼接方法 - Google Patents

Abstract

本发明公开了一种基于平面特征匹配的点云粗拼接方法，其包括如下步骤：首先以人机交互的方式，选中点云数据中某一平面上的部分数据作为种子平面，利用特征值法计算出种子平面的参数方程并进行生长；其次，利用RANSAC算法进行优化处理，可以有效降低噪声点的影响；然后，选取四对或四对以上的匹配平面并计算其法向量；最后根据四对或四对以上的匹配面的法向量，利用最小二乘法分别求出旋转矩阵及平移量来完成点云的粗拼接。

Description

基于平面特征匹配的点云粗拼接方法

技术领域

本发明涉及一种基于平面特征匹配的点云粗拼接方法。

背景技术

高质量、完整的三维点云数据是点云后处理的基础。但在实际测量中，由于现场环境及三维激光扫描仪自身测量范围的限制，需要对目标地物进行多站式扫描，因此，将不同视点下的点云数据有效地拼接起来显得至关重要。目前，国内外的拼接技术一般分为两步：粗拼接和精拼接。粗拼接将不同坐标系下的点云大致对准到同一坐标系下，为精拼接提供初始值。现有的最常用的粗拼接方法主要是三点法和基于点特征的点云粗拼接方法。其中，

三点法是在两站点云数据的重合部分选取不共线的三对以上的同名点，同名点可以是已知的特征点(例如拐角点，边界点)或者是高反射率的点，利用SVD矩阵分解法确定点云数据的旋转矩阵和平移量。此种方法的缺点在于：对点云数据的形状要求比较苛刻，需要存在明显同名点的点云数据。

基于点特征的粗拼接方法最常用的是点签名的拼接方法，该方法是通过在点云数据中的每个点上定义一个特征描述因子，然后通过对两片或多片点云数据的每个点的特征描述因子进行比较，计算点云数据的转换参数来完成点云的拼接。此种方法的缺点在于：计算量大，且对噪声点敏感度较高，拼接的精度较低。

发明内容

本发明的目的在于提出一种基于平面特征匹配的点云粗拼接方法，采用如下技术方案：

基于平面特征匹配的点云粗拼接方法，包括如下步骤：

a提取点云中的平面

a.1种子平面的参数获取及生长

a.11以人机交互的方式，选中点云数据中某一平面上的部分数据作为种子平面，记作P₀，利用特征值法计算出种子平面的参数方程：a₀x+b₀y+c₀z-d₀＝0；

其中，(a₀,b₀,c₀)为平面的单位法向量，d₀为坐标原点至平面的距离；

a.12设置种子平面生长的阈值d_max；

a.13选中的范围在三维空间内的方向扩大一倍，自动选中更多的点云数据；

a.14判断被选中进入的点云数据到平面P₀的距离d_k，若：d_k＝|a₀x_k+b₀y_k+c₀z_k-d₀|≤d_max，k≥1，则将此点视为有效点并加入到点集n₀，反之将此点剔除；其中，(x_k,y_k,z_k)表示被选中进入的点云数据中各点的三维坐标；

a.15若有效点的数量不再增加，则结束生长，否则返回步骤a.13；

a.2利用随机抽样一致性算法对平面进行优化处理

a.21规定迭代次数I，确定有效阈值e以及有效点比例阈值p；

a.22在点集n₀中随机选择三个不共线的点,直接计算对应平面方程：a₁x+b₁y+z＝d₁,然后计算点云至该平面距离：d_k＝|a₁x_k+b₁y_k+z_k-d₁|；

其中，(a₁,b₁,1)表示平面的法向量，d₁表示原点到平面的距离；

a.23若d_k≤e，则被认为是有效点，否则为无效点；

a.24统计得出平面M₁的有效点比例f，若f≥p，M₁为有效平面，否则为无效平面；

a.25根据迭代次数I，得出有效平面M₁′…M_l′,l≥1；通过比较上述有效平面选出有效点比例最高的平面作为匹配面；

a.26利用特征值法对步骤a.25中选取的匹配平面重新进行计算,得出匹配平面的参数方程：Ax+By+Cz＝D，则单位法向量为(A,B,C)；

利用步骤a.2描述的方法在两片点云上选取m对不相互平行的匹配平面，计算其法向量，其中，m≥4；

b基于匹配面法向量的坐标转换

b.1转换模型

在选取匹配面的过程中，计算得到的一对匹配平面的参数方程分别表示为：

P₁:A₁x+B₁y+C₁z＝D₁，P₁′:A₁′x+B₁′y+C₁′z＝D₁′；

其中，(A₁,B₁,C₁)和(A₁′,B₁′,C₁′)表示匹配平面的法向量，记作n₁和n₁′，D₁和D₁′分别表示原点到匹配平面的距离；由计算机图形学知识，可得到转换模型为：

将上式分块并展开得到：n₁＝Rn₁′，D₁＝t·n₁′+D₁′；其中，t为偏移量；

b.2利用法向量计算旋转矩阵R

b.21利用罗德里格矩阵求旋转矩阵的初值

旋转矩阵的初始值R⁰由罗德里格矩阵3个独立参数a、-b、c表示，其中，a、-b、c相互独立；R⁰由反对称矩阵S构成罗德里格矩阵：

其中，E是单位矩阵，根据任意两对匹配平面的法向量及罗德里格矩阵性质构建方程组，整理可得：

求解a、-b、c，得到旋转矩阵的初始值R⁰；

b.22利用最小二乘求解旋转矩阵的平差值

b.221四元数表示法

一个四元数q表示为一个四维向量(λ₀,λ₁,λ₂,λ₃)^T，旋转矩阵R用一个唯一的单位四元数q来表示，即：

任意一对对应匹配平面的法向量(A₁,B₁,C₁)和(A₁′,B₁′,C₁′)满足：

b.222建立平差模型

b.2221以匹配平面的法向量为观测值列出的观测方程及约束条件方程：

其中， 和为对应匹配平面法向量的平差值，i为平面编号，v_i′、v_i分别为n_i′和n_i的改正数；

为λ_j的近似值，为λ_j的平差值，为λ_j的改正数；

b.2222Gauss-Helmert平差模型的一般形式为：

其中，为未知参数，A为对未知参数求一阶偏导后的设计矩阵，v表示观测值的改正数，B为对观测量求一阶偏导后的设计矩阵，w为闭合差向量；

对观测方程线性化得Gauss-Helmert模型：

式中，

当有m对匹配平面时，

对约束条件方程线性化得：

令

b.2223根据Gauss-Helmert平差模型和测量平差的原理可知:

其中，P为单位权阵；

将和代入即可得到λ₀、λ₁、λ₂、λ₃的平差值，得到旋转矩阵的平差值；

b.3计算平移量t

由式D₁＝t·n₁′+D₁′可得D_i＝t·n_i′+D_i′，其中i为平面编号；利用m对匹配平面，由平差知识得到：t＝(N^TN)^-1N^TL；

式中，

本发明具有如下优点：

本发明方法首先以人机交互的方式，选中点云数据中某一平面上的部分数据作为种子平面，利用特征值法计算出种子平面的参数方程并进行生长；其次，利用RANSAC算法进行优化处理，可以有效降低噪声点的影响；然后，选取四对或四对以上的匹配平面并计算其法向量，由于只需求出匹配平面的法向量即可，因此不需要存在同名点；最后根据四对或四对以上的匹配平面的法向量，利用最小二乘法分别求出旋转矩阵及平移量来完成点云的粗拼接，提高了点云拼接的精度。

具体实施方式

下面结合具体实施方式对本发明作进一步详细说明：

基于平面特征匹配的点云粗拼接方法，包括如下步骤：

a提取点云中的平面

a.1种子平面的参数获取及生长

a.11以人机交互的方式，选中点云数据中某一平面上的部分数据作为种子平面，记作P₀，利用特征值法计算出种子平面的参数方程：a₀x+b₀y+c₀z-d₀＝0；

其中，(a₀,b₀,c₀)为平面的单位法向量，d₀为坐标原点至平面的距离；

a.12设置种子平面生长的阈值d_max；

a.13选中的范围在三维空间内的方向扩大一倍，自动选中更多的点云数据；

a.14判断被选中进入的点云数据到平面P₀的距离d_k，若：d_k＝|a₀x_k+b₀y_k+c₀z_k-d₀|≤d_max，k≥1，则将此点视为有效点并加入到点集n₀，反之将此点剔除；其中，(x_k,y_k,z_k)表示被选中进入的点云数据中各点的三维坐标；

a.15若有效点的数量不再增加，则结束生长，否则返回步骤a.13；

a.2利用随机抽样一致性算法对平面进行优化处理

a.21规定迭代次数I，确定有效阈值e以及有效点比例阈值p；

a.22在点集n₀中随机选择三个不共线的点,直接计算对应平面方程：a₁x+b₁y+z＝d₁,然后计算点云至该平面距离：d_k＝|a₁x_k+b₁y_k+z_k-d₁|；

其中，(a₁,b₁,1)表示平面的法向量，d₁表示原点到平面的距离；

a.23若d_k≤e，则被认为是有效点，否则为无效点；

a.24统计得出平面M₁的有效点比例f，若f≥p，M₁为有效平面，否则为无效平面；

a.25根据迭代次数I，得出有效平面M₁′…M_l′，l≥1；通过比较上述有效平面选出有效点比例最高的平面作为匹配面；

a.26利用特征值法对步骤a.25中选取的匹配平面重新进行计算,得出匹配平面的参数方程：Ax+By+Cz＝D，则单位法向量为(A,B,C)；

利用步骤a.2描述的方法在两片点云上选取m对不相互平行的匹配平面，计算其法向量其中，m≥4；

b基于匹配面法向量的坐标转换

b.1转换模型

在选取匹配面的过程中，计算得到的一对匹配平面的参数方程分别表示为：P₁:A₁x+B₁y+C₁z＝D₁，P₁′:A₁′x+B₁′y+C₁′z＝D₁′；

其中，(A₁,B₁,C₁)和(A₁′,B₁′,C₁′)表示匹配平面的法向量，记作n₁和n₁′，D₁和D₁′分别表示原点到匹配平面的距离；由计算机图形学知识，可得到转换模型为：

将上式分块并展开得到：n₁＝Rn₁′,D₁＝t·n₁′+D₁′；其中，t为偏移量；

b.2利用法向量计算旋转矩阵R

b.21利用罗德里格矩阵求旋转矩阵的初值

旋转矩阵的初始值R⁰由罗德里格矩阵3个独立参数a、-b、c表示，其中，a、-b、c相互独立；R⁰由反对称矩阵S构成罗德里格矩阵：

其中，E是单位矩阵，根据任意两对匹配平面的法向量及罗德里格矩阵性质构建方程组，整理可得：

求解a、-b、c，得到旋转矩阵的初始值R⁰；

b.22利用最小二乘求解旋转矩阵的平差值

b.221四元数表示法

一个四元数q表示为一个四维向量(λ₀,λ₁,λ₂,λ₃)^T，旋转矩阵R用一个唯一的单位四元数q来表示，即：

任意一对对应匹配平面的法向量(A₁,B₁,C₁)和(A₁′,B₁′,C₁′)满足：

b.222建立平差模型

b.2221以匹配平面的法向量为观测值列出的观测方程及约束条件方程：

其中， 和为对应匹配平面法向量的平差值，i为平面编号，v_i′、v_i分别为n_i′和n_i的改正数；

为λ_j的近似值，为λ_j的平差值，为λ_j的改正数；

b.2222Gauss-Helmert平差模型的一般形式为：

其中，为未知参数，A为对未知参数求一阶偏导后的设计矩阵，v表示观测值的改正数，B为对观测量求一阶偏导后的设计矩阵，w为闭合差向量；

对观测方程线性化得Gauss-Helmert模型：

式中，

当有m对匹配平面时，

对约束条件方程线性化得：

令

b.2223根据Gauss-Helmert平差模型和测量平差的原理可知:

其中，P为单位权阵；

将和代入即可得到λ₀、λ₁、λ₂、λ₃的平差值，得到旋转矩阵的平差值；

b.3计算平移量t

由式D₁＝t·n₁′+D₁′可得D_i＝t·n_i′+D_i′，其中i为平面编号；利用m对匹配平面，由平差知识得到：t＝(N^TN)^-1N^TL；

式中，

当然，以上说明仅仅为本发明的较佳实施例，本发明并不限于列举上述实施例，应当说明的是，任何熟悉本领域的技术人员在本说明书的教导下，所做出的所有等同替代、明显变形形式，均落在本说明书的实质范围之内，理应受到本发明的保护。

Claims (1)

1.基于平面特征匹配的点云粗拼接方法，其特征在于，包括如下步骤：

a提取点云中的平面

a.1种子平面的参数获取及生长

a.11以人机交互的方式，选中点云数据中某一平面上的部分数据作为种子平面，记作P₀，利用特征值法计算出种子平面的参数方程：a₀x+b₀y+c₀z-d₀＝0；

其中，(a₀,b₀,c₀)为平面的单位法向量，d₀为坐标原点至平面的距离；

a.12设置种子平面生长的阈值d_max；

a.13选中的范围在三维空间内的方向扩大一倍，自动选中更多的点云数据；

a.14判断被选中进入的点云数据到平面P₀的距离d_k，若：d_k＝|a₀x_k+b₀y_k+c₀z_k-d₀|≤d_max，k≥1，则将此点视为有效点并加入到点集n₀，反之将此点剔除；其中，(x_k,y_k,z_k)表示被选中进入的点云数据中各点的三维坐标；

a.15若有效点的数量不再增加，则结束生长，否则返回步骤a.13；

a.2利用随机抽样一致性算法对平面进行优化处理

a.21规定迭代次数I，确定有效阈值e以及有效点比例阈值p；

a.22在点集n₀中随机选择三个不共线的点,直接计算对应平面方程：a₁x+b₁y+z＝d₁,然后计算点云至该平面距离：d_k＝|a₁x_k+b₁y_k+z_k-d₁|；

其中，(a₁,b₁,1)表示平面的法向量，d₁表示原点到平面的距离；

a.23若d_k≤e，则被认为是有效点，否则为无效点；

a.24统计得出平面M₁的有效点比例f，若f≥p，M₁为有效平面，否则为无效平面；

a.25根据迭代次数I，得出有效平面M₁′…M_l′,l≥1；通过比较上述有效平面选出有效点比例最高的平面作为匹配面；

a.26利用特征值法对步骤a.25中选取的匹配平面重新进行计算,得出匹配平面的参数方程：Ax+By+Cz＝D，则单位法向量为(A,B,C)；

利用步骤a.2描述的方法在两片点云上选取m对不相互平行的匹配平面，计算其法向量，其中，m≥4；

b基于匹配面法向量的坐标转换

b.1转换模型

在选取匹配面的过程中，计算得到的一对匹配平面的参数方程分别表示为：

P₁:A₁x+B₁y+C₁z＝D₁，P₁′:A₁′x+B₁′y+C₁′z＝D₁′；

其中，(A₁,B₁,C₁)和(A₁′,B₁′,C₁′)表示匹配平面的法向量，记作n₁和n₁′，D₁和D₁′分别表示原点到匹配平面的距离；由计算机图形学知识，可得到转换模型为：

$\left[\begin{array}{c}{A}_1\\ B_1\\ C_1\\ D_1\end{array}\right]=\left[\begin{array}{cccc}{R}_{11}& R_{12}& R_{13}& 0\\ R_{21}& R_{22}& R_{23}& 0\\ R_{31}& R_{32}& R_{33}& 0\\ t_1& t_2& t_3& 1\end{array}\right]\left[\begin{array}{c}{A_1}^{\′}\\ {B_1}^{\′}\\ {C_1}^{\′}\\ {D_1}^{\′}\end{array}\right];$

将上式分块并展开得到：n₁＝Rn₁′，D₁＝t·n₁′+D₁′；其中，t为偏移量；

b.2利用法向量计算旋转矩阵R

b.21利用罗德里格矩阵求旋转矩阵的初值

旋转矩阵的初始值R⁰由罗德里格矩阵3个独立参数a、-b、c表示，其中，a、-b、c相互独立；R⁰由反对称矩阵S构成罗德里格矩阵：

$\begin{array}{c}{R}^0=(E+S){(E-S)}^{-1}\\ =\frac{1}{1+a^2+b^2+c^2}\left[\begin{array}{ccc}1+a^2-b^2-c^2& 2ab-2c& 2b+2ac\\ 2c+2ab& 1-a^2+b^2-c^2& -2a+2bc\\ 2ac-2b& 2a+2bc& 1-a^2-b^2+c^2\end{array}\right]\end{array},$

其中，E是单位矩阵，根据任意两对匹配平面的法向量及罗德里格矩阵性质构建方程组，整理可得：

$\left[\begin{array}{ccc}0& C_1+{C_1}^{\′}& B_1+{B_1}^{\′}\\ C_1+{C_1}^{\′}& 0& -A_1-{A_1}^{\′}\\ -B_1-{B_1}^{\′}& -A_1-{A_1}^{\′}& 0\\ 0& C_2+{C_2}^{\′}& B_2+{B_2}^{\′}\\ C_2+{C_2}^{\′}& 0& -A_2-{A_2}^{\′}\\ -B_2-{B_2}^{\′}& -A_2-{A_2}^{\′}& 0\end{array}\right]\left[\begin{array}{c}a\\ -b\\ c\end{array}\right]=\left[\begin{array}{c}{A}_1-{A_1}^{\′}\\ B_1-{B_1}^{\′}\\ C_1-{C_1}^{\′}\\ A_2-{A_2}^{\′}\\ B_2-{B_2}^{\′}\\ C_2-{C_2}^{\′}\end{array}\right];$

求解a、-b、c，得到旋转矩阵的初始值R⁰；

b.22利用最小二乘求解旋转矩阵的平差值

b.221四元数表示法

一个四元数q表示为一个四维向量(λ₀,λ₁,λ₂,λ₃)^T，旋转矩阵R用一个唯一的单位四元数q来表示，即：

$R=\left[\begin{array}{ccc}{{\mathrm{\λ}}_0}^2+{{\mathrm{\λ}}_1}^2-{{\mathrm{\λ}}_2}^2-{{\mathrm{\λ}}_3}^2& 2({\mathrm{\λ}}_1{\mathrm{\λ}}_2-{\mathrm{\λ}}_0{\mathrm{\λ}}_3)& 2({\mathrm{\λ}}_1{\mathrm{\λ}}_3+{\mathrm{\λ}}_0{\mathrm{\λ}}_2)\\ 2({\mathrm{\λ}}_1{\mathrm{\λ}}_2+{\mathrm{\λ}}_0{\mathrm{\λ}}_3)& {{\mathrm{\λ}}_0}^2-{{\mathrm{\λ}}_1}^2+{{\mathrm{\λ}}_2}^2-{{\mathrm{\λ}}_3}^2& 2({\mathrm{\λ}}_2{\mathrm{\λ}}_3-{\mathrm{\λ}}_0{\mathrm{\λ}}_1)\\ 2({\mathrm{\λ}}_1{\mathrm{\λ}}_3-{\mathrm{\λ}}_0{\mathrm{\λ}}_2)& 2({\mathrm{\λ}}_2{\mathrm{\λ}}_3+{\mathrm{\λ}}_0{\mathrm{\λ}}_1)& {{\mathrm{\λ}}_0}^2-{{\mathrm{\λ}}_1}^2-{{\mathrm{\λ}}_2}^2+{{\mathrm{\λ}}_3}^2\end{array}\right];$

任意一对对应匹配平面的法向量(A₁,B₁,C₁)和(A₁′,B₁′,C₁′)满足：

$\left[\begin{array}{c}{A}_1\\ B_1\\ C_1\end{array}\right]=\left[\begin{array}{ccc}{{\mathrm{\λ}}_0}^2+{{\mathrm{\λ}}_1}^2-{{\mathrm{\λ}}_2}^2-{{\mathrm{\λ}}_3}^2& 2({\mathrm{\λ}}_1{\mathrm{\λ}}_2-{\mathrm{\λ}}_0{\mathrm{\λ}}_3)& 2({\mathrm{\λ}}_1{\mathrm{\λ}}_3+{\mathrm{\λ}}_0{\mathrm{\λ}}_2)\\ 2({\mathrm{\λ}}_1{\mathrm{\λ}}_2+{\mathrm{\λ}}_0{\mathrm{\λ}}_3)& {{\mathrm{\λ}}_0}^2-{{\mathrm{\λ}}_1}^2+{{\mathrm{\λ}}_2}^2-{{\mathrm{\λ}}_3}^2& 2({\mathrm{\λ}}_2{\mathrm{\λ}}_3-{\mathrm{\λ}}_0{\mathrm{\λ}}_1)\\ 2({\mathrm{\λ}}_1{\mathrm{\λ}}_3-{\mathrm{\λ}}_0{\mathrm{\λ}}_2)& 2({\mathrm{\λ}}_2{\mathrm{\λ}}_3+{\mathrm{\λ}}_0{\mathrm{\λ}}_1)& {{\mathrm{\λ}}_0}^2-{{\mathrm{\λ}}_1}^2-{{\mathrm{\λ}}_2}^2+{{\mathrm{\λ}}_3}^2\end{array}\right]\left[\begin{array}{c}{A_1}^{\′}\\ {B_1}^{\′}\\ {C_1}^{\′}\end{array}\right];$

b.222建立平差模型

b.2221以匹配平面的法向量为观测值列出的观测方程及约束条件方程：

其中， 和为对应匹配平面法向量的平差值，i为平面编号，v_i′、v_i分别为n_i′和n_i的改正数；

为λ_j的近似值，为λ_j的平差值，为λ_j的改正数；

b.2222 Gauss-Helmert平差模型的一般形式为：

其中，为未知参数，A为对未知参数求一阶偏导后的设计矩阵，v表示观测值的改正数，B为对观测量求一阶偏导后的设计矩阵，w为闭合差向量；

对观测方程线性化得Gauss-Helmert模型：

$\begin{array}{c}\left[\begin{array}{cccc}{\left(\frac{\∂F_1}{\∂{\widehat{\mathrm{\λ}}}_0}\right)}_0& {\left(\frac{\∂F_1}{\∂{\widehat{\mathrm{\λ}}}_1}\right)}_0& {\left(\frac{\∂F_1}{\∂{\widehat{\mathrm{\λ}}}_2}\right)}_0& {\left(\frac{\∂F_1}{\∂{\widehat{\mathrm{\λ}}}_3}\right)}_0\\ {\left(\frac{\∂G_1}{\∂{\widehat{\mathrm{\λ}}}_0}\right)}_0& {\left(\frac{\∂G_1}{\∂{\widehat{\mathrm{\λ}}}_1}\right)}_0& {\left(\frac{\∂G_1}{\∂{\widehat{\mathrm{\λ}}}_2}\right)}_0& {\left(\frac{\∂G_1}{\∂{\widehat{\mathrm{\λ}}}_3}\right)}_0\\ {\left(\frac{\∂H_1}{\∂{\widehat{\mathrm{\λ}}}_0}\right)}_0& {\left(\frac{\∂H_1}{\∂{\widehat{\mathrm{\λ}}}_1}\right)}_0& {\left(\frac{\∂H_1}{\∂{\widehat{\mathrm{\λ}}}_2}\right)}_0& {\left(\frac{\∂H_1}{\∂{\widehat{\mathrm{\λ}}}_3}\right)}_0\end{array}\right]\left[\begin{array}{c}{x}_{{\widehat{\mathrm{\λ}}}_0}\\ x_{{\widehat{\mathrm{\λ}}}_1}\\ x_{{\widehat{\mathrm{\λ}}}_2}\\ x_{{\widehat{\mathrm{\λ}}}_3}\end{array}\right]+\left[\begin{array}{cccccc}{\left(\frac{\∂F_1}{\∂{\hat{A}}_1}\right)}_0& {\left(\frac{\∂F_1}{\∂{\hat{B}}_1}\right)}_0& {\left(\frac{\∂F_1}{\∂{\hat{C}}_1}\right)}_0& {\left(\frac{\∂F_1}{\∂{{\hat{A}}_1}^{\′}}\right)}_0& {\left(\frac{\∂F_1}{\∂{{\hat{B}}_1}^{\′}}\right)}_0& {\left(\frac{\∂F_1}{\∂{{\hat{C}}_1}^{\′}}\right)}_0\\ {\left(\frac{\∂G_1}{\∂{\hat{A}}_1}\right)}_0& {\left(\frac{\∂G_1}{\∂{\hat{B}}_1}\right)}_0& {\left(\frac{\∂G_1}{\∂{\hat{C}}_1}\right)}_0& {\left(\frac{\∂G_1}{\∂{{\hat{A}}_1}^{\′}}\right)}_0& {\left(\frac{\∂G_1}{\∂{{\hat{B}}_1}^{\′}}\right)}_0& {\left(\frac{\∂G_1}{\∂{{\hat{C}}_1}^{\′}}\right)}_0\\ {\left(\frac{\∂H_1}{\∂{\hat{A}}_1}\right)}_0& {\left(\frac{\∂H_1}{\∂{\hat{B}}_1}\right)}_0& {\left(\frac{\∂H_1}{\∂{\hat{C}}_1}\right)}_0& {\left(\frac{\∂H_1}{\∂{{\hat{A}}_1}^{\′}}\right)}_0& {\left(\frac{\∂H_1}{\∂{{\hat{B}}_1}^{\′}}\right)}_0& {\left(\frac{\∂H_1}{\∂{{\hat{C}}_1}^{\′}}\right)}_0\end{array}\right]\left[\begin{array}{c}{v}_{A_1}\\ v_{B_1}\\ v_{C_1}\\ v_{{{\hat{A}}_1}^{\′}}\\ v_{{{\hat{B}}_1}^{\′}}\\ v_{{{\hat{C}}_1}^{\′}}\end{array}\right]\\ +\left[\begin{array}{c}-F_1({\mathrm{\λ}}_0^0,{\mathrm{\λ}}_1^0,{\mathrm{\λ}}_2^0,{\mathrm{\λ}}_3^0,A_1,B_1,C_1,{A_1}^{\′},{B_1}^{\′},{C_1}^{\′})\\ -G_1({\mathrm{\λ}}_0^0,{\mathrm{\λ}}_1^0,{\mathrm{\λ}}_2^0,{\mathrm{\λ}}_3^0,A_1,B_1,C_1,{A_1}^{\′},{B_1}^{\′},{C_1}^{\′})\\ -H_1({\mathrm{\λ}}_0^0,{\mathrm{\λ}}_1^0,{\mathrm{\λ}}_2^0,{\mathrm{\λ}}_3^0,A_1,B_1,C_1,{A_1}^{\′},{B_1}^{\′},{C_1}^{\′})\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right]\end{array},$

式中，

${\mathrm{\λ}}_0^0=\frac{1}{\sqrt{1+a^2+b^2+c^2}},{\mathrm{\λ}}_1^0=\frac{a}{\sqrt{1+a^2+b^2+c^2}},{\mathrm{\λ}}_2^0=\frac{-b}{\sqrt{1+a^2+b^2+c^2}},{\mathrm{\λ}}_3^0=\frac{c}{\sqrt{1+a^2+b^2+c^2}};$

$F_1=({{\widehat{\mathrm{\λ}}}_0}^2+{{\widehat{\mathrm{\λ}}}_1}^2-{{\widehat{\mathrm{\λ}}}_2}^2-{{\widehat{\mathrm{\λ}}}_3}^2){{\hat{A}}_1}^{\′}+2({\widehat{\mathrm{\λ}}}_1{\widehat{\mathrm{\λ}}}_2-{\widehat{\mathrm{\λ}}}_0{\widehat{\mathrm{\λ}}}_3){{\hat{B}}_1}^{\′}+2({\widehat{\mathrm{\λ}}}_1{\widehat{\mathrm{\λ}}}_3+{\widehat{\mathrm{\λ}}}_0{\widehat{\mathrm{\λ}}}_2){{\hat{C}}_1}^{\′}-{\hat{A}}_1;$

$G_1=2({\widehat{\mathrm{\λ}}}_1{\widehat{\mathrm{\λ}}}_2+{\widehat{\mathrm{\λ}}}_0{\widehat{\mathrm{\λ}}}_3){{\hat{A}}_1}^{\′}+({{\widehat{\mathrm{\λ}}}_0}^2-{{\widehat{\mathrm{\λ}}}_1}^2+{{\widehat{\mathrm{\λ}}}_2}^2-{{\widehat{\mathrm{\λ}}}_3}^2){{\hat{B}}_1}^{\′}+2({\widehat{\mathrm{\λ}}}_2{\widehat{\mathrm{\λ}}}_3-{\widehat{\mathrm{\λ}}}_0{\widehat{\mathrm{\λ}}}_1){{\hat{C}}_1}^{\′}-{\hat{B}}_1;$

$H_1=2({\widehat{\mathrm{\λ}}}_1{\widehat{\mathrm{\λ}}}_3-{\widehat{\mathrm{\λ}}}_0{\widehat{\mathrm{\λ}}}_2){{\hat{A}}_1}^{\′}+2({\widehat{\mathrm{\λ}}}_2{\widehat{\mathrm{\λ}}}_3+{\widehat{\mathrm{\λ}}}_0{\widehat{\mathrm{\λ}}}_1){{\hat{B}}_1}^{\′}+({{\widehat{\mathrm{\λ}}}_0}^2-{{\widehat{\mathrm{\λ}}}_1}^2-{{\widehat{\mathrm{\λ}}}_2}^2+{{\widehat{\mathrm{\λ}}}_3}^2){{\hat{C}}_1}^{\′}-{\hat{C}}_1;$

当有m对匹配平面时，

$A=\begin{array}{c}\left[\begin{array}{cccc}{\left(\frac{\∂F_1}{\∂{\widehat{\mathrm{\λ}}}_0}\right)}_0& {\left(\frac{\∂F_1}{\∂{\widehat{\mathrm{\λ}}}_1}\right)}_0& {\left(\frac{\∂F_1}{\∂{\widehat{\mathrm{\λ}}}_2}\right)}_0& {\left(\frac{\∂F_1}{\∂{\widehat{\mathrm{\λ}}}_3}\right)}_0\\ {\left(\frac{\∂G_1}{\∂{\widehat{\mathrm{\λ}}}_0}\right)}_0& {\left(\frac{\∂G_1}{\∂{\widehat{\mathrm{\λ}}}_1}\right)}_0& {\left(\frac{\∂G_1}{\∂{\widehat{\mathrm{\λ}}}_2}\right)}_0& {\left(\frac{\∂G_1}{\∂{\widehat{\mathrm{\λ}}}_3}\right)}_0\\ {\left(\frac{\∂H_1}{\∂{\widehat{\mathrm{\λ}}}_0}\right)}_0& {\left(\frac{\∂H_1}{\∂{\widehat{\mathrm{\λ}}}_1}\right)}_0& {\left(\frac{\∂H_1}{\∂{\widehat{\mathrm{\λ}}}_2}\right)}_0& {\left(\frac{\∂H_1}{\∂{\widehat{\mathrm{\λ}}}_3}\right)}_0\\ .& .& .& .\\ .& .& .& .\\ .& .& .& .\\ {\left(\frac{\∂F_i}{\∂{\widehat{\mathrm{\λ}}}_0}\right)}_0& {\left(\frac{\∂F_i}{\∂{\widehat{\mathrm{\λ}}}_1}\right)}_0& {\left(\frac{\∂F_i}{\∂{\widehat{\mathrm{\λ}}}_2}\right)}_0& {\left(\frac{\∂F_i}{\∂{\widehat{\mathrm{\λ}}}_3}\right)}_0\\ {\left(\frac{\∂G_i}{\∂{\widehat{\mathrm{\λ}}}_0}\right)}_0& {\left(\frac{\∂G_i}{\∂{\widehat{\mathrm{\λ}}}_1}\right)}_0& {\left(\frac{\∂G_i}{\∂{\widehat{\mathrm{\λ}}}_2}\right)}_0& {\left(\frac{\∂G_i}{\∂{\widehat{\mathrm{\λ}}}_3}\right)}_0\\ {\left(\frac{\∂H_i}{\∂{\widehat{\mathrm{\λ}}}_0}\right)}_0& {\left(\frac{\∂H_i}{\∂{\widehat{\mathrm{\λ}}}_1}\right)}_0& {\left(\frac{\∂H_i}{\∂{\widehat{\mathrm{\λ}}}_2}\right)}_0& {\left(\frac{\∂H_i}{\∂{\widehat{\mathrm{\λ}}}_3}\right)}_0\end{array}\right]\\ 3m\×4\end{array},w=\begin{array}{c}\left[\begin{array}{c}{F}_1({\mathrm{\λ}}_0^0,{\mathrm{\λ}}_1^0,{\mathrm{\λ}}_2^0,{\mathrm{\λ}}_3^0,A_1,B_1,C_1,{A_1}^{\′},{B_1}^{\′},{C_1}^{\′})\\ G_1({\mathrm{\λ}}_0^0,{\mathrm{\λ}}_1^0,{\mathrm{\λ}}_2^0,{\mathrm{\λ}}_3^0,A_1,B_1,C_1,{A_1}^{\′},{B_1}^{\′},{C_1}^{\′})\\ H_1({\mathrm{\λ}}_0^0,{\mathrm{\λ}}_1^0,{\mathrm{\λ}}_2^0,{\mathrm{\λ}}_3^0,A_1,B_1,C_1,{A_1}^{\′},{B_1}^{\′},{C_1}^{\′})\\ .\\ .\\ .\\ F_i({\mathrm{\λ}}_0^0,{\mathrm{\λ}}_1^0,{\mathrm{\λ}}_2^0,{\mathrm{\λ}}_3^0,A_i,B_i,C_i,{A_i}^{\′},{B_i}^{\′},{C_i}^{\′})\\ G_i({\mathrm{\λ}}_0^0,{\mathrm{\λ}}_1^0,{\mathrm{\λ}}_2^0,{\mathrm{\λ}}_3^0,A_i,B_i,C_i,{A_i}^{\′},{B_i}^{\′},{C_i}^{\′})\\ H_i({\mathrm{\λ}}_0^0,{\mathrm{\λ}}_1^0,{\mathrm{\λ}}_2^0,{\mathrm{\λ}}_3^0,A_i,B_i,C_i,{A_i}^{\′},{B_i}^{\′},{C_i}^{\′})\end{array}\right]\\ 3m\×1\end{array};$

$B=\begin{array}{c}\left[\begin{array}{ccccccccccccc}{\left(\frac{\∂F_1}{\∂{\hat{A}}_1}\right)}_0& {\left(\frac{\∂F_1}{\∂{\hat{B}}_1}\right)}_0& {\left(\frac{\∂F_1}{\∂{\hat{C}}_1}\right)}_0& {\left(\frac{\∂F_1}{\∂{{\hat{A}}_1}^{\′}}\right)}_0& {\left(\frac{\∂F_1}{\∂{{\hat{B}}_1}^{\′}}\right)}_0& {\left(\frac{\∂F_1}{\∂{{\hat{C}}_1}^{\′}}\right)}_0& \mathrm{...}& {\left(\frac{\∂F_i}{\∂{\hat{A}}_i}\right)}_0& {\left(\frac{\∂F_i}{\∂{\hat{B}}_i}\right)}_0& {\left(\frac{\∂F_i}{\∂{\hat{C}}_i}\right)}_0& {\left(\frac{\∂F_i}{\∂{{\hat{A}}_i}^{\′}}\right)}_0& {\left(\frac{\∂F_i}{\∂{{\hat{B}}_i}^{\′}}\right)}_0& {\left(\frac{\∂F_i}{\∂{{\hat{C}}_i}^{\′}}\right)}_0\\ {\left(\frac{\∂G_1}{\∂{\hat{A}}_1}\right)}_0& {\left(\frac{\∂G_1}{\∂{\hat{B}}_1}\right)}_0& {\left(\frac{\∂G_1}{\∂{\hat{C}}_1}\right)}_0& {\left(\frac{\∂G_1}{\∂{{\hat{A}}_1}^{\′}}\right)}_0& {\left(\frac{\∂G_1}{\∂{{\hat{B}}_1}^{\′}}\right)}_0& {\left(\frac{\∂G_1}{\∂{{\hat{C}}_1}^{\′}}\right)}_0& \mathrm{...}& {\left(\frac{\∂G_i}{\∂{\hat{A}}_i}\right)}_0& {\left(\frac{\∂G_i}{\∂{\hat{B}}_i}\right)}_0& {\left(\frac{\∂G_i}{\∂{\hat{C}}_i}\right)}_0& {\left(\frac{\∂G_i}{\∂{{\hat{A}}_i}^{\′}}\right)}_0& {\left(\frac{\∂G_i}{\∂{{\hat{B}}_i}^{\′}}\right)}_0& {\left(\frac{\∂G_i}{\∂{{\hat{C}}_i}^{\′}}\right)}_0\\ {\left(\frac{\∂H_1}{\∂{\hat{A}}_1}\right)}_0& {\left(\frac{\∂H_1}{\∂{\hat{B}}_1}\right)}_0& {\left(\frac{\∂H_1}{\∂{\hat{C}}_1}\right)}_0& {\left(\frac{\∂H_1}{\∂{{\hat{A}}_1}^{\′}}\right)}_0& {\left(\frac{\∂H_1}{\∂{{\hat{B}}_1}^{\′}}\right)}_0& {\left(\frac{\∂H_1}{\∂{{\hat{C}}_1}^{\′}}\right)}_0& \mathrm{...}& {\left(\frac{\∂H_i}{\∂{\hat{A}}_i}\right)}_0& {\left(\frac{\∂H_i}{\∂{\hat{B}}_i}\right)}_0& {\left(\frac{\∂H_i}{\∂{\hat{C}}_i}\right)}_0& {\left(\frac{\∂H_i}{\∂{{\hat{A}}_i}^{\′}}\right)}_0& {\left(\frac{\∂H_i}{\∂{{\hat{B}}_i}^{\′}}\right)}_0& {\left(\frac{\∂H_i}{\∂{{\hat{C}}_i}^{\′}}\right)}_0\\ .& .& .& .& .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .& .& .& .& .\\ .& .& .& .& .& .& .& .& .& .& .& .& .\\ {\left(\frac{\∂F_i}{\∂{\hat{A}}_i}\right)}_0& {\left(\frac{\∂F_i}{\∂{\hat{B}}_i}\right)}_0& {\left(\frac{\∂F_i}{\∂{\hat{C}}_i}\right)}_0& {\left(\frac{\∂F_i}{\∂{{\hat{A}}_i}^{\′}}\right)}_0& {\left(\frac{\∂F_i}{\∂{{\hat{B}}_i}^{\′}}\right)}_0& {\left(\frac{\∂F_i}{\∂{{\hat{C}}_i}^{\′}}\right)}_0& \mathrm{...}& {\left(\frac{\∂F_i}{\∂{\hat{A}}_i}\right)}_0& {\left(\frac{\∂F_i}{\∂{\hat{B}}_i}\right)}_0& {\left(\frac{\∂F_i}{\∂{\hat{C}}_i}\right)}_0& {\left(\frac{\∂F_i}{\∂{{\hat{A}}_i}^{\′}}\right)}_0& {\left(\frac{\∂F_i}{\∂{{\hat{B}}_i}^{\′}}\right)}_0& {\left(\frac{\∂F_i}{\∂{{\hat{C}}_i}^{\′}}\right)}_0\\ {\left(\frac{\∂G_i}{\∂{\hat{A}}_i}\right)}_0& {\left(\frac{\∂G_i}{\∂{\hat{B}}_i}\right)}_0& {\left(\frac{\∂G_i}{\∂{\hat{C}}_i}\right)}_0& {\left(\frac{\∂G_i}{\∂{{\hat{A}}_i}^{\′}}\right)}_0& {\left(\frac{\∂G_i}{\∂{{\hat{B}}_i}^{\′}}\right)}_0& {\left(\frac{\∂G_i}{\∂{{\hat{C}}_i}^{\′}}\right)}_0& \mathrm{...}& {\left(\frac{\∂G_i}{\∂{\hat{A}}_i}\right)}_0& {\left(\frac{\∂G_i}{\∂{\hat{B}}_i}\right)}_0& {\left(\frac{\∂G_i}{\∂{\hat{C}}_i}\right)}_0& {\left(\frac{\∂G_i}{\∂{{\hat{A}}_i}^{\′}}\right)}_0& {\left(\frac{\∂G_i}{\∂{{\hat{B}}_i}^{\′}}\right)}_0& {\left(\frac{\∂G_i}{\∂{{\hat{C}}_i}^{\′}}\right)}_0\\ {\left(\frac{\∂H_i}{\∂{\hat{A}}_i}\right)}_0& {\left(\frac{\∂H_i}{\∂{\hat{B}}_i}\right)}_0& {\left(\frac{\∂H_i}{\∂{\hat{C}}_i}\right)}_0& {\left(\frac{\∂H_i}{\∂{{\hat{A}}_i}^{\′}}\right)}_0& {\left(\frac{\∂H_i}{\∂{{\hat{B}}_i}^{\′}}\right)}_0& {\left(\frac{\∂H_i}{\∂{{\hat{C}}_i}^{\′}}\right)}_0& \mathrm{...}& {\left(\frac{\∂H_i}{\∂{\hat{A}}_i}\right)}_0& {\left(\frac{\∂H_i}{\∂{\hat{B}}_i}\right)}_0& {\left(\frac{\∂H_i}{\∂{\hat{C}}_i}\right)}_0& {\left(\frac{\∂H_i}{\∂{{\hat{A}}_i}^{\′}}\right)}_0& {\left(\frac{\∂H_i}{\∂{{\hat{B}}_i}^{\′}}\right)}_0& {\left(\frac{\∂H_i}{\∂{{\hat{C}}_i}^{\′}}\right)}_0\end{array}\right]\\ 3m\×6m\end{array};$

对约束条件方程线性化得：

$2\[\begin{array}{cccc}{\mathrm{\λ}}_0^0& {\mathrm{\λ}}_1^0& {\mathrm{\λ}}_2^0& {\mathrm{\λ}}_3^0\end{array}\]\left[\begin{array}{c}{x}_{{\widehat{\mathrm{\λ}}}_0}\\ x_{{\widehat{\mathrm{\λ}}}_1}\\ x_{{\widehat{\mathrm{\λ}}}_2}\\ x_{{\widehat{\mathrm{\λ}}}_3}\end{array}\right]+({\left({\mathrm{\λ}}_0^0\right)}^2+{\left({\mathrm{\λ}}_1^0\right)}^2+{\left({\mathrm{\λ}}_2^0\right)}^2+{\left({\mathrm{\λ}}_3^0\right)}^2-1)=0;$

令

b.2223根据Gauss-Helmert平差模型和测量平差的原理可知:

$x_{{\widehat{\mathrm{\λ}}}_j}=(N_{aa}^{-1}-N_{aa}^{-1}{C}^T{N}_{cc}^{-1}{\mathrm{CN}}_{aa}^{-1})w_e+N_{aa}^{-1}{C}^T{N}_{cc}^{-1}{w}_x,$

其中，P为单位权阵；

将和代入即可得到λ₀、λ₁、λ₂、λ₃的平差值，得到旋转矩阵的平差值；

b.3计算平移量t

由式D₁＝t·n₁′+D₁′可得D_i＝t·n_i′+D_i′，其中i为平面编号；利用m对匹配平面，由平差知识得到：t＝(N^TN)^-1N^TL；

式中，