public static void projectiveToMetricKnownK ( DMatrixRMaj cameraMatrix , DMatrixRMaj H , DMatrixRMaj K , Se3_F64 worldToView ) { DMatrixRMaj tmp = new DMatrixRMaj ( 3 , 4 ) ; CommonOps_DDRM . mult ( cameraMatrix , H , tmp ) ; DMatrixRMaj K_inv = new DMatrixRMaj ( 3 , 3 ) ; CommonOps_DDRM . invert ( K , K_inv ) ; DMatrixRMaj P = new DMatrixRMaj ( 3 , 4 ) ; CommonOps_DDRM . mult ( K_inv , tmp , P ) ; CommonOps_DDRM . extract ( P , 0 , 0 , worldToView . R ) ; worldToView . T . x = P . get ( 0 , 3 ) ; worldToView . T . y = P . get ( 1 , 3 ) ; worldToView . T . z = P . get ( 2 , 3 ) ; SingularValueDecomposition_F64 < DMatrixRMaj > svd = DecompositionFactory_DDRM . svd ( true , true , true ) ; DMatrixRMaj R = worldToView . R ; if ( ! svd . decompose ( R ) ) throw new RuntimeException ( "SVD Failed" ) ; CommonOps_DDRM . multTransB ( svd . getU ( null , false ) , svd . getV ( null , false ) , R ) ; // determinant should be +1 double det = CommonOps_DDRM . det ( R ) ; if ( det < 0 ) { CommonOps_DDRM . scale ( - 1 , R ) ; worldToView . T . scale ( - 1 ) ; } }
Convert the projective camera matrix into a metric transform given the rectifying homography and a known calibration matrix .