一种稳健的绝对定向算法
阅读说明：本技术 一种稳健的绝对定向算法 (Steady absolute orientation algorithm ) 是由 张雄锋 于 2021-07-13 设计创作，主要内容包括：本发明涉及一种稳健的绝对定向算法,其包括步骤S1,构建基于加权系数的绝对定向问题求解模型；步骤S2,将每个坐标点的权值设置为步骤S3,推导绝对定向问题求解模型的解析求解算法,计算S2中权值对应的旋转矩阵R和平移向量t；步骤S4,计算重合误差e-(i)＝||Rr’-(A,i)-r’-(B,i)||,使用S估计方法更新权值后,进入步骤S3,计算当前权值对应的旋转矩阵和平移向量；步骤S5,不断循环步骤S4和S3,当S估计方法的迭代次数超过设置的最大迭代次数时,输出旋转矩阵R和平移向量t。本发明通过绝对定向问题求解模型的构建和S估计方法更新权值的应用,再通过绝对定向问题求解模型的解析算法,可自动识别粗差点,提高算法的稳健性,保证算法的求解精度。(The invention relates to a steady absolute orientation algorithm, which comprises a step S1 of constructing a weighting coefficient-based absolute orientation problem solving model; step S2, setting the weight value of each coordinate point as Step S3, deducing an analytic solving algorithm of an absolute orientation problem solving model, and calculating a rotation matrix R and a translational vector t corresponding to the weight in S2; step S4, calculating the superposition error e i ＝||Rr' A,i ‑r' B,i If the weight is updated by S estimation method, thenStep S3, calculating a rotation matrix and a translation vector corresponding to the current weight; and step S5, continuously looping the steps S4 and S3, and outputting the rotation matrix R and the translation vector t when the iteration number of the S estimation method exceeds the set maximum iteration number. According to the method, the coarse difference point can be automatically identified through the construction of the absolute orientation problem solving model and the application of the weight value updated by the S estimation method, and then through the analytic algorithm of the absolute orientation problem solving model, the robustness of the algorithm is improved, and the solving precision of the algorithm is ensured.)
技术领域
本发明涉及摄影测量领域，具体说是一种稳健的绝对定向算法。
背景技术
根据给定的n个空间点在两个坐标系A、B中的三维坐标，求解坐标系A相对于坐标系B的旋转矩阵R和平移向量t的问题，称为绝对定向问题。绝对定向是计算机视觉中的一个重要而基础的问题，在点云匹配、摄影测量学等领域具有广泛应用。
目前绝对定向问题的求解算法，主要有解析算法和迭代优化算法两类。其中解析算法主要有基于正交矩阵的绝对定向算法、基于四元数的绝对定向算法等，这些算法的主要思想是通过矩阵最优化技术，求解得到绝对定向问题的最优解。迭代优化算法包括光束平差算法等非线性优化方法，这类算法的主要思想是使用最小二乘法等数值最优化求解算法，通过不断迭代减小残差，直到残差小于阈值后，得到绝对定向问题的近似最优解。
在使用实际数据求解绝对定向问题的过程中，三维空间点的测量往往存在观测噪声和粗差。例如在点云配准中，不同方向扫描得到的三维点云数据之间进行配准时，由于测量精度有限、点云密度不同、部分点云数据缺失、特征点匹配错误等原因，导致对应坐标存在粗差。传统的两种解析算法均是在理想情况下进行求解的，并未考虑粗差的影响，且在求解过程中使用了算术均值等非稳健统计量，导致两种求解方法稳健性不强。基于MF估计子的绝对定向算法通过对误差和粗差因素进行建模，建立起可抑制误差影响的目标函数，在一定程度上减少了粗差的影响，然而该算法并未给出解析解，求解效率受到限制。
发明内容
针对上述问题，本发明提供一种自动识别粗差点、提高算法稳健性的稳健的绝对定向算法。
本发明解决上述技术问题所采用的技术方案为：一种稳健的绝对定向算法，包括以下步骤：
S1、构建基于加权系数的绝对定向问题求解模型，
假设n个空间点在坐标系A和B之中的三维坐标分别为rA,i和rB,i(i＝1,2,…,n)，通过加权的方法，使用加权形式的绝对定向问题求解模型
的最优解作为旋转矩阵和平移向量的估计值，其中ωi≥0表示第i个坐标点对应的权值且SO(3)表示行列式为1的三阶正交矩阵集合，|| ||表示2范数；
S2、将每个坐标点的权值设置为即
S3、推导绝对定向问题求解模型的解析求解算法，得到(2)式的最优解，计算S2中权值对应的旋转矩阵R和平移向量t，
记A、B坐标系中的加权质心表达式为和(2)式的模型中，旋转矩阵和平移向量的求解存在耦合，为进行解耦，引入第i个空间点的新坐标和理想情况下其中Rtrue和ttrue分别为旋转矩阵和平移向量的真值，进而有
r'B,i＝Rtruer'A,i， (3)
成立，因此，先通过求解最优化模型
得到当前权值对应的旋转矩阵R*，再通过
得到位移参数中的平移向量t*，从而实现优化参数解耦的目的，
由于
前两项与旋转矩阵无关，因此，(4)式的优化模型等价于
其中tr()为矩阵的迹函数，为求解(7)式，将矩阵M进行奇异值分解得到其中U和V为3×3的正交矩阵，σj≥0(j＝1,2,3)为矩阵M的奇异值，当
R*＝VTU, (8)
(7)式取得最大值，
为得到R*对应的平移向量，将(5)式对t求导得令该导数为0，得到权值对应的平移向量为
S4、计算重合误差ei＝||Rr'A,i-r'B,i||，使用S估计方法更新权值后，进入步骤S3，根据(8)式计算当前权值对应的旋转矩阵，并计算重合误差，再依据(9)式计算当前权值对应的平移向量；
S5、不断循环步骤S4和S3，当S估计方法的迭代次数超过设置的最大迭代次数时，输出旋转矩阵R和平移向量t。
作为优选，所述步骤S4中S估计方法具体为，
当循环次数为1时，对误差进行归一化其中为尺度估计量，表示的|ei|中位数，0.6754为S估计中的常数，使用
作为新权值，当循环次数大于1的时候，则使用对观测误差进行归一化使用
作为新的权值。
与现有技术相比，本发明具有以下有益效果：
1、本发明通过绝对定向问题求解模型的构建和S估计方法更新权值的应用，再通过绝对定向问题求解模型的解析算法，可自动识别粗差点，有效抑制粗差点带来的影响，在粗差点较多的情况下，仍能保证求解精度，提高绝对定向算法的稳健性；
2、本发明中基于S估计方法提出了一种迭代式的权值计算方法，该方法可自动识别粗差点，并自动计算各坐标点合适的权值，实现对粗差点的抑制。
附图说明
图1是旋转矩阵和平移向量的平均解算误差随着粗差点百分比增长的变化情况；
其中，图(a)对应旋转矩阵误差，图(b)对应平移向量误差。
具体实施方式
下面将结合图1详细说明本发明，在此本发明的示意性实施例以及说明用来解释本发明，但并不作为对本发明的限定，本发明的绝对定向算法即为Proposed method算法。
一种稳健的绝对定向算法，其包括以下步骤：
S1、构建基于加权系数的绝对定向问题求解模型，
假设n个空间点在坐标系A和B之中的三维坐标分别为rA,i和rB,i(i＝1,2,…,n)，通过加权的方法，使用加权形式的绝对定向问题求解模型
的最优解作为旋转矩阵和平移向量的估计值，其中ωi≥0表示第i个坐标点对应的权值且SO(3)表示行列式为1的三阶正交矩阵集合，|| ||表示2范数,如果权值的选择满足第i个坐标点的粗差程度越大，ωi越接近于0的原则，则可有效减小粗差点对(2)式优化目标函数的影响，从而通过求解(2)式的最优化模型可得到正确的旋转矩阵和平移向量；
S2、将每个坐标点的权值设置为即
S3、推导绝对定向问题求解模型的解析求解算法，得到(2)式的最优解，计算S2中权值对应的旋转矩阵R和平移向量t，
记A、B坐标系中的加权质心表达式为和(2)式的模型中，旋转矩阵和平移向量的求解存在耦合，为进行解耦，引入第i个空间点的新坐标和理想情况下其中Rtrue和ttrue分别为旋转矩阵和平移向量的真值，进而有
r'B,i＝Rtruer'A,i， (3)
成立，因此，先通过求解最优化模型
得到当前权值对应的旋转矩阵R*，再通过
得到位移参数中的平移向量t*，从而实现优化参数解耦的目的，
由于
前两项与旋转矩阵无关，因此，(4)式的优化模型等价于
其中tr()为矩阵的迹函数，为求解(7)式，将矩阵M进行奇异值分解得到其中U和V为3×3的正交矩阵，σj≥0(j＝1,2,3)为矩阵M的奇异值，当
R*＝VTU, (8)
(7)式取得最大值，
为得到R*对应的平移向量，将(5)式对t求导得令该导数为0，得到权值对应的平移向量为
S4、计算重合误差ei＝||Rr'A,i-r'B,i||，使用S估计方法更新权值后，进入步骤S3，根据(8)式计算当前权值对应的旋转矩阵，并计算重合误差，再依据(9)式计算当前权值对应的平移向量，
当循环次数为1时，对误差进行归一化其中为尺度估计量，表示的|ei|中位数，0.6754为S估计中的常数，使用
作为新权值，当循环次数大于1的时候，则使用对观测误差进行归一化使用
作为新的权值；
S5、不断循环步骤S4和S3，当S估计方法的迭代次数超过设置的最大迭代次数时，输出旋转矩阵R和平移向量t。
为验证本发明所提出的稳健绝对定向问题求解算法的有效性，在稳健性方面与基于正交矩阵的经典AbsPose算法进行对比。
在以下的仿真实验中，空间点坐标、平移向量坐标单位均为毫米。仿真测试中的旋转矩阵为随机生成的SO(3)矩阵，平移向量t的三个分量为在[0,1000]中选取的随机数。A坐标系中的20个理想空间点坐标均匀分布在[-30,30]×[-30,30]×[-30,30]中，将A坐标系中的理想空间点坐标进行旋转和平移后，得到B坐标系中的理想空间点坐标。为测试稳健性，首先将A、B坐标系中的理想空间点坐标加入均值为0，方差为0.2的高斯噪声，然后在B坐标系中随机选择m(m＝1,...，10)个点，分别加入[0,50]的均匀分布噪声用以仿真粗差。旋转矩阵的误差定义为其中rij,true和rij分别表示旋转矩阵真值和解算值的第i行第j列的元素，平移向量误差定义为其中ttrue和t分别表示平移向量真值和解算值。对于每个m，进行独立实验1000次。
图1是旋转矩阵和平移向量的平均解算误差随着粗差点百分比增长的变化情况，其中图(a)对应旋转矩阵误差，图(b)对应平移向量误差，圆点为AbsPose算法的解算结果，方点为本发明提出的算法的解算结果。从图中可看出，随着粗差点个数的增加，AbsPose算法的旋转矩阵误差和平移向量误差均呈现增长的趋势，且增长速度远大于本发明提出的稳健绝对定向问题解算算法。在粗差点百分比相同的情况下，本发明提出的算法可大幅提高旋转矩阵和平移向量的测量精度。当粗差点百分比为50％时，AbsPose算法得到的平均平移向量相对误差高达74.0％，而本发明算法的平移向量相对误差仅有2.3％，较AbsPose提高了一个量级的求解精度。因此，本发明所提的算法可有效抑制粗差点带来的影响，在粗差点较多的情况下，仍能保证求解精度，具有良好的稳健性。
以上对本发明实施例所提供的技术方案进行了详细介绍，本文中应用了具体个例对本发明实施例的原理以及实施方式进行了阐述，以上实施例的说明只适用于帮助理解本发明实施例的原理；同时，对于本领域的一般技术人员，依据本发明实施例，在具体实施方式以及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。