aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorcnlohr <lohr85@gmail.com>2018-06-23 12:11:33 -0400
committercnlohr <lohr85@gmail.com>2018-06-23 12:11:33 -0400
commit9291b950c5508513ea261059229e19760c050a47 (patch)
tree2cb0217e67896382c3bc395ad91d438c779a32cf
parent86bbd12c3889db3290e22ff61934ca1a218ce114 (diff)
parent87b6229f6fc4e434b9472e6c7722e40497ac97a1 (diff)
downloadlibsurvive-9291b950c5508513ea261059229e19760c050a47.tar.gz
libsurvive-9291b950c5508513ea261059229e19760c050a47.tar.bz2
Merge branch 'master' into new_config_system
-rw-r--r--Makefile2
-rw-r--r--redist/Makefile4
-rw-r--r--redist/minimal_opencv.c43
-rw-r--r--redist/minimal_opencvtest.c39
-rw-r--r--redist/os_generic.h16
-rw-r--r--src/ootx_decoder.c9
-rw-r--r--src/ootx_decoder.h1
-rwxr-xr-xsrc/survive_cal.c7
-rw-r--r--src/survive_playback.c17
-rw-r--r--tools/viz/survive_viewer.js7
10 files changed, 113 insertions, 32 deletions
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
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 <assert.h>
+#include <math.h>
+#include <stdbool.h>
+#include <stdio.h>
+
+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;
+}
+
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
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 cf65166..f270065 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -50,6 +50,11 @@ STATIC_CONFIG_ITEM( CONFIG_POSER, "configposer", 's', "Poser used for calibratio
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;
@@ -196,7 +201,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 df922e2..ebf7e3f 100644
--- a/src/survive_playback.c
+++ b/src/survive_playback.c
@@ -167,8 +167,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 {
@@ -186,16 +187,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 56ef47f..075e56a 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 = [];
@@ -143,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;
@@ -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);
}