public boolean process ( DMatrixRMaj P ) { if ( ! svd . decompose ( P ) ) return false ; svd . getU ( Ut , true ) ; svd . getV ( V , false ) ; double sv [ ] = svd . getSingularValues ( ) ; SingularOps_DDRM . descendingOrder ( Ut , true , sv , 3 , V , false ) ; // compute W+, which is transposed and non-negative inverted for ( int i = 0 ; i < 3 ; i ++ ) { Wt . unsafe_set ( i , i , 1.0 / sv [ i ] ) ; } // get the pseudo inverse // A+ = V*(W+)*U' CommonOps_DDRM . mult ( V , Wt , tmp ) ; CommonOps_DDRM . mult ( tmp , Ut , PA ) ; // Vector U, which is P*U = 0 SpecializedOps_DDRM . subvector ( V , 0 , 3 , V . numRows , false , 0 , ns ) ; return true ; }
Compute projective transform that converts P into identity