From 38dace073c7bb4e793749e6dc512a5bda53030b5 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 14 Mar 2018 00:03:51 -0600 Subject: Allowed overriding of CC in makefile --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index c2aba18..1ab32d2 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,6 @@ all : lib data_recorder test calibrate calibrate_client simple_pose_test -CC:=gcc +CC?=gcc CFLAGS:=-Iinclude/libsurvive -fPIC -g -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -- cgit v1.2.3 From f7420a8a77e1de0480ffbc4d725869dbe28147e9 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Wed, 14 Mar 2018 00:47:41 -0600 Subject: Added default action for pose process --- src/survive_process.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/survive_process.c b/src/survive_process.c index e45a979..fe6dc45 100644 --- a/src/survive_process.c +++ b/src/survive_process.c @@ -109,7 +109,8 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8 void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { // print the pose; //printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); - + so->OutPose = *pose; + so->FromLHPose[lighthouse] = *pose; } void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose) { -- cgit v1.2.3 From 3f8346dcc4b38116ea15543b62a1e859b6a47e85 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 15 Mar 2018 10:36:09 -0600 Subject: Added first draft of epnp code --- Makefile | 22 +- src/epnp/epnp.c | 772 +++++++++++++++++++++ src/epnp/epnp.h | 60 ++ src/epnp/opencv_shim.c | 421 ++++++++++++ src/epnp/opencv_shim.h | 29 + src/epnp/shim_types_c.h | 1633 ++++++++++++++++++++++++++++++++++++++++++++ src/epnp/test_epnp.c | 128 ++++ src/epnp/test_minimal_cv.c | 142 ++++ 8 files changed, 3204 insertions(+), 3 deletions(-) create mode 100644 src/epnp/epnp.c create mode 100644 src/epnp/epnp.h create mode 100644 src/epnp/opencv_shim.c create mode 100644 src/epnp/opencv_shim.h create mode 100644 src/epnp/shim_types_c.h create mode 100644 src/epnp/test_epnp.c create mode 100644 src/epnp/test_minimal_cv.c diff --git a/Makefile b/Makefile index 1ab32d2..c61ce8e 100644 --- a/Makefile +++ b/Makefile @@ -2,8 +2,7 @@ all : lib data_recorder test calibrate calibrate_client simple_pose_test CC?=gcc - -CFLAGS:=-Iinclude/libsurvive -fPIC -g -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic +CFLAGS:=-Iinclude/libsurvive -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -fsanitize=address -fsanitize=undefined -llapacke -lcblas -lm #LDFLAGS:=-L/usr/local/lib -lpthread -lusb-1.0 -lz -lm -flto -g LDFLAGS:=-L/usr/local/lib -lpthread -lz -lm -flto -g @@ -37,7 +36,7 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic. ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o +LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/poser_imu.o src/survive_imu.o src/epnp/epnp.c src/epnp/opencv_shim.c #If you want to use HIDAPI on Linux. @@ -84,6 +83,23 @@ calibrate_client : calibrate_client.c ./lib/libsurvive.so redist/os_generic.c $ static_calibrate : calibrate.c redist/os_generic.c $(DRAWFUNCTIONS) $(LIBSURVIVE_C) tcc -o $@ $^ $(CFLAGS) $(LDFLAGS) -DTCC +test_minimal_cv: ./src/epnp/test_minimal_cv.c ./lib/libsurvive.so + $(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS) + +test_epnp: ./src/epnp/test_epnp.c ./lib/libsurvive.so + $(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS) + +test_epnp_ocv: ./src/epnp/test_epnp.c ./src/epnp/epnp.c + $(CC) -o $@ $^ -DWITH_OPENCV -lpthread -lz -lm -flto -g -lX11 -lusb-1.0 -Iinclude/libsurvive -fPIC -g -O4 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -fsanitize=address -fsanitize=undefined -llapack -lm -lopencv_core + + +unit_test_epnp: ./src/epnp/unit_test_epnp.c ./lib/libsurvive.so + $(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS) + +test_epnp_results: test_epnp test_epnp_ocv + -./test_epnp_ocv > opencv_output + -./test_epnp > our_output + lib: mkdir lib 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 +#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 +#include + +#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 +#else +#include +#endif +#endif +#elif defined __IPL_H__ +#define HAVE_IPL +#endif + +#include "opencv2/core/cvdef.h" + +#ifndef SKIP_INCLUDES +#include +#include +#include +#include +#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 + CvRect(const cv::Rect_<_Tp> &r) + : x(cv::saturate_cast(r.x)), y(cv::saturate_cast(r.y)), width(cv::saturate_cast(r.width)), + height(cv::saturate_cast(r.height)) {} + template 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 CvPoint(const cv::Point_<_Tp> &pt) : x((int)pt.x), y((int)pt.y) {} + template 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 CvPoint2D32f(const cv::Point_<_Tp> &pt) : x((float)pt.x), y((float)pt.y) {} + template 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 CvPoint3D32f(const cv::Point3_<_Tp> &pt) : x((float)pt.x), y((float)pt.y), z((float)pt.z) {} + template 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 + CvSize(const cv::Size_<_Tp> &sz) + : width(cv::saturate_cast(sz.width)), height(cv::saturate_cast(sz.height)) {} + template 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 + CvSize2D32f(const cv::Size_<_Tp> &sz) + : width(cv::saturate_cast(sz.width)), height(cv::saturate_cast(sz.height)) {} + template 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 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 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 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 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 /**= 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; +} -- cgit v1.2.3 From 2e980136715acd4692dd61be4442dc823219764d Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 15 Mar 2018 14:43:58 -0600 Subject: Results out of epnp --- Makefile | 2 +- data_recorder.c | 20 ++++++-- redist/json_helpers.c | 5 +- redist/linmath.c | 31 ++++++++++++ redist/linmath.h | 1 + src/epnp/opencv_shim.h | 2 + src/poser_epnp.c | 130 +++++++++++++++++++++++++++++++++++++++++++++++++ src/survive.c | 4 +- 8 files changed, 186 insertions(+), 9 deletions(-) create mode 100644 src/poser_epnp.c diff --git a/Makefile b/Makefile index c61ce8e..b619970 100644 --- a/Makefile +++ b/Makefile @@ -31,7 +31,7 @@ GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGXDriver.o endif -POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o src/poser_octavioradii.o src/poser_turveytori.o +POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o src/poser_octavioradii.o src/poser_turveytori.o src/poser_epnp.o REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic.o ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c diff --git a/data_recorder.c b/data_recorder.c index 8116946..fbc9bdc 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -62,6 +62,16 @@ void write_to_output(const char *format, ...) { va_end(args); } +void my_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose) { + survive_default_lighthouse_pose_process(ctx, lighthouse, pose); + write_to_output("LH_POSE %d %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", lighthouse, pose->Pos[0], pose->Pos[1], + pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); +} +void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { + survive_default_raw_pose_process(so, lighthouse, pose); + write_to_output("POSE %s %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", so->codename, pose->Pos[0], pose->Pos[1], + pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); +} void my_light_process(struct SurviveObject *so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, @@ -69,13 +79,14 @@ void my_light_process(struct SurviveObject *so, int sensor_id, int acode, survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lh); - if (acode == -1) { + if (acode == -1 || sensor_id < 0) { write_to_output("A %s %d %d %d %u %u %u\n", so->codename, sensor_id, acode, timeinsweep, timecode, length, lh); return; } int jumpoffset = sensor_id; + if (strcmp(so->codename, "WM0") == 0) jumpoffset += 32; else if (strcmp(so->codename, "WM1") == 0) @@ -170,7 +181,8 @@ void *SurviveThread(void *junk) { survive_install_light_fn(ctx, my_light_process); survive_install_imu_fn(ctx, my_imu_process); - + survive_install_lighthouse_pose_fn(ctx, my_lighthouse_process); + survive_cal_install(ctx); if (!ctx) { fprintf(stderr, "Fatal. Could not start\n"); exit(1); @@ -194,7 +206,8 @@ int main(int argc, char **argv) { } else { output_file = stdout; } - + SurviveThread(0); + /* // Create the libsurvive thread OGCreateThread(SurviveThread, 0); @@ -205,4 +218,5 @@ int main(int argc, char **argv) { // Run the Gui in the main thread GuiThread(0); + */ } diff --git a/redist/json_helpers.c b/redist/json_helpers.c index 109708a..4aeb399 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -185,9 +185,6 @@ void json_load_file(const char* path) { char* tag = substr(JSON_STRING, tag_t->start, tag_t->end, JSON_STRING_LEN); char* value = substr(JSON_STRING, value_t->start, value_t->end, JSON_STRING_LEN); - printf("%d %d c:%d %d %s \n", tag_t->start, tag_t->end, tag_t->size, tag_t->type, tag); - - if (value_t->type == JSMN_ARRAY) { i += json_load_array(JSON_STRING, tokens+i+2,value_t->size, tag); //look at array children } else if (value_t->type == JSMN_OBJECT) { @@ -244,4 +241,4 @@ int parse_float_array(char* str, jsmntok_t* token, FLT** f, uint8_t count) { return count; -} \ No newline at end of file +} diff --git a/redist/linmath.c b/redist/linmath.c index 76a723d..c302f5b 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -300,6 +300,37 @@ void quattomatrix(FLT * matrix44, const FLT * qin) matrix44[15] = 1; } +void quatfrommatrix33(FLT *q, const FLT *m) { + FLT m00 = m[0], m01 = m[1], m02 = m[2], m10 = m[3], m11 = m[4], m12 = m[5], m20 = m[6], m21 = m[7], m22 = m[8]; + + FLT tr = m00 + m11 + m22; + + if (tr > 0) { + FLT S = sqrt(tr + 1.0) * 2; // S=4*qw + q[0] = 0.25 * S; + q[1] = (m21 - m12) / S; + q[2] = (m02 - m20) / S; + q[3] = (m10 - m01) / S; + } else if ((m00 > m11) & (m00 > m22)) { + FLT S = sqrt(1.0 + m00 - m11 - m22) * 2; // S=4*q[1] + q[0] = (m21 - m12) / S; + q[1] = 0.25 * S; + q[2] = (m01 + m10) / S; + q[3] = (m02 + m20) / S; + } else if (m11 > m22) { + FLT S = sqrt(1.0 + m11 - m00 - m22) * 2; // S=4*q[2] + q[0] = (m02 - m20) / S; + q[1] = (m01 + m10) / S; + q[2] = 0.25 * S; + q[3] = (m12 + m21) / S; + } else { + FLT S = sqrt(1.0 + m22 - m00 - m11) * 2; // S=4*q[3] + q[0] = (m10 - m01) / S; + q[1] = (m02 + m20) / S; + q[2] = (m12 + m21) / S; + q[3] = 0.25 * S; + } +} void quatfrommatrix( FLT * q, const FLT * matrix44 ) { diff --git a/redist/linmath.h b/redist/linmath.h index ff00c94..57b98d3 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -87,6 +87,7 @@ FLT quatinvsqmagnitude( const FLT * q ); void quatnormalize( FLT * qout, const FLT * qin ); //Safe for in to be same as out. void quattomatrix( FLT * matrix44, const FLT * q ); void quatfrommatrix( FLT * q, const FLT * matrix44 ); +void quatfrommatrix33(FLT *q, const FLT *matrix33); void quatgetconjugate( FLT * qout, const FLT * qin ); void quatgetreciprocal( FLT * qout, const FLT * qin ); void quatsub( FLT * qout, const FLT * a, const FLT * b ); diff --git a/src/epnp/opencv_shim.h b/src/epnp/opencv_shim.h index 5806971..5701f1d 100644 --- a/src/epnp/opencv_shim.h +++ b/src/epnp/opencv_shim.h @@ -15,6 +15,8 @@ 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); +void cvTranspose(const CvMat *M, CvMat *dst); +void print_mat(const CvMat *M); #define CV_SVD 1 #define CV_SVD_MODIFY_A 1 diff --git a/src/poser_epnp.c b/src/poser_epnp.c new file mode 100644 index 0000000..4c3c8b7 --- /dev/null +++ b/src/poser_epnp.c @@ -0,0 +1,130 @@ +#include "PersistentScene.h" + +#ifndef USE_DOUBLE +#define FLT double +#define USE_DOUBLE +#endif + +#include +#include + +#include "epnp/epnp.h" +#include "linmath.h" +#include "math.h" +#include "stdio.h" + +static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool cameraToWorld) { + SurvivePose rtn = {}; + // std::cerr << "Solving for " << cal_imagePoints.size() << " correspondents" << std::endl; + if (pnp->number_of_correspondences <= 4) { + SurviveContext *ctx = so->ctx; + SV_INFO("Can't solve for only %u points\n", pnp->number_of_correspondences); + return rtn; + } + + double r[3][3]; + + double err = epnp_compute_pose(pnp, r, rtn.Pos); + + CvMat R = cvMat(3, 3, CV_64F, r); + CvMat T = cvMat(3, 1, CV_64F, rtn.Pos); + // Requested output is camera -> world, so invert + if (cameraToWorld) { + FLT tmp[3]; + CvMat Tmp = cvMat(3, 1, CV_64F, tmp); + cvCopyTo(&T, &Tmp); + + // Flip the Rotation matrix + cvTranspose(&R, &R); + // Then 'tvec = -R * tvec' + cvGEMM(&R, &Tmp, -1, 0, 0, &T, 0); + print_mat(&R); + print_mat(&T); + } + + FLT tmp[4]; + quatfrommatrix33(tmp, r[0]); + + // Typical camera applications have Z facing forward; the vive is contrarian and has Z going out of the + // back of the lighthouse. Think of this as a rotation on the Y axis a full 180 degrees -- the quat for that is + // [0 0x 1y 0z] + const FLT rt[4] = {0, 0, 1, 0}; + quatrotateabout(rtn.Rot, tmp, rt); + if (!cameraToWorld) { + // We have to pre-multiply the rt transform here, which means we have to also offset our position by + quatrotateabout(rtn.Rot, rt, tmp); + rtn.Pos[0] = -rtn.Pos[0]; + rtn.Pos[2] = -rtn.Pos[2]; + } + + return rtn; +} + +static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) { + + for (int lh = 0; lh < 2; lh++) { + epnp pnp = {.fu = 1, .fv = 1}; + epnp_set_maximum_number_of_correspondences(&pnp, so->nr_locations); + + for (size_t i = 0; i < so->nr_locations; i++) { + FLT *lengths = pdfs->lengths[i][lh]; + FLT *ang = pdfs->angles[i][lh]; + if (lengths[0] < 0 || lengths[1] < 0) + continue; + + epnp_add_correspondence(&pnp, so->sensor_locations[i * 3 + 0], so->sensor_locations[i * 3 + 1], + so->sensor_locations[i * 3 + 2], tan(ang[0]), tan(ang[1])); + } + + SurviveContext *ctx = so->ctx; + SV_INFO("Solving for %d correspondents", pnp.number_of_correspondences); + if (pnp.number_of_correspondences <= 4) { + SV_INFO("Can't solve for only %d points on lh %d\n", pnp.number_of_correspondences, lh); + continue; + } + + SurvivePose lighthouse = solve_correspondence(so, &pnp, true); + PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &lighthouse); + } + return 0; +} + +int PoserEPNP(SurviveObject *so, PoserData *pd) { + switch (pd->pt) { + case POSERDATA_IMU: { + // Really should use this... + PoserDataIMU *imuData = (PoserDataIMU *)pd; + return 0; + } + case POSERDATA_LIGHT: { + /* + PersistentScene* scene; + PoserDataLight * lightData = pd; + + PersistentScene_add(scene, so, lightData); + + if (so->ctx->bsd[lh].PositionSet) { + auto pose = solve_correspondence(so, cal_objectPoints, cal_imagePoints, false); + + SurvivePose txPose = {}; + quatrotatevector(txPose.Pos, so->ctx->bsd[lh].Pose.Rot, pose.Pos); + for (int i = 0; i < 3; i++) { + txPose.Pos[i] += so->ctx->bsd[lh].Pose.Pos[i]; + } + quatrotateabout(txPose.Rot, so->ctx->bsd[lh].Pose.Rot, pose.Rot); + + // scene->integratePose(txPose, lightData->timecode); + // txPose = scene->currentPose; + PoserData_poser_raw_pose_func(pd, so, lh, &txPose); + } + */ + return -1; + } + case POSERDATA_FULL_SCENE: { + return opencv_solver_fullscene(so, (PoserDataFullScene *)(pd)); + } + } + return -1; +} + +REGISTER_LINKTIME(PoserEPNP); diff --git a/src/survive.c b/src/survive.c index 61bfc86..e09ae13 100755 --- a/src/survive.c +++ b/src/survive.c @@ -297,7 +297,9 @@ int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int d int i; for( i = 0; i < oldct; i++ ) { - ctx->drivermagics[i]( ctx, ctx->drivers[i], magic_code, data, datalen ); + if (ctx->drivermagics[i]) { + ctx->drivermagics[i](ctx, ctx->drivers[i], magic_code, data, datalen); + } } return 0; } -- cgit v1.2.3 From 1a43149022d6c9fa156e2c929fe9f16021269a09 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 15 Mar 2018 14:48:52 -0600 Subject: Added some tmp file stings to gitignore --- .gitignore | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/.gitignore b/.gitignore index 4c22fa3..99adf7f 100644 --- a/.gitignore +++ b/.gitignore @@ -18,3 +18,10 @@ winbuild/.vs/libsurvive/v15/.suo *.tlog winbuild/calibrate.exe winbuild/calibrate.def +*~ +*/#*# +*.so +calinfo +calibrate_client +calibrate +test \ No newline at end of file -- cgit v1.2.3 From 49bade6bbcb351900f627f066b14a6826f6d7984 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 15 Mar 2018 15:59:08 -0600 Subject: Removed printfs, fixed a segfault --- Makefile | 2 +- data_recorder.c | 3 ++- src/epnp/epnp.c | 32 +----------------------- src/epnp/opencv_shim.c | 2 +- src/poser_epnp.c | 67 +++++++++++++++++++++++++++++++++----------------- test.c | 1 + 6 files changed, 50 insertions(+), 57 deletions(-) diff --git a/Makefile b/Makefile index b619970..2eb4859 100644 --- a/Makefile +++ b/Makefile @@ -36,7 +36,7 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic. ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/poser_imu.o src/survive_imu.o src/epnp/epnp.c src/epnp/opencv_shim.c +LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/poser_imu.o src/survive_imu.o src/epnp/epnp.c src/epnp/opencv_shim.c src/persistent_scene.o #If you want to use HIDAPI on Linux. diff --git a/data_recorder.c b/data_recorder.c index fbc9bdc..e835c73 100644 --- a/data_recorder.c +++ b/data_recorder.c @@ -67,7 +67,7 @@ void my_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose write_to_output("LH_POSE %d %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", lighthouse, pose->Pos[0], pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); } -void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { +void my_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) { survive_default_raw_pose_process(so, lighthouse, pose); write_to_output("POSE %s %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", so->codename, pose->Pos[0], pose->Pos[1], pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]); @@ -182,6 +182,7 @@ void *SurviveThread(void *junk) { survive_install_light_fn(ctx, my_light_process); survive_install_imu_fn(ctx, my_imu_process); survive_install_lighthouse_pose_fn(ctx, my_lighthouse_process); + survive_install_raw_pose_fn(ctx, my_raw_pose_process); survive_cal_install(ctx); if (!ctx) { fprintf(stderr, "Fatal. Could not start\n"); diff --git a/src/epnp/epnp.c b/src/epnp/epnp.c index 48b7d8a..4b888aa 100644 --- a/src/epnp/epnp.c +++ b/src/epnp/epnp.c @@ -79,12 +79,7 @@ void epnp_compute_rho(epnp *self, double *rho) { 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) { @@ -149,13 +144,10 @@ void epnp_choose_control_points(epnp *self) { 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++) { @@ -174,19 +166,8 @@ void epnp_compute_barycentric_coordinates(epnp *self) { 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; @@ -289,10 +270,7 @@ void find_betas_approx_1(const CvMat *L_6x10, const CvMat *Rho, double *betas) { 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); @@ -512,8 +490,6 @@ double epnp_compute_pose(epnp *self, double R[3][3], double t[3]) { 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); @@ -552,9 +528,6 @@ double epnp_compute_pose(epnp *self, double R[3][3], double t[3]) { 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); @@ -570,17 +543,14 @@ double epnp_compute_pose(epnp *self, double R[3][3], double t[3]) { 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]) diff --git a/src/epnp/opencv_shim.c b/src/epnp/opencv_shim.c index 4680b1f..d7886d0 100644 --- a/src/epnp/opencv_shim.c +++ b/src/epnp/opencv_shim.c @@ -388,7 +388,7 @@ void cvSVD(CvMat *aarr, CvMat *warr, CvMat *uarr, CvMat *varr, int flags) { lapack_int ulda = uarr ? uarr->cols : 1; lapack_int plda = varr ? varr->cols : acols; - double *superb = malloc(MIN(arows, acols)); + double *superb = malloc(sizeof(double) * 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); diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 4c3c8b7..21e61ae 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -1,4 +1,4 @@ -#include "PersistentScene.h" +#include "persistent_scene.h" #ifndef USE_DOUBLE #define FLT double @@ -89,6 +89,19 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) return 0; } +struct add_correspondence_for_lh { + epnp *pnp; + int lh; +}; + +void add_correspondence_for_lh(SurviveObject *so, int lh, int sensor_idx, FLT *angles, void *_user) { + struct add_correspondence_for_lh *user = (struct add_correspondence_for_lh *)_user; + if (user->lh == lh) + epnp_add_correspondence(user->pnp, so->sensor_locations[sensor_idx * 3 + 0], + so->sensor_locations[sensor_idx * 3 + 1], so->sensor_locations[sensor_idx * 3 + 2], + tan(angles[0]), tan(angles[1])); +} + int PoserEPNP(SurviveObject *so, PoserData *pd) { switch (pd->pt) { case POSERDATA_IMU: { @@ -97,28 +110,36 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { return 0; } case POSERDATA_LIGHT: { - /* - PersistentScene* scene; - PoserDataLight * lightData = pd; - - PersistentScene_add(scene, so, lightData); - - if (so->ctx->bsd[lh].PositionSet) { - auto pose = solve_correspondence(so, cal_objectPoints, cal_imagePoints, false); - - SurvivePose txPose = {}; - quatrotatevector(txPose.Pos, so->ctx->bsd[lh].Pose.Rot, pose.Pos); - for (int i = 0; i < 3; i++) { - txPose.Pos[i] += so->ctx->bsd[lh].Pose.Pos[i]; - } - quatrotateabout(txPose.Rot, so->ctx->bsd[lh].Pose.Rot, pose.Rot); - - // scene->integratePose(txPose, lightData->timecode); - // txPose = scene->currentPose; - PoserData_poser_raw_pose_func(pd, so, lh, &txPose); - } - */ - return -1; + static PersistentScene _scene = {.tolerance = 1500000}; + PersistentScene *scene = &_scene; + PoserDataLight *lightData = (PoserDataLight *)pd; + + PersistentScene_add(scene, so, lightData); + + int lh = lightData->lh; + if (so->ctx->bsd[lh].PositionSet) { + epnp pnp = {.fu = 1, .fv = 1}; + epnp_set_maximum_number_of_correspondences(&pnp, so->nr_locations); + + struct add_correspondence_for_lh user = {.lh = lh, .pnp = &pnp}; + PersistentScene_ForEachCorrespondence(scene, add_correspondence_for_lh, so, lightData->timecode, &user); + + if (pnp.number_of_correspondences > 4) { + + SurvivePose pose = solve_correspondence(so, &pnp, false); + + SurvivePose txPose = {}; + quatrotatevector(txPose.Pos, so->ctx->bsd[lh].Pose.Rot, pose.Pos); + for (int i = 0; i < 3; i++) { + txPose.Pos[i] += so->ctx->bsd[lh].Pose.Pos[i]; + } + + quatrotateabout(txPose.Rot, so->ctx->bsd[lh].Pose.Rot, pose.Rot); + PoserData_poser_raw_pose_func(pd, so, lh, &txPose); + } + } + + return 0; } case POSERDATA_FULL_SCENE: { return opencv_solver_fullscene(so, (PoserDataFullScene *)(pd)); diff --git a/test.c b/test.c index d4b3d6f..aaf980f 100644 --- a/test.c +++ b/test.c @@ -145,6 +145,7 @@ int main() survive_install_button_fn(ctx, testprog_button_process); survive_install_raw_pose_fn(ctx, testprog_raw_pose_process); + survive_install_imu_fn(ctx, testprog_imu_process); survive_install_lighthouse_pose_fn(ctx, testprog_lighthouse_process); -- cgit v1.2.3 From a3faba1727a741fa43d4ad2a7b43cf93dcc1cadf Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 15 Mar 2018 16:26:11 -0600 Subject: Cleaned up memory leaks --- redist/json_helpers.c | 1 + src/epnp/epnp.h | 1 + src/epnp/opencv_shim.c | 53 ++----- src/poser_epnp.c | 4 + src/survive.c | 7 +- src/survive_playback.c | 29 ++-- tools/viz/README.md | 13 ++ tools/viz/index.html | 23 +++ tools/viz/survive_viewer.js | 378 ++++++++++++++++++++++++++++++++++++++++++++ 9 files changed, 456 insertions(+), 53 deletions(-) create mode 100644 tools/viz/README.md create mode 100644 tools/viz/index.html create mode 100644 tools/viz/survive_viewer.js diff --git a/redist/json_helpers.c b/redist/json_helpers.c index 4aeb399..8704a93 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -210,6 +210,7 @@ void json_load_file(const char* path) { free(value); } + free(tokens); free(JSON_STRING); } diff --git a/src/epnp/epnp.h b/src/epnp/epnp.h index 9ca3a2e..0a42b04 100644 --- a/src/epnp/epnp.h +++ b/src/epnp/epnp.h @@ -46,6 +46,7 @@ typedef struct { double cws_determinant; } epnp; +void epnp_dtor(epnp *self); void epnp_set_internal_parameters(epnp *self, double uc, double vc, double fu, double fv); void epnp_set_maximum_number_of_correspondences(epnp *self, int n); void epnp_reset_correspondences(epnp *self); diff --git a/src/epnp/opencv_shim.c b/src/epnp/opencv_shim.c index d7886d0..df2765b 100644 --- a/src/epnp/opencv_shim.c +++ b/src/epnp/opencv_shim.c @@ -28,29 +28,7 @@ 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; @@ -85,7 +63,7 @@ void cvMulTransposed(const CvMat *src, CvMat *dst, int order, const CvMat *delta 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 + assert(delta == 0 && "This isn't implemented yet"); double beta = 0; bool isAT = order == 1; @@ -93,16 +71,12 @@ void cvMulTransposed(const CvMat *src, CvMat *dst, int order, const CvMat *delta lapack_int dstCols = dst->cols; - cblas_dgemm(CblasRowMajor, isAT ? CblasTrans : CblasNoTrans, isBT ? CblasTrans : CblasNoTrans, - cols, // isAT ? cols : rows, - dstCols, - rows, // isAT ? rows : cols, + cblas_dgemm(CblasRowMajor, isAT ? CblasTrans : CblasNoTrans, isBT ? CblasTrans : CblasNoTrans, cols, dstCols, rows, 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); } @@ -242,14 +216,15 @@ double cvInvert(const CvMat *srcarr, CvMat *dstarr, int method) { free(ipiv); } else if (method == DECOMP_SVD) { - + // TODO: There is no way this needs this many allocations, + // but in my defense I was very tired when I wrote this code 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); + cvSVD(dstarr, w, u, v, 0); + cvSetZero(um); for (int i = 0; i < w->cols; i++) { cvmSet(um, i, i, 1. / w->data.db[i]); @@ -258,6 +233,12 @@ double cvInvert(const CvMat *srcarr, CvMat *dstarr, int method) { 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); + + cvReleaseMat(&tmp); + cvReleaseMat(&w); + cvReleaseMat(&u); + cvReleaseMat(&v); + cvReleaseMat(&um); } return 0; } @@ -333,11 +314,9 @@ int cvSolve(const CvMat *Aarr, const CvMat *xarr, CvMat *Barr, int method) { assert(Barr->cols == xCpy->cols); xCpy->rows = acols; cvCopyTo(xCpy, Barr); -/* -Barr->data = xCpy->data; -Barr->rows = acols; -Barr->cols = xCpy->cols; -*/ + + cvReleaseMat(&aCpy); + cvReleaseMat(&xCpy); #ifdef DEBUG_PRINT print_mat(Barr); #endif diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 21e61ae..7749c7b 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -85,6 +85,8 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) SurvivePose lighthouse = solve_correspondence(so, &pnp, true); PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &lighthouse); + + epnp_dtor(&pnp); } return 0; } @@ -137,6 +139,8 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { quatrotateabout(txPose.Rot, so->ctx->bsd[lh].Pose.Rot, pose.Rot); PoserData_poser_raw_pose_func(pd, so, lh, &txPose); } + + epnp_dtor(&pnp); } return 0; diff --git a/src/survive.c b/src/survive.c index e09ae13..a27ba3e 100755 --- a/src/survive.c +++ b/src/survive.c @@ -340,7 +340,8 @@ void survive_close( SurviveContext * ctx ) { PoserData pd; pd.pt = POSERDATA_DISASSOCIATE; - if( ctx->objs[i]->PoserFn ) ctx->objs[i]->PoserFn( ctx->objs[i], &pd ); + if (ctx->objs[i]->PoserFn) + ctx->objs[i]->PoserFn(ctx->objs[i], &pd); } for( i = 0; i < oldct; i++ ) @@ -354,6 +355,10 @@ void survive_close( SurviveContext * ctx ) destroy_config_group(ctx->global_config_values); destroy_config_group(ctx->lh_config); + for (i = 0; i < ctx->objs_ct; i++) { + free(ctx->objs[i]); + } + free( ctx->objs ); free( ctx->drivers ); free( ctx->driverpolls ); diff --git a/src/survive_playback.c b/src/survive_playback.c index fe7af2b..c4564c4 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -160,6 +160,7 @@ static int playback_close(struct SurviveContext *ctx, void *_driver) { if (driver->playback_file) fclose(driver->playback_file); driver->playback_file = 0; + return 0; } @@ -184,7 +185,11 @@ static int LoadConfig(SurvivePlaybackData *sv, SurviveObject *so) { ct0conf[len] = 0; printf("Loading config: %d\n", len); - return survive_load_htc_config_format(ct0conf, len, so); + int rtn = survive_load_htc_config_format(ct0conf, len, so); + + free(ct0conf); + + return rtn; } int DriverRegPlayback(SurviveContext *ctx) { @@ -217,20 +222,14 @@ int DriverRegPlayback(SurviveContext *ctx) { SurviveObject *tr0 = survive_create_tr0(ctx, "Playback", sp); SurviveObject *ww0 = survive_create_ww0(ctx, "Playback", sp); - if (!LoadConfig(sp, hmd)) { - survive_add_object(ctx, hmd); - } - if (!LoadConfig(sp, wm0)) { - survive_add_object(ctx, wm0); - } - if (!LoadConfig(sp, wm1)) { - survive_add_object(ctx, wm1); - } - if (!LoadConfig(sp, tr0)) { - survive_add_object(ctx, tr0); - } - if (!LoadConfig(sp, ww0)) { - survive_add_object(ctx, ww0); + SurviveObject *objs[] = {hmd, wm0, wm1, tr0, ww0, 0}; + + for (SurviveObject **obj = objs; *obj; obj++) { + if (!LoadConfig(sp, *obj)) { + survive_add_object(ctx, *obj); + } else { + free(*obj); + } } survive_add_driver(ctx, sp, playback_poll, playback_close, 0); diff --git a/tools/viz/README.md b/tools/viz/README.md new file mode 100644 index 0000000..6afbd5c --- /dev/null +++ b/tools/viz/README.md @@ -0,0 +1,13 @@ +# How to use + +- Download and install: http://websocketd.com/ +- Build the repo +- Run data_recorder through websocketd like so: + +``` websocketd --port=8080 ./data_recorder``` + +- Navigate to the `index.html` page in this directory on chrome. + +When lighthouses, poses, or angle information is found, it should add it the scene. + + diff --git a/tools/viz/index.html b/tools/viz/index.html new file mode 100644 index 0000000..5085faf --- /dev/null +++ b/tools/viz/index.html @@ -0,0 +1,23 @@ + + + + + + + + + +
+
+ + +
+ + diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js new file mode 100644 index 0000000..aaa0340 --- /dev/null +++ b/tools/viz/survive_viewer.js @@ -0,0 +1,378 @@ +var sphere, axes; + +function add_lighthouse(idx, p, q) { + var group = new THREE.Group(); + + var lh = new THREE.AxesHelper(1); + + group.position.fromArray(p); + group.quaternion.fromArray([ q[1], q[2], q[3], q[0] ]); + + var height = 3; + var geometry = new THREE.ConeGeometry(Math.sin(1.0472) * height, height, 4, 1, true); + var material = new THREE.MeshBasicMaterial({ + wireframe : true, + vertexColor : true, + color : 0x111111, + opacity : 0.09, + transparent : true, + blending : THREE.AdditiveBlending, + side : THREE.BothSides + }); + var cone = new THREE.Mesh(geometry, material); + + var lhBoxGeom = new THREE.CubeGeometry(.1, .1, .1); + var lhBoxMaterial = new THREE.MeshLambertMaterial({color : 0x111111, side : THREE.FrontSide}); + var lhBox = new THREE.Mesh(lhBoxGeom, lhBoxMaterial); + group.add(lhBox); + + cone.translateZ(-height / 2) + cone.rotateZ(Math.PI / 4) + cone.rotateX(Math.PI / 2) + // cone.position.z + + group.add(cone); + + group.add(lh); + scene.add(group); + // DrawCoordinateSystem(p[0], p[1], p[2], q[0], q[1], q[2], q[3]); + } +var downAxes = {}; +var angles = {}; +var ctx; +var canvas; +var oldDrawTime = 0; +var lastWhen = {}; + +$(function() { $("#toggleBtn").click(function() { $("#cam").toggle(); }); }); + +function redrawCanvas(when) { + oldDrawTime = new Date().getTime(); + if (!ctx) { + canvas = document.getElementById("camcanvas"); + ctx = canvas.getContext("2d"); + } + if (!$(canvas).is(":visible")) { + return true; + } + ctx.clearRect(0, 0, canvas.width, canvas.height); + + var fov_degrees = 150; + var fov_radians = fov_degrees / 180 * Math.PI; + + function rad_to_x(ang) { + var half_fov = fov_radians / 2; + return ang / half_fov * canvas.width / 2 + canvas.width / 2; + } + var rad_to_y = rad_to_x; + + ctx.strokeStyle = "#ffffff"; + ctx.beginPath(); + for (var x = -fov_degrees; x < fov_degrees; x += 10) { + var length = Math.abs(x) == 60 ? canvas.width : 10; + ctx.moveTo(rad_to_x(x / 180 * Math.PI), 0); + ctx.lineTo(rad_to_x(x / 180 * Math.PI), length); + + ctx.moveTo(0, rad_to_x(x / 180 * Math.PI)); + ctx.lineTo(length, rad_to_x(x / 180 * Math.PI)); + + ctx.moveTo(rad_to_x(x / 180 * Math.PI), canvas.width); + ctx.lineTo(rad_to_x(x / 180 * Math.PI), canvas.width - length); + + ctx.moveTo(canvas.width, rad_to_x(x / 180 * Math.PI)); + ctx.lineTo(canvas.width - length, rad_to_x(x / 180 * Math.PI)); + } + + ctx.stroke(); + + for (var key in angles) { + for (var lh = 0; lh < 2; lh++) { + var bvalue = {"WW0" : "FF", "TR0" : "00"}; + ctx.strokeStyle = (lh === 0 ? "#FF00" : "#00FF") + bvalue[key]; + + if (angles[key][lh]) + + for (var id in angles[key][lh]) { + var ang = angles[key][lh][id]; + + if (ang[0] === undefined || ang[1] === undefined || ang[1][1] < when[key] - 48000000 || + ang[0][1] < when[key] - 48000000) + continue; + + var half_fov = 1.0472 * 2; + var x = ang[0][0] / half_fov * canvas.width / 2 + canvas.width / 2; + var y = -ang[1][0] / half_fov * canvas.height / 2 + canvas.height / 2; + + ctx.fillStyle = "white"; + ctx.font = "14px Arial"; + // ctx.fillText(id, x, y); + + ctx.beginPath(); + ctx.arc(x, y, 1, 0, 2 * Math.PI); + ctx.stroke(); + } + } + } + } + +var objs = {}; +var sensorGeometry = new THREE.SphereGeometry(.01, 32, 16); +// use a "lambert" material rather than "basic" for realistic lighting. +// (don't forget to add (at least one) light!) + +function create_object(info) { + var group = new THREE.Group(); + + for (var idx in info.points) { + var p = info.points[idx]; + var color = 0xFFFFFF; // / info.points.length * idx; + if (idx == 10) + color = 0x00ff00; + if (idx == 12) + color = 0x0000ff; + var sensorMaterial = new THREE.MeshLambertMaterial({color : color}); + var newSensor = new THREE.Mesh(sensorGeometry, sensorMaterial); + newSensor.position.set(p[0], p[1], p[2]); + + group.add(newSensor); + } + + var axes = new THREE.AxesHelper(1); + group.add(axes); + + objs[info.tracker] = group; + scene.add(group); + } + +var timecode = {}; +$(function() { + + function parseLine(msg) { + var s = msg.split(' '); + + var command_mappings = { + "LH_POSE" : function(v) { + return { + type : "lighthouse_pose", + lighthouse : parseInt(v[2]), + position : [ parseFloat(v[3]), parseFloat(v[4]), parseFloat(v[5]) ], + quat : [ parseFloat(v[6]), parseFloat(v[7]), parseFloat(v[8]), parseFloat(v[9]) ] + }; + }, + "POSE" : function(v) { + return { + type: "pose", tracker: v[2], position: [ parseFloat(v[3]), parseFloat(v[4]), parseFloat(v[5]) ], + quat: [ parseFloat(v[6]), parseFloat(v[7]), parseFloat(v[8]), parseFloat(v[9]) ] + } + } + }; + if (command_mappings[s[1]]) { + var rtn = command_mappings[s[1]](s); + rtn.time = parseFloat(s[0]); + return rtn; + } + return {}; + } + var ws; + if (window.location.protocol === "file:") { + ws = new WebSocket("ws://localhost:8080/ws"); + } else { + ws = new WebSocket(((window.location.protocol === "https:") ? "wss://" : "ws://") + window.location.host + + "/ws"); + } + + ws.onopen = function(evt) { + // ws.send("!"); + }; + ws.onmessage = function(evt) { + var msg = evt.data; + var obj; + if (msg[0] == "{") + obj = JSON.parse(msg); + else + obj = parseLine(msg); + + // console.log(obj); + if (obj.type === "pose") { + if (!objs[obj.tracker]) { + create_object(obj); + } + + objs[obj.tracker].position.set(obj.position[0], obj.position[1], obj.position[2]); + objs[obj.tracker].quaternion.set(obj.quat[1], obj.quat[2], obj.quat[3], obj.quat[0]); + + } else if (obj.type === "lighthouse_pose") { + add_lighthouse(obj.lighthouse, obj.position, obj.quat); + } else if (obj.type === "tracker_calibration") { + create_object(obj); + } else if (obj.type === "imu") { + if (objs[obj.tracker]) { + if (!downAxes[obj.tracker]) { + downAxes[obj.tracker] = new THREE.Geometry(); + downAxes[obj.tracker].vertices.push( + new THREE.Vector3(0, 0, 0), + new THREE.Vector3(obj.accelgyro[0], obj.accelgyro[1], obj.accelgyro[2])); + + var line = new THREE.Line(downAxes[obj.tracker], new THREE.LineBasicMaterial({color : 0xffffff})); + objs[obj.tracker].add(line); + } else { + var q = obj.accelgyro; + downAxes[obj.tracker].vertices[1].fromArray(q); + downAxes[obj.tracker].verticesNeedUpdate = true; + } + } + + } else if (obj.type === "angle") { + angles[obj.tracker] = angles[obj.tracker] || {}; + angles[obj.tracker][obj.lighthouse] = angles[obj.tracker][obj.lighthouse] || {}; + angles[obj.tracker][obj.lighthouse][obj.sensor_id] = + angles[obj.tracker][obj.lighthouse][obj.sensor_id] || {}; + + angles[obj.tracker][obj.lighthouse][obj.sensor_id][obj.acode] = [ obj.angle, obj.timecode ]; + timecode[obj.tracker] = obj.timecode; + } + + // ws.send("!"); + }; +}); + +////////// +// MAIN // +////////// + +// standard global variables +var container, scene, camera, renderer, controls, stats; +var clock = new THREE.Clock(); + +// custom global variables +var cube; +$(function() { + // initialization + init(); + + // animation loop / game loop + animate(); +}) + +/////////////// +// FUNCTIONS // +/////////////// + +function +init() { + /////////// + // SCENE // + /////////// + scene = new THREE.Scene(); + + //////////// + // CAMERA // + //////////// + + // set the view size in pixels (custom or according to window size) + // var SCREEN_WIDTH = 400, SCREEN_HEIGHT = 300; + var SCREEN_WIDTH = window.innerWidth, SCREEN_HEIGHT = window.innerHeight; + // camera attributes + var VIEW_ANGLE = 45, ASPECT = SCREEN_WIDTH / SCREEN_HEIGHT, NEAR = 0.01, FAR = 200; + // set up camera + camera = new THREE.PerspectiveCamera(VIEW_ANGLE, ASPECT, NEAR, FAR); + camera.up = new THREE.Vector3(0, 0, 1); + // add the camera to the scene + scene.add(camera); + // the camera defaults to position (0,0,0) + // so pull it back (z = 400) and up (y = 100) and set the angle towards the + // scene origin + camera.position.set(5, 2, 5.00); + camera.lookAt(scene.position); + + ////////////// + // RENDERER // + ////////////// + + renderer = new THREE.WebGLRenderer({antialias : true}); + + renderer.setSize(SCREEN_WIDTH, SCREEN_HEIGHT); + + // attach div element to variable to contain the renderer + container = document.getElementById('ThreeJS'); + // alternatively: to create the div at runtime, use: + // container = document.createElement( 'div' ); + // document.body.appendChild( container ); + + // attach renderer to the container div + container.appendChild(renderer.domElement); + + //////////// + // EVENTS // + //////////// + + /* + // automatically resize renderer + THREEx.WindowResize(renderer, camera); + // toggle full-screen on given key press + THREEx.FullScreen.bindKey({ charCode : 'm'.charCodeAt(0) }); +*/ + ////////////// + // CONTROLS // + ////////////// + + // move mouse and: left click to rotate, + // middle click to zoom, + // right click to pan + controls = new THREE.OrbitControls(camera, renderer.domElement); + + /////////// + // LIGHT // + /////////// + + // create a light + var light = new THREE.PointLight(0xffffff); + light.position.set(0, 5, 0); + scene.add(light); + var ambientLight = new THREE.AmbientLight(0x111111); + // scene.add(ambientLight); + + var floorTexture = new THREE.ImageUtils.loadTexture('images/checkerboard.jpg'); + floorTexture.wrapS = floorTexture.wrapT = THREE.RepeatWrapping; + floorTexture.repeat.set(10, 10); + // DoubleSide: render texture on both sides of mesh + var floorMaterial = + new THREE.MeshBasicMaterial({color : 0x000000, opacity : 0.15, transparent : true, side : THREE.FrontSide}); + var floorGeometry = new THREE.PlaneGeometry(10, 10); + var floor = new THREE.Mesh(floorGeometry, floorMaterial); + floor.position.z = -1; + + scene.add(floor); + + ///////// + // SKY // + ///////// + + // recommend either a skybox or fog effect (can't use both at the same time) + // without one of these, the scene's background color is determined by + // webpage background + + var skyBoxGeometry = new THREE.CubeGeometry(50, 50, 50); + var skyBoxMaterial = new THREE.MeshBasicMaterial({color : 0x888888, side : THREE.BackSide}); + var skyBox = new THREE.Mesh(skyBoxGeometry, skyBoxMaterial); + scene.add(skyBox); + + // fog must be added to scene before first render + // scene.fog = new THREE.FogExp2(0xffffff, 0.025); +} + +function animate() { + requestAnimationFrame(animate); + render(); + update(); + redrawCanvas(timecode); + } + +function update() { + // delta = change in time since last call (in seconds) + var delta = clock.getDelta(); + + // controls.update(); + } + +function render() { renderer.render(scene, camera); } -- cgit v1.2.3 From 0c3d7e1b48a209b4bdbb4553837fad419f0d0777 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 15 Mar 2018 16:30:12 -0600 Subject: Removed cruft from Makefile --- Makefile | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/Makefile b/Makefile index 2eb4859..4d492eb 100644 --- a/Makefile +++ b/Makefile @@ -2,7 +2,7 @@ all : lib data_recorder test calibrate calibrate_client simple_pose_test CC?=gcc -CFLAGS:=-Iinclude/libsurvive -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -fsanitize=address -fsanitize=undefined -llapacke -lcblas -lm +CFLAGS:=-Iinclude/libsurvive -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke -lcblas -lm #LDFLAGS:=-L/usr/local/lib -lpthread -lusb-1.0 -lz -lm -flto -g LDFLAGS:=-L/usr/local/lib -lpthread -lz -lm -flto -g @@ -92,14 +92,6 @@ test_epnp: ./src/epnp/test_epnp.c ./lib/libsurvive.so test_epnp_ocv: ./src/epnp/test_epnp.c ./src/epnp/epnp.c $(CC) -o $@ $^ -DWITH_OPENCV -lpthread -lz -lm -flto -g -lX11 -lusb-1.0 -Iinclude/libsurvive -fPIC -g -O4 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -fsanitize=address -fsanitize=undefined -llapack -lm -lopencv_core - -unit_test_epnp: ./src/epnp/unit_test_epnp.c ./lib/libsurvive.so - $(CC) -o $@ $^ $(LDFLAGS) $(CFLAGS) - -test_epnp_results: test_epnp test_epnp_ocv - -./test_epnp_ocv > opencv_output - -./test_epnp > our_output - lib: mkdir lib @@ -111,7 +103,7 @@ calibrate_tcc : $(LIBSURVIVE_C) tcc -DRUNTIME_SYMNUM $(CFLAGS) -o $@ $^ $(LDFLAGS) calibrate.c redist/os_generic.c $(DRAWFUNCTIONS) redist/symbol_enumerator.c clean : - rm -rf *.o src/*.o *~ src/*~ test simple_pose_test data_recorder calibrate testCocoa lib/libsurvive.so redist/*.o redist/*~ + rm -rf *.o src/*.o *~ src/*~ test simple_pose_test data_recorder calibrate testCocoa lib/libsurvive.so test_minimal_cv test_epnp test_epnp_ocv calibrate_client redist/*.o redist/*~ -- cgit v1.2.3 From 26da6d75305d02e17e7b011ab88f6d50dcea02f0 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 15 Mar 2018 17:04:30 -0600 Subject: Refactored the minimal ocv to be in redist --- Makefile | 4 +- redist/minimal_opencv.c | 372 +++++++++++ redist/minimal_opencv.h | 212 ++++++ src/epnp/epnp.h | 2 +- src/epnp/opencv_shim.c | 400 ------------ src/epnp/opencv_shim.h | 31 - src/epnp/shim_types_c.h | 1633 ----------------------------------------------- 7 files changed, 587 insertions(+), 2067 deletions(-) create mode 100644 redist/minimal_opencv.c create mode 100644 redist/minimal_opencv.h delete mode 100644 src/epnp/opencv_shim.c delete mode 100644 src/epnp/opencv_shim.h delete mode 100644 src/epnp/shim_types_c.h diff --git a/Makefile b/Makefile index 4d492eb..d0071a1 100644 --- a/Makefile +++ b/Makefile @@ -32,11 +32,11 @@ endif POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o src/poser_octavioradii.o src/poser_turveytori.o src/poser_epnp.o -REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic.o +REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic.o redist/minimal_opencv.o ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/poser_imu.o src/survive_imu.o src/epnp/epnp.c src/epnp/opencv_shim.c src/persistent_scene.o +LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/poser_imu.o src/survive_imu.o src/epnp/epnp.c src/persistent_scene.o #If you want to use HIDAPI on Linux. diff --git a/redist/minimal_opencv.c b/redist/minimal_opencv.c new file mode 100644 index 0000000..3f7bed7 --- /dev/null +++ b/redist/minimal_opencv.c @@ -0,0 +1,372 @@ +//#include "/home/justin/source/CLAPACK/INCLUDE/f2c.h" +//#include "/home/justin/source/CLAPACK/INCLUDE/clapack.h" +#include +#include + +#include "math.h" +#include "minimal_opencv.h" +#include "stdbool.h" +#include "stdio.h" +#include "string.h" + +#include +//#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; + +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); +} + +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, dstCols, rows, + scale, + + src->data.db, cols, src->data.db, cols, beta, + + dst->data.db, dstCols); +} + +void *cvAlloc(size_t size) { return malloc(size); } + +static void icvCheckHuge(CvMat *arr) { + if ((int64_t)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(CvMat *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_t _total_size = (int64_t)step * mat->rows + sizeof(int) + CV_MALLOC_ALIGN; + total_size = (size_t)_total_size; + if (_total_size != (int64_t)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)) { + assert("There is no support for ND types"); + } 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) { + // TODO: There is no way this needs this many allocations, + // but in my defense I was very tired when I wrote this code + 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); + CvMat *um = cvCreateMat(w->cols, w->cols, w->type); + + cvSVD(dstarr, w, u, v, 0); + + 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); + + cvReleaseMat(&tmp); + cvReleaseMat(&w); + cvReleaseMat(&u); + cvReleaseMat(&v); + cvReleaseMat(&um); + } + 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); + + cvReleaseMat(&aCpy); + cvReleaseMat(&xCpy); +#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(sizeof(double) * 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/redist/minimal_opencv.h b/redist/minimal_opencv.h new file mode 100644 index 0000000..fa07c84 --- /dev/null +++ b/redist/minimal_opencv.h @@ -0,0 +1,212 @@ +#include "assert.h" +#include "stdint.h" +#include "stdlib.h" + +#define CV_Error(code, msg) assert(0 && msg); // cv::error( code, msg, CV_Func, __FILE__, __LINE__ ) + +#define CV_8U 0 +#define CV_8S 1 +#define CV_16U 2 +#define CV_16S 3 +#define CV_32S 4 +#define CV_32F 5 +#define CV_64F 6 + +#define CV_32FC1 CV_32F +#define CV_64FC1 CV_64F + +#define CV_MAGIC_MASK 0xFFFF0000 +#define CV_MAT_MAGIC_VAL 0x42420000 + +#define CV_CN_MAX 512 +#define CV_CN_SHIFT 3 +#define CV_DEPTH_MAX (1 << CV_CN_SHIFT) + +#define CV_MAT_DEPTH_MASK (CV_DEPTH_MAX - 1) +#define CV_MAT_DEPTH(flags) ((flags)&CV_MAT_DEPTH_MASK) + +#define CV_MAKETYPE(depth, cn) (CV_MAT_DEPTH(depth) + (((cn)-1) << CV_CN_SHIFT)) +#define CV_MAKE_TYPE CV_MAKETYPE + +#define CV_MAT_CN_MASK ((CV_CN_MAX - 1) << CV_CN_SHIFT) +#define CV_MAT_CN(flags) ((((flags)&CV_MAT_CN_MASK) >> CV_CN_SHIFT) + 1) +#define CV_MAT_TYPE_MASK (CV_DEPTH_MAX * CV_CN_MAX - 1) +#define CV_MAT_TYPE(flags) ((flags)&CV_MAT_TYPE_MASK) +#define CV_MAT_CONT_FLAG_SHIFT 14 +#define CV_MAT_CONT_FLAG (1 << CV_MAT_CONT_FLAG_SHIFT) +#define CV_IS_MAT_CONT(flags) ((flags)&CV_MAT_CONT_FLAG) +#define CV_IS_CONT_MAT CV_IS_MAT_CONT +#define CV_SUBMAT_FLAG_SHIFT 15 +#define CV_SUBMAT_FLAG (1 << CV_SUBMAT_FLAG_SHIFT) +#define CV_IS_SUBMAT(flags) ((flags)&CV_MAT_SUBMAT_FLAG) + +typedef uint8_t uchar; + +#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 CV_IS_MATND_HDR(mat) ((mat) != NULL && (((const CvMat *)(mat))->type & CV_MAGIC_MASK) == CV_MATND_MAGIC_VAL) + +#define CV_IS_MATND(mat) (CV_IS_MATND_HDR(mat) && ((const CvMat *)(mat))->data.ptr != NULL) +#define CV_MATND_MAGIC_VAL 0x42430000 + +/** 0x3a50 = 11 10 10 01 01 00 00 ~ array of log2(sizeof(arr_type_elem)) */ +#define CV_ELEM_SIZE(type) \ + (CV_MAT_CN(type) << ((((sizeof(size_t) / 4 + 1) * 16384 | 0x3a50) >> CV_MAT_DEPTH(type) * 2) & 3)) + +#ifndef MIN +#define MIN(a, b) ((a) > (b) ? (b) : (a)) +#endif + +#ifndef MAX +#define MAX(a, b) ((a) < (b) ? (b) : (a)) +#endif + +/** 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; + + int rows; + int cols; + +} CvMat; + +/** Inline constructor. No data is allocated internally!!! + * (Use together with cvCreateData, or use cvCreateMat instead to + * get a matrix with allocated data): + */ +static inline CvMat cvMat(int rows, int cols, int type, void *data) { + 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 = 0; + m.hdr_refcount = 0; + + return m; +} + +/* +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 + */ +static 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 + */ +static 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; + } +} + +/** 0x3a50 = 11 10 10 01 01 00 00 ~ array of log2(sizeof(arr_type_elem)) */ +#define CV_ELEM_SIZE(type) \ + (CV_MAT_CN(type) << ((((sizeof(size_t) / 4 + 1) * 16384 | 0x3a50) >> CV_MAT_DEPTH(type) * 2) & 3)) + +//#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); +void cvTranspose(const CvMat *M, CvMat *dst); +void print_mat(const CvMat *M); + +#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/epnp.h b/src/epnp/epnp.h index 0a42b04..f87e1bf 100644 --- a/src/epnp/epnp.h +++ b/src/epnp/epnp.h @@ -29,7 +29,7 @@ #define epnp_h #ifndef WITH_OPENCV -#include "opencv_shim.h" +#include "../../redist/minimal_opencv.h" #else #include #endif diff --git a/src/epnp/opencv_shim.c b/src/epnp/opencv_shim.c deleted file mode 100644 index df2765b..0000000 --- a/src/epnp/opencv_shim.c +++ /dev/null @@ -1,400 +0,0 @@ -//#include "/home/justin/source/CLAPACK/INCLUDE/f2c.h" -//#include "/home/justin/source/CLAPACK/INCLUDE/clapack.h" -#include -#include - -#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##_ - -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, dstCols, rows, - scale, - - src->data.db, cols, src->data.db, cols, beta, - - dst->data.db, dstCols); -} - -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) { - // TODO: There is no way this needs this many allocations, - // but in my defense I was very tired when I wrote this code - 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); - CvMat *um = cvCreateMat(w->cols, w->cols, w->type); - - cvSVD(dstarr, w, u, v, 0); - - 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); - - cvReleaseMat(&tmp); - cvReleaseMat(&w); - cvReleaseMat(&u); - cvReleaseMat(&v); - cvReleaseMat(&um); - } - 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); - - cvReleaseMat(&aCpy); - cvReleaseMat(&xCpy); -#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(sizeof(double) * 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 deleted file mode 100644 index 5701f1d..0000000 --- a/src/epnp/opencv_shim.h +++ /dev/null @@ -1,31 +0,0 @@ -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); -void cvTranspose(const CvMat *M, CvMat *dst); -void print_mat(const CvMat *M); - -#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 deleted file mode 100644 index 56a61fe..0000000 --- a/src/epnp/shim_types_c.h +++ /dev/null @@ -1,1633 +0,0 @@ -/*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 -#else -#include -#endif -#endif -#elif defined __IPL_H__ -#define HAVE_IPL -#endif - -#include "opencv2/core/cvdef.h" - -#ifndef SKIP_INCLUDES -#include -#include -#include -#include -#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 - CvRect(const cv::Rect_<_Tp> &r) - : x(cv::saturate_cast(r.x)), y(cv::saturate_cast(r.y)), width(cv::saturate_cast(r.width)), - height(cv::saturate_cast(r.height)) {} - template 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 CvPoint(const cv::Point_<_Tp> &pt) : x((int)pt.x), y((int)pt.y) {} - template 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 CvPoint2D32f(const cv::Point_<_Tp> &pt) : x((float)pt.x), y((float)pt.y) {} - template 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 CvPoint3D32f(const cv::Point3_<_Tp> &pt) : x((float)pt.x), y((float)pt.y), z((float)pt.z) {} - template 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 - CvSize(const cv::Size_<_Tp> &sz) - : width(cv::saturate_cast(sz.width)), height(cv::saturate_cast(sz.height)) {} - template 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 - CvSize2D32f(const cv::Size_<_Tp> &sz) - : width(cv::saturate_cast(sz.width)), height(cv::saturate_cast(sz.height)) {} - template 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 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 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 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 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 /**= 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. */ -- cgit v1.2.3 From 029b2909b37cc58ba35e0f46be1802866ee59730 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 15 Mar 2018 21:17:42 -0600 Subject: Forgot files --- Makefile | 3 +-- src/persistent_scene.c | 36 ++++++++++++++++++++++++++++++++++++ src/persistent_scene.h | 32 ++++++++++++++++++++++++++++++++ 3 files changed, 69 insertions(+), 2 deletions(-) create mode 100644 src/persistent_scene.c create mode 100644 src/persistent_scene.h diff --git a/Makefile b/Makefile index d0071a1..99d29a8 100644 --- a/Makefile +++ b/Makefile @@ -30,13 +30,12 @@ GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGXDriver.o endif - POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o src/poser_octavioradii.o src/poser_turveytori.o src/poser_epnp.o REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic.o redist/minimal_opencv.o ifeq ($(UNAME), Darwin) REDISTS:=$(REDISTS) redist/hid-osx.c endif -LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/poser_imu.o src/survive_imu.o src/epnp/epnp.c src/persistent_scene.o +LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.c src/persistent_scene.o #If you want to use HIDAPI on Linux. diff --git a/src/persistent_scene.c b/src/persistent_scene.c new file mode 100644 index 0000000..9bbbf41 --- /dev/null +++ b/src/persistent_scene.c @@ -0,0 +1,36 @@ +#include "persistent_scene.h" +#include "linmath.h" +#include +#include +#include + +bool PersistentScene_isStillValid(const PersistentScene *self, uint32_t timecode_now, uint32_t idx, int lh) { + const uint32_t *data_timecode = self->timecode[idx][lh]; + return !(timecode_now - data_timecode[0] > self->tolerance || timecode_now - data_timecode[1] > self->tolerance); +} + +void PersistentScene_add(PersistentScene *self, SurviveObject *so, PoserDataLight *lightData) { + int axis = (lightData->acode & 1); + uint32_t *data_timecode = &self->timecode[lightData->sensor_id][lightData->lh][axis]; + FLT *angle = &self->angles[lightData->sensor_id][lightData->lh][axis]; + + *angle = lightData->angle; + *data_timecode = lightData->timecode; +} + +void PersistentScene_ForEachCorrespondence(PersistentScene *self, PersistentScene_ForEachCorrespondence_fn fn, + SurviveObject *so, uint32_t timecode_now, void *user) { + for (int lh = 0; lh < NUM_LIGHTHOUSES; lh++) { + for (size_t i = 0; i < so->nr_locations; i++) { + if (PersistentScene_isStillValid(self, timecode_now, i, lh)) { + double *pts = self->angles[i][lh]; + fn(so, lh, i, pts, user); + } + } + } +} + +void PersistentScene_ctor(PersistentScene *self) { + memset(self, 0, sizeof(PersistentScene)); + self->tolerance = 1500000; +} diff --git a/src/persistent_scene.h b/src/persistent_scene.h new file mode 100644 index 0000000..07d9056 --- /dev/null +++ b/src/persistent_scene.h @@ -0,0 +1,32 @@ +#pragma once +#include "persistent_scene.h" +#include "stdbool.h" + +#ifndef USE_DOUBLE +#define FLT double +#define USE_DOUBLE +#endif + +#include "linmath.h" +#include + +typedef struct { + uint32_t tolerance; + + // If "lengths[...]" < 0, means not a valid piece of sweep information. + FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // 2 Axes (Angles in LH space) + uint32_t timecode[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; + + PoserDataIMU lastimu; + +} PersistentScene; + +typedef void (*PersistentScene_ForEachCorrespondence_fn)(SurviveObject *so, int lh, int sensor_idx, FLT *angles, + void *); +void PersistentScene_ForEachCorrespondence(PersistentScene *self, PersistentScene_ForEachCorrespondence_fn fn, + SurviveObject *so, uint32_t timecode_now, void *user); + +void PersistentScene_add(PersistentScene *self, SurviveObject *so, PoserDataLight *lightData); + +bool PersistentScene_isStillValid(const PersistentScene *self, uint32_t timecode_now, uint32_t idx, int lh); +void PersistentScene_ctor(PersistentScene *self); -- cgit v1.2.3