/** find_homography computes a homography from the four points
 * defined in <i>from</i> into the four points defined into <i>to</i>.
 * 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;
}