1
+ /**
2
+ * @class CoordTransfer
3
+ * @version 11.2.0
4
+ * @private
5
+ * @classdesc 坐标转换
6
+ * @param {Object } options - 配准参数。
7
+ * @param {number } [options.pitch] - 俯仰角。
8
+ * @param {number } [options.roll] - 侧偏角。
9
+ * @param {number } [options.yaw] - 偏航角。
10
+ * @param {number } [options.x] - 视频 x 坐标。
11
+ * @param {number } [options.y] - 视频 y 坐标。
12
+ * @param {number } [options.z] - 视频 z 坐标。
13
+ * @param {number } [options.fx] - 水平视场角。
14
+ * @param {number } [options.fy] - 垂直视场角。
15
+ * @param {number } [options.centerX] - 相机中心的水平坐标。
16
+ * @param {number } [options.centerY] - 相机中心的垂直坐标。
17
+ */
18
+
19
+ export default class CoordTransfer {
20
+ constructor ( cv , configs ) {
21
+ this . configs = configs ;
22
+ this . cv = cv ;
23
+ }
24
+
25
+ /**
26
+ * @function CoordTransfer.prototype.init
27
+ * @description 初始化。
28
+ */
29
+ init ( ) {
30
+ return new Promise ( ( resolve ) => {
31
+ this . cv . then ( ( ) => {
32
+ this . setCameraLocation ( this . configs ) ;
33
+ resolve ( this ) ;
34
+ } ) ;
35
+ } ) ;
36
+ }
37
+
38
+ /**
39
+ * @function CoordTransfer.prototype.setCameraLocation
40
+ * @description 设置视频配准参数。
41
+ * @param {Object } options - 配准参数。
42
+ * @param {Object } [options.pitch] - 俯仰角。
43
+ * @param {Object } [options.roll] - 侧偏角。
44
+ * @param {Object } [options.yaw] - 偏航角。
45
+ * @param {Object } [options.x] - 视频 x 坐标。
46
+ * @param {Object } [options.y] - 视频 y 坐标。
47
+ * @param {Object } [options.z] - 视频 z 坐标。
48
+ * @param {Object } [options.fx] - 相机水平焦距。
49
+ * @param {Object } [options.fy] - 相机垂直焦距。
50
+ * @param {Object } [options.centerX] - 相机中心的水平坐标。
51
+ * @param {Object } [options.centerY] - 相机中心的垂直坐标。
52
+ */
53
+ setCameraLocation ( configs ) {
54
+ const { pitch, roll, yaw, x, y, z, fx, fy, centerX, centerY } = configs ;
55
+ if ( ! this . cv ) {
56
+ return ;
57
+ }
58
+ this . rotationMatrix = this . toRotationMatrix ( pitch , roll , yaw ) ;
59
+ this . translationMatrix = this . toTranslationMatrix ( x , y , z ) ;
60
+ this . k = this . toCameraMatrix ( fx , fy , centerX , centerY ) ;
61
+ }
62
+
63
+ /**
64
+ * @function CoordTransfer.prototype.toRotationMatrix
65
+ * @description 计算旋转矩阵。
66
+ * @param {number } pitch - 俯仰角
67
+ * @param {number } roll - 侧偏角
68
+ * @param {number } yaw - 偏航角
69
+ * @returns {Array } 旋转矩阵。
70
+ */
71
+ toRotationMatrix ( pitch , roll , yaw ) {
72
+ if ( ! this . cv ) {
73
+ return ;
74
+ }
75
+ let x = ( pitch - 90 ) * ( Math . PI / 180 ) ; // pitch
76
+ let y = roll * ( Math . PI / 180 ) ; // roll
77
+ let z = yaw * - 1 * ( Math . PI / 180 ) ; // yaw
78
+ let rotationMatrix = new this . cv . Mat ( 3 , 1 , this . cv . CV_64FC1 ) ;
79
+ let rx = this . cv . matFromArray ( 3 , 3 , this . cv . CV_64FC1 , [
80
+ 1 ,
81
+ 0 ,
82
+ 0 ,
83
+ 0 ,
84
+ Math . cos ( x ) ,
85
+ Math . sin ( x ) ,
86
+ 0 ,
87
+ - Math . sin ( x ) ,
88
+ Math . cos ( x )
89
+ ] ) ;
90
+ let ry = this . cv . matFromArray ( 3 , 3 , this . cv . CV_64FC1 , [
91
+ Math . cos ( y ) ,
92
+ 0 ,
93
+ - Math . sin ( y ) ,
94
+ 0 ,
95
+ 1 ,
96
+ 0 ,
97
+ Math . sin ( y ) ,
98
+ 0 ,
99
+ Math . cos ( y )
100
+ ] ) ;
101
+
102
+ let rz = this . cv . matFromArray ( 3 , 3 , this . cv . CV_64FC1 , [
103
+ Math . cos ( z ) ,
104
+ Math . sin ( z ) ,
105
+ 0 ,
106
+ - Math . sin ( z ) ,
107
+ Math . cos ( z ) ,
108
+ 0 ,
109
+ 0 ,
110
+ 0 ,
111
+ 1
112
+ ] ) ;
113
+
114
+ let tempResult = new this . cv . Mat ( 3 , 3 , this . cv . CV_64FC1 ) ;
115
+ this . cv . gemm ( rx , ry , 1 , new this . cv . Mat ( ) , 0 , tempResult ) ;
116
+ this . cv . gemm ( tempResult , rz , 1 , new this . cv . Mat ( ) , 0 , tempResult ) ;
117
+ this . cv . Rodrigues ( tempResult , rotationMatrix ) ;
118
+ return rotationMatrix ;
119
+ }
120
+
121
+ /**
122
+ * @function CoordTransfer.prototype.toTranslationMatrix
123
+ * @description 计算偏移矩阵
124
+ * @param {number } x - 视频 x 坐标
125
+ * @param {number } y - 视频 y 坐标
126
+ * @param {number } z - 视频 z 坐标
127
+ * @returns {Array } 平移矩阵。
128
+ */
129
+ toTranslationMatrix ( x , y , z ) {
130
+ if ( ! this . cv ) {
131
+ return ;
132
+ }
133
+ let translationMatrix = new this . cv . Mat ( ) ;
134
+ let tvecs = this . cv . matFromArray ( 3 , 1 , this . cv . CV_64FC1 , [ - x , - y , - z ] ) ;
135
+
136
+ let rotationMatrix = new this . cv . Mat ( 3 , 3 , this . cv . CV_64FC1 ) ;
137
+ this . cv . Rodrigues ( this . rotationMatrix , rotationMatrix ) ;
138
+ this . cv . gemm ( rotationMatrix , tvecs , 1.0 , new this . cv . Mat ( ) , 0 , translationMatrix ) ;
139
+ return translationMatrix ;
140
+ }
141
+
142
+ /**
143
+ * @function CoordTransfer.prototype.toCameraMatrix
144
+ * @description 计算相机矩阵。
145
+ * @param {Object } fx - 水平焦距。
146
+ * @param {Object } fy - 垂直焦距。
147
+ * @param {Object } centerX - 相机中心的水平坐标
148
+ * @param {Object } centerY - 相机中心的垂直坐标。
149
+ * @returns {Array } 视频矩阵。
150
+ */
151
+ toCameraMatrix ( fx , fy , centerX , centerY ) {
152
+ return this . cv . matFromArray ( 3 , 3 , this . cv . CV_64FC1 , [ fx , 0 , centerX , 0 , fy , centerY , 0 , 0 , 1 ] ) ;
153
+ }
154
+ /**
155
+ * @function CoordTransfer.prototype.toVideoCoordinate
156
+ * @description 转换视频像素坐标到空间地理坐标。
157
+ * @param {Array } coord - 空间坐标。
158
+ * @returns {Array } 视频像素坐标。
159
+ */
160
+ toVideoCoordinate ( coord ) {
161
+ if ( ! this . cv || ! this . cv . Mat ) {
162
+ return [ ] ;
163
+ }
164
+ let emptyMat = new this . cv . Mat ( ) ;
165
+ let point3 = this . cv . matFromArray ( 3 , 1 , this . cv . CV_64FC1 , [ coord [ 0 ] , coord [ 1 ] , 0 ] ) ;
166
+ let point2 = new this . cv . Mat ( 2 , 1 , this . cv . CV_64FC1 ) ;
167
+ this . cv . projectPoints ( point3 , this . rotationMatrix , this . translationMatrix , this . k , emptyMat , point2 ) ;
168
+ return point2 ;
169
+ }
170
+ /**
171
+ * @function CoordTransfer.prototype.toSpatialCoordinate
172
+ * @description 转换视频像素坐标到空间地理坐标
173
+ * @param {Array } point - 像素坐标。
174
+ * @returns {Array } 空间地理坐标。
175
+ */
176
+ toSpatialCoordinate ( videoPoint ) {
177
+ if ( ! this . cv || ! this . cv . Mat ) {
178
+ return [ ] ;
179
+ }
180
+ let uvPoint = new this . cv . matFromArray ( 3 , 1 , this . cv . CV_64FC1 , [ videoPoint [ 0 ] , videoPoint [ 1 ] , 1.0 ] ) ;
181
+ let rotationMatrix = new this . cv . Mat ( 3 , 3 , this . cv . CV_64FC1 ) ;
182
+ this . cv . Rodrigues ( this . rotationMatrix , rotationMatrix ) ;
183
+ let tempMat = new this . cv . Mat ( ) ;
184
+ this . cv . gemm ( rotationMatrix . inv ( 3 ) , this . k . inv ( 3 ) , 1 , new this . cv . Mat ( ) , 0 , tempMat ) ;
185
+ this . cv . gemm ( tempMat , uvPoint , 1 , new this . cv . Mat ( ) , 0 , tempMat ) ;
186
+ let tempMat2 = new this . cv . Mat ( ) ;
187
+ this . cv . gemm ( rotationMatrix . inv ( 3 ) , this . translationMatrix , 1 , new this . cv . Mat ( ) , 0 , tempMat2 ) ;
188
+ let zConst = 0 ;
189
+ let s = zConst + tempMat2 . data64F [ 2 ] ;
190
+ s /= tempMat . data64F [ 2 ] ;
191
+ let result = new this . cv . Mat ( ) ;
192
+ this . cv . gemm ( this . k . inv ( 3 ) , uvPoint , 1 , new this . cv . Mat ( ) , 0 , result ) ;
193
+ this . cv . multiply ( result , new this . cv . matFromArray ( 4 , 1 , this . cv . CV_64FC1 , [ s , 0 , 0 , 0 ] ) , result ) ;
194
+ this . cv . subtract ( result , this . translationMatrix , result ) ;
195
+ this . cv . gemm ( rotationMatrix . inv ( 3 ) , result , 1 , new this . cv . Mat ( ) , 0 , result ) ;
196
+ return result . data64F ;
197
+ }
198
+ }
0 commit comments