Merge pull request #113 from cnlohr/epnp

Epnp

10 files changed, 1321 insertions, 18 deletions

diff --git a/src/epnp/epnp.c b/src/epnp/epnp.c
new file mode 100644
index 0000000..4b888aa
--- /dev/null
+++ b/src/epnp/epnp.c
@@ -0,0 +1,742 @@
+// Copyright (c) 2009, V. Lepetit, EPFL
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+
+// 1. Redistributions of source code must retain the above copyright notice, this
+//    list of conditions and the following disclaimer.
+// 2. Redistributions in binary form must reproduce the above copyright notice,
+//    this list of conditions and the following disclaimer in the documentation
+//    and/or other materials provided with the distribution.
+
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+// The views and conclusions contained in the software and documentation are those
+// of the authors and should not be interpreted as representing official policies,
+//   either expressed or implied, of the FreeBSD Project.
+#include "epnp.h"
+#include "math.h"
+#include "stdbool.h"
+#include "stdio.h"
+#include "stdlib.h"
+
+void print_mat(const CvMat *M) {
+	if (!M) {
+		printf("null\n");
+		return;
+	}
+	printf("%d x %d:\n", M->rows, M->cols);
+	for (unsigned i = 0; i < M->rows; i++) {
+		for (unsigned j = 0; j < M->cols; j++) {
+			printf("%.17g, ", cvmGet(M, i, j));
+		}
+		printf("\n");
+	}
+	printf("\n");
+}
+
+void epnp_epnp(epnp *self) {
+	self->maximum_number_of_correspondences = 0;
+	self->number_of_correspondences = 0;
+
+	self->pws = 0;
+	self->us = 0;
+	self->alphas = 0;
+	self->pcs = 0;
+}
+
+void epnp_dtor(epnp *self) {
+	free(self->pws);
+	free(self->us);
+	free(self->alphas);
+	free(self->pcs);
+}
+double epnp_compute_R_and_t(epnp *self, const double *ut, const double *betas, double R[3][3], double t[3]);
+
+double dot(const double *v1, const double *v2) { return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]; }
+
+double dist2(const double *p1, const double *p2) {
+	return (p1[0] - p2[0]) * (p1[0] - p2[0]) + (p1[1] - p2[1]) * (p1[1] - p2[1]) + (p1[2] - p2[2]) * (p1[2] - p2[2]);
+}
+
+void epnp_compute_rho(epnp *self, double *rho) {
+	rho[0] = dist2(self->cws[0], self->cws[1]);
+	rho[1] = dist2(self->cws[0], self->cws[2]);
+	rho[2] = dist2(self->cws[0], self->cws[3]);
+	rho[3] = dist2(self->cws[1], self->cws[2]);
+	rho[4] = dist2(self->cws[1], self->cws[3]);
+	rho[5] = dist2(self->cws[2], self->cws[3]);
+
+	CvMat cws = cvMat(4, 3, CV_64F, self->cws);
+	CvMat ccs = cvMat(4, 3, CV_64F, self->ccs);
+	CvMat pws = cvMat(self->maximum_number_of_correspondences, 3, CV_64F, self->pws);
+}
+
+void epnp_set_internal_parameters(epnp *self, double uc, double vc, double fu, double fv) {
+	self->uc = uc;
+	self->vc = vc;
+	self->fu = fu;
+	self->fv = fv;
+}
+
+void epnp_set_maximum_number_of_correspondences(epnp *self, int n) {
+	if (self->maximum_number_of_correspondences < n) {
+		if (self->pws != 0)
+			free(self->pws);
+		if (self->us != 0)
+			free(self->us);
+		if (self->alphas != 0)
+			free(self->alphas);
+		if (self->pcs != 0)
+			free(self->pcs);
+
+		self->maximum_number_of_correspondences = n;
+		self->pws = calloc(sizeof(double), 3 * self->maximum_number_of_correspondences);
+		self->us = calloc(sizeof(double), 2 * self->maximum_number_of_correspondences);
+		self->alphas = calloc(sizeof(double), 4 * self->maximum_number_of_correspondences);
+		self->pcs = calloc(sizeof(double), 3 * self->maximum_number_of_correspondences);
+	}
+}
+
+void epnp_reset_correspondences(epnp *self) { self->number_of_correspondences = 0; }
+
+void epnp_add_correspondence(epnp *self, double X, double Y, double Z, double u, double v) {
+	self->pws[3 * self->number_of_correspondences] = X;
+	self->pws[3 * self->number_of_correspondences + 1] = Y;
+	self->pws[3 * self->number_of_correspondences + 2] = Z;
+
+	self->us[2 * self->number_of_correspondences] = u;
+	self->us[2 * self->number_of_correspondences + 1] = v;
+
+	self->number_of_correspondences++;
+}
+
+void epnp_choose_control_points(epnp *self) {
+	// Take C0 as the reference points centroid:
+	self->cws[0][0] = self->cws[0][1] = self->cws[0][2] = 0;
+	for (int i = 0; i < self->number_of_correspondences; i++)
+		for (int j = 0; j < 3; j++)
+			self->cws[0][j] += self->pws[3 * i + j];
+
+	for (int j = 0; j < 3; j++)
+		self->cws[0][j] /= self->number_of_correspondences;
+
+	// Take C1, C2, and C3 from PCA on the reference points:
+	CvMat *PW0 = cvCreateMat(self->number_of_correspondences, 3, CV_64F);
+
+	double pw0tpw0[3 * 3] = {}, dc[3], uct[3 * 3];
+	CvMat PW0tPW0 = cvMat(3, 3, CV_64F, pw0tpw0);
+	CvMat DC = cvMat(3, 1, CV_64F, dc);
+	CvMat UCt = cvMat(3, 3, CV_64F, uct);
+
+	for (int i = 0; i < self->number_of_correspondences; i++)
+		for (int j = 0; j < 3; j++)
+			PW0->data.db[3 * i + j] = self->pws[3 * i + j] - self->cws[0][j];
+
+	cvMulTransposed(PW0, &PW0tPW0, 1, 0, 1);
+
+	cvSVD(&PW0tPW0, &DC, &UCt, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);
+	assert(UCt.data.db == uct);
+
+	cvReleaseMat(&PW0);
+
+	for (int i = 1; i < 4; i++) {
+		double k = sqrt(dc[i - 1] / self->number_of_correspondences);
+		for (int j = 0; j < 3; j++)
+			self->cws[i][j] = self->cws[0][j] + k * uct[3 * (i - 1) + j];
+	}
+}
+
+void epnp_compute_barycentric_coordinates(epnp *self) {
+	double cc[3 * 3], cc_inv[3 * 3];
+	CvMat CC = cvMat(3, 3, CV_64F, cc);
+	CvMat CC_inv = cvMat(3, 3, CV_64F, cc_inv);
+
+	for (int i = 0; i < 3; i++)
+		for (int j = 1; j < 4; j++)
+			cc[3 * i + j - 1] = self->cws[j][i] - self->cws[0][i];
+
+	cvInvert(&CC, &CC_inv, 1);
+
+	double *ci = cc_inv;
+	for (int i = 0; i < self->number_of_correspondences; i++) {
+		double *pi = self->pws + 3 * i;
+		double *a = self->alphas + 4 * i;
+
+		for (int j = 0; j < 3; j++)
+			a[1 + j] = ci[3 * j] * (pi[0] - self->cws[0][0]) + ci[3 * j + 1] * (pi[1] - self->cws[0][1]) +
+					   ci[3 * j + 2] * (pi[2] - self->cws[0][2]);
+		a[0] = 1.0f - a[1] - a[2] - a[3];
+	}
+}
+
+void epnp_fill_M(epnp *self, CvMat *M, const int row, const double *as, const double u, const double v) {
+	double *M1 = M->data.db + row * 12;
+	double *M2 = M1 + 12;
+
+	for (int i = 0; i < 4; i++) {
+		M1[3 * i] = as[i] * self->fu;
+		M1[3 * i + 1] = 0.0;
+		M1[3 * i + 2] = as[i] * (self->uc - u);
+
+		M2[3 * i] = 0.0;
+		M2[3 * i + 1] = as[i] * self->fv;
+		M2[3 * i + 2] = as[i] * (self->vc - v);
+	}
+}
+
+void epnp_compute_ccs(epnp *self, const double *betas, const double *ut) {
+	for (int i = 0; i < 4; i++)
+		self->ccs[i][0] = self->ccs[i][1] = self->ccs[i][2] = 0.0f;
+
+	for (int i = 0; i < 4; i++) {
+		const double *v = ut + 12 * (11 - i);
+		for (int j = 0; j < 4; j++)
+			for (int k = 0; k < 3; k++)
+				self->ccs[j][k] += betas[i] * v[3 * j + k];
+	}
+}
+
+void epnp_compute_pcs(epnp *self) {
+	for (int i = 0; i < self->number_of_correspondences; i++) {
+		double *a = self->alphas + 4 * i;
+		double *pc = self->pcs + 3 * i;
+
+		for (int j = 0; j < 3; j++)
+			pc[j] = a[0] * self->ccs[0][j] + a[1] * self->ccs[1][j] + a[2] * self->ccs[2][j] + a[3] * self->ccs[3][j];
+	}
+}
+
+void epnp_compute_L_6x10(epnp *self, const double *ut, double *l_6x10) {
+	const double *v[4];
+
+	v[0] = ut + 12 * 11;
+	v[1] = ut + 12 * 10;
+	v[2] = ut + 12 * 9;
+	v[3] = ut + 12 * 8;
+
+	double dv[4][6][3];
+
+	for (int i = 0; i < 4; i++) {
+		int a = 0, b = 1;
+		for (int j = 0; j < 6; j++) {
+			dv[i][j][0] = v[i][3 * a] - v[i][3 * b];
+			dv[i][j][1] = v[i][3 * a + 1] - v[i][3 * b + 1];
+			dv[i][j][2] = v[i][3 * a + 2] - v[i][3 * b + 2];
+
+			b++;
+			if (b > 3) {
+				a++;
+				b = a + 1;
+			}
+		}
+	}
+
+	for (int i = 0; i < 6; i++) {
+		double *row = l_6x10 + 10 * i;
+
+		row[0] = dot(dv[0][i], dv[0][i]);
+		row[1] = 2.0f * dot(dv[0][i], dv[1][i]);
+		row[2] = dot(dv[1][i], dv[1][i]);
+		row[3] = 2.0f * dot(dv[0][i], dv[2][i]);
+		row[4] = 2.0f * dot(dv[1][i], dv[2][i]);
+		row[5] = dot(dv[2][i], dv[2][i]);
+		row[6] = 2.0f * dot(dv[0][i], dv[3][i]);
+		row[7] = 2.0f * dot(dv[1][i], dv[3][i]);
+		row[8] = 2.0f * dot(dv[2][i], dv[3][i]);
+		row[9] = dot(dv[3][i], dv[3][i]);
+	}
+}
+
+void find_betas_approx_1(const CvMat *L_6x10, const CvMat *Rho, double *betas) {
+	double l_6x4[6 * 4], b4[4];
+	CvMat L_6x4 = cvMat(6, 4, CV_64F, l_6x4);
+	CvMat B4 = cvMat(4, 1, CV_64F, b4);
+
+	for (int i = 0; i < 6; i++) {
+		cvmSet(&L_6x4, i, 0, cvmGet(L_6x10, i, 0));
+		cvmSet(&L_6x4, i, 1, cvmGet(L_6x10, i, 1));
+		cvmSet(&L_6x4, i, 2, cvmGet(L_6x10, i, 3));
+		cvmSet(&L_6x4, i, 3, cvmGet(L_6x10, i, 6));
+	}
+
+	cvSolve(&L_6x4, Rho, &B4, CV_SVD);
+
+	assert(B4.data.db == b4);
+
+	if (b4[0] < 0) {
+		betas[0] = sqrt(-b4[0]);
+		betas[1] = -b4[1] / betas[0];
+		betas[2] = -b4[2] / betas[0];
+		betas[3] = -b4[3] / betas[0];
+	} else {
+		betas[0] = sqrt(b4[0]);
+		betas[1] = b4[1] / betas[0];
+		betas[2] = b4[2] / betas[0];
+		betas[3] = b4[3] / betas[0];
+	}
+}
+
+void compute_A_and_b_gauss_newton(const double *l_6x10, const double *rho, double betas[4], CvMat *A, CvMat *b) {
+	for (int i = 0; i < 6; i++) {
+		const double *rowL = l_6x10 + i * 10;
+		double *rowA = A->data.db + i * 4;
+
+		rowA[0] = 2 * rowL[0] * betas[0] + rowL[1] * betas[1] + rowL[3] * betas[2] + rowL[6] * betas[3];
+		rowA[1] = rowL[1] * betas[0] + 2 * rowL[2] * betas[1] + rowL[4] * betas[2] + rowL[7] * betas[3];
+		rowA[2] = rowL[3] * betas[0] + rowL[4] * betas[1] + 2 * rowL[5] * betas[2] + rowL[8] * betas[3];
+		rowA[3] = rowL[6] * betas[0] + rowL[7] * betas[1] + rowL[8] * betas[2] + 2 * rowL[9] * betas[3];
+
+		cvmSet(b, i, 0,
+			   rho[i] - (rowL[0] * betas[0] * betas[0] + rowL[1] * betas[0] * betas[1] + rowL[2] * betas[1] * betas[1] +
+						 rowL[3] * betas[0] * betas[2] + rowL[4] * betas[1] * betas[2] + rowL[5] * betas[2] * betas[2] +
+						 rowL[6] * betas[0] * betas[3] + rowL[7] * betas[1] * betas[3] + rowL[8] * betas[2] * betas[3] +
+						 rowL[9] * betas[3] * betas[3]));
+	}
+}
+
+void qr_solve(CvMat *A, CvMat *b, CvMat *X) {
+	static int max_nr = 0;
+	static double *A1, *A2;
+
+	const int nr = A->rows;
+	const int nc = A->cols;
+
+	if (max_nr != 0 && max_nr < nr) {
+		free(A1);
+		free(A2);
+	}
+	if (max_nr < nr) {
+		max_nr = nr;
+		A1 = malloc(sizeof(double) * nr);
+		A2 = malloc(sizeof(double) * nr);
+	}
+
+	double *pA = A->data.db, *ppAkk = pA;
+	for (int k = 0; k < nc; k++) {
+		double *ppAik = ppAkk, eta = fabs(*ppAik);
+		for (int i = k + 1; i < nr; i++) {
+			double elt = fabs(*ppAik);
+			if (eta < elt)
+				eta = elt;
+			ppAik += nc;
+		}
+
+		if (eta == 0) {
+			A1[k] = A2[k] = 0.0;
+			// cerr << "God damnit, A is singular, this shouldn't happen." << endl;
+			return;
+		} else {
+			double *ppAik = ppAkk, sum = 0.0, inv_eta = 1. / eta;
+			for (int i = k; i < nr; i++) {
+				*ppAik *= inv_eta;
+				sum += *ppAik * *ppAik;
+				ppAik += nc;
+			}
+			double sigma = sqrt(sum);
+			if (*ppAkk < 0)
+				sigma = -sigma;
+			*ppAkk += sigma;
+			A1[k] = sigma * *ppAkk;
+			A2[k] = -eta * sigma;
+			for (int j = k + 1; j < nc; j++) {
+				double *ppAik = ppAkk, sum = 0;
+				for (int i = k; i < nr; i++) {
+					sum += *ppAik * ppAik[j - k];
+					ppAik += nc;
+				}
+				double tau = sum / A1[k];
+				ppAik = ppAkk;
+				for (int i = k; i < nr; i++) {
+					ppAik[j - k] -= tau * *ppAik;
+					ppAik += nc;
+				}
+			}
+		}
+		ppAkk += nc + 1;
+	}
+
+	// b <- Qt b
+	double *ppAjj = pA, *pb = b->data.db;
+	for (int j = 0; j < nc; j++) {
+		double *ppAij = ppAjj, tau = 0;
+		for (int i = j; i < nr; i++) {
+			tau += *ppAij * pb[i];
+			ppAij += nc;
+		}
+		tau /= A1[j];
+		ppAij = ppAjj;
+		for (int i = j; i < nr; i++) {
+			pb[i] -= tau * *ppAij;
+			ppAij += nc;
+		}
+		ppAjj += nc + 1;
+	}
+
+	// X = R-1 b
+	double *pX = X->data.db;
+	pX[nc - 1] = pb[nc - 1] / A2[nc - 1];
+	for (int i = nc - 2; i >= 0; i--) {
+		double *ppAij = pA + i * nc + (i + 1), sum = 0;
+
+		for (int j = i + 1; j < nc; j++) {
+			sum += *ppAij * pX[j];
+			ppAij++;
+		}
+		pX[i] = (pb[i] - sum) / A2[i];
+	}
+}
+
+void gauss_newton(const CvMat *L_6x10, const CvMat *Rho, double betas[4]) {
+	const int iterations_number = 5;
+
+	double a[6 * 4], b[6], x[4];
+	CvMat A = cvMat(6, 4, CV_64F, a);
+	CvMat B = cvMat(6, 1, CV_64F, b);
+	CvMat X = cvMat(4, 1, CV_64F, x);
+
+	for (int k = 0; k < iterations_number; k++) {
+		compute_A_and_b_gauss_newton(L_6x10->data.db, Rho->data.db, betas, &A, &B);
+		qr_solve(&A, &B, &X);
+
+		for (int i = 0; i < 4; i++)
+			betas[i] += x[i];
+	}
+}
+
+void find_betas_approx_2(const CvMat *L_6x10, const CvMat *Rho, double *betas) {
+	double l_6x3[6 * 3], b3[3];
+	CvMat L_6x3 = cvMat(6, 3, CV_64F, l_6x3);
+	CvMat B3 = cvMat(3, 1, CV_64F, b3);
+
+	for (int i = 0; i < 6; i++) {
+		cvmSet(&L_6x3, i, 0, cvmGet(L_6x10, i, 0));
+		cvmSet(&L_6x3, i, 1, cvmGet(L_6x10, i, 1));
+		cvmSet(&L_6x3, i, 2, cvmGet(L_6x10, i, 2));
+	}
+
+	cvSolve(&L_6x3, Rho, &B3, CV_SVD);
+
+	if (b3[0] < 0) {
+		betas[0] = sqrt(-b3[0]);
+		betas[1] = (b3[2] < 0) ? sqrt(-b3[2]) : 0.0;
+	} else {
+		betas[0] = sqrt(b3[0]);
+		betas[1] = (b3[2] > 0) ? sqrt(b3[2]) : 0.0;
+	}
+
+	if (b3[1] < 0)
+		betas[0] = -betas[0];
+
+	betas[2] = 0.0;
+	betas[3] = 0.0;
+}
+
+// betas10        = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
+// betas_approx_3 = [B11 B12 B22 B13 B23                    ]
+
+void epnp_find_betas_approx_3(epnp *self, const CvMat *L_6x10, const CvMat *Rho, double *betas) {
+	double l_6x5[6 * 5], b5[5];
+	CvMat L_6x5 = cvMat(6, 5, CV_64F, l_6x5);
+	CvMat B5 = cvMat(5, 1, CV_64F, b5);
+
+	for (int i = 0; i < 6; i++) {
+		cvmSet(&L_6x5, i, 0, cvmGet(L_6x10, i, 0));
+		cvmSet(&L_6x5, i, 1, cvmGet(L_6x10, i, 1));
+		cvmSet(&L_6x5, i, 2, cvmGet(L_6x10, i, 2));
+		cvmSet(&L_6x5, i, 3, cvmGet(L_6x10, i, 3));
+		cvmSet(&L_6x5, i, 4, cvmGet(L_6x10, i, 4));
+	}
+
+	cvSolve(&L_6x5, Rho, &B5, CV_SVD);
+
+	if (b5[0] < 0) {
+		betas[0] = sqrt(-b5[0]);
+		betas[1] = (b5[2] < 0) ? sqrt(-b5[2]) : 0.0;
+	} else {
+		betas[0] = sqrt(b5[0]);
+		betas[1] = (b5[2] > 0) ? sqrt(b5[2]) : 0.0;
+	}
+	if (b5[1] < 0)
+		betas[0] = -betas[0];
+	betas[2] = b5[3] / betas[0];
+	betas[3] = 0.0;
+}
+
+void copy_R_and_t(const double R_src[3][3], const double t_src[3], double R_dst[3][3], double t_dst[3]) {
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++)
+			R_dst[i][j] = R_src[i][j];
+		t_dst[i] = t_src[i];
+	}
+}
+
+double epnp_compute_pose(epnp *self, double R[3][3], double t[3]) {
+	epnp_choose_control_points(self);
+	epnp_compute_barycentric_coordinates(self);
+
+	CvMat *M = cvCreateMat(2 * self->number_of_correspondences, 12, CV_64F);
+
+	for (int i = 0; i < self->number_of_correspondences; i++)
+		epnp_fill_M(self, M, 2 * i, self->alphas + 4 * i, self->us[2 * i], self->us[2 * i + 1]);
+
+
+	double mtm[12 * 12], d[12], ut[12 * 12];
+	CvMat MtM = cvMat(12, 12, CV_64F, mtm);
+	CvMat D = cvMat(12, 1, CV_64F, d);
+	CvMat Ut = cvMat(12, 12, CV_64F, ut);
+
+	cvMulTransposed(M, &MtM, 1, 0, 1);
+
+	cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);
+	cvReleaseMat(&M);
+
+	/*  double gt[] = {0.907567, -0.00916941, -0.0637565, -0.239863, 0.00224965, 0.0225974, -0.239574, 0.00209046,
+	   0.0176213, -0.237255, 0.00251711, -0.0108157,
+		   0.00910763, 0.909518, 0.0026331, -0.00232957, -0.239824, 0.00409253, -0.00243169, -0.239934, 0.00316978,
+	   -0.0024403, -0.239909, -0.0016784,
+		   -0.00657473, -0.00182409, -0.118455, -0.418384, -0.0208829, -0.00537926, -0.341435, -0.198683, 0.0639791,
+	   0.777439, 0.211238, 0.0351144,
+		   -0.000558729, -0.00120335, 0.0410987, 0.435735, 0.470224, -0.0117729, -0.330236, -0.651751, 0.0877612,
+	   -0.112846, 0.179057, -0.0293607,
+		   0.000207011, -0.000114796, 1.30348e-05, -0.150349, -0.396757, -0.0336814, 0.362168, -0.332794, -0.0038853,
+	   -0.215378, 0.728371, 3.59307e-05,
+		   -0.000236456, 3.59257e-05, -0.00240085, -0.516359, 0.533741, 8.75851e-05, 0.550447, -0.29792, -0.00101687,
+	   -0.0338867, -0.235687, -0.00652534,
+		   0.367037, -0.0382166, -0.268689, 0.518886, -0.0415839, 0.198992, 0.504361, -0.0564282, 0.00252704, 0.456381,
+	   -0.0480543, 0.11356,
+		   -0.0477438, -0.404345, -0.0789953, -0.0475805, -0.514161, -0.108317, -0.0431554, -0.498573, 0.134824,
+	   -0.0719115, -0.52184, 0.0593704,
+		   0.172473, -0.0624523, 0.798148, 0.0821341, -0.0877883, -0.120482, 0.105865, -0.083816, -0.254253, 0.24317,
+	   -0.056877, -0.393827,
+		   -0.0555454, -0.0526344, 0.0122309, -0.0649974, -0.0336308, 0.479865, -0.117645, -0.135477, -0.783616,
+	   -0.0585432, -0.034449, 0.327881,
+		   0.0797424, 0.032575, 0.168567, 0.0597489, 0.0568341, -0.66392, 0.0387932, 0.0297936, -0.142108, 0.0542191,
+	   0.0221337, 0.700399,
+		   -0.00310509, 0.000734298, -0.485965, 0.0476647, 0.0218702, -0.51114, -0.00347318, -0.0252922, -0.520376,
+	   0.00830308, -0.0120006, -0.477658 };
+		   for(int i = 0;i < 144;i++) ut[i] = gt[i];*/
+	assert(Ut.data.db == ut);
+
+	double l_6x10[6 * 10], rho[6];
+	CvMat L_6x10 = cvMat(6, 10, CV_64F, l_6x10);
+	CvMat Rho = cvMat(6, 1, CV_64F, rho);
+
+	epnp_compute_L_6x10(self, ut, l_6x10);
+
+	epnp_compute_rho(self, rho);
+
+	double Betas[4][4], rep_errors[4];
+	double Rs[4][3][3], ts[4][3];
+
+	find_betas_approx_1(&L_6x10, &Rho, Betas[1]);
+	gauss_newton(&L_6x10, &Rho, Betas[1]);
+
+	rep_errors[1] = epnp_compute_R_and_t(self, ut, Betas[1], Rs[1], ts[1]);
+
+	find_betas_approx_2(&L_6x10, &Rho, Betas[2]);
+	gauss_newton(&L_6x10, &Rho, Betas[2]);
+	rep_errors[2] = epnp_compute_R_and_t(self, ut, Betas[2], Rs[2], ts[2]);
+
+	epnp_find_betas_approx_3(self, &L_6x10, &Rho, Betas[3]);
+	gauss_newton(&L_6x10, &Rho, Betas[3]);
+	rep_errors[3] = epnp_compute_R_and_t(self, ut, Betas[3], Rs[3], ts[3]);
+
+	int N = 1;
+	if (rep_errors[2] < rep_errors[1])
+		N = 2;
+	if (rep_errors[3] < rep_errors[N])
+		N = 3;
+
+	copy_R_and_t(Rs[N], ts[N], R, t);
+
+	return rep_errors[N];
+}
+
+double epnp_reprojection_error(epnp *self, const double R[3][3], const double t[3]) {
+	double sum2 = 0.0;
+
+	for (int i = 0; i < self->number_of_correspondences; i++) {
+		double *pw = self->pws + 3 * i;
+		double Xc = dot(R[0], pw) + t[0];
+		double Yc = dot(R[1], pw) + t[1];
+		double inv_Zc = 1.0 / (dot(R[2], pw) + t[2]);
+		double ue = self->uc + self->fu * Xc * inv_Zc;
+		double ve = self->vc + self->fv * Yc * inv_Zc;
+		double u = self->us[2 * i], v = self->us[2 * i + 1];
+
+		sum2 += sqrt((u - ue) * (u - ue) + (v - ve) * (v - ve));
+	}
+
+	return sum2 / self->number_of_correspondences;
+}
+
+void epnp_estimate_R_and_t(epnp *self, double R[3][3], double t[3]) {
+	double pc0[3], pw0[3];
+
+	pc0[0] = pc0[1] = pc0[2] = 0.0;
+	pw0[0] = pw0[1] = pw0[2] = 0.0;
+
+	for (int i = 0; i < self->number_of_correspondences; i++) {
+		const double *pc = self->pcs + 3 * i;
+		const double *pw = self->pws + 3 * i;
+
+		for (int j = 0; j < 3; j++) {
+			pc0[j] += pc[j];
+			pw0[j] += pw[j];
+		}
+	}
+	for (int j = 0; j < 3; j++) {
+		pc0[j] /= self->number_of_correspondences;
+		pw0[j] /= self->number_of_correspondences;
+	}
+
+	double abt[3 * 3], abt_d[3], abt_u[3 * 3], abt_v[3 * 3];
+	CvMat ABt = cvMat(3, 3, CV_64F, abt);
+	CvMat ABt_D = cvMat(3, 1, CV_64F, abt_d);
+	CvMat ABt_U = cvMat(3, 3, CV_64F, abt_u);
+	CvMat ABt_V = cvMat(3, 3, CV_64F, abt_v);
+
+	cvSetZero(&ABt);
+
+	for (int i = 0; i < self->number_of_correspondences; i++) {
+		double *pc = self->pcs + 3 * i;
+		double *pw = self->pws + 3 * i;
+
+		for (int j = 0; j < 3; j++) {
+			abt[3 * j] += (pc[j] - pc0[j]) * (pw[0] - pw0[0]);
+			abt[3 * j + 1] += (pc[j] - pc0[j]) * (pw[1] - pw0[1]);
+			abt[3 * j + 2] += (pc[j] - pc0[j]) * (pw[2] - pw0[2]);
+		}
+	}
+
+	cvSVD(&ABt, &ABt_D, &ABt_U, &ABt_V, CV_SVD_MODIFY_A);
+
+	for (int i = 0; i < 3; i++)
+		for (int j = 0; j < 3; j++)
+			R[i][j] = dot(abt_u + 3 * i, abt_v + 3 * j);
+
+	const double det = R[0][0] * R[1][1] * R[2][2] + R[0][1] * R[1][2] * R[2][0] + R[0][2] * R[1][0] * R[2][1] -
+					   R[0][2] * R[1][1] * R[2][0] - R[0][1] * R[1][0] * R[2][2] - R[0][0] * R[1][2] * R[2][1];
+
+	if (det < 0) {
+		R[2][0] = -R[2][0];
+		R[2][1] = -R[2][1];
+		R[2][2] = -R[2][2];
+	}
+
+	t[0] = pc0[0] - dot(R[0], pw0);
+	t[1] = pc0[1] - dot(R[1], pw0);
+	t[2] = pc0[2] - dot(R[2], pw0);
+}
+
+void print_pose(const double R[3][3], const double t[3]) {
+	for (unsigned i = 0; i < 3; i++) {
+		for (unsigned j = 0; j < 3; j++) {
+			printf("%g ", R[i][j]);
+		}
+		printf("%g ", t[i]);
+		printf("\n");
+	}
+	printf("\n");
+}
+
+void epnp_solve_for_sign(epnp *self) {
+	if (self->pcs[2] < 0.0) {
+		for (int i = 0; i < 4; i++)
+			for (int j = 0; j < 3; j++)
+				self->ccs[i][j] = -self->ccs[i][j];
+
+		for (int i = 0; i < self->number_of_correspondences; i++) {
+			self->pcs[3 * i] = -self->pcs[3 * i];
+			self->pcs[3 * i + 1] = -self->pcs[3 * i + 1];
+			self->pcs[3 * i + 2] = -self->pcs[3 * i + 2];
+		}
+	}
+}
+
+double epnp_compute_R_and_t(epnp *self, const double *ut, const double *betas, double R[3][3], double t[3]) {
+	epnp_compute_ccs(self, betas, ut);
+	epnp_compute_pcs(self);
+
+	epnp_solve_for_sign(self);
+
+	epnp_estimate_R_and_t(self, R, t);
+
+	return epnp_reprojection_error(self, R, t);
+}
+
+// betas10        = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
+// betas_approx_1 = [B11 B12     B13         B14]
+
+// betas10        = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
+// betas_approx_2 = [B11 B12 B22                            ]
+
+void mat_to_quat(const double R[3][3], double q[4]) {
+	double tr = R[0][0] + R[1][1] + R[2][2];
+	double n4;
+
+	if (tr > 0.0f) {
+		q[0] = R[1][2] - R[2][1];
+		q[1] = R[2][0] - R[0][2];
+		q[2] = R[0][1] - R[1][0];
+		q[3] = tr + 1.0f;
+		n4 = q[3];
+	} else if ((R[0][0] > R[1][1]) && (R[0][0] > R[2][2])) {
+		q[0] = 1.0f + R[0][0] - R[1][1] - R[2][2];
+		q[1] = R[1][0] + R[0][1];
+		q[2] = R[2][0] + R[0][2];
+		q[3] = R[1][2] - R[2][1];
+		n4 = q[0];
+	} else if (R[1][1] > R[2][2]) {
+		q[0] = R[1][0] + R[0][1];
+		q[1] = 1.0f + R[1][1] - R[0][0] - R[2][2];
+		q[2] = R[2][1] + R[1][2];
+		q[3] = R[2][0] - R[0][2];
+		n4 = q[1];
+	} else {
+		q[0] = R[2][0] + R[0][2];
+		q[1] = R[2][1] + R[1][2];
+		q[2] = 1.0f + R[2][2] - R[0][0] - R[1][1];
+		q[3] = R[0][1] - R[1][0];
+		n4 = q[2];
+	}
+	double scale = 0.5f / (sqrt(n4));
+
+	q[0] *= scale;
+	q[1] *= scale;
+	q[2] *= scale;
+	q[3] *= scale;
+}
+
+void relative_error(double *rot_err, double *transl_err, const double Rtrue[3][3], const double ttrue[3],
+					const double Rest[3][3], const double test[3]) {
+	double qtrue[4], qest[4];
+
+	mat_to_quat(Rtrue, qtrue);
+	mat_to_quat(Rest, qest);
+
+	double rot_err1 = sqrt((qtrue[0] - qest[0]) * (qtrue[0] - qest[0]) + (qtrue[1] - qest[1]) * (qtrue[1] - qest[1]) +
+						   (qtrue[2] - qest[2]) * (qtrue[2] - qest[2]) + (qtrue[3] - qest[3]) * (qtrue[3] - qest[3])) /
+					  sqrt(qtrue[0] * qtrue[0] + qtrue[1] * qtrue[1] + qtrue[2] * qtrue[2] + qtrue[3] * qtrue[3]);
+
+	double rot_err2 = sqrt((qtrue[0] + qest[0]) * (qtrue[0] + qest[0]) + (qtrue[1] + qest[1]) * (qtrue[1] + qest[1]) +
+						   (qtrue[2] + qest[2]) * (qtrue[2] + qest[2]) + (qtrue[3] + qest[3]) * (qtrue[3] + qest[3])) /
+					  sqrt(qtrue[0] * qtrue[0] + qtrue[1] * qtrue[1] + qtrue[2] * qtrue[2] + qtrue[3] * qtrue[3]);
+
+	*rot_err = fmin(rot_err1, rot_err2);
+
+	*transl_err = sqrt((ttrue[0] - test[0]) * (ttrue[0] - test[0]) + (ttrue[1] - test[1]) * (ttrue[1] - test[1]) +
+					   (ttrue[2] - test[2]) * (ttrue[2] - test[2])) /
+				  sqrt(ttrue[0] * ttrue[0] + ttrue[1] * ttrue[1] + ttrue[2] * ttrue[2]);
+}
diff --git a/src/epnp/epnp.h b/src/epnp/epnp.h
new file mode 100644
index 0000000..f87e1bf
--- /dev/null
+++ b/src/epnp/epnp.h
@@ -0,0 +1,61 @@
+// Copyright (c) 2009, V. Lepetit, EPFL
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+
+// 1. Redistributions of source code must retain the above copyright notice, this
+//    list of conditions and the following disclaimer.
+// 2. Redistributions in binary form must reproduce the above copyright notice,
+//    this list of conditions and the following disclaimer in the documentation
+//    and/or other materials provided with the distribution.
+
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+// The views and conclusions contained in the software and documentation are those
+// of the authors and should not be interpreted as representing official policies,
+//   either expressed or implied, of the FreeBSD Project.
+
+#ifndef epnp_h
+#define epnp_h
+
+#ifndef WITH_OPENCV
+#include "../../redist/minimal_opencv.h"
+#else
+#include <opencv/cv.h>
+#endif
+
+typedef struct {
+
+	double uc, vc, fu, fv;
+
+	double *pws, *us, *alphas, *pcs;
+	int maximum_number_of_correspondences;
+	int number_of_correspondences;
+
+	double cws[4][3], ccs[4][3];
+	double cws_determinant;
+} epnp;
+
+void epnp_dtor(epnp *self);
+void epnp_set_internal_parameters(epnp *self, double uc, double vc, double fu, double fv);
+void epnp_set_maximum_number_of_correspondences(epnp *self, int n);
+void epnp_reset_correspondences(epnp *self);
+void epnp_add_correspondence(epnp *self, double X, double Y, double Z, double u, double v);
+double epnp_compute_pose(epnp *self, double R[3][3], double t[3]);
+void relative_error(double *rot_err, double *transl_err, const double Rtrue[3][3], const double ttrue[3],
+					const double Rest[3][3], const double test[3]);
+void epnp_print_pose(epnp *self, const double R[3][3], const double t[3]);
+double epnp_reprojection_error(epnp *self, const double R[3][3], const double t[3]);
+void print_pose(const double R[3][3], const double t[3]);
+
+#endif
diff --git a/src/epnp/test_epnp.c b/src/epnp/test_epnp.c
new file mode 100644
index 0000000..d4c5391
--- /dev/null
+++ b/src/epnp/test_epnp.c
@@ -0,0 +1,128 @@
+// Copyright (c) 2009, V. Lepetit, EPFL
+// All rights reserved.
+
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+
+// 1. Redistributions of source code must retain the above copyright notice, this
+//    list of conditions and the following disclaimer.
+// 2. Redistributions in binary form must reproduce the above copyright notice,
+//    this list of conditions and the following disclaimer in the documentation
+//    and/or other materials provided with the distribution.
+
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+// ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+// The views and conclusions contained in the software and documentation are those
+// of the authors and should not be interpreted as representing official policies,
+//   either expressed or implied, of the FreeBSD Project.
+
+#include "epnp.h"
+#include "math.h"
+#include "stdio.h"
+#include "time.h"
+
+const double uc = 320;
+const double vc = 240;
+const double fu = 800;
+const double fv = 800;
+
+// MtM takes more time than 12x12 opencv SVD with about 180 points and more:
+
+const int n = 10;
+const double noise = 10;
+
+double epnp_rand(double min, double max) { return min + (max - min) * (double)(rand()) / RAND_MAX; }
+
+void random_pose(double R[3][3], double t[3]) {
+	const double range = 1;
+
+	double phi = epnp_rand(0, range * 3.14159 * 2);
+	double theta = epnp_rand(0, range * 3.14159);
+	double psi = epnp_rand(0, range * 3.14159 * 2);
+
+	R[0][0] = cos(psi) * cos(phi) - cos(theta) * sin(phi) * sin(psi);
+	R[0][1] = cos(psi) * sin(phi) + cos(theta) * cos(phi) * sin(psi);
+	R[0][2] = sin(psi) * sin(theta);
+
+	R[1][0] = -sin(psi) * cos(phi) - cos(theta) * sin(phi) * cos(psi);
+	R[1][1] = -sin(psi) * sin(phi) + cos(theta) * cos(phi) * cos(psi);
+	R[1][2] = cos(psi) * sin(theta);
+
+	R[2][0] = sin(theta) * sin(phi);
+	R[2][1] = -sin(theta) * cos(phi);
+	R[2][2] = cos(theta);
+
+	t[0] = 0.0f;
+	t[1] = 0.0f;
+	t[2] = 6.0f;
+}
+
+void random_point(double *Xw, double *Yw, double *Zw) {
+	double theta = epnp_rand(0, 3.14159), phi = epnp_rand(0, 2 * 3.14159), R = epnp_rand(0, +2);
+
+	*Xw = sin(theta) * sin(phi) * R;
+	*Yw = -sin(theta) * cos(phi) * R;
+	*Zw = cos(theta) * R;
+}
+
+void project_with_noise(double R[3][3], double t[3], double Xw, double Yw, double Zw, double *u, double *v) {
+	double Xc = R[0][0] * Xw + R[0][1] * Yw + R[0][2] * Zw + t[0];
+	double Yc = R[1][0] * Xw + R[1][1] * Yw + R[1][2] * Zw + t[1];
+	double Zc = R[2][0] * Xw + R[2][1] * Yw + R[2][2] * Zw + t[2];
+
+	double nu = epnp_rand(-noise, +noise);
+	double nv = epnp_rand(-noise, +noise);
+	*u = uc + fu * Xc / Zc + nu;
+	*v = vc + fv * Yc / Zc + nv;
+}
+
+int main(int argc, char **argv) {
+	epnp PnP = {};
+
+	srand(0);
+
+	epnp_set_internal_parameters(&PnP, uc, vc, fu, fv);
+	epnp_set_maximum_number_of_correspondences(&PnP, n);
+
+	double R_true[3][3], t_true[3];
+	random_pose(R_true, t_true);
+
+	epnp_reset_correspondences(&PnP);
+	for (int i = 0; i < n; i++) {
+		double Xw, Yw, Zw, u, v;
+
+		random_point(&Xw, &Yw, &Zw);
+		printf("%f %f %f\n", Xw, Yw, Zw);
+		project_with_noise(R_true, t_true, Xw, Yw, Zw, &u, &v);
+		epnp_add_correspondence(&PnP, Xw, Yw, Zw, u, v);
+	}
+
+	printf("\n");
+
+	double R_est[3][3], t_est[3];
+	double err2 = epnp_compute_pose(&PnP, R_est, t_est);
+	double rot_err, transl_err;
+
+	relative_error(&rot_err, &transl_err, R_true, t_true, R_est, t_est);
+	printf("Reprojection error: %g\n", err2);
+	printf("rot_err: %g, %g \n", rot_err, transl_err);
+	printf("\n");
+	printf("'True reprojection error': %g\n\n", epnp_reprojection_error(&PnP, R_true, t_true));
+
+	printf("True pose:\n");
+	print_pose(R_true, t_true);
+	printf("\n");
+	printf("Found pose:\n");
+	print_pose(R_est, t_est);
+
+	return 0;
+}
diff --git a/src/epnp/test_minimal_cv.c b/src/epnp/test_minimal_cv.c
new file mode 100644
index 0000000..53f4613
--- /dev/null
+++ b/src/epnp/test_minimal_cv.c
@@ -0,0 +1,142 @@
+#include "epnp.h"
+#include "stdio.h"
+
+/* Parameters */
+
+void test_svd() {
+	double wkopt;
+	double *work;
+/* Local arrays */
+/* iwork dimension should be at least 8*min(m,n) */
+#define COLS 4
+#define ROWS 6
+
+#define LDA ROWS
+#define LDU ROWS
+#define LDVT COLS
+
+	double s[COLS], u[LDU * ROWS], vt[LDVT * COLS];
+	double a[ROWS * COLS] = {7.52,  -1.10, -7.95, 1.08, -0.76, 0.62, 9.34,  -7.10, 5.13,  6.62,  -5.66, 0.87,
+							 -4.75, 8.52,  5.75,  5.30, 1.33,  4.91, -5.49, -3.52, -2.40, -6.77, 2.34,  3.95};
+
+	CvMat A = cvMat(ROWS, COLS, CV_64F, a);
+	CvMat S = cvMat(1, COLS, CV_64F, s);
+	CvMat U = cvMat(LDU, ROWS, CV_64F, u);
+	CvMat VT = cvMat(LDVT, COLS, CV_64F, vt);
+
+	cvSVD(&A, &S, &U, &VT, 0);
+
+	print_mat(&A);
+	print_mat(&S);
+	print_mat(&U);
+	print_mat(&VT);
+
+	double n[LDVT * COLS];
+	CvMat N = cvMat(LDVT, COLS, CV_64F, n);
+
+	printf("Tf:\n");
+	cvMulTransposed(&VT, &N, 1, 0, 1);
+	print_mat(&N);
+}
+
+void test_solve() {
+	int msize = 10;
+	CvMat *A = cvCreateMat(msize, msize, CV_64F);
+	for (unsigned i = 0; i < A->rows; i++) {
+		for (unsigned j = 0; j < A->cols; j++) {
+			cvmSet(A, i, j, 1000. * rand() / (double)RAND_MAX);
+		}
+	}
+
+	int nsize = 1;
+	CvMat *X = cvCreateMat(msize, nsize, CV_64F);
+	for (unsigned i = 0; i < X->rows; i++) {
+		for (unsigned j = 0; j < X->cols; j++) {
+			cvmSet(X, i, j, 1000. * rand() / (double)RAND_MAX);
+		}
+	}
+
+	double b_m[nsize * msize];
+	CvMat B = cvMat(msize, nsize, CV_64F, b_m);
+	cvSolve(A, X, &B, 0);
+
+	double check_m[msize * nsize];
+	CvMat check = cvMat(msize, nsize, CV_64F, check_m);
+
+	cvGEMM(A, &B, 1, &check, 0, &check, 0);
+
+	printf("A: \n");
+	print_mat(A);
+	printf("B: \n");
+	print_mat(&B);
+
+	printf("X: \n");
+	print_mat(X);
+	printf("A*B: \n");
+	print_mat(&check);
+
+	cvReleaseMat(&A);
+	cvReleaseMat(&X);
+}
+
+void test_invert() {
+	int msize = 10;
+	CvMat *M = cvCreateMat(msize, msize, CV_64F);
+	for (unsigned i = 0; i < M->rows; i++) {
+		for (unsigned j = 0; j < M->cols; j++) {
+			cvmSet(M, i, j, 1000. * rand() / (double)RAND_MAX);
+		}
+	}
+
+	double inv_a[msize * msize];
+	CvMat inv = cvMat(msize, msize, CV_64F, inv_a);
+	cvInvert(M, &inv, CV_SVD);
+
+	double check_m[msize * msize];
+	CvMat check = cvMat(msize, msize, CV_64F, check_m);
+
+	cvGEMM(&inv, M, 1, &check, 0, &check, 0);
+	print_mat(M);
+	print_mat(&inv);
+	print_mat(&check);
+
+	cvReleaseMat(&M);
+}
+
+void test_transpose_mult() {
+	int msize = 10;
+	CvMat *M = cvCreateMat(msize, msize, CV_64F);
+	for (unsigned i = 0; i < M->rows; i++) {
+		for (unsigned j = 0; j < M->cols; j++) {
+			cvmSet(M, i, j, rand() / (double)RAND_MAX);
+		}
+	}
+
+	double n[msize * msize];
+	CvMat N = cvMat(msize, msize, CV_64F, n);
+
+	cvMulTransposed(M, &N, 1, 0, 1);
+
+	print_mat(&N);
+	cvReleaseMat(&M);
+
+	{
+		double m[] = {1, 2, 0, 3};
+		CvMat M = cvMat(2, 2, CV_64F, m);
+		double m1[] = {1, 2, 0, 3};
+		CvMat M1 = cvMat(2, 2, CV_64F, m1);
+
+		cvMulTransposed(&M, &M1, 1, 0, 1);
+
+		print_mat(&M);
+		print_mat(&M1);
+	}
+}
+
+int main() {
+	test_invert();
+	test_solve();
+	test_svd();
+	test_transpose_mult();
+	return 0;
+}
diff --git a/src/persistent_scene.c b/src/persistent_scene.c
new file mode 100644
index 0000000..9bbbf41
--- /dev/null
+++ b/src/persistent_scene.c
@@ -0,0 +1,36 @@
+#include "persistent_scene.h"
+#include "linmath.h"
+#include <stdlib.h>
+#include <string.h>
+#include <survive.h>
+
+bool PersistentScene_isStillValid(const PersistentScene *self, uint32_t timecode_now, uint32_t idx, int lh) {
+	const uint32_t *data_timecode = self->timecode[idx][lh];
+	return !(timecode_now - data_timecode[0] > self->tolerance || timecode_now - data_timecode[1] > self->tolerance);
+}
+
+void PersistentScene_add(PersistentScene *self, SurviveObject *so, PoserDataLight *lightData) {
+	int axis = (lightData->acode & 1);
+	uint32_t *data_timecode = &self->timecode[lightData->sensor_id][lightData->lh][axis];
+	FLT *angle = &self->angles[lightData->sensor_id][lightData->lh][axis];
+
+	*angle = lightData->angle;
+	*data_timecode = lightData->timecode;
+}
+
+void PersistentScene_ForEachCorrespondence(PersistentScene *self, PersistentScene_ForEachCorrespondence_fn fn,
+										   SurviveObject *so, uint32_t timecode_now, void *user) {
+	for (int lh = 0; lh < NUM_LIGHTHOUSES; lh++) {
+		for (size_t i = 0; i < so->nr_locations; i++) {
+			if (PersistentScene_isStillValid(self, timecode_now, i, lh)) {
+				double *pts = self->angles[i][lh];
+				fn(so, lh, i, pts, user);
+			}
+		}
+	}
+}
+
+void PersistentScene_ctor(PersistentScene *self) {
+	memset(self, 0, sizeof(PersistentScene));
+	self->tolerance = 1500000;
+}
diff --git a/src/persistent_scene.h b/src/persistent_scene.h
new file mode 100644
index 0000000..07d9056
--- /dev/null
+++ b/src/persistent_scene.h
@@ -0,0 +1,32 @@
+#pragma once
+#include "persistent_scene.h"
+#include "stdbool.h"
+
+#ifndef USE_DOUBLE
+#define FLT double
+#define USE_DOUBLE
+#endif
+
+#include "linmath.h"
+#include <survive.h>
+
+typedef struct {
+	uint32_t tolerance;
+
+	// If "lengths[...]" < 0, means not a valid piece of sweep information.
+	FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // 2 Axes  (Angles in LH space)
+	uint32_t timecode[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
+
+	PoserDataIMU lastimu;
+
+} PersistentScene;
+
+typedef void (*PersistentScene_ForEachCorrespondence_fn)(SurviveObject *so, int lh, int sensor_idx, FLT *angles,
+														 void *);
+void PersistentScene_ForEachCorrespondence(PersistentScene *self, PersistentScene_ForEachCorrespondence_fn fn,
+										   SurviveObject *so, uint32_t timecode_now, void *user);
+
+void PersistentScene_add(PersistentScene *self, SurviveObject *so, PoserDataLight *lightData);
+
+bool PersistentScene_isStillValid(const PersistentScene *self, uint32_t timecode_now, uint32_t idx, int lh);
+void PersistentScene_ctor(PersistentScene *self);
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
new file mode 100644
index 0000000..7749c7b
--- /dev/null
+++ b/src/poser_epnp.c
@@ -0,0 +1,155 @@
+#include "persistent_scene.h"
+
+#ifndef USE_DOUBLE
+#define FLT double
+#define USE_DOUBLE
+#endif
+
+#include <poser.h>
+#include <survive.h>
+
+#include "epnp/epnp.h"
+#include "linmath.h"
+#include "math.h"
+#include "stdio.h"
+
+static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool cameraToWorld) {
+	SurvivePose rtn = {};
+	// std::cerr << "Solving for " << cal_imagePoints.size() << " correspondents" << std::endl;
+	if (pnp->number_of_correspondences <= 4) {
+		SurviveContext *ctx = so->ctx;
+		SV_INFO("Can't solve for only %u points\n", pnp->number_of_correspondences);
+		return rtn;
+	}
+
+	double r[3][3];
+
+	double err = epnp_compute_pose(pnp, r, rtn.Pos);
+
+	CvMat R = cvMat(3, 3, CV_64F, r);
+	CvMat T = cvMat(3, 1, CV_64F, rtn.Pos);
+	// Requested output is camera -> world, so invert
+	if (cameraToWorld) {
+		FLT tmp[3];
+		CvMat Tmp = cvMat(3, 1, CV_64F, tmp);
+		cvCopyTo(&T, &Tmp);
+
+		// Flip the Rotation matrix
+		cvTranspose(&R, &R);
+		// Then 'tvec = -R * tvec'
+		cvGEMM(&R, &Tmp, -1, 0, 0, &T, 0);
+		print_mat(&R);
+		print_mat(&T);
+	}
+
+	FLT tmp[4];
+	quatfrommatrix33(tmp, r[0]);
+
+	// Typical camera applications have Z facing forward; the vive is contrarian and has Z going out of the
+	// back of the lighthouse. Think of this as a rotation on the Y axis a full 180 degrees -- the quat for that is
+	// [0 0x 1y 0z]
+	const FLT rt[4] = {0, 0, 1, 0};
+	quatrotateabout(rtn.Rot, tmp, rt);
+	if (!cameraToWorld) {
+		// We have to pre-multiply the rt transform here, which means we have to also offset our position by
+		quatrotateabout(rtn.Rot, rt, tmp);
+		rtn.Pos[0] = -rtn.Pos[0];
+		rtn.Pos[2] = -rtn.Pos[2];
+	}
+
+	return rtn;
+}
+
+static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) {
+
+	for (int lh = 0; lh < 2; lh++) {
+		epnp pnp = {.fu = 1, .fv = 1};
+		epnp_set_maximum_number_of_correspondences(&pnp, so->nr_locations);
+
+		for (size_t i = 0; i < so->nr_locations; i++) {
+			FLT *lengths = pdfs->lengths[i][lh];
+			FLT *ang = pdfs->angles[i][lh];
+			if (lengths[0] < 0 || lengths[1] < 0)
+				continue;
+
+			epnp_add_correspondence(&pnp, so->sensor_locations[i * 3 + 0], so->sensor_locations[i * 3 + 1],
+									so->sensor_locations[i * 3 + 2], tan(ang[0]), tan(ang[1]));
+		}
+
+		SurviveContext *ctx = so->ctx;
+		SV_INFO("Solving for %d correspondents", pnp.number_of_correspondences);
+		if (pnp.number_of_correspondences <= 4) {
+			SV_INFO("Can't solve for only %d points on lh %d\n", pnp.number_of_correspondences, lh);
+			continue;
+		}
+
+		SurvivePose lighthouse = solve_correspondence(so, &pnp, true);
+		PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &lighthouse);
+
+		epnp_dtor(&pnp);
+	}
+	return 0;
+}
+
+struct add_correspondence_for_lh {
+	epnp *pnp;
+	int lh;
+};
+
+void add_correspondence_for_lh(SurviveObject *so, int lh, int sensor_idx, FLT *angles, void *_user) {
+	struct add_correspondence_for_lh *user = (struct add_correspondence_for_lh *)_user;
+	if (user->lh == lh)
+		epnp_add_correspondence(user->pnp, so->sensor_locations[sensor_idx * 3 + 0],
+								so->sensor_locations[sensor_idx * 3 + 1], so->sensor_locations[sensor_idx * 3 + 2],
+								tan(angles[0]), tan(angles[1]));
+}
+
+int PoserEPNP(SurviveObject *so, PoserData *pd) {
+	switch (pd->pt) {
+	case POSERDATA_IMU: {
+		// Really should use this...
+		PoserDataIMU *imuData = (PoserDataIMU *)pd;
+		return 0;
+	}
+	case POSERDATA_LIGHT: {
+		static PersistentScene _scene = {.tolerance = 1500000};
+		PersistentScene *scene = &_scene;
+		PoserDataLight *lightData = (PoserDataLight *)pd;
+
+		PersistentScene_add(scene, so, lightData);
+
+		int lh = lightData->lh;
+		if (so->ctx->bsd[lh].PositionSet) {
+			epnp pnp = {.fu = 1, .fv = 1};
+			epnp_set_maximum_number_of_correspondences(&pnp, so->nr_locations);
+
+			struct add_correspondence_for_lh user = {.lh = lh, .pnp = &pnp};
+			PersistentScene_ForEachCorrespondence(scene, add_correspondence_for_lh, so, lightData->timecode, &user);
+
+			if (pnp.number_of_correspondences > 4) {
+
+				SurvivePose pose = solve_correspondence(so, &pnp, false);
+
+				SurvivePose txPose = {};
+				quatrotatevector(txPose.Pos, so->ctx->bsd[lh].Pose.Rot, pose.Pos);
+				for (int i = 0; i < 3; i++) {
+					txPose.Pos[i] += so->ctx->bsd[lh].Pose.Pos[i];
+				}
+
+				quatrotateabout(txPose.Rot, so->ctx->bsd[lh].Pose.Rot, pose.Rot);
+				PoserData_poser_raw_pose_func(pd, so, lh, &txPose);
+			}
+
+			epnp_dtor(&pnp);
+		}
+
+		return 0;
+	}
+	case POSERDATA_FULL_SCENE: {
+		return opencv_solver_fullscene(so, (PoserDataFullScene *)(pd));
+	}
+	}
+	return -1;
+}
+
+REGISTER_LINKTIME(PoserEPNP);
diff --git a/src/survive.c b/src/survive.c
index 61bfc86..a27ba3e 100755
--- a/src/survive.c
+++ b/src/survive.c
@@ -297,7 +297,9 @@ int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int d
 	int i;
 	for( i = 0; i < oldct; i++ )
 	{
-		ctx->drivermagics[i]( ctx, ctx->drivers[i], magic_code, data, datalen );
+		if (ctx->drivermagics[i]) {
+			ctx->drivermagics[i](ctx, ctx->drivers[i], magic_code, data, datalen);
+		}
 	}
 	return 0;
 }
@@ -338,7 +340,8 @@ void survive_close( SurviveContext * ctx )
 	{
 		PoserData pd;
 		pd.pt = POSERDATA_DISASSOCIATE;
-		if( ctx->objs[i]->PoserFn ) ctx->objs[i]->PoserFn( ctx->objs[i], &pd );
+		if (ctx->objs[i]->PoserFn)
+			ctx->objs[i]->PoserFn(ctx->objs[i], &pd);
 	}
 
 	for( i = 0; i < oldct; i++ )
@@ -352,6 +355,10 @@ void survive_close( SurviveContext * ctx )
 	destroy_config_group(ctx->global_config_values);
 	destroy_config_group(ctx->lh_config);
 
+	for (i = 0; i < ctx->objs_ct; i++) {
+		free(ctx->objs[i]);
+	}
+
 	free( ctx->objs );
 	free( ctx->drivers );
 	free( ctx->driverpolls );
diff --git a/src/survive_playback.c b/src/survive_playback.c
index fe7af2b..c4564c4 100644
--- a/src/survive_playback.c
+++ b/src/survive_playback.c
@@ -160,6 +160,7 @@ static int playback_close(struct SurviveContext *ctx, void *_driver) {
 	if (driver->playback_file)
 		fclose(driver->playback_file);
 	driver->playback_file = 0;
+
 	return 0;
 }
 
@@ -184,7 +185,11 @@ static int LoadConfig(SurvivePlaybackData *sv, SurviveObject *so) {
 	ct0conf[len] = 0;
 
 	printf("Loading config: %d\n", len);
-	return survive_load_htc_config_format(ct0conf, len, so);
+	int rtn = survive_load_htc_config_format(ct0conf, len, so);
+
+	free(ct0conf);
+
+	return rtn;
 }
 
 int DriverRegPlayback(SurviveContext *ctx) {
@@ -217,20 +222,14 @@ int DriverRegPlayback(SurviveContext *ctx) {
 	SurviveObject *tr0 = survive_create_tr0(ctx, "Playback", sp);
 	SurviveObject *ww0 = survive_create_ww0(ctx, "Playback", sp);
 
-	if (!LoadConfig(sp, hmd)) {
-		survive_add_object(ctx, hmd);
-	}
-	if (!LoadConfig(sp, wm0)) {
-		survive_add_object(ctx, wm0);
-	}
-	if (!LoadConfig(sp, wm1)) {
-		survive_add_object(ctx, wm1);
-	}
-	if (!LoadConfig(sp, tr0)) {
-		survive_add_object(ctx, tr0);
-	}
-	if (!LoadConfig(sp, ww0)) {
-		survive_add_object(ctx, ww0);
+	SurviveObject *objs[] = {hmd, wm0, wm1, tr0, ww0, 0};
+
+	for (SurviveObject **obj = objs; *obj; obj++) {
+		if (!LoadConfig(sp, *obj)) {
+			survive_add_object(ctx, *obj);
+		} else {
+			free(*obj);
+		}
 	}
 
 	survive_add_driver(ctx, sp, playback_poll, playback_close, 0);
diff --git a/src/survive_process.c b/src/survive_process.c
index e45a979..fe6dc45 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -109,7 +109,8 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8
 void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
 	// print the pose;
 	//printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
-
+	so->OutPose = *pose;
+	so->FromLHPose[lighthouse] = *pose;
 }
 
 void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose) {