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