void solveAndPrint(int N, double **AMatrix, double *bMatrix, double *xMatrix){ int j = 0; LUdecomp *LUdecomp = malloc(sizeof(LUdecomp)); LUdecomp = LUdecompose(2*N, AMatrix); // perform decomp LUsolve(LUdecomp, bMatrix, xMatrix); // solve x for(int i = 0; i < 2*N; i++){ // print to stdout printf("%.10f ", xMatrix[i]); //print j++; if(j == 3){ // 3 values, move to next row j = 0; printf("\n"); } } printf("1.0000000000"); // print 1 at the end }
int main(){ /* Create N Create matrix to hold the original set of x,y coordinates (xy_source) Create matrix to hold the transformed set of x,y coordinates (xy_target) */ int N = 0; //get N from first line of text file scanf("%d", &N); double xy_source[N][2]; double xy_target[N][2]; //get the x and y values from input file for(int i = 0; i < N; i++){ scanf("%lf %lf", &xy_source[i][0], &xy_source[i][1]); } for(int i = 0; i < N; i++){ scanf("%lf %lf", &xy_target[i][0], &xy_target[i][1]); } //Create matrix to hold A double** A = (double**)malloc(2*N*sizeof(double*)); //make room on the heap for A for(int i = 0; i < 2*N; i++) A[i] = (double*)malloc(8*sizeof(double)); /* begin filling Matrix A */ int count = 0; //column 1 for(int j = 0; j < 2*N; j ++) if(j % 2 == 0) A[j][0] = xy_source[count++][0]; else A[j][0] = 0; count = 0; //column 2 for(int j = 0; j < 2*N; j++) if(j % 2 == 0) A[j][1] = xy_source[count++][1]; else A[j][1] = 0; count = 0; //column 3 for(int j = 0; j < 2*N; j++) if(j % 2 == 0) A[j][2] = 1; else A[j][2] = 0; count = 0; //column 4 for(int j = 0; j < 2*N; j++) if(j % 2 == 0) A[j][3] = 0; else A[j][3] = xy_source[count++][0]; count = 0; //column 5 for(int j = 0; j < 2*N; j++) if(j % 2 ==0) A[j][4] = 0; else A[j][4] = xy_source[count++][1]; count = 0; //column 6 for(int j = 0; j < 2*N; j++) if(j % 2 == 0) A[j][5] = 0; else A[j][5] = 1; count = 0; //column 7 for(int j = 0; j < 2*N; j++){ if(j % 2 == 0){ A[j][6] = (-1)*(xy_source[count][0])*(xy_target[count][0]); }else{ A[j][6] = (-1)*(xy_source[count][0])*(xy_target[count][1]); count++; } } count = 0; //column 8 for(int j = 0; j < 2*N; j++){ if(j % 2 == 0){ A[j][7] = (-1)*(xy_source[count][1])*(xy_target[count][0]); }else{ A[j][7] = (-1)*(xy_source[count][1])*(xy_target[count][1]); count++; } } //Create and make room on heap for matrix b double* b = (double*)malloc(2*N*sizeof(double)); //populate matrix b from xy_target count = 0; for(int j = 0; j < 2*N; j += 2){ b[j] = xy_target[count][0]; b[j+1] = xy_target[count++][1]; } //Create and make room on heap for matrix x double* x = (double*)malloc(8*sizeof(double)); for(int j = 0; j < 8; j++) x[j] = 0.0; /* Create matrix to hold A transpose A */ double** A_T_A = (double**)malloc(8*sizeof(double*)); for(int i = 0; i < 8; i++){ A_T_A[i] = (double*)malloc(8*sizeof(double)); } //Create matrix to hold A transpose b double* A_T_b = (double*)malloc(8*sizeof(double)); /* If N > 4 we have an overdetermined system and must use the method of least-squares */ if(N > 4){ double sum = 0.0; for(int i = 0; i < 8; i++){ for(int j = i; j < 8; j++){ sum = 0.0; for(int k = 0; k < 2*N; k++){ sum += A[k][i]*A[k][j]; } A_T_A[i][j] = sum; } } for(int i = 0; i < 8; i++) for(int j = 0; j < i; j++) A_T_A[i][j] = A_T_A[j][i]; sum = 0.0; for(int i = 0; i < 8; i++){ sum = 0.0; for(int j = 0; j < 2*N; j++) sum += A[j][i] * b[j]; A_T_b[i] = sum; } } if(N == 4){ LUdecomp* A_decompose = LUdecompose(2*N, (const double**)A); LUsolve(A_decompose, b, x); }else{ LUdecomp* A_T_decompose = LUdecompose(8, (const double**)A_T_A); LUsolve(A_T_decompose, A_T_b, x); } /* Print out the homography in row major order */ for(int i = 0; i < 3; i++) printf("%lf ", x[i]); printf("\n"); for(int i = 3; i < 6; i++) printf("%lf ", x[i]); printf("\n"); for(int i = 6; i < 8; i++) printf("%lf ", x[i]); double one = 1.0; printf("%lf ", one); return 0; }