Added first draft of epnp code

7 files changed, 3185 insertions, 0 deletions

diff --git a/src/epnp/epnp.c b/src/epnp/epnp.c
new file mode 100644
index 0000000..48b7d8a
--- /dev/null
+++ b/src/epnp/epnp.c
@@ -0,0 +1,772 @@
+// 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);
+	printf("Rho:\n");
+	print_mat(&cws);
+	print_mat(&ccs);
+
+	CvMat pws = cvMat(self->maximum_number_of_correspondences, 3, CV_64F, self->pws);
+	print_mat(&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);
+	printf("PW0tPW0\n");
+	print_mat(&PW0tPW0);
+
+	cvSVD(&PW0tPW0, &DC, &UCt, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);
+	assert(UCt.data.db == uct);
+	print_mat(&DC);
+	print_mat(&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];
+
+	printf("CC_inv\n");
+	print_mat(&CC);
+	cvInvert(&CC, &CC_inv, 1);
+
+	/*    double gt[] = {
+	  -0.39443, 0.639333, 1.16496 ,
+	  -0.550589, -1.45206, 0.610476 ,
+	  3.54726, -0.682609, 1.57564
+	};
+	for(int i = 0;i < 9;i++) CC_inv.data.db[i] = gt[i];
+	*/
+	print_mat(&CC_inv);
+
+	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));
+	}
+
+	print_mat(&L_6x4);
+	cvSolve(&L_6x4, Rho, &B4, CV_SVD);
+	print_mat(Rho);
+	print_mat(&B4);
+
+	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]);
+
+	printf("M\n");
+	print_mat(M);
+
+	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);
+
+	print_mat(&Ut);
+	print_mat(&D);
+
+	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]);
+	printf("r1: %f\n", rep_errors[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]);
+	printf("r2: %f\n", rep_errors[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]);
+	printf("r3: %f\n", rep_errors[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..9ca3a2e
--- /dev/null
+++ b/src/epnp/epnp.h
@@ -0,0 +1,60 @@
+// 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 "opencv_shim.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_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/opencv_shim.c b/src/epnp/opencv_shim.c
new file mode 100644
index 0000000..4680b1f
--- /dev/null
+++ b/src/epnp/opencv_shim.c
@@ -0,0 +1,421 @@
+//#include "/home/justin/source/CLAPACK/INCLUDE/f2c.h"
+//#include "/home/justin/source/CLAPACK/INCLUDE/clapack.h"
+#include <cblas.h>
+#include <lapacke.h>
+
+#include "math.h"
+#include "opencv_shim.h"
+#include "stdbool.h"
+#include "stdio.h"
+
+//#define DEBUG_PRINT
+
+int cvRound(float f) { return roundf(f); }
+#define CV_Error(code, msg) assert(0 && msg); // cv::error( code, msg, CV_Func, __FILE__, __LINE__ )
+
+const int DECOMP_SVD = 1;
+const int DECOMP_LU = 2;
+
+#include "shim_types_c.h"
+
+void print_mat(const CvMat *M);
+void cvCopyTo(const CvMat *srcarr, CvMat *dstarr) {
+	memcpy(dstarr->data.db, srcarr->data.db, sizeof(double) * dstarr->rows * dstarr->cols);
+}
+/*
+const int CV_64F = 0;
+*/
+typedef double doublereal;
+
+#define F77_FUNC(func) func##_
+/*
+extern int F77_FUNC(dgetrs)(char *trans, int *n, int *nrhs, double *a, int *lda, int *ipiv, double *b, int *ldb, int
+*info);
+
+extern int F77_FUNC(dgetri)(int *n, double *a, int *lda, int *ipiv, double *work, int *lwork, int *info);
+extern int F77_FUNC(dgetrf)(int *m, int *n, double *a, int *lda, int *ipiv, int *info); /* blocked LU
+
+extern int F77_FUNC(dgesvd)(char *jobu, char *jobvt,
+				int *m, int *n,
+				double *a, int *lda, double *s, double *u, int *ldu,
+				double *vt, int *ldvt, double *work, int *lwork,
+				int *info);
+
+extern int F77_FUNC(dgesdd)(char *jobz,
+				int *m, int *n, double *a, int *lda,
+		   double *s, double *u, int *ldu, double *vt, int *ldvt,
+		   double *work, int *lwork, int *iwork, int *info);
+
+extern int dgemm_(char *transa, char *transb, lapack_lapack_int *m, lapack_lapack_int *
+	n, lapack_lapack_int *k, double *alpha, double *a, lapack_lapack_int *lda,
+	double *b, lapack_lapack_int *ldb, double *beta, double *c, lapack_lapack_int
+	   *ldc);
+*/
+void cvGEMM(const CvMat *src1, const CvMat *src2, double alpha, const CvMat *src3, double beta, CvMat *dst, int tABC) {
+	lapack_int rows1 = src1->rows;
+	lapack_int cols1 = src1->cols;
+
+	lapack_int rows2 = src2->rows;
+	lapack_int cols2 = src2->cols;
+
+	lapack_int lda = cols1;
+	lapack_int ldb = cols2;
+
+	assert(src1->cols == src2->rows);
+	assert(src1->rows == dst->rows);
+	assert(src2->cols == dst->cols);
+
+	if (src3)
+		cvCopyTo(src3, dst);
+	else
+		beta = 0;
+
+	cblas_dgemm(CblasRowMajor, (tABC & GEMM_1_T) ? CblasTrans : CblasNoTrans,
+				(tABC & GEMM_2_T) ? CblasTrans : CblasNoTrans, src1->rows, dst->cols, src1->cols, alpha,
+
+				src1->data.db, lda, src2->data.db, ldb, beta,
+
+				dst->data.db, dst->cols);
+}
+
+void cvMulTransposed(const CvMat *src, CvMat *dst, int order, const CvMat *delta, double scale) {
+	lapack_int rows = src->rows;
+	lapack_int cols = src->cols;
+
+	lapack_int drows = dst->rows;
+	assert(drows == cols);
+	assert(order == 1 ? (dst->cols == src->cols) : (dst->cols == src->rows));
+	assert(delta == 0); // THIS ISN'T IMPLEMENTED YET
+	double beta = 0;
+
+	bool isAT = order == 1;
+	bool isBT = !isAT;
+
+	lapack_int dstCols = dst->cols;
+
+	cblas_dgemm(CblasRowMajor, isAT ? CblasTrans : CblasNoTrans, isBT ? CblasTrans : CblasNoTrans,
+				cols, // isAT ? cols : rows,
+				dstCols,
+				rows, // isAT ? rows : cols,
+				scale,
+
+				src->data.db, cols, src->data.db, cols, beta,
+
+				dst->data.db, dstCols);
+	// const CvMat* delta, double scale
+}
+
+void *cvAlloc(size_t size) { return malloc(size); }
+
+static void icvCheckHuge(CvMat *arr) {
+	if ((int64)arr->step * arr->rows > INT_MAX)
+		arr->type &= ~CV_MAT_CONT_FLAG;
+}
+
+CvMat *cvCreateMatHeader(int rows, int cols, int type) {
+	type = CV_MAT_TYPE(type);
+
+	assert(!(rows < 0 || cols < 0));
+
+	int min_step = CV_ELEM_SIZE(type);
+	assert(!(min_step <= 0));
+	min_step *= cols;
+
+	CvMat *arr = (CvMat *)cvAlloc(sizeof(*arr));
+
+	arr->step = min_step;
+	arr->type = CV_MAT_MAGIC_VAL | type | CV_MAT_CONT_FLAG;
+	arr->rows = rows;
+	arr->cols = cols;
+	arr->data.ptr = 0;
+	arr->refcount = 0;
+	arr->hdr_refcount = 1;
+
+	icvCheckHuge(arr);
+	return arr;
+}
+
+/* the alignment of all the allocated buffers */
+#define CV_MALLOC_ALIGN 16
+
+/* IEEE754 constants and macros */
+#define CV_TOGGLE_FLT(x) ((x) ^ ((int)(x) < 0 ? 0x7fffffff : 0))
+#define CV_TOGGLE_DBL(x) ((x) ^ ((int64)(x) < 0 ? CV_BIG_INT(0x7fffffffffffffff) : 0))
+
+#define CV_DbgAssert assert
+
+static inline void *cvAlignPtr(const void *ptr, int align) {
+	CV_DbgAssert((align & (align - 1)) == 0);
+	return (void *)(((size_t)ptr + align - 1) & ~(size_t)(align - 1));
+}
+
+static inline int cvAlign(int size, int align) {
+	CV_DbgAssert((align & (align - 1)) == 0 && size < INT_MAX);
+	return (size + align - 1) & -align;
+}
+
+void cvCreateData(CvArr *arr) {
+	if (CV_IS_MAT_HDR_Z(arr)) {
+		size_t step, total_size;
+		CvMat *mat = (CvMat *)arr;
+		step = mat->step;
+
+		if (mat->rows == 0 || mat->cols == 0)
+			return;
+
+		if (mat->data.ptr != 0)
+			CV_Error(CV_StsError, "Data is already allocated");
+
+		if (step == 0)
+			step = CV_ELEM_SIZE(mat->type) * mat->cols;
+
+		int64 _total_size = (int64)step * mat->rows + sizeof(int) + CV_MALLOC_ALIGN;
+		total_size = (size_t)_total_size;
+		if (_total_size != (int64)total_size)
+			CV_Error(CV_StsNoMem, "Too big buffer is allocated");
+		mat->refcount = (int *)cvAlloc((size_t)total_size);
+		mat->data.ptr = (uchar *)cvAlignPtr(mat->refcount + 1, CV_MALLOC_ALIGN);
+		*mat->refcount = 1;
+	} else if (CV_IS_MATND_HDR(arr)) {
+		CvMatND *mat = (CvMatND *)arr;
+		size_t total_size = CV_ELEM_SIZE(mat->type);
+
+		if (mat->dim[0].size == 0)
+			return;
+
+		if (mat->data.ptr != 0)
+			CV_Error(CV_StsError, "Data is already allocated");
+
+		if (CV_IS_MAT_CONT(mat->type)) {
+			total_size = (size_t)mat->dim[0].size * (mat->dim[0].step != 0 ? (size_t)mat->dim[0].step : total_size);
+		} else {
+			int i;
+			for (i = mat->dims - 1; i >= 0; i--) {
+				size_t size = (size_t)mat->dim[i].step * mat->dim[i].size;
+
+				if (total_size < size)
+					total_size = size;
+			}
+		}
+
+		mat->refcount = (int *)cvAlloc(total_size + sizeof(int) + CV_MALLOC_ALIGN);
+		mat->data.ptr = (uchar *)cvAlignPtr(mat->refcount + 1, CV_MALLOC_ALIGN);
+		*mat->refcount = 1;
+	} else
+		CV_Error(CV_StsBadArg, "unrecognized or unsupported array type");
+}
+
+CvMat *cvCreateMat(int height, int width, int type) {
+	CvMat *arr = cvCreateMatHeader(height, width, type);
+	cvCreateData(arr);
+
+	return arr;
+}
+
+double cvInvert(const CvMat *srcarr, CvMat *dstarr, int method) {
+	lapack_int inf;
+	lapack_int rows = srcarr->rows;
+	lapack_int cols = srcarr->cols;
+	lapack_int lda = srcarr->cols;
+
+	cvCopyTo(srcarr, dstarr);
+	double *a = dstarr->data.db;
+
+#ifdef DEBUG_PRINT
+	printf("a: \n");
+	print_mat(srcarr);
+#endif
+	if (method == DECOMP_LU) {
+		lapack_int *ipiv = malloc(sizeof(lapack_int) * MIN(srcarr->rows, srcarr->cols));
+		inf = LAPACKE_dgetrf(LAPACK_ROW_MAJOR, rows, cols, a, lda, ipiv);
+		assert(inf == 0);
+		print_mat(dstarr);
+
+		inf = LAPACKE_dgetri(LAPACK_ROW_MAJOR, rows, a, lda, ipiv);
+		print_mat(dstarr);
+		assert(inf >= 0);
+		if (inf > 0) {
+			printf("Warning: Singular matrix: \n");
+			print_mat(srcarr);
+		}
+
+		free(ipiv);
+
+	} else if (method == DECOMP_SVD) {
+
+		CvMat *w = cvCreateMat(1, MIN(dstarr->rows, dstarr->cols), dstarr->type);
+		CvMat *u = cvCreateMat(dstarr->cols, dstarr->cols, dstarr->type);
+		CvMat *v = cvCreateMat(dstarr->rows, dstarr->rows, dstarr->type);
+		cvSVD(dstarr, w, u, v, 0);
+
+		CvMat *um = cvCreateMat(w->cols, w->cols, w->type);
+
+		cvSetZero(um);
+		for (int i = 0; i < w->cols; i++) {
+			cvmSet(um, i, i, 1. / w->data.db[i]);
+		}
+
+		CvMat *tmp = cvCreateMat(dstarr->cols, dstarr->rows, dstarr->type);
+		cvGEMM(v, um, 1, 0, 0, tmp, GEMM_1_T);
+		cvGEMM(tmp, u, 1, 0, 0, dstarr, GEMM_2_T);
+	}
+	return 0;
+}
+
+CvMat *cvCloneMat(const CvMat *mat) {
+	CvMat *rtn = cvCreateMat(mat->rows, mat->cols, mat->type);
+	cvCopyTo(mat, rtn);
+	return rtn;
+}
+
+int cvSolve(const CvMat *Aarr, const CvMat *xarr, CvMat *Barr, int method) {
+	lapack_int inf;
+	lapack_int arows = Aarr->rows;
+	lapack_int acols = Aarr->cols;
+	lapack_int xcols = xarr->cols;
+	lapack_int xrows = xarr->rows;
+	lapack_int lda = acols; // Aarr->step / sizeof(double);
+	lapack_int type = CV_MAT_TYPE(Aarr->type);
+
+	if (method == DECOMP_LU) {
+		assert(Aarr->cols == Barr->rows);
+		assert(xarr->rows == Aarr->rows);
+		assert(Barr->cols == xarr->cols);
+		assert(type == CV_MAT_TYPE(Barr->type) && (type == CV_32F || type == CV_64F));
+
+		cvCopyTo(xarr, Barr);
+		CvMat *a_ws = cvCloneMat(Aarr);
+
+		lapack_int brows = Barr->rows;
+		lapack_int bcols = Barr->cols;
+		lapack_int ldb = bcols; // Barr->step / sizeof(double);
+
+		lapack_int *ipiv = malloc(sizeof(lapack_int) * MIN(Aarr->rows, Aarr->cols));
+
+		inf = LAPACKE_dgetrf(LAPACK_ROW_MAJOR, arows, acols, a_ws->data.db, lda, ipiv);
+		assert(inf >= 0);
+		if (inf > 0) {
+			printf("Warning: Singular matrix: \n");
+			print_mat(a_ws);
+		}
+
+#ifdef DEBUG_PRINT
+		printf("Solve A * x = B:\n");
+		print_mat(a_ws);
+		print_mat(Barr);
+#endif
+
+		inf =
+			LAPACKE_dgetrs(LAPACK_ROW_MAJOR, CblasNoTrans, arows, bcols, a_ws->data.db, lda, ipiv, Barr->data.db, ldb);
+		assert(inf == 0);
+
+		free(ipiv);
+		cvReleaseMat(&a_ws);
+	} else if (method == DECOMP_SVD) {
+
+#ifdef DEBUG_PRINT
+		printf("Solve |b - A * x|:\n");
+		print_mat(Aarr);
+		print_mat(xarr);
+#endif
+
+		CvMat *aCpy = cvCloneMat(Aarr);
+		CvMat *xCpy = cvCloneMat(xarr);
+		double *S = malloc(sizeof(double) * MIN(arows, acols));
+		double rcond = -1;
+		lapack_int *rank = malloc(sizeof(lapack_int) * MIN(arows, acols));
+		lapack_int inf = LAPACKE_dgelss(LAPACK_ROW_MAJOR, arows, acols, xcols, aCpy->data.db, acols, xCpy->data.db,
+										xcols, S, rcond, rank);
+		free(rank);
+		free(S);
+
+		assert(Barr->rows == acols);
+		assert(Barr->cols == xCpy->cols);
+		xCpy->rows = acols;
+		cvCopyTo(xCpy, Barr);
+/*
+Barr->data = xCpy->data;
+Barr->rows = acols;
+Barr->cols = xCpy->cols;
+*/
+#ifdef DEBUG_PRINT
+		print_mat(Barr);
+#endif
+		assert(inf == 0);
+	}
+	return 0;
+}
+
+void cvTranspose(const CvMat *M, CvMat *dst) {
+	bool inPlace = M == dst || M->data.db == dst->data.db;
+	double *src = M->data.db;
+
+	CvMat *tmp = 0;
+	if (inPlace) {
+		tmp = cvCloneMat(dst);
+		src = tmp->data.db;
+	}
+
+	assert(M->rows == dst->cols);
+	assert(M->cols == dst->rows);
+	for (unsigned i = 0; i < M->rows; i++) {
+		for (unsigned j = 0; j < M->cols; j++) {
+			dst->data.db[j * M->rows + i] = src[i * M->cols + j];
+		}
+	}
+
+	if (inPlace) {
+		cvReleaseMat(&tmp);
+	}
+}
+
+void cvSVD(CvMat *aarr, CvMat *warr, CvMat *uarr, CvMat *varr, int flags) {
+	char jobu = 'A';
+	char jobvt = 'A';
+
+	lapack_int inf;
+
+	if ((flags & CV_SVD_MODIFY_A) == 0) {
+		aarr = cvCloneMat(aarr);
+	}
+
+	if (uarr == 0)
+		jobu = 'N';
+	if (varr == 0)
+		jobvt = 'N';
+
+	lapack_int arows = aarr->rows, acols = aarr->cols;
+	lapack_int ulda = uarr ? uarr->cols : 1;
+	lapack_int plda = varr ? varr->cols : acols;
+
+	double *superb = malloc(MIN(arows, acols));
+	inf = LAPACKE_dgesvd(LAPACK_ROW_MAJOR, jobu, jobvt, arows, acols, aarr->data.db, acols, warr ? warr->data.db : 0,
+						 uarr ? uarr->data.db : 0, ulda, varr ? varr->data.db : 0, plda, superb);
+
+	free(superb);
+	assert(inf == 0);
+	if (uarr && (flags & CV_SVD_U_T)) {
+		cvTranspose(uarr, uarr);
+	}
+
+	if (varr && (flags & CV_SVD_V_T) == 0) {
+		cvTranspose(varr, varr);
+	}
+
+	if ((flags & CV_SVD_MODIFY_A) == 0) {
+		cvReleaseMat(&aarr);
+	}
+}
+
+void cvSetZero(CvMat *arr) {
+	for (int i = 0; i < arr->rows; i++)
+		for (int j = 0; j < arr->cols; j++)
+			arr->data.db[i * arr->cols + j] = 0;
+}
+
+void cvReleaseMat(CvMat **mat) {
+	assert(*(*mat)->refcount == 1);
+	free((*mat)->refcount);
+	free(*mat);
+	*mat = 0;
+}
diff --git a/src/epnp/opencv_shim.h b/src/epnp/opencv_shim.h
new file mode 100644
index 0000000..5806971
--- /dev/null
+++ b/src/epnp/opencv_shim.h
@@ -0,0 +1,29 @@
+int cvRound(float f);
+#define CV_Error(code, msg) assert(0 && msg); // cv::error( code, msg, CV_Func, __FILE__, __LINE__ )
+
+#include "shim_types_c.h"
+
+void print_mat(const CvMat *M);
+
+CvMat *cvCreateMat(int height, int width, int type);
+double cvInvert(const CvMat *srcarr, CvMat *dstarr, int method);
+void cvGEMM(const CvMat *src1, const CvMat *src2, double alpha, const CvMat *src3, double beta, CvMat *dst, int tABC);
+int cvSolve(const CvMat *Aarr, const CvMat *Barr, CvMat *xarr, int method);
+void cvSetZero(CvMat *arr);
+void cvCopyTo(const CvMat *src, CvMat *dest);
+CvMat *cvCloneMat(const CvMat *mat);
+void cvReleaseMat(CvMat **mat);
+void cvSVD(CvMat *aarr, CvMat *warr, CvMat *uarr, CvMat *varr, int flags);
+void cvMulTransposed(const CvMat *src, CvMat *dst, int order, const CvMat *delta, double scale);
+
+#define CV_SVD 1
+#define CV_SVD_MODIFY_A 1
+#define CV_SVD_SYM 2
+#define CV_SVD_U_T 2
+#define CV_SVD_V_T 4
+extern const int DECOMP_SVD;
+extern const int DECOMP_LU;
+
+#define GEMM_1_T 1
+#define GEMM_2_T 2
+#define GEMM_3_T 4
diff --git a/src/epnp/shim_types_c.h b/src/epnp/shim_types_c.h
new file mode 100644
index 0000000..56a61fe
--- /dev/null
+++ b/src/epnp/shim_types_c.h
@@ -0,0 +1,1633 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                          License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Copyright (C) 2013, OpenCV Foundation, all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's 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.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// 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 Intel Corporation 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.
+//
+//M*/
+
+#ifndef OPENCV_CORE_TYPES_H
+#define OPENCV_CORE_TYPES_H
+
+#ifdef HAVE_IPL
+#ifndef __IPL_H__
+#if defined _WIN32
+#include <ipl.h>
+#else
+#include <ipl/ipl.h>
+#endif
+#endif
+#elif defined __IPL_H__
+#define HAVE_IPL
+#endif
+
+#include "opencv2/core/cvdef.h"
+
+#ifndef SKIP_INCLUDES
+#include <assert.h>
+#include <float.h>
+#include <stdlib.h>
+#include <string.h>
+#endif // SKIP_INCLUDES
+
+#if defined _WIN32
+#define CV_CDECL __cdecl
+#define CV_STDCALL __stdcall
+#else
+#define CV_CDECL
+#define CV_STDCALL
+#endif
+
+#ifndef CV_DEFAULT
+#ifdef __cplusplus
+#define CV_DEFAULT(val) = val
+#else
+#define CV_DEFAULT(val)
+#endif
+#endif
+
+#ifndef CV_EXTERN_C_FUNCPTR
+#ifdef __cplusplus
+#define CV_EXTERN_C_FUNCPTR(x)                                                                                         \
+	extern "C" {                                                                                                       \
+	typedef x;                                                                                                         \
+	}
+#else
+#define CV_EXTERN_C_FUNCPTR(x) typedef x
+#endif
+#endif
+
+#ifndef CVAPI
+#define CVAPI(rettype) CV_EXTERN_C CV_EXPORTS rettype CV_CDECL
+#endif
+
+#ifndef CV_IMPL
+#define CV_IMPL CV_EXTERN_C
+#endif
+
+#ifdef __cplusplus
+#include "opencv2/core.hpp"
+#endif
+
+/** @addtogroup core_c
+	@{
+*/
+
+/** @brief This is the "metatype" used *only* as a function parameter.
+
+It denotes that the function accepts arrays of multiple types, such as IplImage*, CvMat* or even
+CvSeq* sometimes. The particular array type is determined at runtime by analyzing the first 4
+bytes of the header. In C++ interface the role of CvArr is played by InputArray and OutputArray.
+ */
+typedef void CvArr;
+
+typedef int CVStatus;
+
+/** @see cv::Error::Code */
+enum {
+	CV_StsOk = 0,			  /**< everything is ok                */
+	CV_StsBackTrace = -1,	 /**< pseudo error for back trace     */
+	CV_StsError = -2,		  /**< unknown /unspecified error      */
+	CV_StsInternal = -3,	  /**< internal error (bad state)      */
+	CV_StsNoMem = -4,		  /**< insufficient memory             */
+	CV_StsBadArg = -5,		  /**< function arg/param is bad       */
+	CV_StsBadFunc = -6,		  /**< unsupported function            */
+	CV_StsNoConv = -7,		  /**< iter. didn't converge           */
+	CV_StsAutoTrace = -8,	 /**< tracing                         */
+	CV_HeaderIsNull = -9,	 /**< image header is NULL            */
+	CV_BadImageSize = -10,	/**< image size is invalid           */
+	CV_BadOffset = -11,		  /**< offset is invalid               */
+	CV_BadDataPtr = -12,	  /**/
+	CV_BadStep = -13,		  /**< image step is wrong, this may happen for a non-continuous matrix */
+	CV_BadModelOrChSeq = -14, /**/
+	CV_BadNumChannels =
+		-15, /**< bad number of channels, for example, some functions accept only single channel matrices */
+	CV_BadNumChannel1U = -16,			/**/
+	CV_BadDepth = -17,					/**< input image depth is not supported by the function */
+	CV_BadAlphaChannel = -18,			/**/
+	CV_BadOrder = -19,					/**< number of dimensions is out of range */
+	CV_BadOrigin = -20,					/**< incorrect input origin               */
+	CV_BadAlign = -21,					/**< incorrect input align                */
+	CV_BadCallBack = -22,				/**/
+	CV_BadTileSize = -23,				/**/
+	CV_BadCOI = -24,					/**< input COI is not supported           */
+	CV_BadROISize = -25,				/**< incorrect input roi                  */
+	CV_MaskIsTiled = -26,				/**/
+	CV_StsNullPtr = -27,				/**< null pointer */
+	CV_StsVecLengthErr = -28,			/**< incorrect vector length */
+	CV_StsFilterStructContentErr = -29, /**< incorrect filter structure content */
+	CV_StsKernelStructContentErr = -30, /**< incorrect transform kernel content */
+	CV_StsFilterOffsetErr = -31,		/**< incorrect filter offset value */
+	CV_StsBadSize = -201,				/**< the input/output structure size is incorrect  */
+	CV_StsDivByZero = -202,				/**< division by zero */
+	CV_StsInplaceNotSupported = -203,   /**< in-place operation is not supported */
+	CV_StsObjectNotFound = -204,		/**< request can't be completed */
+	CV_StsUnmatchedFormats = -205,		/**< formats of input/output arrays differ */
+	CV_StsBadFlag = -206,				/**< flag is wrong or not supported */
+	CV_StsBadPoint = -207,				/**< bad CvPoint */
+	CV_StsBadMask = -208,				/**< bad format of mask (neither 8uC1 nor 8sC1)*/
+	CV_StsUnmatchedSizes = -209,		/**< sizes of input/output structures do not match */
+	CV_StsUnsupportedFormat = -210,		/**< the data format/type is not supported by the function*/
+	CV_StsOutOfRange = -211,			/**< some of parameters are out of range */
+	CV_StsParseError = -212,			/**< invalid syntax/structure of the parsed file */
+	CV_StsNotImplemented = -213,		/**< the requested function/feature is not implemented */
+	CV_StsBadMemBlock = -214,			/**< an allocated block has been corrupted */
+	CV_StsAssert = -215,				/**< assertion failed   */
+	CV_GpuNotSupported = -216,			/**< no CUDA support    */
+	CV_GpuApiCallError = -217,			/**< GPU API call error */
+	CV_OpenGlNotSupported = -218,		/**< no OpenGL support  */
+	CV_OpenGlApiCallError = -219,		/**< OpenGL API call error */
+	CV_OpenCLApiCallError = -220,		/**< OpenCL API call error */
+	CV_OpenCLDoubleNotSupported = -221,
+	CV_OpenCLInitError = -222, /**< OpenCL initialization error */
+	CV_OpenCLNoAMDBlasFft = -223
+};
+
+/****************************************************************************************\
+*                             Common macros and inline functions                         *
+\****************************************************************************************/
+
+#define CV_SWAP(a, b, t) ((t) = (a), (a) = (b), (b) = (t))
+
+/** min & max without jumps */
+#define CV_IMIN(a, b) ((a) ^ (((a) ^ (b)) & (((a) < (b)) - 1)))
+
+#define CV_IMAX(a, b) ((a) ^ (((a) ^ (b)) & (((a) > (b)) - 1)))
+
+/** absolute value without jumps */
+#ifndef __cplusplus
+#define CV_IABS(a) (((a) ^ ((a) < 0 ? -1 : 0)) - ((a) < 0 ? -1 : 0))
+#else
+#define CV_IABS(a) abs(a)
+#endif
+#define CV_CMP(a, b) (((a) > (b)) - ((a) < (b)))
+#define CV_SIGN(a) CV_CMP((a), 0)
+
+#define cvInvSqrt(value) ((float)(1. / sqrt(value)))
+#define cvSqrt(value) ((float)sqrt(value))
+
+/*************** Random number generation *******************/
+
+typedef uint64 CvRNG;
+
+#define CV_RNG_COEFF 4164903690U
+
+/** @brief Initializes a random number generator state.
+
+The function initializes a random number generator and returns the state. The pointer to the state
+can be then passed to the cvRandInt, cvRandReal and cvRandArr functions. In the current
+implementation a multiply-with-carry generator is used.
+@param seed 64-bit value used to initiate a random sequence
+@sa the C++ class RNG replaced CvRNG.
+ */
+CV_INLINE CvRNG cvRNG(int64 seed CV_DEFAULT(-1)) {
+	CvRNG rng = seed ? (uint64)seed : (uint64)(int64)-1;
+	return rng;
+}
+
+/** @brief Returns a 32-bit unsigned integer and updates RNG.
+
+The function returns a uniformly-distributed random 32-bit unsigned integer and updates the RNG
+state. It is similar to the rand() function from the C runtime library, except that OpenCV functions
+always generates a 32-bit random number, regardless of the platform.
+@param rng CvRNG state initialized by cvRNG.
+ */
+CV_INLINE unsigned cvRandInt(CvRNG *rng) {
+	uint64 temp = *rng;
+	temp = (uint64)(unsigned)temp * CV_RNG_COEFF + (temp >> 32);
+	*rng = temp;
+	return (unsigned)temp;
+}
+
+/** @brief Returns a floating-point random number and updates RNG.
+
+The function returns a uniformly-distributed random floating-point number between 0 and 1 (1 is not
+included).
+@param rng RNG state initialized by cvRNG
+ */
+CV_INLINE double cvRandReal(CvRNG *rng) { return cvRandInt(rng) * 2.3283064365386962890625e-10 /* 2^-32 */; }
+
+/****************************************************************************************\
+*                                  Image type (IplImage)                                 *
+\****************************************************************************************/
+
+#ifndef HAVE_IPL
+
+/*
+ * The following definitions (until #endif)
+ * is an extract from IPL headers.
+ * Copyright (c) 1995 Intel Corporation.
+ */
+#define IPL_DEPTH_SIGN 0x80000000
+
+#define IPL_DEPTH_1U 1
+#define IPL_DEPTH_8U 8
+#define IPL_DEPTH_16U 16
+#define IPL_DEPTH_32F 32
+
+#define IPL_DEPTH_8S (IPL_DEPTH_SIGN | 8)
+#define IPL_DEPTH_16S (IPL_DEPTH_SIGN | 16)
+#define IPL_DEPTH_32S (IPL_DEPTH_SIGN | 32)
+
+#define IPL_DATA_ORDER_PIXEL 0
+#define IPL_DATA_ORDER_PLANE 1
+
+#define IPL_ORIGIN_TL 0
+#define IPL_ORIGIN_BL 1
+
+#define IPL_ALIGN_4BYTES 4
+#define IPL_ALIGN_8BYTES 8
+#define IPL_ALIGN_16BYTES 16
+#define IPL_ALIGN_32BYTES 32
+
+#define IPL_ALIGN_DWORD IPL_ALIGN_4BYTES
+#define IPL_ALIGN_QWORD IPL_ALIGN_8BYTES
+
+#define IPL_BORDER_CONSTANT 0
+#define IPL_BORDER_REPLICATE 1
+#define IPL_BORDER_REFLECT 2
+#define IPL_BORDER_WRAP 3
+
+/** The IplImage is taken from the Intel Image Processing Library, in which the format is native. OpenCV
+only supports a subset of possible IplImage formats, as outlined in the parameter list above.
+
+In addition to the above restrictions, OpenCV handles ROIs differently. OpenCV functions require
+that the image size or ROI size of all source and destination images match exactly. On the other
+hand, the Intel Image Processing Library processes the area of intersection between the source and
+destination images (or ROIs), allowing them to vary independently.
+*/
+typedef struct
+#ifdef __cplusplus
+	CV_EXPORTS
+#endif
+		_IplImage {
+	int nSize;					   /**< sizeof(IplImage) */
+	int ID;						   /**< version (=0)*/
+	int nChannels;				   /**< Most of OpenCV functions support 1,2,3 or 4 channels */
+	int alphaChannel;			   /**< Ignored by OpenCV */
+	int depth;					   /**< Pixel depth in bits: IPL_DEPTH_8U, IPL_DEPTH_8S, IPL_DEPTH_16S,
+									  IPL_DEPTH_32S, IPL_DEPTH_32F and IPL_DEPTH_64F are supported.  */
+	char colorModel[4];			   /**< Ignored by OpenCV */
+	char channelSeq[4];			   /**< ditto */
+	int dataOrder;				   /**< 0 - interleaved color channels, 1 - separate color channels.
+									  cvCreateImage can only create interleaved images */
+	int origin;					   /**< 0 - top-left origin,
+									  1 - bottom-left origin (Windows bitmaps style).  */
+	int align;					   /**< Alignment of image rows (4 or 8).
+									  OpenCV ignores it and uses widthStep instead.    */
+	int width;					   /**< Image width in pixels.                           */
+	int height;					   /**< Image height in pixels.                          */
+	struct _IplROI *roi;		   /**< Image ROI. If NULL, the whole image is selected. */
+	struct _IplImage *maskROI;	 /**< Must be NULL. */
+	void *imageId;				   /**< "           " */
+	struct _IplTileInfo *tileInfo; /**< "           " */
+	int imageSize;				   /**< Image data size in bytes
+									  (==image->height*image->widthStep
+									  in case of interleaved data)*/
+	char *imageData;			   /**< Pointer to aligned image data.         */
+	int widthStep;				   /**< Size of aligned image row in bytes.    */
+	int BorderMode[4];			   /**< Ignored by OpenCV.                     */
+	int BorderConst[4];			   /**< Ditto.                                 */
+	char *imageDataOrigin;		   /**< Pointer to very origin of image data
+									  (not necessarily aligned) -
+									  needed for correct deallocation */
+
+#ifdef __cplusplus
+	_IplImage() {}
+	_IplImage(const cv::Mat &m);
+#endif
+} IplImage;
+
+typedef struct _IplTileInfo IplTileInfo;
+
+typedef struct _IplROI {
+	int coi; /**< 0 - no COI (all channels are selected), 1 - 0th channel is selected ...*/
+	int xOffset;
+	int yOffset;
+	int width;
+	int height;
+} IplROI;
+
+typedef struct _IplConvKernel {
+	int nCols;
+	int nRows;
+	int anchorX;
+	int anchorY;
+	int *values;
+	int nShiftR;
+} IplConvKernel;
+
+typedef struct _IplConvKernelFP {
+	int nCols;
+	int nRows;
+	int anchorX;
+	int anchorY;
+	float *values;
+} IplConvKernelFP;
+
+#define IPL_IMAGE_HEADER 1
+#define IPL_IMAGE_DATA 2
+#define IPL_IMAGE_ROI 4
+
+#endif /*HAVE_IPL*/
+
+/** extra border mode */
+#define IPL_BORDER_REFLECT_101 4
+#define IPL_BORDER_TRANSPARENT 5
+
+#define IPL_IMAGE_MAGIC_VAL ((int)sizeof(IplImage))
+#define CV_TYPE_NAME_IMAGE "opencv-image"
+
+#define CV_IS_IMAGE_HDR(img) ((img) != NULL && ((const IplImage *)(img))->nSize == sizeof(IplImage))
+
+#define CV_IS_IMAGE(img) (CV_IS_IMAGE_HDR(img) && ((IplImage *)img)->imageData != NULL)
+
+/** for storing double-precision
+   floating point data in IplImage's */
+#define IPL_DEPTH_64F 64
+
+/** get reference to pixel at (col,row),
+   for multi-channel images (col) should be multiplied by number of channels */
+#define CV_IMAGE_ELEM(image, elemtype, row, col)                                                                       \
+	(((elemtype *)((image)->imageData + (image)->widthStep * (row)))[(col)])
+
+/****************************************************************************************\
+*                                  Matrix type (CvMat)                                   *
+\****************************************************************************************/
+
+#define CV_AUTO_STEP 0x7fffffff
+#define CV_WHOLE_ARR cvSlice(0, 0x3fffffff)
+
+#define CV_MAGIC_MASK 0xFFFF0000
+#define CV_MAT_MAGIC_VAL 0x42420000
+#define CV_TYPE_NAME_MAT "opencv-matrix"
+
+/** Matrix elements are stored row by row. Element (i, j) (i - 0-based row index, j - 0-based column
+index) of a matrix can be retrieved or modified using CV_MAT_ELEM macro:
+
+	uchar pixval = CV_MAT_ELEM(grayimg, uchar, i, j)
+	CV_MAT_ELEM(cameraMatrix, float, 0, 2) = image.width*0.5f;
+
+To access multiple-channel matrices, you can use
+CV_MAT_ELEM(matrix, type, i, j\*nchannels + channel_idx).
+
+@deprecated CvMat is now obsolete; consider using Mat instead.
+ */
+typedef struct CvMat {
+	int type;
+	int step;
+
+	/* for internal use only */
+	int *refcount;
+	int hdr_refcount;
+
+	union {
+		uchar *ptr;
+		short *s;
+		int *i;
+		float *fl;
+		double *db;
+	} data;
+
+#ifdef __cplusplus
+	union {
+		int rows;
+		int height;
+	};
+
+	union {
+		int cols;
+		int width;
+	};
+#else
+	int rows;
+	int cols;
+#endif
+
+#ifdef __cplusplus
+	CvMat() {}
+	CvMat(const CvMat &m) { memcpy(this, &m, sizeof(CvMat)); }
+	CvMat(const cv::Mat &m);
+#endif
+
+} CvMat;
+
+#define CV_IS_MAT_HDR(mat)                                                                                             \
+	((mat) != NULL && (((const CvMat *)(mat))->type & CV_MAGIC_MASK) == CV_MAT_MAGIC_VAL &&                            \
+	 ((const CvMat *)(mat))->cols > 0 && ((const CvMat *)(mat))->rows > 0)
+
+#define CV_IS_MAT_HDR_Z(mat)                                                                                           \
+	((mat) != NULL && (((const CvMat *)(mat))->type & CV_MAGIC_MASK) == CV_MAT_MAGIC_VAL &&                            \
+	 ((const CvMat *)(mat))->cols >= 0 && ((const CvMat *)(mat))->rows >= 0)
+
+#define CV_IS_MAT(mat) (CV_IS_MAT_HDR(mat) && ((const CvMat *)(mat))->data.ptr != NULL)
+
+#define CV_IS_MASK_ARR(mat) (((mat)->type & (CV_MAT_TYPE_MASK & ~CV_8SC1)) == 0)
+
+#define CV_ARE_TYPES_EQ(mat1, mat2) ((((mat1)->type ^ (mat2)->type) & CV_MAT_TYPE_MASK) == 0)
+
+#define CV_ARE_CNS_EQ(mat1, mat2) ((((mat1)->type ^ (mat2)->type) & CV_MAT_CN_MASK) == 0)
+
+#define CV_ARE_DEPTHS_EQ(mat1, mat2) ((((mat1)->type ^ (mat2)->type) & CV_MAT_DEPTH_MASK) == 0)
+
+#define CV_ARE_SIZES_EQ(mat1, mat2) ((mat1)->rows == (mat2)->rows && (mat1)->cols == (mat2)->cols)
+
+#define CV_IS_MAT_CONST(mat) (((mat)->rows | (mat)->cols) == 1)
+
+#define IPL2CV_DEPTH(depth)                                                                                            \
+	((((CV_8U) + (CV_16U << 4) + (CV_32F << 8) + (CV_64F << 16) + (CV_8S << 20) + (CV_16S << 24) + (CV_32S << 28)) >>  \
+	  ((((depth)&0xF0) >> 2) + (((depth)&IPL_DEPTH_SIGN) ? 20 : 0))) &                                                 \
+	 15)
+
+/** Inline constructor. No data is allocated internally!!!
+ * (Use together with cvCreateData, or use cvCreateMat instead to
+ * get a matrix with allocated data):
+ */
+CV_INLINE CvMat cvMat(int rows, int cols, int type, void *data CV_DEFAULT(NULL)) {
+	CvMat m;
+
+	assert((unsigned)CV_MAT_DEPTH(type) <= CV_64F);
+	type = CV_MAT_TYPE(type);
+	m.type = CV_MAT_MAGIC_VAL | CV_MAT_CONT_FLAG | type;
+	m.cols = cols;
+	m.rows = rows;
+	m.step = m.cols * CV_ELEM_SIZE(type);
+	m.data.ptr = (uchar *)data;
+	m.refcount = NULL;
+	m.hdr_refcount = 0;
+
+	return m;
+}
+
+#ifdef __cplusplus
+inline CvMat::CvMat(const cv::Mat &m) {
+	CV_DbgAssert(m.dims <= 2);
+	*this = cvMat(m.rows, m.dims == 1 ? 1 : m.cols, m.type(), m.data);
+	step = (int)m.step[0];
+	type = (type & ~cv::Mat::CONTINUOUS_FLAG) | (m.flags & cv::Mat::CONTINUOUS_FLAG);
+}
+#endif
+
+#define CV_MAT_ELEM_PTR_FAST(mat, row, col, pix_size)                                                                  \
+	(assert((unsigned)(row) < (unsigned)(mat).rows && (unsigned)(col) < (unsigned)(mat).cols),                         \
+	 (mat).data.ptr + (size_t)(mat).step * (row) + (pix_size) * (col))
+
+#define CV_MAT_ELEM_PTR(mat, row, col) CV_MAT_ELEM_PTR_FAST(mat, row, col, CV_ELEM_SIZE((mat).type))
+
+#define CV_MAT_ELEM(mat, elemtype, row, col) (*(elemtype *)CV_MAT_ELEM_PTR_FAST(mat, row, col, sizeof(elemtype)))
+
+/** @brief Returns the particular element of single-channel floating-point matrix.
+
+The function is a fast replacement for cvGetReal2D in the case of single-channel floating-point
+matrices. It is faster because it is inline, it does fewer checks for array type and array element
+type, and it checks for the row and column ranges only in debug mode.
+@param mat Input matrix
+@param row The zero-based index of row
+@param col The zero-based index of column
+ */
+CV_INLINE double cvmGet(const CvMat *mat, int row, int col) {
+	int type;
+
+	type = CV_MAT_TYPE(mat->type);
+	assert((unsigned)row < (unsigned)mat->rows && (unsigned)col < (unsigned)mat->cols);
+
+	if (type == CV_32FC1)
+		return ((float *)(void *)(mat->data.ptr + (size_t)mat->step * row))[col];
+	else {
+		assert(type == CV_64FC1);
+		return ((double *)(void *)(mat->data.ptr + (size_t)mat->step * row))[col];
+	}
+}
+
+/** @brief Sets a specific element of a single-channel floating-point matrix.
+
+The function is a fast replacement for cvSetReal2D in the case of single-channel floating-point
+matrices. It is faster because it is inline, it does fewer checks for array type and array element
+type, and it checks for the row and column ranges only in debug mode.
+@param mat The matrix
+@param row The zero-based index of row
+@param col The zero-based index of column
+@param value The new value of the matrix element
+ */
+CV_INLINE void cvmSet(CvMat *mat, int row, int col, double value) {
+	int type;
+	type = CV_MAT_TYPE(mat->type);
+	assert((unsigned)row < (unsigned)mat->rows && (unsigned)col < (unsigned)mat->cols);
+
+	if (type == CV_32FC1)
+		((float *)(void *)(mat->data.ptr + (size_t)mat->step * row))[col] = (float)value;
+	else {
+		assert(type == CV_64FC1);
+		((double *)(void *)(mat->data.ptr + (size_t)mat->step * row))[col] = value;
+	}
+}
+
+CV_INLINE int cvIplDepth(int type) {
+	int depth = CV_MAT_DEPTH(type);
+	return CV_ELEM_SIZE1(depth) * 8 | (depth == CV_8S || depth == CV_16S || depth == CV_32S ? IPL_DEPTH_SIGN : 0);
+}
+
+/****************************************************************************************\
+*                       Multi-dimensional dense array (CvMatND)                          *
+\****************************************************************************************/
+
+#define CV_MATND_MAGIC_VAL 0x42430000
+#define CV_TYPE_NAME_MATND "opencv-nd-matrix"
+
+#define CV_MAX_DIM 32
+#define CV_MAX_DIM_HEAP 1024
+
+/**
+  @deprecated consider using cv::Mat instead
+  */
+typedef struct
+#ifdef __cplusplus
+	CV_EXPORTS
+#endif
+		CvMatND {
+	int type;
+	int dims;
+
+	int *refcount;
+	int hdr_refcount;
+
+	union {
+		uchar *ptr;
+		float *fl;
+		double *db;
+		int *i;
+		short *s;
+	} data;
+
+	struct {
+		int size;
+		int step;
+	} dim[CV_MAX_DIM];
+
+#ifdef __cplusplus
+	CvMatND() {}
+	CvMatND(const cv::Mat &m);
+#endif
+} CvMatND;
+
+#define CV_IS_MATND_HDR(mat) ((mat) != NULL && (((const CvMatND *)(mat))->type & CV_MAGIC_MASK) == CV_MATND_MAGIC_VAL)
+
+#define CV_IS_MATND(mat) (CV_IS_MATND_HDR(mat) && ((const CvMatND *)(mat))->data.ptr != NULL)
+
+/****************************************************************************************\
+*                      Multi-dimensional sparse array (CvSparseMat)                      *
+\****************************************************************************************/
+
+#define CV_SPARSE_MAT_MAGIC_VAL 0x42440000
+#define CV_TYPE_NAME_SPARSE_MAT "opencv-sparse-matrix"
+
+struct CvSet;
+
+typedef struct
+#ifdef __cplusplus
+	CV_EXPORTS
+#endif
+		CvSparseMat {
+	int type;
+	int dims;
+	int *refcount;
+	int hdr_refcount;
+
+	struct CvSet *heap;
+	void **hashtable;
+	int hashsize;
+	int valoffset;
+	int idxoffset;
+	int size[CV_MAX_DIM];
+
+#ifdef __cplusplus
+	void copyToSparseMat(cv::SparseMat &m) const;
+#endif
+} CvSparseMat;
+
+#ifdef __cplusplus
+CV_EXPORTS CvSparseMat *cvCreateSparseMat(const cv::SparseMat &m);
+#endif
+
+#define CV_IS_SPARSE_MAT_HDR(mat)                                                                                      \
+	((mat) != NULL && (((const CvSparseMat *)(mat))->type & CV_MAGIC_MASK) == CV_SPARSE_MAT_MAGIC_VAL)
+
+#define CV_IS_SPARSE_MAT(mat) CV_IS_SPARSE_MAT_HDR(mat)
+
+/**************** iteration through a sparse array *****************/
+
+typedef struct CvSparseNode {
+	unsigned hashval;
+	struct CvSparseNode *next;
+} CvSparseNode;
+
+typedef struct CvSparseMatIterator {
+	CvSparseMat *mat;
+	CvSparseNode *node;
+	int curidx;
+} CvSparseMatIterator;
+
+#define CV_NODE_VAL(mat, node) ((void *)((uchar *)(node) + (mat)->valoffset))
+#define CV_NODE_IDX(mat, node) ((int *)((uchar *)(node) + (mat)->idxoffset))
+
+/****************************************************************************************\
+*                                         Histogram                                      *
+\****************************************************************************************/
+
+typedef int CvHistType;
+
+#define CV_HIST_MAGIC_VAL 0x42450000
+#define CV_HIST_UNIFORM_FLAG (1 << 10)
+
+/** indicates whether bin ranges are set already or not */
+#define CV_HIST_RANGES_FLAG (1 << 11)
+
+#define CV_HIST_ARRAY 0
+#define CV_HIST_SPARSE 1
+#define CV_HIST_TREE CV_HIST_SPARSE
+
+/** should be used as a parameter only,
+   it turns to CV_HIST_UNIFORM_FLAG of hist->type */
+#define CV_HIST_UNIFORM 1
+
+typedef struct CvHistogram {
+	int type;
+	CvArr *bins;
+	float thresh[CV_MAX_DIM][2]; /**< For uniform histograms.                      */
+	float **thresh2;			 /**< For non-uniform histograms.                  */
+	CvMatND mat;				 /**< Embedded matrix header for array histograms. */
+} CvHistogram;
+
+#define CV_IS_HIST(hist)                                                                                               \
+	((hist) != NULL && (((CvHistogram *)(hist))->type & CV_MAGIC_MASK) == CV_HIST_MAGIC_VAL && (hist)->bins != NULL)
+
+#define CV_IS_UNIFORM_HIST(hist) (((hist)->type & CV_HIST_UNIFORM_FLAG) != 0)
+
+#define CV_IS_SPARSE_HIST(hist) CV_IS_SPARSE_MAT((hist)->bins)
+
+#define CV_HIST_HAS_RANGES(hist) (((hist)->type & CV_HIST_RANGES_FLAG) != 0)
+
+/****************************************************************************************\
+*                      Other supplementary data type definitions                         *
+\****************************************************************************************/
+
+/*************************************** CvRect *****************************************/
+/** @sa Rect_ */
+typedef struct CvRect {
+	int x;
+	int y;
+	int width;
+	int height;
+
+#ifdef __cplusplus
+	CvRect(int _x = 0, int _y = 0, int w = 0, int h = 0) : x(_x), y(_y), width(w), height(h) {}
+	template <typename _Tp>
+	CvRect(const cv::Rect_<_Tp> &r)
+		: x(cv::saturate_cast<int>(r.x)), y(cv::saturate_cast<int>(r.y)), width(cv::saturate_cast<int>(r.width)),
+		  height(cv::saturate_cast<int>(r.height)) {}
+	template <typename _Tp> operator cv::Rect_<_Tp>() const {
+		return cv::Rect_<_Tp>((_Tp)x, (_Tp)y, (_Tp)width, (_Tp)height);
+	}
+#endif
+} CvRect;
+
+/** constructs CvRect structure. */
+CV_INLINE CvRect cvRect(int x, int y, int width, int height) {
+	CvRect r;
+
+	r.x = x;
+	r.y = y;
+	r.width = width;
+	r.height = height;
+
+	return r;
+}
+
+CV_INLINE IplROI cvRectToROI(CvRect rect, int coi) {
+	IplROI roi;
+	roi.xOffset = rect.x;
+	roi.yOffset = rect.y;
+	roi.width = rect.width;
+	roi.height = rect.height;
+	roi.coi = coi;
+
+	return roi;
+}
+
+CV_INLINE CvRect cvROIToRect(IplROI roi) { return cvRect(roi.xOffset, roi.yOffset, roi.width, roi.height); }
+
+/*********************************** CvTermCriteria *************************************/
+
+#define CV_TERMCRIT_ITER 1
+#define CV_TERMCRIT_NUMBER CV_TERMCRIT_ITER
+#define CV_TERMCRIT_EPS 2
+
+/** @sa TermCriteria
+ */
+typedef struct CvTermCriteria {
+	int type; /**< may be combination of
+				 CV_TERMCRIT_ITER
+				 CV_TERMCRIT_EPS */
+	int max_iter;
+	double epsilon;
+
+#ifdef __cplusplus
+	CvTermCriteria(int _type = 0, int _iter = 0, double _eps = 0) : type(_type), max_iter(_iter), epsilon(_eps) {}
+	CvTermCriteria(const cv::TermCriteria &t) : type(t.type), max_iter(t.maxCount), epsilon(t.epsilon) {}
+	operator cv::TermCriteria() const { return cv::TermCriteria(type, max_iter, epsilon); }
+#endif
+
+} CvTermCriteria;
+
+CV_INLINE CvTermCriteria cvTermCriteria(int type, int max_iter, double epsilon) {
+	CvTermCriteria t;
+
+	t.type = type;
+	t.max_iter = max_iter;
+	t.epsilon = (float)epsilon;
+
+	return t;
+}
+
+/******************************* CvPoint and variants ***********************************/
+
+typedef struct CvPoint {
+	int x;
+	int y;
+
+#ifdef __cplusplus
+	CvPoint(int _x = 0, int _y = 0) : x(_x), y(_y) {}
+	template <typename _Tp> CvPoint(const cv::Point_<_Tp> &pt) : x((int)pt.x), y((int)pt.y) {}
+	template <typename _Tp> operator cv::Point_<_Tp>() const {
+		return cv::Point_<_Tp>(cv::saturate_cast<_Tp>(x), cv::saturate_cast<_Tp>(y));
+	}
+#endif
+} CvPoint;
+
+/** constructs CvPoint structure. */
+CV_INLINE CvPoint cvPoint(int x, int y) {
+	CvPoint p;
+
+	p.x = x;
+	p.y = y;
+
+	return p;
+}
+
+typedef struct CvPoint2D32f {
+	float x;
+	float y;
+
+#ifdef __cplusplus
+	CvPoint2D32f(float _x = 0, float _y = 0) : x(_x), y(_y) {}
+	template <typename _Tp> CvPoint2D32f(const cv::Point_<_Tp> &pt) : x((float)pt.x), y((float)pt.y) {}
+	template <typename _Tp> operator cv::Point_<_Tp>() const {
+		return cv::Point_<_Tp>(cv::saturate_cast<_Tp>(x), cv::saturate_cast<_Tp>(y));
+	}
+#endif
+} CvPoint2D32f;
+
+/** constructs CvPoint2D32f structure. */
+CV_INLINE CvPoint2D32f cvPoint2D32f(double x, double y) {
+	CvPoint2D32f p;
+
+	p.x = (float)x;
+	p.y = (float)y;
+
+	return p;
+}
+
+/** converts CvPoint to CvPoint2D32f. */
+CV_INLINE CvPoint2D32f cvPointTo32f(CvPoint point) { return cvPoint2D32f((float)point.x, (float)point.y); }
+
+/** converts CvPoint2D32f to CvPoint. */
+CV_INLINE CvPoint cvPointFrom32f(CvPoint2D32f point) {
+	CvPoint ipt;
+	ipt.x = cvRound(point.x);
+	ipt.y = cvRound(point.y);
+
+	return ipt;
+}
+
+typedef struct CvPoint3D32f {
+	float x;
+	float y;
+	float z;
+
+#ifdef __cplusplus
+	CvPoint3D32f(float _x = 0, float _y = 0, float _z = 0) : x(_x), y(_y), z(_z) {}
+	template <typename _Tp> CvPoint3D32f(const cv::Point3_<_Tp> &pt) : x((float)pt.x), y((float)pt.y), z((float)pt.z) {}
+	template <typename _Tp> operator cv::Point3_<_Tp>() const {
+		return cv::Point3_<_Tp>(cv::saturate_cast<_Tp>(x), cv::saturate_cast<_Tp>(y), cv::saturate_cast<_Tp>(z));
+	}
+#endif
+} CvPoint3D32f;
+
+/** constructs CvPoint3D32f structure. */
+CV_INLINE CvPoint3D32f cvPoint3D32f(double x, double y, double z) {
+	CvPoint3D32f p;
+
+	p.x = (float)x;
+	p.y = (float)y;
+	p.z = (float)z;
+
+	return p;
+}
+
+typedef struct CvPoint2D64f {
+	double x;
+	double y;
+} CvPoint2D64f;
+
+/** constructs CvPoint2D64f structure.*/
+CV_INLINE CvPoint2D64f cvPoint2D64f(double x, double y) {
+	CvPoint2D64f p;
+
+	p.x = x;
+	p.y = y;
+
+	return p;
+}
+
+typedef struct CvPoint3D64f {
+	double x;
+	double y;
+	double z;
+} CvPoint3D64f;
+
+/** constructs CvPoint3D64f structure. */
+CV_INLINE CvPoint3D64f cvPoint3D64f(double x, double y, double z) {
+	CvPoint3D64f p;
+
+	p.x = x;
+	p.y = y;
+	p.z = z;
+
+	return p;
+}
+
+/******************************** CvSize's & CvBox **************************************/
+
+typedef struct CvSize {
+	int width;
+	int height;
+
+#ifdef __cplusplus
+	CvSize(int w = 0, int h = 0) : width(w), height(h) {}
+	template <typename _Tp>
+	CvSize(const cv::Size_<_Tp> &sz)
+		: width(cv::saturate_cast<int>(sz.width)), height(cv::saturate_cast<int>(sz.height)) {}
+	template <typename _Tp> operator cv::Size_<_Tp>() const {
+		return cv::Size_<_Tp>(cv::saturate_cast<_Tp>(width), cv::saturate_cast<_Tp>(height));
+	}
+#endif
+} CvSize;
+
+/** constructs CvSize structure. */
+CV_INLINE CvSize cvSize(int width, int height) {
+	CvSize s;
+
+	s.width = width;
+	s.height = height;
+
+	return s;
+}
+
+typedef struct CvSize2D32f {
+	float width;
+	float height;
+
+#ifdef __cplusplus
+	CvSize2D32f(float w = 0, float h = 0) : width(w), height(h) {}
+	template <typename _Tp>
+	CvSize2D32f(const cv::Size_<_Tp> &sz)
+		: width(cv::saturate_cast<float>(sz.width)), height(cv::saturate_cast<float>(sz.height)) {}
+	template <typename _Tp> operator cv::Size_<_Tp>() const {
+		return cv::Size_<_Tp>(cv::saturate_cast<_Tp>(width), cv::saturate_cast<_Tp>(height));
+	}
+#endif
+} CvSize2D32f;
+
+/** constructs CvSize2D32f structure. */
+CV_INLINE CvSize2D32f cvSize2D32f(double width, double height) {
+	CvSize2D32f s;
+
+	s.width = (float)width;
+	s.height = (float)height;
+
+	return s;
+}
+
+/** @sa RotatedRect
+ */
+typedef struct CvBox2D {
+	CvPoint2D32f center; /**< Center of the box.                          */
+	CvSize2D32f size;	/**< Box width and length.                       */
+	float angle;		 /**< Angle between the horizontal axis           */
+						 /**< and the first side (i.e. length) in degrees */
+
+#ifdef __cplusplus
+	CvBox2D(CvPoint2D32f c = CvPoint2D32f(), CvSize2D32f s = CvSize2D32f(), float a = 0)
+		: center(c), size(s), angle(a) {}
+	CvBox2D(const cv::RotatedRect &rr) : center(rr.center), size(rr.size), angle(rr.angle) {}
+	operator cv::RotatedRect() const { return cv::RotatedRect(center, size, angle); }
+#endif
+} CvBox2D;
+
+/** Line iterator state: */
+typedef struct CvLineIterator {
+	/** Pointer to the current point: */
+	uchar *ptr;
+
+	/* Bresenham algorithm state: */
+	int err;
+	int plus_delta;
+	int minus_delta;
+	int plus_step;
+	int minus_step;
+} CvLineIterator;
+
+/************************************* CvSlice ******************************************/
+#define CV_WHOLE_SEQ_END_INDEX 0x3fffffff
+#define CV_WHOLE_SEQ cvSlice(0, CV_WHOLE_SEQ_END_INDEX)
+
+typedef struct CvSlice {
+	int start_index, end_index;
+
+#if defined(__cplusplus) && !defined(__CUDACC__)
+	CvSlice(int start = 0, int end = 0) : start_index(start), end_index(end) {}
+	CvSlice(const cv::Range &r) {
+		*this = (r.start != INT_MIN && r.end != INT_MAX) ? CvSlice(r.start, r.end) : CvSlice(0, CV_WHOLE_SEQ_END_INDEX);
+	}
+	operator cv::Range() const {
+		return (start_index == 0 && end_index == CV_WHOLE_SEQ_END_INDEX) ? cv::Range::all()
+																		 : cv::Range(start_index, end_index);
+	}
+#endif
+} CvSlice;
+
+CV_INLINE CvSlice cvSlice(int start, int end) {
+	CvSlice slice;
+	slice.start_index = start;
+	slice.end_index = end;
+
+	return slice;
+}
+
+/************************************* CvScalar *****************************************/
+/** @sa Scalar_
+ */
+typedef struct CvScalar {
+	double val[4];
+
+#ifdef __cplusplus
+	CvScalar() {}
+	CvScalar(double d0, double d1 = 0, double d2 = 0, double d3 = 0) {
+		val[0] = d0;
+		val[1] = d1;
+		val[2] = d2;
+		val[3] = d3;
+	}
+	template <typename _Tp> CvScalar(const cv::Scalar_<_Tp> &s) {
+		val[0] = s.val[0];
+		val[1] = s.val[1];
+		val[2] = s.val[2];
+		val[3] = s.val[3];
+	}
+	template <typename _Tp> operator cv::Scalar_<_Tp>() const {
+		return cv::Scalar_<_Tp>(cv::saturate_cast<_Tp>(val[0]), cv::saturate_cast<_Tp>(val[1]),
+								cv::saturate_cast<_Tp>(val[2]), cv::saturate_cast<_Tp>(val[3]));
+	}
+	template <typename _Tp, int cn> CvScalar(const cv::Vec<_Tp, cn> &v) {
+		int i;
+		for (i = 0; i < (cn < 4 ? cn : 4); i++)
+			val[i] = v.val[i];
+		for (; i < 4; i++)
+			val[i] = 0;
+	}
+#endif
+} CvScalar;
+
+CV_INLINE CvScalar cvScalar(double val0, double val1 CV_DEFAULT(0), double val2 CV_DEFAULT(0),
+							double val3 CV_DEFAULT(0)) {
+	CvScalar scalar;
+	scalar.val[0] = val0;
+	scalar.val[1] = val1;
+	scalar.val[2] = val2;
+	scalar.val[3] = val3;
+	return scalar;
+}
+
+CV_INLINE CvScalar cvRealScalar(double val0) {
+	CvScalar scalar;
+	scalar.val[0] = val0;
+	scalar.val[1] = scalar.val[2] = scalar.val[3] = 0;
+	return scalar;
+}
+
+CV_INLINE CvScalar cvScalarAll(double val0123) {
+	CvScalar scalar;
+	scalar.val[0] = val0123;
+	scalar.val[1] = val0123;
+	scalar.val[2] = val0123;
+	scalar.val[3] = val0123;
+	return scalar;
+}
+
+/****************************************************************************************\
+*                                   Dynamic Data structures                              *
+\****************************************************************************************/
+
+/******************************** Memory storage ****************************************/
+
+typedef struct CvMemBlock {
+	struct CvMemBlock *prev;
+	struct CvMemBlock *next;
+} CvMemBlock;
+
+#define CV_STORAGE_MAGIC_VAL 0x42890000
+
+typedef struct CvMemStorage {
+	int signature;
+	CvMemBlock *bottom;			 /**< First allocated block.                   */
+	CvMemBlock *top;			 /**< Current memory block - top of the stack. */
+	struct CvMemStorage *parent; /**< We get new blocks from parent as needed. */
+	int block_size;				 /**< Block size.                              */
+	int free_space;				 /**< Remaining free space in current block.   */
+} CvMemStorage;
+
+#define CV_IS_STORAGE(storage)                                                                                         \
+	((storage) != NULL && (((CvMemStorage *)(storage))->signature & CV_MAGIC_MASK) == CV_STORAGE_MAGIC_VAL)
+
+typedef struct CvMemStoragePos {
+	CvMemBlock *top;
+	int free_space;
+} CvMemStoragePos;
+
+/*********************************** Sequence *******************************************/
+
+typedef struct CvSeqBlock {
+	struct CvSeqBlock *prev; /**< Previous sequence block.                   */
+	struct CvSeqBlock *next; /**< Next sequence block.                       */
+	int start_index;		 /**< Index of the first element in the block +  */
+							 /**< sequence->first->start_index.              */
+	int count;				 /**< Number of elements in the block.           */
+	schar *data;			 /**< Pointer to the first element of the block. */
+} CvSeqBlock;
+
+#define CV_TREE_NODE_FIELDS(node_type)                                                                                 \
+	int flags;				  /**< Miscellaneous flags.     */                                                         \
+	int header_size;		  /**< Size of sequence header. */                                                         \
+	struct node_type *h_prev; /**< Previous sequence.       */                                                         \
+	struct node_type *h_next; /**< Next sequence.           */                                                         \
+	struct node_type *v_prev; /**< 2nd previous sequence.   */                                                         \
+	struct node_type *v_next  /**< 2nd next sequence.       */
+
+/**
+   Read/Write sequence.
+   Elements can be dynamically inserted to or deleted from the sequence.
+*/
+#define CV_SEQUENCE_FIELDS()                                                                                           \
+	CV_TREE_NODE_FIELDS(CvSeq);                                                                                        \
+	int total;				 /**< Total number of elements.            */                                              \
+	int elem_size;			 /**< Size of sequence element in bytes.   */                                              \
+	schar *block_max;		 /**< Maximal bound of the last block.     */                                              \
+	schar *ptr;				 /**< Current write pointer.               */                                              \
+	int delta_elems;		 /**< Grow seq this many at a time.        */                                              \
+	CvMemStorage *storage;   /**< Where the seq is stored.             */                                              \
+	CvSeqBlock *free_blocks; /**< Free blocks list.                    */                                              \
+	CvSeqBlock *first;		 /**< Pointer to the first sequence block. */
+
+typedef struct CvSeq { CV_SEQUENCE_FIELDS() } CvSeq;
+
+#define CV_TYPE_NAME_SEQ "opencv-sequence"
+#define CV_TYPE_NAME_SEQ_TREE "opencv-sequence-tree"
+
+/*************************************** Set ********************************************/
+/** @brief Set
+  Order is not preserved. There can be gaps between sequence elements.
+  After the element has been inserted it stays in the same place all the time.
+  The MSB(most-significant or sign bit) of the first field (flags) is 0 iff the element exists.
+*/
+#define CV_SET_ELEM_FIELDS(elem_type)                                                                                  \
+	int flags;                                                                                                         \
+	struct elem_type *next_free;
+
+typedef struct CvSetElem { CV_SET_ELEM_FIELDS(CvSetElem) } CvSetElem;
+
+#define CV_SET_FIELDS()                                                                                                \
+	CV_SEQUENCE_FIELDS()                                                                                               \
+	CvSetElem *free_elems;                                                                                             \
+	int active_count;
+
+typedef struct CvSet { CV_SET_FIELDS() } CvSet;
+
+#define CV_SET_ELEM_IDX_MASK ((1 << 26) - 1)
+#define CV_SET_ELEM_FREE_FLAG (1 << (sizeof(int) * 8 - 1))
+
+/** Checks whether the element pointed by ptr belongs to a set or not */
+#define CV_IS_SET_ELEM(ptr) (((CvSetElem *)(ptr))->flags >= 0)
+
+/************************************* Graph ********************************************/
+
+/** @name Graph
+
+We represent a graph as a set of vertices. Vertices contain their adjacency lists (more exactly,
+pointers to first incoming or outcoming edge (or 0 if isolated vertex)). Edges are stored in
+another set. There is a singly-linked list of incoming/outcoming edges for each vertex.
+
+Each edge consists of:
+
+- Two pointers to the starting and ending vertices (vtx[0] and vtx[1] respectively).
+
+	A graph may be oriented or not. In the latter case, edges between vertex i to vertex j are not
+distinguished during search operations.
+
+- Two pointers to next edges for the starting and ending vertices, where next[0] points to the
+next edge in the vtx[0] adjacency list and next[1] points to the next edge in the vtx[1]
+adjacency list.
+
+@see CvGraphEdge, CvGraphVtx, CvGraphVtx2D, CvGraph
+@{
+*/
+#define CV_GRAPH_EDGE_FIELDS()                                                                                         \
+	int flags;                                                                                                         \
+	float weight;                                                                                                      \
+	struct CvGraphEdge *next[2];                                                                                       \
+	struct CvGraphVtx *vtx[2];
+
+#define CV_GRAPH_VERTEX_FIELDS()                                                                                       \
+	int flags;                                                                                                         \
+	struct CvGraphEdge *first;
+
+typedef struct CvGraphEdge { CV_GRAPH_EDGE_FIELDS() } CvGraphEdge;
+
+typedef struct CvGraphVtx { CV_GRAPH_VERTEX_FIELDS() } CvGraphVtx;
+
+typedef struct CvGraphVtx2D {
+	CV_GRAPH_VERTEX_FIELDS()
+	CvPoint2D32f *ptr;
+} CvGraphVtx2D;
+
+/**
+   Graph is "derived" from the set (this is set a of vertices)
+   and includes another set (edges)
+*/
+#define CV_GRAPH_FIELDS()                                                                                              \
+	CV_SET_FIELDS()                                                                                                    \
+	CvSet *edges;
+
+typedef struct CvGraph { CV_GRAPH_FIELDS() } CvGraph;
+
+#define CV_TYPE_NAME_GRAPH "opencv-graph"
+
+/** @} */
+
+/*********************************** Chain/Contour *************************************/
+
+typedef struct CvChain {
+	CV_SEQUENCE_FIELDS()
+	CvPoint origin;
+} CvChain;
+
+#define CV_CONTOUR_FIELDS()                                                                                            \
+	CV_SEQUENCE_FIELDS()                                                                                               \
+	CvRect rect;                                                                                                       \
+	int color;                                                                                                         \
+	int reserved[3];
+
+typedef struct CvContour { CV_CONTOUR_FIELDS() } CvContour;
+
+typedef CvContour CvPoint2DSeq;
+
+/****************************************************************************************\
+*                                    Sequence types                                      *
+\****************************************************************************************/
+
+#define CV_SEQ_MAGIC_VAL 0x42990000
+
+#define CV_IS_SEQ(seq) ((seq) != NULL && (((CvSeq *)(seq))->flags & CV_MAGIC_MASK) == CV_SEQ_MAGIC_VAL)
+
+#define CV_SET_MAGIC_VAL 0x42980000
+#define CV_IS_SET(set) ((set) != NULL && (((CvSeq *)(set))->flags & CV_MAGIC_MASK) == CV_SET_MAGIC_VAL)
+
+#define CV_SEQ_ELTYPE_BITS 12
+#define CV_SEQ_ELTYPE_MASK ((1 << CV_SEQ_ELTYPE_BITS) - 1)
+
+#define CV_SEQ_ELTYPE_POINT CV_32SC2 /**< (x,y) */
+#define CV_SEQ_ELTYPE_CODE CV_8UC1   /**< freeman code: 0..7 */
+#define CV_SEQ_ELTYPE_GENERIC 0
+#define CV_SEQ_ELTYPE_PTR CV_USRTYPE1
+#define CV_SEQ_ELTYPE_PPOINT CV_SEQ_ELTYPE_PTR /**< &(x,y) */
+#define CV_SEQ_ELTYPE_INDEX CV_32SC1		   /**< #(x,y) */
+#define CV_SEQ_ELTYPE_GRAPH_EDGE 0			   /**< &next_o, &next_d, &vtx_o, &vtx_d */
+#define CV_SEQ_ELTYPE_GRAPH_VERTEX 0		   /**< first_edge, &(x,y) */
+#define CV_SEQ_ELTYPE_TRIAN_ATR 0			   /**< vertex of the binary tree   */
+#define CV_SEQ_ELTYPE_CONNECTED_COMP 0		   /**< connected component  */
+#define CV_SEQ_ELTYPE_POINT3D CV_32FC3		   /**< (x,y,z)  */
+
+#define CV_SEQ_KIND_BITS 2
+#define CV_SEQ_KIND_MASK (((1 << CV_SEQ_KIND_BITS) - 1) << CV_SEQ_ELTYPE_BITS)
+
+/** types of sequences */
+#define CV_SEQ_KIND_GENERIC (0 << CV_SEQ_ELTYPE_BITS)
+#define CV_SEQ_KIND_CURVE (1 << CV_SEQ_ELTYPE_BITS)
+#define CV_SEQ_KIND_BIN_TREE (2 << CV_SEQ_ELTYPE_BITS)
+
+/** types of sparse sequences (sets) */
+#define CV_SEQ_KIND_GRAPH (1 << CV_SEQ_ELTYPE_BITS)
+#define CV_SEQ_KIND_SUBDIV2D (2 << CV_SEQ_ELTYPE_BITS)
+
+#define CV_SEQ_FLAG_SHIFT (CV_SEQ_KIND_BITS + CV_SEQ_ELTYPE_BITS)
+
+/** flags for curves */
+#define CV_SEQ_FLAG_CLOSED (1 << CV_SEQ_FLAG_SHIFT)
+#define CV_SEQ_FLAG_SIMPLE (0 << CV_SEQ_FLAG_SHIFT)
+#define CV_SEQ_FLAG_CONVEX (0 << CV_SEQ_FLAG_SHIFT)
+#define CV_SEQ_FLAG_HOLE (2 << CV_SEQ_FLAG_SHIFT)
+
+/** flags for graphs */
+#define CV_GRAPH_FLAG_ORIENTED (1 << CV_SEQ_FLAG_SHIFT)
+
+#define CV_GRAPH CV_SEQ_KIND_GRAPH
+#define CV_ORIENTED_GRAPH (CV_SEQ_KIND_GRAPH | CV_GRAPH_FLAG_ORIENTED)
+
+/** point sets */
+#define CV_SEQ_POINT_SET (CV_SEQ_KIND_GENERIC | CV_SEQ_ELTYPE_POINT)
+#define CV_SEQ_POINT3D_SET (CV_SEQ_KIND_GENERIC | CV_SEQ_ELTYPE_POINT3D)
+#define CV_SEQ_POLYLINE (CV_SEQ_KIND_CURVE | CV_SEQ_ELTYPE_POINT)
+#define CV_SEQ_POLYGON (CV_SEQ_FLAG_CLOSED | CV_SEQ_POLYLINE)
+#define CV_SEQ_CONTOUR CV_SEQ_POLYGON
+#define CV_SEQ_SIMPLE_POLYGON (CV_SEQ_FLAG_SIMPLE | CV_SEQ_POLYGON)
+
+/** chain-coded curves */
+#define CV_SEQ_CHAIN (CV_SEQ_KIND_CURVE | CV_SEQ_ELTYPE_CODE)
+#define CV_SEQ_CHAIN_CONTOUR (CV_SEQ_FLAG_CLOSED | CV_SEQ_CHAIN)
+
+/** binary tree for the contour */
+#define CV_SEQ_POLYGON_TREE (CV_SEQ_KIND_BIN_TREE | CV_SEQ_ELTYPE_TRIAN_ATR)
+
+/** sequence of the connected components */
+#define CV_SEQ_CONNECTED_COMP (CV_SEQ_KIND_GENERIC | CV_SEQ_ELTYPE_CONNECTED_COMP)
+
+/** sequence of the integer numbers */
+#define CV_SEQ_INDEX (CV_SEQ_KIND_GENERIC | CV_SEQ_ELTYPE_INDEX)
+
+#define CV_SEQ_ELTYPE(seq) ((seq)->flags & CV_SEQ_ELTYPE_MASK)
+#define CV_SEQ_KIND(seq) ((seq)->flags & CV_SEQ_KIND_MASK)
+
+/** flag checking */
+#define CV_IS_SEQ_INDEX(seq) ((CV_SEQ_ELTYPE(seq) == CV_SEQ_ELTYPE_INDEX) && (CV_SEQ_KIND(seq) == CV_SEQ_KIND_GENERIC))
+
+#define CV_IS_SEQ_CURVE(seq) (CV_SEQ_KIND(seq) == CV_SEQ_KIND_CURVE)
+#define CV_IS_SEQ_CLOSED(seq) (((seq)->flags & CV_SEQ_FLAG_CLOSED) != 0)
+#define CV_IS_SEQ_CONVEX(seq) 0
+#define CV_IS_SEQ_HOLE(seq) (((seq)->flags & CV_SEQ_FLAG_HOLE) != 0)
+#define CV_IS_SEQ_SIMPLE(seq) 1
+
+/** type checking macros */
+#define CV_IS_SEQ_POINT_SET(seq) ((CV_SEQ_ELTYPE(seq) == CV_32SC2 || CV_SEQ_ELTYPE(seq) == CV_32FC2))
+
+#define CV_IS_SEQ_POINT_SUBSET(seq) (CV_IS_SEQ_INDEX(seq) || CV_SEQ_ELTYPE(seq) == CV_SEQ_ELTYPE_PPOINT)
+
+#define CV_IS_SEQ_POLYLINE(seq) (CV_SEQ_KIND(seq) == CV_SEQ_KIND_CURVE && CV_IS_SEQ_POINT_SET(seq))
+
+#define CV_IS_SEQ_POLYGON(seq) (CV_IS_SEQ_POLYLINE(seq) && CV_IS_SEQ_CLOSED(seq))
+
+#define CV_IS_SEQ_CHAIN(seq) (CV_SEQ_KIND(seq) == CV_SEQ_KIND_CURVE && (seq)->elem_size == 1)
+
+#define CV_IS_SEQ_CONTOUR(seq) (CV_IS_SEQ_CLOSED(seq) && (CV_IS_SEQ_POLYLINE(seq) || CV_IS_SEQ_CHAIN(seq)))
+
+#define CV_IS_SEQ_CHAIN_CONTOUR(seq) (CV_IS_SEQ_CHAIN(seq) && CV_IS_SEQ_CLOSED(seq))
+
+#define CV_IS_SEQ_POLYGON_TREE(seq)                                                                                    \
+	(CV_SEQ_ELTYPE(seq) == CV_SEQ_ELTYPE_TRIAN_ATR && CV_SEQ_KIND(seq) == CV_SEQ_KIND_BIN_TREE)
+
+#define CV_IS_GRAPH(seq) (CV_IS_SET(seq) && CV_SEQ_KIND((CvSet *)(seq)) == CV_SEQ_KIND_GRAPH)
+
+#define CV_IS_GRAPH_ORIENTED(seq) (((seq)->flags & CV_GRAPH_FLAG_ORIENTED) != 0)
+
+#define CV_IS_SUBDIV2D(seq) (CV_IS_SET(seq) && CV_SEQ_KIND((CvSet *)(seq)) == CV_SEQ_KIND_SUBDIV2D)
+
+/****************************************************************************************/
+/*                            Sequence writer & reader                                  */
+/****************************************************************************************/
+
+#define CV_SEQ_WRITER_FIELDS()                                                                                         \
+	int header_size;                                                                                                   \
+	CvSeq *seq;		   /**< the sequence written */                                                                    \
+	CvSeqBlock *block; /**< current block */                                                                           \
+	schar *ptr;		   /**< pointer to free space */                                                                   \
+	schar *block_min;  /**< pointer to the beginning of block*/                                                        \
+	schar *block_max;  /**< pointer to the end of block */
+
+typedef struct CvSeqWriter { CV_SEQ_WRITER_FIELDS() } CvSeqWriter;
+
+#define CV_SEQ_READER_FIELDS()                                                                                         \
+	int header_size;                                                                                                   \
+	CvSeq *seq;		   /**< sequence, beign read */                                                                    \
+	CvSeqBlock *block; /**< current block */                                                                           \
+	schar *ptr;		   /**< pointer to element be read next */                                                         \
+	schar *block_min;  /**< pointer to the beginning of block */                                                       \
+	schar *block_max;  /**< pointer to the end of block */                                                             \
+	int delta_index;   /**< = seq->first->start_index   */                                                             \
+	schar *prev_elem;  /**< pointer to previous element */
+
+typedef struct CvSeqReader { CV_SEQ_READER_FIELDS() } CvSeqReader;
+
+/****************************************************************************************/
+/*                                Operations on sequences                               */
+/****************************************************************************************/
+
+#define CV_SEQ_ELEM(seq, elem_type, index)                                                                             \
+	/** assert gives some guarantee that <seq> parameter is valid */                                                   \
+	(assert(sizeof((seq)->first[0]) == sizeof(CvSeqBlock) && (seq)->elem_size == sizeof(elem_type)),                   \
+	 (elem_type *)((seq)->first && (unsigned)index < (unsigned)((seq)->first->count)                                   \
+					   ? (seq)->first->data + (index) * sizeof(elem_type)                                              \
+					   : cvGetSeqElem((CvSeq *)(seq), (index))))
+#define CV_GET_SEQ_ELEM(elem_type, seq, index) CV_SEQ_ELEM((seq), elem_type, (index))
+
+/** Add element to sequence: */
+#define CV_WRITE_SEQ_ELEM_VAR(elem_ptr, writer)                                                                        \
+	{                                                                                                                  \
+		if ((writer).ptr >= (writer).block_max) {                                                                      \
+			cvCreateSeqBlock(&writer);                                                                                 \
+		}                                                                                                              \
+		memcpy((writer).ptr, elem_ptr, (writer).seq->elem_size);                                                       \
+		(writer).ptr += (writer).seq->elem_size;                                                                       \
+	}
+
+#define CV_WRITE_SEQ_ELEM(elem, writer)                                                                                \
+	{                                                                                                                  \
+		assert((writer).seq->elem_size == sizeof(elem));                                                               \
+		if ((writer).ptr >= (writer).block_max) {                                                                      \
+			cvCreateSeqBlock(&writer);                                                                                 \
+		}                                                                                                              \
+		assert((writer).ptr <= (writer).block_max - sizeof(elem));                                                     \
+		memcpy((writer).ptr, &(elem), sizeof(elem));                                                                   \
+		(writer).ptr += sizeof(elem);                                                                                  \
+	}
+
+/** Move reader position forward: */
+#define CV_NEXT_SEQ_ELEM(elem_size, reader)                                                                            \
+	{                                                                                                                  \
+		if (((reader).ptr += (elem_size)) >= (reader).block_max) {                                                     \
+			cvChangeSeqBlock(&(reader), 1);                                                                            \
+		}                                                                                                              \
+	}
+
+/** Move reader position backward: */
+#define CV_PREV_SEQ_ELEM(elem_size, reader)                                                                            \
+	{                                                                                                                  \
+		if (((reader).ptr -= (elem_size)) < (reader).block_min) {                                                      \
+			cvChangeSeqBlock(&(reader), -1);                                                                           \
+		}                                                                                                              \
+	}
+
+/** Read element and move read position forward: */
+#define CV_READ_SEQ_ELEM(elem, reader)                                                                                 \
+	{                                                                                                                  \
+		assert((reader).seq->elem_size == sizeof(elem));                                                               \
+		memcpy(&(elem), (reader).ptr, sizeof((elem)));                                                                 \
+		CV_NEXT_SEQ_ELEM(sizeof(elem), reader)                                                                         \
+	}
+
+/** Read element and move read position backward: */
+#define CV_REV_READ_SEQ_ELEM(elem, reader)                                                                             \
+	{                                                                                                                  \
+		assert((reader).seq->elem_size == sizeof(elem));                                                               \
+		memcpy(&(elem), (reader).ptr, sizeof((elem)));                                                                 \
+		CV_PREV_SEQ_ELEM(sizeof(elem), reader)                                                                         \
+	}
+
+#define CV_READ_CHAIN_POINT(_pt, reader)                                                                               \
+	{                                                                                                                  \
+		(_pt) = (reader).pt;                                                                                           \
+		if ((reader).ptr) {                                                                                            \
+			CV_READ_SEQ_ELEM((reader).code, (reader));                                                                 \
+			assert(((reader).code & ~7) == 0);                                                                         \
+			(reader).pt.x += (reader).deltas[(int)(reader).code][0];                                                   \
+			(reader).pt.y += (reader).deltas[(int)(reader).code][1];                                                   \
+		}                                                                                                              \
+	}
+
+#define CV_CURRENT_POINT(reader) (*((CvPoint *)((reader).ptr)))
+#define CV_PREV_POINT(reader) (*((CvPoint *)((reader).prev_elem)))
+
+#define CV_READ_EDGE(pt1, pt2, reader)                                                                                 \
+	{                                                                                                                  \
+		assert(sizeof(pt1) == sizeof(CvPoint) && sizeof(pt2) == sizeof(CvPoint) &&                                     \
+			   reader.seq->elem_size == sizeof(CvPoint));                                                              \
+		(pt1) = CV_PREV_POINT(reader);                                                                                 \
+		(pt2) = CV_CURRENT_POINT(reader);                                                                              \
+		(reader).prev_elem = (reader).ptr;                                                                             \
+		CV_NEXT_SEQ_ELEM(sizeof(CvPoint), (reader));                                                                   \
+	}
+
+/************ Graph macros ************/
+
+/** Return next graph edge for given vertex: */
+#define CV_NEXT_GRAPH_EDGE(edge, vertex)                                                                               \
+	(assert((edge)->vtx[0] == (vertex) || (edge)->vtx[1] == (vertex)), (edge)->next[(edge)->vtx[1] == (vertex)])
+
+/****************************************************************************************\
+*             Data structures for persistence (a.k.a serialization) functionality        *
+\****************************************************************************************/
+
+/** "black box" file storage */
+typedef struct CvFileStorage CvFileStorage;
+
+/** Storage flags: */
+#define CV_STORAGE_READ 0
+#define CV_STORAGE_WRITE 1
+#define CV_STORAGE_WRITE_TEXT CV_STORAGE_WRITE
+#define CV_STORAGE_WRITE_BINARY CV_STORAGE_WRITE
+#define CV_STORAGE_APPEND 2
+#define CV_STORAGE_MEMORY 4
+#define CV_STORAGE_FORMAT_MASK (7 << 3)
+#define CV_STORAGE_FORMAT_AUTO 0
+#define CV_STORAGE_FORMAT_XML 8
+#define CV_STORAGE_FORMAT_YAML 16
+#define CV_STORAGE_FORMAT_JSON 24
+#define CV_STORAGE_BASE64 64
+#define CV_STORAGE_WRITE_BASE64 (CV_STORAGE_BASE64 | CV_STORAGE_WRITE)
+
+/** @brief List of attributes. :
+
+In the current implementation, attributes are used to pass extra parameters when writing user
+objects (see cvWrite). XML attributes inside tags are not supported, aside from the object type
+specification (type_id attribute).
+@see cvAttrList, cvAttrValue
+ */
+typedef struct CvAttrList {
+	const char **attr;		 /**< NULL-terminated array of (attribute_name,attribute_value) pairs. */
+	struct CvAttrList *next; /**< Pointer to next chunk of the attributes list.                    */
+} CvAttrList;
+
+/** initializes CvAttrList structure */
+CV_INLINE CvAttrList cvAttrList(const char **attr CV_DEFAULT(NULL), CvAttrList *next CV_DEFAULT(NULL)) {
+	CvAttrList l;
+	l.attr = attr;
+	l.next = next;
+
+	return l;
+}
+
+struct CvTypeInfo;
+
+#define CV_NODE_NONE 0
+#define CV_NODE_INT 1
+#define CV_NODE_INTEGER CV_NODE_INT
+#define CV_NODE_REAL 2
+#define CV_NODE_FLOAT CV_NODE_REAL
+#define CV_NODE_STR 3
+#define CV_NODE_STRING CV_NODE_STR
+#define CV_NODE_REF 4 /**< not used */
+#define CV_NODE_SEQ 5
+#define CV_NODE_MAP 6
+#define CV_NODE_TYPE_MASK 7
+
+#define CV_NODE_TYPE(flags) ((flags)&CV_NODE_TYPE_MASK)
+
+/** file node flags */
+#define CV_NODE_FLOW 8 /**<Used only for writing structures in YAML format. */
+#define CV_NODE_USER 16
+#define CV_NODE_EMPTY 32
+#define CV_NODE_NAMED 64
+
+#define CV_NODE_IS_INT(flags) (CV_NODE_TYPE(flags) == CV_NODE_INT)
+#define CV_NODE_IS_REAL(flags) (CV_NODE_TYPE(flags) == CV_NODE_REAL)
+#define CV_NODE_IS_STRING(flags) (CV_NODE_TYPE(flags) == CV_NODE_STRING)
+#define CV_NODE_IS_SEQ(flags) (CV_NODE_TYPE(flags) == CV_NODE_SEQ)
+#define CV_NODE_IS_MAP(flags) (CV_NODE_TYPE(flags) == CV_NODE_MAP)
+#define CV_NODE_IS_COLLECTION(flags) (CV_NODE_TYPE(flags) >= CV_NODE_SEQ)
+#define CV_NODE_IS_FLOW(flags) (((flags)&CV_NODE_FLOW) != 0)
+#define CV_NODE_IS_EMPTY(flags) (((flags)&CV_NODE_EMPTY) != 0)
+#define CV_NODE_IS_USER(flags) (((flags)&CV_NODE_USER) != 0)
+#define CV_NODE_HAS_NAME(flags) (((flags)&CV_NODE_NAMED) != 0)
+
+#define CV_NODE_SEQ_SIMPLE 256
+#define CV_NODE_SEQ_IS_SIMPLE(seq) (((seq)->flags & CV_NODE_SEQ_SIMPLE) != 0)
+
+typedef struct CvString {
+	int len;
+	char *ptr;
+} CvString;
+
+/** All the keys (names) of elements in the readed file storage
+   are stored in the hash to speed up the lookup operations: */
+typedef struct CvStringHashNode {
+	unsigned hashval;
+	CvString str;
+	struct CvStringHashNode *next;
+} CvStringHashNode;
+
+typedef struct CvGenericHash CvFileNodeHash;
+
+/** Basic element of the file storage - scalar or collection: */
+typedef struct CvFileNode {
+	int tag;
+	struct CvTypeInfo *info; /**< type information
+			(only for user-defined object, for others it is 0) */
+	union {
+		double f;			 /**< scalar floating-point number */
+		int i;				 /**< scalar integer number */
+		CvString str;		 /**< text string */
+		CvSeq *seq;			 /**< sequence (ordered collection of file nodes) */
+		CvFileNodeHash *map; /**< map (collection of named file nodes) */
+	} data;
+} CvFileNode;
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+typedef int(CV_CDECL *CvIsInstanceFunc)(const void *struct_ptr);
+typedef void(CV_CDECL *CvReleaseFunc)(void **struct_dblptr);
+typedef void *(CV_CDECL *CvReadFunc)(CvFileStorage *storage, CvFileNode *node);
+typedef void(CV_CDECL *CvWriteFunc)(CvFileStorage *storage, const char *name, const void *struct_ptr,
+									CvAttrList attributes);
+typedef void *(CV_CDECL *CvCloneFunc)(const void *struct_ptr);
+#ifdef __cplusplus
+}
+#endif
+
+/** @brief Type information
+
+The structure contains information about one of the standard or user-defined types. Instances of the
+type may or may not contain a pointer to the corresponding CvTypeInfo structure. In any case, there
+is a way to find the type info structure for a given object using the cvTypeOf function.
+Alternatively, type info can be found by type name using cvFindType, which is used when an object
+is read from file storage. The user can register a new type with cvRegisterType that adds the type
+information structure into the beginning of the type list. Thus, it is possible to create
+specialized types from generic standard types and override the basic methods.
+ */
+typedef struct CvTypeInfo {
+	int flags;					  /**< not used */
+	int header_size;			  /**< sizeof(CvTypeInfo) */
+	struct CvTypeInfo *prev;	  /**< previous registered type in the list */
+	struct CvTypeInfo *next;	  /**< next registered type in the list */
+	const char *type_name;		  /**< type name, written to file storage */
+	CvIsInstanceFunc is_instance; /**< checks if the passed object belongs to the type */
+	CvReleaseFunc release;		  /**< releases object (memory etc.) */
+	CvReadFunc read;			  /**< reads object from file storage */
+	CvWriteFunc write;			  /**< writes object to file storage */
+	CvCloneFunc clone;			  /**< creates a copy of the object */
+} CvTypeInfo;
+
+/**** System data types ******/
+
+typedef struct CvPluginFuncInfo {
+	void **func_addr;
+	void *default_func_addr;
+	const char *func_names;
+	int search_modules;
+	int loaded_from;
+} CvPluginFuncInfo;
+
+typedef struct CvModuleInfo {
+	struct CvModuleInfo *next;
+	const char *name;
+	const char *version;
+	CvPluginFuncInfo *func_tab;
+} CvModuleInfo;
+
+/** @} */
+
+#endif /*OPENCV_CORE_TYPES_H*/
+
+/* End of file. */
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;
+}