docs: 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];
  		}
  	}