protected void computePsiDataPsiGradient ( GrayF32 image1 , GrayF32 image2 , GrayF32 deriv1x , GrayF32 deriv1y , GrayF32 deriv2x , GrayF32 deriv2y , GrayF32 deriv2xx , GrayF32 deriv2yy , GrayF32 deriv2xy , GrayF32 du , GrayF32 dv , GrayF32 psiData , GrayF32 psiGradient ) { int N = image1 . width * image1 . height ; for ( int i = 0 ; i < N ; i ++ ) { float du_ = du . data [ i ] ; float dv_ = dv . data [ i ] ; // compute Psi-data float taylor2 = image2 . data [ i ] + deriv2x . data [ i ] * du_ + deriv2y . data [ i ] * dv_ ; float v = taylor2 - image1 . data [ i ] ; psiData . data [ i ] = ( float ) ( 1.0 / ( 2.0 * Math . sqrt ( v * v + EPSILON * EPSILON ) ) ) ; // compute Psi-gradient float dIx = deriv2x . data [ i ] + deriv2xx . data [ i ] * du_ + deriv2xy . data [ i ] * dv_ - deriv1x . data [ i ] ; float dIy = deriv2y . data [ i ] + deriv2xy . data [ i ] * du_ + deriv2yy . data [ i ] * dv_ - deriv1y . data [ i ] ; float dI2 = dIx * dIx + dIy * dIy ; psiGradient . data [ i ] = ( float ) ( 1.0 / ( 2.0 * Math . sqrt ( dI2 + EPSILON * EPSILON ) ) ) ; } }
Compute Psi - data using equation 6 and approximation in equation 5