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;
}
