void NN_print_matrix(const char* msg, const NN_Matrix* A) { unsigned int i, j, nrows, ncols; printf("%s\n\n", msg); if(!A) { fprintf(stderr, "\tEmpty matrix\n\n"); return; } element* Adp = A->data; nrows = A->nrows; ncols = A->ncols; for(i=0; i<nrows; i++) { for(j=0; j<ncols; j++) { printf("\t%.4f\t", MATRIX_ELEMENT(Adp,nrows,ncols,i,j)); } printf("\n"); } printf("\n"); }
void Homography::fit(matd &x1, matd &x2, RX::mat3 &H) { int _lwork; double * _work; double *_A, *_U, *_VT, *_S; // preallocate svd memory _A = new double[72]; _U = new double[64]; _VT = new double[81]; _S = new double[8]; _work = new double[10]; _lwork = -1; int info, m = 8, n = 9; char a = 'A'; // query for right lwork dgesvd_(&a, &a, &m, &n, _A, &m, _S, _U, &m, _VT, &n, _work, &_lwork, &info); _lwork = _work[0]; delete[] _work; _work = new double[_lwork]; for(int i = 0; i < 4; ++i) { double _x1 = x1[0][i], _y1 = x1[1][i]; double _X1 = x2[0][i], _Y1 = x2[1][i]; _A[(2*i)] = _x1; _A[(2*i)+8] = _y1; _A[(2*i)+2*8] = 1; _A[(2*i)+3*8] = 0; _A[(2*i)+4*8] = 0; _A[(2*i)+5*8] = 0; _A[(2*i)+6*8] = -_x1*_X1; _A[(2*i)+7*8] = -_y1*_X1; _A[(2*i)+8*8] = -_X1; _A[(2*i+1)] = 0; _A[(2*i+1)+8] = 0; _A[(2*i+1)+2*8] = 0; _A[(2*i+1)+3*8] = _x1; _A[(2*i+1)+4*8] = _y1; _A[(2*i+1)+5*8] = 1; _A[(2*i+1)+6*8] = -_x1*_Y1; _A[(2*i+1)+7*8] = -_y1*_Y1; _A[(2*i+1)+8*8] = -_Y1; } dgesvd_(&a, &a, &m, &n, _A, &m, _S, _U, &m, _VT, &n, _work, &_lwork, &info); if (info != 0) { fprintf(stderr, "Warning: dgesvd returned with a non-zero status (info = %d)\n", info); } for(int i=0; i<3; i++) { for(int j=0; j<3; j++) { int b = 3*i+j; H.set(i,j, MATRIX_ELEMENT(_VT, 9, 9, 8, b)); } } for(int i=0; i<3; i++) for(int j=0; j<3; j++) H.set(i,j, H.at(i, j)/H.at(2,2)); delete[] _A; delete[] _U; delete[] _VT; delete[] _S; delete[] _work; }