/*
Solve the overconstrained linear system Ma = b using a least
squares error (pseudo inverse) approach.
*/
int solve_system (dmat M, dmat a, dmat b)
/*
int solve_system (M, a, b)
dmat M,
a,
b;
*/
{
dmat Mt,MtM,Mdag;
int error;
if ((M.ub1 - M.lb1) < (M.ub2 - M.lb2))
{
fprintf (stderr, "solve_system: matrix M has more columns than rows\n");
return (-1);
}
Mt = newdmat (M.lb2, M.ub2, M.lb1, M.ub1, &error);
if (error)
{ fprintf (stderr, "solve_system: unable to allocate matrix M_transpose\n"); return (-1); }
if (transpose (M, Mt)!=0)
{ fprintf (stderr, "solve_system: unable to transpose matrix M\n"); return (-1); }
MtM = newdmat (M.lb2, M.ub2, M.lb2, M.ub2, &error);
if (error)
{ fprintf (stderr, "solve_system: unable to allocate matrix M_transpose_M\n"); return (-1); }
if (matmul (Mt, M, MtM)!=0)
{ fprintf (stderr, "solve_system: unable to compute matrix product of M_transpose and M\n"); return (-1); }
if (fabs (matinvert (MtM)) < 0.001)
{
fprintf (stderr, "solve_system: determinant of matrix M_transpose_M is too small\n");
return (-1);
}
/*
if (errno) {
fprintf (stderr, "solve_system: error during matrix inversion\n");
return (-1);
}
*/
Mdag = newdmat (M.lb2, M.ub2, M.lb1, M.ub1, &error);
if (error)
{ fprintf (stderr, "solve_system: unable to allocate matrix M_diag\n"); return (-1); }
if (matmul (MtM, Mt, Mdag)!=0)
{
fprintf (stderr, "solve_system: unable to compute matrix product of M_transpose_M and M_transpose\n");
return (-1);
}
if (matmul (Mdag, b, a)!=0)
{
fprintf (stderr, "solve_system: unable to compute matrix product of M_diag and b\n");
return (-1);
}
freemat (Mt); freemat (MtM); freemat (Mdag);
return 0;
}
/*
Perform the matrix multiplication c = a*b where
a, b, and c are dynamic Iliffe matrices.
The matrix dimensions must be commensurate. This means that
c and a must have the same number of rows; c and b must have
the same number of columns; and a must have the same number
of columns as b has rows. The actual index origins are
immaterial.
If c is the same matrix as a or b, the result will be
correct at the expense of more time.
*/
int matmul (dmat a,dmat b,dmat c)
{
int i,
j,
k,
broff,
croff,
ccoff, /* coordinate origin offsets */
mem_alloced,
error;
double t;
dmat d; /* temporary workspace matrix */
if (a.ub2 - a.lb2 != b.ub1 - b.lb1
|| a.ub1 - a.lb1 != c.ub1 - c.lb1
|| b.ub2 - b.lb2 != c.ub2 - c.lb2) {
errno = EDOM;
return (-1);
}
if (a.mat_sto != c.mat_sto && b.mat_sto != c.mat_sto) {
d = c;
mem_alloced = FALSE;
} else {
d = newdmat (c.lb1, c.ub1, c.lb2, c.ub2, &error);
if (error) {
fprintf (stderr, "Matmul: out of storage.\n");
errno = ENOMEM;
return (-1);
}
mem_alloced = TRUE;
}
broff = b.lb1 - a.lb2; /* B's row offset from A */
croff = c.lb1 - a.lb1; /* C's row offset from A */
ccoff = c.lb2 - b.lb2; /* C's column offset from B */
for (i = a.lb1; i <= a.ub1; i++)
for (j = b.lb2; j <= b.ub2; j++) {
t = 0.0;
for (k = a.lb2; k <= a.ub2; k++)
t += a.el[i][k] * b.el[k + broff][j];
d.el[i + croff][j + ccoff] = t;
}
if (mem_alloced) {
for (i = c.lb1; i <= c.ub1; i++)
for (j = c.lb2; j <= c.ub2; j++)
c.el[i][j] = d.el[i][j];
freemat (d);
}
return (0);
}
/*
Generate the transpose of a dynamic Iliffe matrix.
*/
int transpose (dmat A, dmat ATrans)
{
int i,
j,
rowsize,
colsize,
error;
double **a = A.el,
**atrans = ATrans.el,
temp;
dmat TMP;
rowsize = A.ub1 - A.lb1;
colsize = A.ub2 - A.lb2;
if (rowsize < 0 || rowsize != ATrans.ub2 - ATrans.lb2 ||
colsize < 0 || colsize != ATrans.ub1 - ATrans.lb1) {
errno = EDOM;
return (-1);
}
if (A.mat_sto == ATrans.mat_sto
&& A.lb1 == ATrans.lb1
&& A.lb2 == ATrans.lb2) {
for (i = 0; i <= rowsize; i++)
for (j = i + 1; j <= colsize; j++) {
temp = a[A.lb1 + i][A.lb2 + j];
atrans[A.lb1 + i][A.lb2 + j] = a[A.lb1 + j][A.lb2 + i];
atrans[A.lb1 + j][A.lb2 + i] = temp;
}
} else if (A.mat_sto == ATrans.mat_sto) {
TMP = newdmat (ATrans.lb1, ATrans.ub1, ATrans.lb2, ATrans.ub2, &error);
if (error)
return (-2);
for (i = 0; i <= rowsize; i++)
for (j = 0; j <= colsize; j++) {
TMP.el[ATrans.lb1 + j][ATrans.lb2 + i] =
a[A.lb1 + i][A.lb2 + j];
}
matcopy (TMP, ATrans);
freemat (TMP);
} else {
for (i = 0; i <= rowsize; i++)
for (j = 0; j <= colsize; j++)
atrans[ATrans.lb1 + j][ATrans.lb2 + i]
= a[A.lb1 + i][A.lb2 + j];
}
return (0);
}
/*
Solve the overconstrained linear system Ma = b using a least
squares error (pseudo inverse) approach.
*/
int solve_system (dmat M, dmat a,dmat b)
{
dmat Mt,
MtM,
Mdag;
//AfxMessageBox("S1");
if ((M.ub1 - M.lb1) < (M.ub2 - M.lb2)) {
fprintf (stderr, "solve_system: matrix M has more columns than rows\n");
return (-1);
}
//AfxMessageBox("S2");
Mt = newdmat (M.lb2, M.ub2, M.lb1, M.ub1, &errno);
if (errno) {
fprintf (stderr, "solve_system: unable to allocate matrix M_transpose\n");
return (-2);
}
//AfxMessageBox("S3");
transpose (M, Mt);
if (errno) {
fprintf (stderr, "solve_system: unable to transpose matrix M\n");
return (-3);
}
//AfxMessageBox("S4");
MtM = newdmat (M.lb2, M.ub2, M.lb2, M.ub2, &errno);
if (errno) {
fprintf (stderr, "solve_system: unable to allocate matrix M_transpose_M\n");
return (-4);
}
//AfxMessageBox("S5");
matmul (Mt, M, MtM);
if (errno) {
fprintf (stderr, "solve_system: unable to compute matrix product of M_transpose and M\n");
return (-5);
}
//modified by Dickson
//AfxMessageBox("S6");
double aa=fabs (matinvert (MtM));
//AfxMessageBox("S7");
//if (aa < 0.001) {
//CString str;
//str.Format("determinant=%f",aa);
//AfxMessageBox("S71");
//AfxMessageBox(str);
//AfxMessageBox("S8");
if (aa < 0.001) {
fprintf (stderr, "solve_system: determinant of matrix M_transpose_M is too small\n");
return (-6);
}
if (errno) {
fprintf (stderr, "solve_system: error during matrix inversion\n");
return (-7);
}
Mdag = newdmat (M.lb2, M.ub2, M.lb1, M.ub1, &errno);
if (errno) {
fprintf (stderr, "solve_system: unable to allocate matrix M_diag\n");
return (-8);
}
matmul (MtM, Mt, Mdag);
if (errno) {
fprintf (stderr, "solve_system: unable to compute matrix product of M_transpose_M and M_transpose\n");
return (-9);
}
matmul (Mdag, b, a);
if (errno) {
fprintf (stderr, "solve_system: unable to compute matrix product of M_diag and b\n");
return (-10);
}
freemat (Mt);
freemat (MtM);
freemat (Mdag);
return 0;
}
/*******************************************************************************
Given a set of matches, compute the "best fitting" homography.
matches: matching points
numMatches: number of matching points
h: returned homography
isForward: direction of the projection (true = image1 -> image2, false = image2 -> image1)
*******************************************************************************/
bool MainWindow::ComputeHomography(CMatches *matches, int numMatches, double h[3][3], bool isForward)
{
int error;
int nEq=numMatches*2;
dmat M=newdmat(0,nEq,0,7,&error);
dmat a=newdmat(0,7,0,0,&error);
dmat b=newdmat(0,nEq,0,0,&error);
double x0, y0, x1, y1;
for (int i=0;i<nEq/2;i++)
{
if(isForward == false)
{
x0 = matches[i].m_X1;
y0 = matches[i].m_Y1;
x1 = matches[i].m_X2;
y1 = matches[i].m_Y2;
}
else
{
x0 = matches[i].m_X2;
y0 = matches[i].m_Y2;
x1 = matches[i].m_X1;
y1 = matches[i].m_Y1;
}
//Eq 1 for corrpoint
M.el[i*2][0]=x1;
M.el[i*2][1]=y1;
M.el[i*2][2]=1;
M.el[i*2][3]=0;
M.el[i*2][4]=0;
M.el[i*2][5]=0;
M.el[i*2][6]=(x1*x0*-1);
M.el[i*2][7]=(y1*x0*-1);
b.el[i*2][0]=x0;
//Eq 2 for corrpoint
M.el[i*2+1][0]=0;
M.el[i*2+1][1]=0;
M.el[i*2+1][2]=0;
M.el[i*2+1][3]=x1;
M.el[i*2+1][4]=y1;
M.el[i*2+1][5]=1;
M.el[i*2+1][6]=(x1*y0*-1);
M.el[i*2+1][7]=(y1*y0*-1);
b.el[i*2+1][0]=y0;
}
int ret=solve_system (M,a,b);
if (ret!=0)
{
freemat(M);
freemat(a);
freemat(b);
return false;
}
else
{
h[0][0]= a.el[0][0];
h[0][1]= a.el[1][0];
h[0][2]= a.el[2][0];
h[1][0]= a.el[3][0];
h[1][1]= a.el[4][0];
h[1][2]= a.el[5][0];
h[2][0]= a.el[6][0];
h[2][1]= a.el[7][0];
h[2][2]= 1;
}
freemat(M);
freemat(a);
freemat(b);
return true;
}