/** find_homography computes a homography from the four points
* defined in *from* into the four points defined into *to*.
* The homography is calculated using QR factorization.
* @param from array of eight floats defining the source points
* @param to array of eight floats defining the destination points
* @returns array of nine floats defining the homography in row-major
**/
float* find_homography(const float* from, const float* to) {
int m = 9;
int n = 8;
int lwork = n*64;
float A[m*m]; /* this needs to be m*m to hold the final Q matrix */
float tau[n];
float work[lwork];
int info;
/* Set-up A matrix to solve
* Ax=0
* i.e. find the nullspace of A.
* A is 8x9, which wont work for nullspace extraction from
* QR factorization (wont work with SVD neither...),
* therefore we assume A to be transposed:
* Define it using row-major but tell BLAS/LAPACK
* column-major (prefered format anyways).
* We have to transpose the result as well then.
*
* Refer to Hartley and Zisserman, "Multiple View Geometry"
* (Section 4.1, the DLT algorithm) for an explanation and
* deduction of matrix A.
*/
for(int i=0; i<4; ++i) {
const float &sx = from[2*i+0];
const float &sy = from[2*i+1];
const float &dx = to[2*i+0];
const float &dy = to[2*i+1];
A[i*18+0] = 0.f; A[i*18+1] = 0.f; A[i*18+2] = 0.f;
A[i*18+3] = -sx; A[i*18+4] = -sy; A[i*18+5] = -1.f;
A[i*18+6] = dy*sx; A[i*18+7] = dy*sy; A[i*18+8] = dy;
//
A[i*18+9] = sx; A[i*18+10] = sy; A[i*18+11] = 1.f;
A[i*18+12] = 0.f; A[i*18+13] = 0.f; A[i*18+14] = 0.f;
A[i*18+15] = -dx*sx; A[i*18+16] = -dx*sy; A[i*18+17] = -dx;
}
/* compute QR factorization using LAPACK */
sgeqrf_(&m, &n, A, &m, tau, work, &lwork, &info);
if(info) {
// error
return NULL;
}
/* extract Q using LAPACK */
sorgqr_(&m, &m, &n, A, &m, tau, work, &lwork, &info);
if(info) {
// error
return NULL;
}
/* The last column of Q consists of the nullspace of
* A, which is the solution and thus the desired
* homography. However, the result now is transposed
* as well (see above) but
* still column-major; therefore it is already
* the correct result in row-major.
* We need to normalize but then we are done. */
float *Res = &A[9*8];
float *H = new float[9];
float norm = 1.f/Res[8];
for(int i=0; i<9; ++i) {
H[i] = Res[i]*norm;
}
return H;
}