一种消除数值错误的三点位姿估计方法

本发明涉及一种消除数值错误的三点位姿估计方法，属于视觉测量。

背景技术：

1、相机位姿估计在计算机视觉、摄影测量、机器人和增强现实领域都有广泛应用。相机的姿态可以用旋转矩阵和平移向量描述，即相机的外参。通过建立世界坐标系和平面像素坐标系来描述特征点的3d-2d匹配关系，以此求解出相机的外参，这个过程叫做pnp问题。当n＝3时，这个问题也被称为透视三点问题(p3p)，目的是通过三个2d-3d点对应关系来确定摄像机在世界参考系中的位置和方向。

2、现有的p3p算法大多都把问题转化为求解四次方程，但这些算法在求解过程中都出现了一定程度上的数值错误，导致三点位姿估计结果不够准确。

技术实现思路

1、目的：为了克服现有技术中存在的不足，本发明提供一种消除数值错误的三点位姿估计方法，该方法考虑了传统求解四次方程过程中的数值错误，将p3p问题转化为求解三次方程，运用代数方法消除了数值错误问题，使得三点位姿估计结果准确性显著提高。

2、技术方案：为解决上述技术问题，本发明采用的技术方案为：

3、第一方面，本发明提供一种消除数值错误的三点位姿估计方法，包括：

4、根据相机的小孔成像模型构建透视图变换方程，在透视图变换方程中引入新的中间坐标系，转换得到新的变换方程；其中所述透视图变换方程中包括世界坐标系下的第一旋转矩阵和第一平移向量所述新的变换方程中包括新的中间坐标系下的第二旋转矩阵和第二平移向量

5、对新的变换方程进行参数化，表示为非齐次线性方程；

6、根据第二旋转矩阵的性质，得到两个二次方程；将两个二次方程的实解看为两个二次曲线的实交点，并用矩阵形式表示；

7、基于所述两个二次曲线的实交点，构建一个退化二次曲线得到一元参数方程，对一元参数方程进行求解计算得到退化二次曲线的值；

8、将退化二次曲线分解为最多两条直线p和q；利用代数法计算退化二次曲线分解的直线与两条二次曲线的交点，得到两个二次方程的实解；

9、基于新的变换方程和非齐次线性方程，根据所述两个二次方程的实解计算得到第二旋转矩阵和第二平移向量的值；

10、根据第二旋转矩阵和第二平移向量的值计算得到世界坐标系下的第一旋转矩阵和第一平移向量的值，得到世界坐标系下的相机位姿估计结果。

11、在一些实施例中，根据相机的小孔成像模型构建透视图变换方程，在透视图变换方程中引入新的中间坐标系，转换得到新的变换方程，包括：

12、根据校准后的相机针孔模型，构建透视图变换方程如下：

13、

14、其中且||mi||＝1，di表示第i个参考点到相机中心的距离，旋转矩阵(3)和平移向量定义从世界坐标系帧到相机帧的转换；pi、mi分别表示三维参考点及其对应的归一化图像点；w指世界坐标系；

15、引入新的中间坐标系o，得变换矩阵n＝[nx ny nz]：

16、

17、

18、ny＝nx×nz

19、得到：

20、

21、其中中间参数p1′、p2′、p3′分别表示三维参考点p1、p2、p3在新的中间坐标系o下的坐标；

22、将透视图变换方程转换为新的变换方程，如下：

23、

24、其中

25、

26、

27、第二旋转矩阵和第二平移向量定义从新的中间坐标系帧到相机帧的转换，xi、yi、zi表示第i个参考点世界坐标系的三轴坐标，r1至r9表示第二旋转矩阵参数，tx、ty、tz表示第二平移向量参数。

28、在一些实施例中，对新的变换方程进行参数化，表示为非齐次线性方程，包括：

29、将构建的新的变换方程表示为非齐次线性方程：

30、mv＝d，

31、其中，

32、

33、v＝[s1 s2 s3 s4 s5 s6]t,s1、s2、s3、s4、s5、s6表示待解向量v中的参数；

34、

35、

36、并且

37、

38、v参数化为：v＝α1b1+α2b2+b3,

39、其中α1、α2表示两个未知参数；

40、

41、

42、从组成部分b5,1还有b6,2，得到α1＝s5和α2＝s6。

43、在一些实施例中，根据第二旋转矩阵的性质，得到两个二次方程，包括：

44、根据第二旋转矩阵以及得

45、r1r2+r4r5+r7r8＝0,

46、

47、

48、由此得两个二次方程：

49、

50、

51、其中中间参数g0至g4，h0至h5为二次方程的系数；

52、

53、在一些实施例中，将两个二次方程的实解看为两个二次曲线的实交点，并用矩阵表示，包括：

54、

55、

56、将两个二次方程写成如下矩阵表示：

57、[1 s5 s6]c1[1 s5 s6]t＝0,

58、[1 s5 s6]c2[1 s5 s6]t＝0.

59、其中c1、c2为3×3矩阵；

60、

61、

62、在一些实施例中，基于所述两个二次曲线的实交点，构建一个退化二次曲线得到一元参数方程，对一元参数方程进行求解计算得到退化二次曲线的值，包括：

63、构建一个3×3的矩阵表示退化二次曲线c：

64、

65、由f(σ)＝det(c)＝det(c1+σc2)＝0得到关于σ的一元参数方程：

66、k3σ3+k2σ2+k1σ+k0＝0.

67、其中k3、k2、k1、k0均为一元参数方程的系数；

68、

69、

70、

71、

72、对一元参数方程求解得到σ值，根据σ值计算得到退化二次曲线c的值。

73、在一些实施例中，将退化二次曲线c分解为最多两条直线p和q，包括：

74、退化二次曲线c包含两条直线p和q或一对重复的直线，写成：

75、c＝pqt+qpt.

76、令

77、

78、p＝[p1 p2 p3]t,q＝[q1 q2 q3]t

79、则退化二次曲线c写成：

80、

81、先求两条直线的交点v＝p×q，v在c的零空间中，取c的任何零向量n，已知v＝αn；找到α∈r，使v的尺度与c(和p,q)一致；

82、v＝[p2q3-p3q2 p3q1-p1q3 p1q2-p2q1]

83、则

84、

85、因此，对n进行适当的缩放，得到在正确比例下的交点v；

86、再提取直线，v的斜对称矩阵[v]×为：

87、[v]×＝pqt-qpt,

88、定义一个新的矩阵d：

89、d＝c+[v]×＝2pqt

90、如果退化二次曲线c包含两条直线p和q，则退化二次曲线c的秩为2；{p,q}对从d的一行和对应的列中找到；

91、如果退化二次曲线c包含一对重复的直线，则退化二次曲线c的秩为1；直接从一行或列中恢复重复行。

92、在一些实施例中，利用代数法计算退化二次曲线分解的直线与两条二次曲线的交点，得到两个二次方程的实解，包括：

93、计算这两条直线和两条二次曲线的交点,假设方程为：

94、p1+p2s5+p3s6＝0

95、q1+q2s5+q3s6＝0

96、计算两条直线分别与两条二次曲线的交点：

97、记两条直线p1+p2s5+p3s6＝0与q1+q2s5+q3s6＝0和二次曲线的交点为sj＝{s5j,s6j}，j∈{0,1,2,3,4}；

98、记两条直线p1+p2s5+p3s6＝0与q1+q2s5+q3s6＝0和二次曲线的交点为sk＝{s5k,s6k}，k∈{0,1,2,3,4}；

99、判断sj与sk是否相等，相等则记为st＝{s5t,s6t}，t∈{0,1,2,3,4}，为两个二次方程的实解，根据两个二次方程的实解确定s5、s6。

100、在一些实施例中，基于新的变换方程和非齐次线性方程，根据所述两个二次方程的实解计算得到第二旋转矩阵和第二平移向量的值，包括：

101、求解得到s5,s6后，由非齐次线性方程中v＝α1b1+α2b2+b3,计算出v中剩余的四个参数s1、s2、s3、s4；

102、根据s1、s2、s3、s4、s5、s6计算得到比例因子d1：

103、

104、将d1代入分别求得tx，ty，tz，r1，r2，r4，r5，r7和r8；被忽略的参数r3、r6和r9由[r1 r4r7]t叉乘[r2 r5 r8]t得到；从而得到和

105、在一些实施例中，根据第二旋转矩阵和第二平移向量的值计算得到世界坐标系下的第一旋转矩阵和第一平移向量的值，包括：

106、

107、

108、其中，n为新的中间坐标系中的变换矩阵，p1为第1个三维参考点。

109、第二方面，本发明提供了一种消除数值错误的三点位姿估计装置，包括处理器及存储介质；

110、所述存储介质用于存储指令；

111、所述处理器用于根据所述指令进行操作以执行根据第一方面所述的方法。

112、第三方面，本发明提供了一种设备，包括，

113、存储器；

114、处理器；

115、以及

116、计算机程序；

117、其中，所述计算机程序存储在所述存储器中，并被配置为由所述处理器执行以实现上述第一方面所述的方法。

118、有益效果:本发明提供的消除数值错误的三点位姿估计方法，具有以下优点：本发明考虑了代数求解过程中出现的系数等数值错误，使用代数方法将p3p问题转化为求解三次方程，消除了转化过程中的数值错误，更加精确迅速的求出相机的绝对位姿。