@@ -58,15 +58,26 @@ export default class CoordTransfer {
5858 if ( this . rotationMatrix ) {
5959 this . rotationMatrix . delete ( ) ;
6060 }
61+ if ( this . rotationMat3 ) {
62+ this . rotationMat3 . delete ( ) ;
63+ }
6164 if ( this . translationMatrix ) {
6265 this . translationMatrix . delete ( ) ;
6366 }
6467 if ( this . k ) {
6568 this . k . delete ( ) ;
6669 }
6770 this . rotationMatrix = this . toRotationMatrix ( pitch , roll , yaw ) ;
71+ this . rotationMat3 = new this . cv . Mat ( 3 , 3 , this . cv . CV_64FC1 ) ;
72+ this . cv . Rodrigues ( this . rotationMatrix , this . rotationMat3 ) ;
6873 this . translationMatrix = this . toTranslationMatrix ( x , y , z ) ;
6974 this . k = this . toCameraMatrix ( fx , fy , centerX , centerY ) ;
75+ this . _R = this . rotationMat3 . data64F ? Array . from ( this . rotationMat3 . data64F ) : null ;
76+ this . _t = this . translationMatrix . data64F ? Array . from ( this . translationMatrix . data64F ) : null ;
77+ this . _fx = fx ;
78+ this . _fy = fy ;
79+ this . _cx = centerX ;
80+ this . _cy = centerY ;
7081 }
7182
7283 /**
@@ -170,25 +181,65 @@ export default class CoordTransfer {
170181 toCameraMatrix ( fx , fy , centerX , centerY ) {
171182 return this . cv . matFromArray ( 3 , 3 , this . cv . CV_64FC1 , [ fx , 0 , centerX , 0 , fy , centerY , 0 , 0 , 1 ] ) ;
172183 }
184+ /**
185+ * @function CoordTransfer.prototype.toCameraCoordinate
186+ * @description 将空间坐标转换到相机坐标系。Z > 0 表示在相机前方。
187+ * @param {Array<number> } coord - 空间坐标 [x, y] 或 [x, y, z](EPSG:3857)。
188+ * @returns {Array<number>|null } 相机坐标 [Xc, Yc, Zc]。
189+ */
190+ toCameraCoordinate ( coord ) {
191+ if ( ! this . _R || this . _R . length < 9 || ! this . _t || this . _t . length < 3 || ! coord || coord . length < 2 ) {
192+ return null ;
193+ }
194+ const x = coord [ 0 ] ;
195+ const y = coord [ 1 ] ;
196+ const z = coord . length > 2 ? coord [ 2 ] : 0 ;
197+ const R = this . _R ;
198+ const t = this . _t ;
199+ return [
200+ R [ 0 ] * x + R [ 1 ] * y + R [ 2 ] * z + t [ 0 ] ,
201+ R [ 3 ] * x + R [ 4 ] * y + R [ 5 ] * z + t [ 1 ] ,
202+ R [ 6 ] * x + R [ 7 ] * y + R [ 8 ] * z + t [ 2 ]
203+ ] ;
204+ }
205+
206+ /**
207+ * @function CoordTransfer.prototype.projectCameraToVideo
208+ * @description 将相机坐标投影为视频像素坐标。相机后方的点返回 null。
209+ * @param {Array<number> } camera - 相机坐标 [Xc, Yc, Zc]。
210+ * @param {number } [zNear=0.01] - 近裁剪面,单位与空间坐标一致。
211+ * @returns {Array<number>|null } 视频像素坐标 [u, v]。
212+ */
213+ projectCameraToVideo ( camera , zNear = 0.01 ) {
214+ if ( ! camera || this . _fx == null || this . _fy == null ) {
215+ return null ;
216+ }
217+ const depth = camera [ 2 ] ;
218+ if ( ! ( depth > zNear ) ) {
219+ return null ;
220+ }
221+ return [
222+ this . _fx * camera [ 0 ] / depth + this . _cx ,
223+ this . _fy * camera [ 1 ] / depth + this . _cy
224+ ] ;
225+ }
226+
173227 /**
174228 * @function CoordTransfer.prototype.toVideoCoordinate
175- * @description 转换视频像素坐标到空间地理坐标 。
229+ * @description 将空间地理坐标转换为视频像素坐标。相机后方的点 data64F 为空数组 。
176230 * @param {Array } coord - 空间坐标。
177- * @returns {Array } 视频像素坐标 。
231+ * @returns {{data64F: Array<number>, depth: (number|null)} } 视频像素坐标及相机深度 。
178232 */
179233 toVideoCoordinate ( coord ) {
180- if ( ! this . cv || ! this . cv . Mat ) {
181- return { data64F : [ ] } ;
234+ const camera = this . toCameraCoordinate ( coord ) ;
235+ if ( ! camera ) {
236+ return { data64F : [ ] , depth : null } ;
182237 }
183- let emptyMat = new this . cv . Mat ( ) ;
184- let point3 = this . cv . matFromArray ( 3 , 1 , this . cv . CV_64FC1 , [ coord [ 0 ] , coord [ 1 ] , 0 ] ) ;
185- let point2 = new this . cv . Mat ( 2 , 1 , this . cv . CV_64FC1 ) ;
186- this . cv . projectPoints ( point3 , this . rotationMatrix , this . translationMatrix , this . k , emptyMat , point2 ) ;
187- const data64F = point2 . data64F . length ? [ point2 . data64F [ 0 ] , point2 . data64F [ 1 ] ] : [ ] ;
188- emptyMat . delete ( ) ;
189- point3 . delete ( ) ;
190- point2 . delete ( ) ;
191- return { data64F } ;
238+ const uv = this . projectCameraToVideo ( camera ) ;
239+ return {
240+ data64F : uv || [ ] ,
241+ depth : camera [ 2 ]
242+ } ;
192243 }
193244 /**
194245 * @function CoordTransfer.prototype.toSpatialCoordinate
0 commit comments