public static void removeRadial ( float x , float y , float [ ] radial , float t1 , float t2 , Point2D_F32 out , float tol ) { float origX = x ; float origY = y ; float prevSum = 0 ; for ( int iter = 0 ; iter < 500 ; iter ++ ) { // estimate the radial distance float r2 = x * x + y * y ; float ri2 = r2 ; float sum = 0 ; for ( int i = 0 ; i < radial . length ; i ++ ) { sum += radial [ i ] * ri2 ; ri2 *= r2 ; } float tx = 2.0f * t1 * x * y + t2 * ( r2 + 2.0f * x * x ) ; float ty = t1 * ( r2 + 2.0f * y * y ) + 2.0f * t2 * x * y ; x = ( origX - tx ) / ( 1.0f + sum ) ; y = ( origY - ty ) / ( 1.0f + sum ) ; if ( ( float ) Math . abs ( prevSum - sum ) <= tol ) { break ; } else { prevSum = sum ; } } out . set ( x , y ) ; }
Static function for removing radial and tangential distortion