Matrix weightedLeastSquares(Matrix A, Matrix b, Matrix W) {
int prealloc = alloc_matrix;
Matrix WA = multiplyMatrix(W,A);
Matrix WAt = transposeMatrix(WA);
Matrix WAtW = multiplyMatrix(WAt,W);
Matrix WAtWA = multiplyMatrix(WAtW,A);
Matrix WAtWAi = invertRREF(WAtWA);
Matrix WAtWAiWAt = multiplyMatrix(WAtWAi,WAt);
Matrix WAtWAiWAtW = multiplyMatrix(WAtWAiWAt,W);
Matrix a = multiplyMatrix(WAtWAiWAtW,b);
freeMatrix(WA);
freeMatrix(WAt);
freeMatrix(WAtW);
freeMatrix(WAtWA);
freeMatrix(WAtWAi);
freeMatrix(WAtWAiWAt);
freeMatrix(WAtWAiWAtW);
if(prealloc != alloc_matrix - 1) {
printf("Error deallocating matricies <%s>: pre=%d post=%d",__FUNCTION__, prealloc, alloc_matrix);
exit(1);
}
return a;
}
Matrix leastSquares(Matrix A, Matrix b) {
int prealloc = alloc_matrix;
Matrix At = transposeMatrix(A);
Matrix AtA = transposeMultiplyMatrixL(A,A);
Matrix AtAi = invertRREF(AtA);
Matrix Atb = multiplyMatrix(At,b);
Matrix a = multiplyMatrix(AtAi,Atb);
freeMatrix(At);
freeMatrix(AtA);
freeMatrix(AtAi);
freeMatrix(Atb);
if(prealloc != alloc_matrix - 1) {
printf("Error deallocating matricies <%s>: pre=%d post=%d",__FUNCTION__, prealloc, alloc_matrix);
exit(1);
}
return a;
}
/* This function performs a 3X3 two dimentional perspective transform to an image
This is used to perform geomentric normalization */
Image transformImage(Image source, int newWidth, int newHeight, const Matrix m){
Image dest = makeImage(newWidth, newHeight, source->channels);
Matrix point = makeMatrix(3,1);
Matrix inv;
Matrix pt;
int x, y, c;
assert(m->row_dim == 3);
assert(m->col_dim == 3);
/* initialize homogenius point */
ME(point,2,0) = 1;
/* find the inverse transformation to convert from dest to source */
inv = invertRREF(m);
for(x = 0; x < dest->width; x++){
for(y = 0; y < dest->height; y++){
/* calculate source point */
ME(point,0,0) = x;
ME(point,1,0) = y;
ME(point,2,0) = 1.0;
pt = multiplyMatrix(inv, point);
ME(pt,0,0) = ME(pt,0,0) / ME(pt,2,0);
ME(pt,1,0) = ME(pt,1,0) / ME(pt,2,0);
for(c = 0; c < dest->channels; c++){
/* interpolate value */
IE(dest,x,y,c) = interpLinear(source, ME(pt,0,0),ME(pt,1,0),c);
}
/* clean up */
freeMatrix(pt);
}
}
freeMatrix(point);
freeMatrix(inv);
return dest;
}