import { Matrix , inverse } from 'ml-matrix' ; import { solveHomography } from '../utils/homography' ; export const estimate = ( { screenCoords , worldCoords , projectionTransform } ) => { const Harray = solveHomography ( worldCoords . map ( ( p ) => [ p . x , p . y ] ) , screenCoords . map ( ( p ) => [ p . x , p . y ] ) ) as number [ ] ; const H = new Matrix ( [ [ Harray [ 0 ] , Harray [ 1 ] , Harray [ 2 ] ] , [ Harray [ 3 ] , Harray [ 4 ] , Harray [ 5 ] ] , [ Harray [ 6 ] , Harray [ 7 ] , Harray [ 8 ] ] , ] ) ; const K = new Matrix ( projectionTransform ) ; const KInv = inverse ( K ) ; const _KInvH = KInv . mmul ( H ) ; const KInvH = _KInvH . to1DArray ( ) ; const norm1 = Math . sqrt ( KInvH [ 0 ] * KInvH [ 0 ] + KInvH [ 3 ] * KInvH [ 3 ] + KInvH [ 6 ] * KInvH [ 6 ] ) ; const norm2 = Math . sqrt ( KInvH [ 1 ] * KInvH [ 1 ] + KInvH [ 4 ] * KInvH [ 4 ] + KInvH [ 7 ] * KInvH [ 7 ] ) ; const tnorm = ( norm1 + norm2 ) / 2 ; const rotate = [ ] ; rotate [ 0 ] = KInvH [ 0 ] / norm1 ; rotate [ 3 ] = KInvH [ 3 ] / norm1 ; rotate [ 6 ] = KInvH [ 6 ] / norm1 ; rotate [ 1 ] = KInvH [ 1 ] / norm2 ; rotate [ 4 ] = KInvH [ 4 ] / norm2 ; rotate [ 7 ] = KInvH [ 7 ] / norm2 ; rotate [ 2 ] = rotate [ 3 ] * rotate [ 7 ] - rotate [ 6 ] * rotate [ 4 ] ; rotate [ 5 ] = rotate [ 6 ] * rotate [ 1 ] - rotate [ 0 ] * rotate [ 7 ] ; rotate [ 8 ] = rotate [ 0 ] * rotate [ 4 ] - rotate [ 1 ] * rotate [ 3 ] ; const norm3 = Math . sqrt ( rotate [ 2 ] * rotate [ 2 ] + rotate [ 5 ] * rotate [ 5 ] + rotate [ 8 ] * rotate [ 8 ] ) ; rotate [ 2 ] /= norm3 ; rotate [ 5 ] /= norm3 ; rotate [ 8 ] /= norm3 ; const tran = [ ] ; tran [ 0 ] = KInvH [ 2 ] / tnorm ; tran [ 1 ] = KInvH [ 5 ] / tnorm ; tran [ 2 ] = KInvH [ 8 ] / tnorm ; let initialModelViewTransform = [ [ rotate [ 0 ] , rotate [ 1 ] , rotate [ 2 ] , tran [ 0 ] ] , [ rotate [ 3 ] , rotate [ 4 ] , rotate [ 5 ] , tran [ 1 ] ] , [ rotate [ 6 ] , rotate [ 7 ] , rotate [ 8 ] , tran [ 2 ] ] ] ; return initialModelViewTransform ; } ;