From 0cb304caa5311e5cd59ab89ec6ff129b65423f8c Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 27 Apr 2018 16:30:36 +0000 Subject: Adding api compilation unit; must have gotten dropped in a merge --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 6a7dcd7..eb2177a 100644 --- a/Makefile +++ b/Makefile @@ -32,7 +32,7 @@ endif SBA:=redist/sba/sba_chkjac.c redist/sba/sba_crsm.c redist/sba/sba_lapack.c redist/sba/sba_levmar.c redist/sba/sba_levmar_wrap.c redist/minimal_opencv.c src/poser_epnp.c src/poser_sba.c src/epnp/epnp.c src/poser_general_optimizer.c -LIBSURVIVE_CORE+=src/survive.c src/survive_process.c src/ootx_decoder.c src/survive_driverman.c src/survive_default_devices.c src/survive_playback.c src/survive_config.c src/survive_cal.c src/poser.c src/survive_sensor_activations.c src/survive_disambiguator.c src/survive_imu.c src/survive_kalman.c +LIBSURVIVE_CORE+=src/survive.c src/survive_process.c src/ootx_decoder.c src/survive_driverman.c src/survive_default_devices.c src/survive_playback.c src/survive_config.c src/survive_cal.c src/poser.c src/survive_sensor_activations.c src/survive_disambiguator.c src/survive_imu.c src/survive_kalman.c src/survive_api.c MINIMAL_NEEDED+=src/survive_usb.c src/survive_charlesbiguator.c src/survive_vive.c src/survive_reproject.c AUX_NEEDED+=src/survive_turveybiguator.c src/survive_statebased_disambiguator.c src/survive_driver_dummy.c src/survive_driver_udp.c POSERS:=src/poser_dummy.c src/poser_imu.c src/poser_charlesrefine.c -- cgit v1.2.3 From 9224b6e2ce9747f09d154be2117a43f0d14511ac Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 7 May 2018 10:39:01 -0600 Subject: Added basic checks for using viz tool as library --- tools/viz/survive_viewer.js | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js index 56ef47f..cbd4fce 100644 --- a/tools/viz/survive_viewer.js +++ b/tools/viz/survive_viewer.js @@ -49,6 +49,9 @@ function add_lighthouse(idx, p, q) { } function recolorTrackers(when) { + if(ctx == undefined) + return; + for (var key in angles) { var colors = []; @@ -501,6 +504,6 @@ function animate() { } function render() { - var use_fpv = $("#fpv")[0].checked; + var use_fpv = $("#fpv").length > 0 && $("#fpv")[0].checked; renderer.render(scene, use_fpv ? fpv_camera : camera); } -- cgit v1.2.3 From fcf3528bfeba5e4646dbb0626a87c297d940a787 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 7 Jun 2018 23:42:05 -0600 Subject: Added debug message to ootx; mag reading to playback --- src/ootx_decoder.c | 9 ++++++++- src/ootx_decoder.h | 1 + src/survive_cal.c | 7 ++++++- src/survive_playback.c | 17 +++++++++++------ tools/viz/survive_viewer.js | 2 +- 5 files changed, 27 insertions(+), 9 deletions(-) diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c index 0620284..7d45626 100644 --- a/src/ootx_decoder.c +++ b/src/ootx_decoder.c @@ -19,11 +19,17 @@ #define MAX_BUFF_SIZE 64 +void (*ootx_error_clbk)(ootx_decoder_context *ctx, const char *msg) = NULL; void (*ootx_packet_clbk)(ootx_decoder_context * ctx, ootx_packet* packet) = NULL; void (*ootx_bad_crc_clbk)(ootx_decoder_context * ctx, ootx_packet* packet, uint32_t crc) = NULL; void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit); +void ootx_error(ootx_decoder_context *ctx, const char *msg) { + if (ootx_error_clbk) + ootx_error_clbk(ctx, msg); +} + void ootx_init_decoder_context(ootx_decoder_context *ctx) { ctx->buf_offset = 0; ctx->bits_written = 0; @@ -106,7 +112,7 @@ void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) { if ( ootx_detect_preamble(ctx, dbit) ) { /* data stream can start over at any time so we must always look for preamble bits */ - //printf("Preamble found\n"); + ootx_error(ctx, "Preamble found"); ootx_reset_buffer(ctx); ctx->bits_processed = 0; ctx->found_preamble = 1; @@ -117,6 +123,7 @@ void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) { if( !dbit ) { //printf("Bad sync bit\n"); + ootx_error(ctx, "OOTX Decoder: Bad sync bit"); ootx_reset_buffer(ctx); } ctx->bits_processed = 0; diff --git a/src/ootx_decoder.h b/src/ootx_decoder.h index 8ddf527..a96cb03 100644 --- a/src/ootx_decoder.h +++ b/src/ootx_decoder.h @@ -66,6 +66,7 @@ void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit); uint8_t ootx_decode_bit(uint32_t length); +extern void (*ootx_error_clbk)(ootx_decoder_context *ctx, const char *msg); extern void (*ootx_packet_clbk)(ootx_decoder_context *ctx, ootx_packet* packet); extern void (*ootx_bad_crc_clbk)(ootx_decoder_context *ctx, ootx_packet* packet, uint32_t crc); diff --git a/src/survive_cal.c b/src/survive_cal.c index ad62d22..f809809 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -45,6 +45,11 @@ int mkdir(const char *); static void handle_calibration( struct SurviveCalData *cd ); static void reset_calibration( struct SurviveCalData * cd ); +void ootx_error_clbk_d(ootx_decoder_context *ct, const char *msg) { + SurviveContext *ctx = (SurviveContext *)(ct->user); + SV_INFO("%s", msg); +} + void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet) { static uint8_t lighthouses_completed = 0; @@ -191,7 +196,7 @@ void survive_cal_install( struct SurviveContext * ctx ) cd->ConfigPoserFn = GetDriverByConfig(ctx, "Poser", "configposer", "SBA", 0); ootx_packet_clbk = ootx_packet_clbk_d; - + ootx_error_clbk = ootx_error_clbk_d; ctx->calptr = cd; } diff --git a/src/survive_playback.c b/src/survive_playback.c index a5c4519..50b2e5c 100644 --- a/src/survive_playback.c +++ b/src/survive_playback.c @@ -166,8 +166,9 @@ void survive_recording_imu_process(struct SurviveObject *so, int mask, FLT *acce if (recordingData == 0) return; - write_to_output(recordingData, "%s I %d %u %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %d\n", so->codename, mask, timecode, - accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id); + write_to_output(recordingData, "%s I %d %u %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %d\n", + so->codename, mask, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], + accelgyro[5], accelgyro[6], accelgyro[7], accelgyro[8], id); } struct SurvivePlaybackData { @@ -184,16 +185,20 @@ typedef struct SurvivePlaybackData SurvivePlaybackData; static int parse_and_run_imu(const char *line, SurvivePlaybackData *driver) { char dev[10]; int timecode = 0; - FLT accelgyro[6]; + FLT accelgyro[9] = {}; int mask; int id; int rr = sscanf(line, "%s I %d %d " FLT_format " " FLT_format " " FLT_format " " FLT_format " " FLT_format - " " FLT_format "%d", + " " FLT_format " " FLT_format " " FLT_format " " FLT_format "%d", dev, &mask, &timecode, &accelgyro[0], &accelgyro[1], &accelgyro[2], &accelgyro[3], &accelgyro[4], - &accelgyro[5], &id); + &accelgyro[5], &accelgyro[6], &accelgyro[7], &accelgyro[8], &id); - if (rr != 10) { + if (rr == 10) { + // Older formats might not have mag data + id = accelgyro[6]; + accelgyro[6] = 0; + } else if (rr != 13) { fprintf(stderr, "Warning: On line %d, only %d values read: '%s'\n", driver->lineno, rr, line); return -1; } diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js index cbd4fce..075e56a 100644 --- a/tools/viz/survive_viewer.js +++ b/tools/viz/survive_viewer.js @@ -146,7 +146,7 @@ function redrawCanvas(when) { ang[0][1] < when[key] - visible_tolerance) continue; - var half_fov = 1.0472 * 2 / 5.; + 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; -- cgit v1.2.3 From a7a44dc9cc0f4a74adbc22ff86ad081ecf2383ba Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 21 Jun 2018 16:22:24 +0000 Subject: Expanded minimal opencv functionality to properly use transpose flags --- redist/Makefile | 4 ++++ redist/minimal_opencv.c | 43 ++++++++++++++++++++++++++++--------------- redist/minimal_opencvtest.c | 39 +++++++++++++++++++++++++++++++++++++++ 3 files changed, 71 insertions(+), 15 deletions(-) create mode 100644 redist/minimal_opencvtest.c diff --git a/redist/Makefile b/redist/Makefile index 58536c9..5598c9b 100644 --- a/redist/Makefile +++ b/redist/Makefile @@ -6,11 +6,15 @@ jsmntest : jsmntest.c jsmn.c lintest : lintest.c linmath.c linmath.h gcc -g -O0 -o $@ $^ -lm +minimal_opencvtest : minimal_opencvtest.c minimal_opencv.c minimal_opencv.h + gcc -g -O0 -o $@ $^ -lcblas -lm -llapacke + test_dcl : test_dcl.c dclhelpers.c minimal_opencv.c ../src/epnp/epnp.c gcc -o $@ $^ os_generic.c -DFLT=double -lpthread -lcblas -lm -llapacke -O3 -msse2 -ftree-vectorize .run_tests: clean all ./lintest + ./minimal_opencvtest ./test_dcl ./jsmntest diff --git a/redist/minimal_opencv.c b/redist/minimal_opencv.c index d569d96..c9cacf3 100644 --- a/redist/minimal_opencv.c +++ b/redist/minimal_opencv.c @@ -30,27 +30,39 @@ SURVIVE_LOCAL_ONLY void cvCopyTo(const CvMat *srcarr, CvMat *dstarr) { SURVIVE_LOCAL_ONLY 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; + int rows1 = (tABC & GEMM_1_T) ? src1->cols : src1->rows; + int cols1 = (tABC & GEMM_1_T) ? src1->rows : src1->cols; - lapack_int lda = cols1; - lapack_int ldb = cols2; + int rows2 = (tABC & GEMM_2_T) ? src2->cols : src2->rows; + int cols2 = (tABC & GEMM_2_T) ? src2->rows : src2->cols; - assert(src1->cols == src2->rows); - assert(src1->rows == dst->rows); - assert(src2->cols == dst->cols); + assert(cols1 == rows2); + assert(rows1 == dst->rows); + assert(cols2 == dst->cols); + lapack_int lda = src1->cols; + lapack_int ldb = src2->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); + cblas_dgemm(CblasRowMajor, + (tABC & GEMM_1_T) ? CblasTrans : CblasNoTrans, + (tABC & GEMM_2_T) ? CblasTrans : CblasNoTrans, + dst->rows, + dst->cols, + cols1, + alpha, + src1->data.db, + lda, + src2->data.db, + ldb, + beta, + dst->data.db, + dst->cols); } SURVIVE_LOCAL_ONLY void cvMulTransposed(const CvMat *src, CvMat *dst, int order, const CvMat *delta, double scale) { @@ -296,10 +308,11 @@ SURVIVE_LOCAL_ONLY void cvTranspose(const CvMat *M, CvMat *dst) { if (inPlace) { tmp = cvCloneMat(dst); src = tmp->data.db; + } else { + assert(M->rows == dst->cols); + assert(M->cols == dst->rows); } - 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]; @@ -329,7 +342,7 @@ SURVIVE_LOCAL_ONLY void cvSVD(CvMat *aarr, CvMat *warr, CvMat *uarr, CvMat *varr 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); diff --git a/redist/minimal_opencvtest.c b/redist/minimal_opencvtest.c new file mode 100644 index 0000000..6320a27 --- /dev/null +++ b/redist/minimal_opencvtest.c @@ -0,0 +1,39 @@ +#include "minimal_opencv.h" +#include +#include +#include +#include + +void print_mat(const CvMat *M) {} + +void test_gemm() { + double _2x3[2*3] = {1, 2, 3, 4, 5, 6}; + CvMat m2x3 = cvMat(2, 3, CV_64F, _2x3); + + double _3x2[2*3] = {1, 2, 3, 4, 5, 6}; + CvMat m3x2 = cvMat(3, 2, CV_64F, _3x2); + + double _2x2[2*2] = {1, 2, 3, 4}; + CvMat m2x2 = cvMat(2, 2, CV_64F, _2x2); + + double _3x3[3*3] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; + CvMat m3x3 = cvMat(3, 3, CV_64F, _3x3); + + cvGEMM(&m2x3, &m3x2, 1, 0, 0, &m2x2, 0); + cvGEMM(&m3x2, &m2x3, 1, 0, 0, &m3x3, 0); + + cvGEMM(&m2x3, &m2x3, 1, 0, 0, &m3x3, GEMM_1_T); + cvGEMM(&m2x3, &m2x3, 1, 0, 0, &m2x2, GEMM_2_T); + + cvGEMM(&m2x3, &m3x2, 1, 0, 0, &m3x3, GEMM_1_T | GEMM_2_T); + // cvGEMM(&m3x2, &m2x3, 1, 0, 0, &m2x2, GEMM_1_T | GEMM_2_T); + +} + +int main() +{ + test_gemm(); + + return 0; +} + -- cgit v1.2.3 From 87b6229f6fc4e434b9472e6c7722e40497ac97a1 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 21 Jun 2018 10:30:11 -0600 Subject: Updated os_generic to work properly when imported into C++ context --- redist/os_generic.h | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/redist/os_generic.h b/redist/os_generic.h index 0d1c7e7..c0cb4f8 100644 --- a/redist/os_generic.h +++ b/redist/os_generic.h @@ -205,7 +205,7 @@ OSG_INLINE double OGGetFileTime(const char *file) { } OSG_INLINE og_thread_t OGCreateThread(void *(routine)(void *), void *parameter) { - pthread_t *ret = malloc(sizeof(pthread_t)); + pthread_t *ret = (pthread_t *)malloc(sizeof(pthread_t)); int r = pthread_create(ret, 0, routine, parameter); if (r) { free(ret); @@ -268,26 +268,30 @@ OSG_INLINE void OGDeleteMutex(og_mutex_t om) { } OSG_INLINE og_sema_t OGCreateSema() { - sem_t *sem = malloc(sizeof(sem_t)); + sem_t *sem = (sem_t *)malloc(sizeof(sem_t)); sem_init(sem, 0, 0); return (og_sema_t)sem; } OSG_INLINE int OGGetSema(og_sema_t os) { int valp; - sem_getvalue(os, &valp); + sem_getvalue((sem_t *)os, &valp); return valp; } -OSG_INLINE void OGLockSema(og_sema_t os) { sem_wait(os); } +OSG_INLINE void OGLockSema(og_sema_t os) { sem_wait((sem_t *)os); } -OSG_INLINE void OGUnlockSema(og_sema_t os) { sem_post(os); } +OSG_INLINE void OGUnlockSema(og_sema_t os) { sem_post((sem_t *)os); } OSG_INLINE void OGDeleteSema(og_sema_t os) { - sem_destroy(os); + sem_destroy((sem_t *)os); free(os); } #endif #endif + +#ifdef __cplusplus +} +#endif -- cgit v1.2.3