void constructA678 ( ) { final int N = X1 . numRows ; // Pseudo-inverse of hat(p) computePseudo ( X1 , P_plus ) ; DMatrixRMaj PPpXP = new DMatrixRMaj ( 1 , 1 ) ; DMatrixRMaj PPpYP = new DMatrixRMaj ( 1 , 1 ) ; computePPXP ( X1 , P_plus , X2 , 0 , PPpXP ) ; computePPXP ( X1 , P_plus , X2 , 1 , PPpYP ) ; DMatrixRMaj PPpX = new DMatrixRMaj ( 1 , 1 ) ; DMatrixRMaj PPpY = new DMatrixRMaj ( 1 , 1 ) ; computePPpX ( X1 , P_plus , X2 , 0 , PPpX ) ; computePPpX ( X1 , P_plus , X2 , 1 , PPpY ) ; //============ Equations 20 computeEq20 ( X2 , X1 , XP_bar ) ; //============ Equation 21 A . reshape ( N * 2 , 3 ) ; double XP_bar_x = XP_bar [ 0 ] ; double XP_bar_y = XP_bar [ 1 ] ; double YP_bar_x = XP_bar [ 2 ] ; double YP_bar_y = XP_bar [ 3 ] ; // Compute the top half of A for ( int i = 0 , index = 0 , indexA = 0 ; i < N ; i ++ , index += 2 ) { double x = - X2 . data [ i * 2 ] ; // X' double P_hat_x = X1 . data [ index ] ; // hat{P}[0] double P_hat_y = X1 . data [ index + 1 ] ; // hat{P}[1] // x'*hat{p} - bar{X*P} - PPpXP A . data [ indexA ++ ] = x * P_hat_x - XP_bar_x - PPpXP . data [ index ] ; A . data [ indexA ++ ] = x * P_hat_y - XP_bar_y - PPpXP . data [ index + 1 ] ; // X'*1 - PPx1 A . data [ indexA ++ ] = x - PPpX . data [ i ] ; } // Compute the bottom half of A for ( int i = 0 , index = 0 , indexA = N * 3 ; i < N ; i ++ , index += 2 ) { double x = - X2 . data [ i * 2 + 1 ] ; double P_hat_x = X1 . data [ index ] ; double P_hat_y = X1 . data [ index + 1 ] ; // x'*hat{p} - bar{X*P} - PPpXP A . data [ indexA ++ ] = x * P_hat_x - YP_bar_x - PPpYP . data [ index ] ; A . data [ indexA ++ ] = x * P_hat_y - YP_bar_y - PPpYP . data [ index + 1 ] ; // X'*1 - PPx1 A . data [ indexA ++ ] = x - PPpY . data [ i ] ; } }
Constructs equation for elements 6 to 8 in H