public boolean process ( ) { int numViews = depths . numRows ; int numFeatures = depths . numCols ; P . reshape ( 3 * numViews , 4 ) ; X . reshape ( 4 , numFeatures ) ; A . reshape ( numViews * 3 , numFeatures ) ; B . reshape ( numViews * 3 , numFeatures ) ; // Scale depths so that they are close to unity normalizeDepths ( depths ) ; // Compute the initial A matirx assignValuesToA ( A ) ; for ( int iter = 0 ; iter < maxIterations ; iter ++ ) { if ( ! svd . decompose ( A ) ) return false ; svd . getU ( U , false ) ; svd . getV ( Vt , true ) ; double sv [ ] = svd . getSingularValues ( ) ; SingularOps_DDRM . descendingOrder ( U , false , sv , A . numCols , Vt , true ) ; // This is equivalent to forcing the rank to be 4 CommonOps_DDRM . extract ( U , 0 , 0 , P ) ; CommonOps_DDRM . multCols ( P , sv ) ; CommonOps_DDRM . extract ( Vt , 0 , 0 , X ) ; // Compute the new value of A CommonOps_DDRM . mult ( P , X , B ) ; // See how much change there is double delta = SpecializedOps_DDRM . diffNormF ( A , B ) / ( A . numCols * A . numRows ) ; // swap arrays for the next iteration DMatrixRMaj tmp = A ; A = B ; B = tmp ; // exit if converged if ( delta <= minimumChangeTol ) break ; } return true ; }
Performs iteration to find camera matrices and feature locations in world frame