From a1027bceebb36afc4fa9d082277478abd8101eda Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 20 Apr 2018 22:31:18 -0600 Subject: Enabled and then cleaned up warnings --- Makefile | 14 +++---- calibrate.c | 6 --- include/libsurvive/poser.h | 1 - include/libsurvive/survive.h | 2 +- include/libsurvive/survive_imu.h | 2 - include/libsurvive/survive_types.h | 1 + redist/CNFGXDriver.c | 8 ++-- redist/json_helpers.c | 5 ++- redist/linmath.h | 2 +- redist/minimal_opencv.c | 5 --- redist/sba/sba_crsm.c | 43 -------------------- redist/sba/sba_levmar.c | 46 ++-------------------- redist/sba/sba_levmar_wrap.c | 8 ++-- src/poser_general_optimizer.c | 4 ++ src/poser_sba.c | 80 ++++++++++++-------------------------- src/survive.c | 4 +- src/survive_cal.c | 2 - src/survive_imu.c | 12 ------ src/survive_sensor_activations.c | 5 ++- src/survive_turveybiguator.c | 6 ++- src/survive_vive.c | 2 + test.c | 1 - tools/viz/survive_viewer.js | 2 +- 23 files changed, 65 insertions(+), 196 deletions(-) diff --git a/Makefile b/Makefile index 7a35b2a..3e460e4 100644 --- a/Makefile +++ b/Makefile @@ -8,6 +8,7 @@ all : $(STATIC_LIBRARY) $(LIBRARY) data_recorder test calibrate calibrate_client OBJDIR:=build +CFLAGS += -Wall -Wno-unused-variable -Wno-switch -Wno-parentheses -Wno-missing-braces ifdef WINDOWS CFLAGS+=-Iinclude/libsurvive -g -O3 -Iredist -DUSE_DOUBLE -std=gnu99 -MD -DNOZLIB -DWINDOWS -DWIN32 -DHIDAPI @@ -16,7 +17,7 @@ ifdef WINDOWS LIBSURVIVE_CORE:=redist/puff.c redist/crc32.c redist/hid-windows.c winbuild/getdelim.c CC:=i686-w64-mingw32-gcc else - CFLAGS+=-Iinclude/libsurvive -fPIC -g -O3 -Iredist -DUSE_DOUBLE -std=gnu99 -rdynamic -MD + CFLAGS+=-Iinclude/libsurvive -fPIC -g -O3 -Iredist -DUSE_DOUBLE -std=gnu99 -MD LDFLAGS+=-L/usr/local/lib -lpthread -lz -lm -g -llapacke -lcblas -lm -lusb-1.0 LDFLAGS_TOOLS+=-Llib -lsurvive -Wl,-rpath,lib -lX11 $(LDFLAGS) endif @@ -25,13 +26,13 @@ CC?=gcc ifdef EXTRA_WARNINGS - CFLAGS+=-fsanitize=address -fsanitize=undefined -Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable -Wno-pointer-sign -Wno-parentheses + CFLAGS+=-fsanitize=address -fsanitize=undefined 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 +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 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 POSERS:=src/poser_dummy.c src/poser_imu.c src/poser_charlesrefine.c @@ -48,12 +49,12 @@ ifeq ($(UNAME), Darwin) # Mac OSX CFLAGS:=$(CFLAGS) -DRASTERIZER -DHIDAPI -I/usr/local/include -x objective-c LDFLAGS:=$(LDFLAGS) -framework OpenGL -framework Cocoa -framework IOKit DRAWFUNCTIONS=redist/CNFGFunctions.c redist/CNFGCocoaNSImageDriver.m - GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGCocoaNSImageDriver.o + GRAPHICS_LOFI:=redist/CNFGFunctions.c redist/CNFGCocoaNSImageDriver.c REDISTS:=$(REDISTS) redist/hid-osx.c else # Linux / FreeBSD LDFLAGS:=$(LDFLAGS) DRAWFUNCTIONS=redist/CNFGFunctions.c redist/CNFGXDriver.c redist/CNFG3D.c - GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGXDriver.o + GRAPHICS_LOFI:=redist/CNFGFunctions.c redist/CNFGXDriver.c endif ifdef MINIMAL @@ -76,7 +77,6 @@ LIBSURVIVE_D:=$(LIBSURVIVE_C:%.c=$(OBJDIR)/%.d) #Include all dependencies so if header files change, it updates. -include $(LIBSURVIVE_D) - #### Tools testCocoa : testCocoa.c $(LIBRARY) @@ -134,7 +134,7 @@ $(OBJDIR): mkdir -p $(OBJDIR)/src/epnp $(LIBRARY): $(LIBSURVIVE_O) $(OBJDIR) - $(CC) -o $@ $(LIBSURVIVE_O) $(CFLAGS) $(LDFLAGS) -shared + $(CC) $(CFLAGS) -shared -o $@ $(LIBSURVIVE_O) $(LDFLAGS) $(STATIC_LIBRARY) : $(LIBSURVIVE_O) $(OBJDIR) ar rcs --plugin=$$(gcc --print-file-name=liblto_plugin.so) lib/libsurvive.a $(LIBSURVIVE_O) diff --git a/calibrate.c b/calibrate.c index e21c310..de7fd3b 100644 --- a/calibrate.c +++ b/calibrate.c @@ -170,12 +170,6 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution) CNFGColor((b << 16) | (g << 8) | r); - int x1, x2, y1, y2; - - x1 = (int)(windowCenterX - minRectSize - ((pose.Pos[2] * 40.0)) + (pose.Pos[0] * sizeScale)); - y1 = (int)(windowCenterY - minRectSize - ((pose.Pos[2] * 40.0)) + (pose.Pos[1] * sizeScale)); - x2 = (int)(windowCenterX + minRectSize + ((pose.Pos[2] * 40.0)) + (pose.Pos[0] * sizeScale)); - y2 = (int)(windowCenterY + minRectSize + ((pose.Pos[2] * 40.0)) + (pose.Pos[1] * sizeScale)); FLT xCenter = windowCenterX + (pose.Pos[0] * sizeScale); FLT yCenter = windowCenterY + (pose.Pos[1] * sizeScale); diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h index 4cddf89..e71b34f 100644 --- a/include/libsurvive/poser.h +++ b/include/libsurvive/poser.h @@ -88,7 +88,6 @@ typedef struct //If "lengths[...]" < 0, means not a valid piece of sweep information. FLT lengths[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; FLT angles [SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; //2 Axes (Angles in LH space) - FLT synctimes[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; PoserDataIMU lastimu; } PoserDataFullScene; diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h index 66993df..9f692e9 100644 --- a/include/libsurvive/survive.h +++ b/include/libsurvive/survive.h @@ -273,7 +273,7 @@ SURVIVE_EXPORT void survive_close(SurviveContext *ctx); SURVIVE_EXPORT SurviveObject *survive_get_so_by_name(SurviveContext *ctx, const char *name); // Utilitiy functions. -int survive_simple_inflate(SurviveContext *ctx, const char *input, int inlen, char *output, int outlen); +int survive_simple_inflate(SurviveContext *ctx, const uint8_t *input, int inlen, uint8_t *output, int outlen); SURVIVE_EXPORT int survive_send_magic(SurviveContext *ctx, int magic_code, void *data, int datalen); // These functions search both the stored-general and temporary sections for a parameter and return it. diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h index 4d03038..892df38 100644 --- a/include/libsurvive/survive_imu.h +++ b/include/libsurvive/survive_imu.h @@ -31,8 +31,6 @@ typedef struct { } SurviveIMUTracker; -void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose); - void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data); void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timecode, SurviveIMUTracker *tracker, SurvivePose *pose, const FLT *variance); diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h index 7fa5e0f..b4faaa7 100644 --- a/include/libsurvive/survive_types.h +++ b/include/libsurvive/survive_types.h @@ -47,6 +47,7 @@ typedef LinmathPose SurvivePose; #define BUTTON_EVENT_AXIS_CHANGED 3 typedef uint32_t survive_timecode; +survive_timecode survive_timecode_difference(survive_timecode most_recent, survive_timecode least_recent); typedef struct SurviveObject SurviveObject; typedef struct SurviveContext SurviveContext; diff --git a/redist/CNFGXDriver.c b/redist/CNFGXDriver.c index 91b7233..6037eff 100644 --- a/redist/CNFGXDriver.c +++ b/redist/CNFGXDriver.c @@ -215,10 +215,10 @@ void CNFGHandleInput() XEvent report; int bKeyDirection = 1; - int r; + while( XPending( CNFGDisplay ) ) { - r=XNextEvent( CNFGDisplay, &report ); + XNextEvent(CNFGDisplay, &report); bKeyDirection = 1; switch (report.type) @@ -264,7 +264,7 @@ void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) static int depth; static int lw, lh; static unsigned char * lbuffer; - int r, ls; + int r; if( !xi ) { @@ -283,8 +283,6 @@ void CNFGUpdateScreenWithBitmap( unsigned long * data, int w, int h ) lh = h; } - ls = lw * lh; - XPutImage(CNFGDisplay, CNFGWindow, CNFGWindowGC, xi, 0, 0, 0, 0, w, h ); } diff --git a/redist/json_helpers.c b/redist/json_helpers.c index ebacd2c..0741c84 100644 --- a/redist/json_helpers.c +++ b/redist/json_helpers.c @@ -120,8 +120,9 @@ char* load_file_to_mem(const char* path) { int len = ftell( f ); fseek( f, 0, SEEK_SET ); char * JSON_STRING = malloc( len + 1); - memset(JSON_STRING,0,len+1); - size_t i = fread( JSON_STRING, len, 1, f ); i = i; //Ignore return value. + memset(JSON_STRING,0,len+1); + size_t i = fread(JSON_STRING, len, 1, f); + (void)i; // Ignore return value. fclose( f ); return JSON_STRING; } diff --git a/redist/linmath.h b/redist/linmath.h index 78cac5c..aff46a3 100644 --- a/redist/linmath.h +++ b/redist/linmath.h @@ -14,7 +14,7 @@ extern "C" { #endif // Yes, I know it's kind of arbitrary. -#define DEFAULT_EPSILON 0.001 +#define DEFAULT_EPSILON 0.0000000001 // For printf #define PFTHREE(x) (x)[0], (x)[1], (x)[2] diff --git a/redist/minimal_opencv.c b/redist/minimal_opencv.c index 988dd0f..d569d96 100644 --- a/redist/minimal_opencv.c +++ b/redist/minimal_opencv.c @@ -116,11 +116,6 @@ static inline void *cvAlignPtr(const void *ptr, int align) { 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; -} - SURVIVE_LOCAL_ONLY void cvCreateData(CvMat *arr) { if (CV_IS_MAT_HDR_Z(arr)) { size_t step, total_size; diff --git a/redist/sba/sba_crsm.c b/redist/sba/sba_crsm.c index 9ba11f1..ea38042 100644 --- a/redist/sba/sba_crsm.c +++ b/redist/sba/sba_crsm.c @@ -22,9 +22,6 @@ #include "sba.h" -static void sba_crsm_print(struct sba_crsm *sm, FILE *fp); -static void sba_crsm_build(struct sba_crsm *sm, int *m, int nr, int nc); - /* allocate a sparse CRS matrix */ void sba_crsm_alloc(struct sba_crsm *sm, int nr, int nc, int nnz) { int msz; @@ -50,46 +47,6 @@ void sba_crsm_free(struct sba_crsm *sm) { sm->val = sm->colidx = sm->rowptr = NULL; } -static void sba_crsm_print(struct sba_crsm *sm, FILE *fp) { - register int i; - - fprintf(fp, "matrix is %dx%d, %d non-zeros\nval: ", sm->nr, sm->nc, sm->nnz); - for (i = 0; i < sm->nnz; ++i) - fprintf(fp, "%d ", sm->val[i]); - fprintf(fp, "\ncolidx: "); - for (i = 0; i < sm->nnz; ++i) - fprintf(fp, "%d ", sm->colidx[i]); - fprintf(fp, "\nrowptr: "); - for (i = 0; i <= sm->nr; ++i) - fprintf(fp, "%d ", sm->rowptr[i]); - fprintf(fp, "\n"); -} - -/* build a sparse CRS matrix from a dense one. intended to serve as an example for sm creation */ -static void sba_crsm_build(struct sba_crsm *sm, int *m, int nr, int nc) { - int nnz; - register int i, j, k; - - /* count nonzeros */ - for (i = nnz = 0; i < nr; ++i) - for (j = 0; j < nc; ++j) - if (m[i * nc + j] != 0) - ++nnz; - - sba_crsm_alloc(sm, nr, nc, nnz); - - /* fill up the sm structure */ - for (i = k = 0; i < nr; ++i) { - sm->rowptr[i] = k; - for (j = 0; j < nc; ++j) - if (m[i * nc + j] != 0) { - sm->val[k] = m[i * nc + j]; - sm->colidx[k++] = j; - } - } - sm->rowptr[nr] = nnz; -} - /* returns the index of the (i, j) element. No bounds checking! */ int sba_crsm_elmidx(struct sba_crsm *sm, int i, int j) { register int low, high, mid, diff; diff --git a/redist/sba/sba_levmar.c b/redist/sba/sba_levmar.c index 2f85f2a..e5c499f 100644 --- a/redist/sba/sba_levmar.c +++ b/redist/sba/sba_levmar.c @@ -99,46 +99,6 @@ static double sba_mean_repr_error(int n, int mnp, double *x, double *hx, struct return err / ((double)(nprojs)); } -/* print the solution in p using sba's text format. If cnp/pnp==0 only points/cameras are printed */ -static void sba_print_sol(int n, int m, double *p, int cnp, int pnp, double *x, int mnp, struct sba_crsm *idxij, - int *rcidxs, int *rcsubs) { - register int i, j, ii; - int nnz; - double *ptr; - - if (cnp) { - /* print camera parameters */ - for (j = 0; j < m; ++j) { - ptr = p + cnp * j; - for (ii = 0; ii < cnp; ++ii) - printf("%g ", ptr[ii]); - printf("\n"); - } - } - - if (pnp) { - /* 3D & 2D point parameters */ - printf("\n\n\n# X Y Z nframes frame0 x0 y0 frame1 x1 y1 ...\n"); - for (i = 0; i < n; ++i) { - ptr = p + cnp * m + i * pnp; - for (ii = 0; ii < pnp; ++ii) // print 3D coordinates - printf("%g ", ptr[ii]); - - nnz = sba_crsm_row_elmidxs(idxij, i, rcidxs, rcsubs); /* find nonzero x_ij, j=0...m-1 */ - printf("%d ", nnz); - - for (j = 0; j < nnz; ++j) { /* point i projecting on camera rcsubs[j] */ - ptr = x + idxij->val[rcidxs[j]] * mnp; - - printf("%d ", rcsubs[j]); - for (ii = 0; ii < mnp; ++ii) // print 2D coordinates - printf("%g ", ptr[ii]); - } - printf("\n"); - } - } -} - /* Compute e=x-y for two n-vectors x and y and return the squared L2 norm of e. * e can coincide with either x or y. * Uses loop unrolling and blocking to reduce bookkeeping overhead & pipeline @@ -623,7 +583,7 @@ int sba_motstr_levmar_x( * measurements vector x */ - double *pa, *pb, *ea, *eb, *dpa, *dpb; /* pointers into p, jac, eab and dp respectively */ + double *ea, *eb, *dpa, *dpb; /* pointers into p, jac, eab and dp respectively */ /* submatrices sizes */ int Asz, Bsz, ABsz, Usz, Vsz, Wsz, Ysz, esz, easz, ebsz, YWtsz, Wtdasz, Sblsz, covsz; @@ -793,8 +753,8 @@ int sba_motstr_levmar_x( jac = W + Wsz + ((Wsz > ABsz) ? nvis * (Wsz - ABsz) : 0); /* set up auxiliary pointers */ - pa = p; - pb = p + m * cnp; + // pa = p; + // pb = p + m * cnp; ea = eab; eb = eab + m * cnp; dpa = dp; diff --git a/redist/sba/sba_levmar_wrap.c b/redist/sba/sba_levmar_wrap.c index 159a040..50948ea 100644 --- a/redist/sba/sba_levmar_wrap.c +++ b/redist/sba/sba_levmar_wrap.c @@ -73,7 +73,7 @@ static void sba_motstr_Qs(double *p, struct sba_crsm *idxij, int *rcidxs, int *r register int i, j; int cnp, pnp, mnp; double *pa, *pb, *paj, *pbi, *pxij; - int n, m, nnz; + int m, nnz; struct wrap_motstr_data_ *wdata; void (*proj)(int j, int i, double *aj, double *bi, double *xij, void *proj_adata); void *proj_adata; @@ -85,7 +85,7 @@ static void sba_motstr_Qs(double *p, struct sba_crsm *idxij, int *rcidxs, int *r proj = wdata->proj; proj_adata = wdata->adata; - n = idxij->nr; + // n = idxij->nr; m = idxij->nc; pa = p; pb = p + m * cnp; @@ -117,7 +117,7 @@ static void sba_motstr_Qs_jac(double *p, struct sba_crsm *idxij, int *rcidxs, in register int i, j; int cnp, pnp, mnp; double *pa, *pb, *paj, *pbi, *pAij, *pBij; - int n, m, nnz, Asz, Bsz, ABsz, idx; + int m, nnz, Asz, Bsz, ABsz, idx; struct wrap_motstr_data_ *wdata; void (*projac)(int j, int i, double *aj, double *bi, double *Aij, double *Bij, void *projac_adata); void *projac_adata; @@ -129,7 +129,7 @@ static void sba_motstr_Qs_jac(double *p, struct sba_crsm *idxij, int *rcidxs, in projac = wdata->projac; projac_adata = wdata->adata; - n = idxij->nr; + // n = idxij->nr; m = idxij->nc; pa = p; pb = p + m * cnp; diff --git a/src/poser_general_optimizer.c b/src/poser_general_optimizer.c index ff82fd4..057ac3d 100644 --- a/src/poser_general_optimizer.c +++ b/src/poser_general_optimizer.c @@ -37,6 +37,10 @@ bool general_optimizer_data_record_success(GeneralOptimizerData *d, FLT error) { d->failures_to_reset_cntr = d->failures_to_reset; return true; } + + if (d->failures_to_reset_cntr > 0) + d->failures_to_reset_cntr--; + return false; } diff --git a/src/poser_sba.c b/src/poser_sba.c index 8c9e328..07d8f25 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -17,6 +17,7 @@ #include "string.h" #include "survive_cal.h" #include "survive_config.h" +#include "survive_kalman.h" #include "survive_reproject.h" typedef struct { @@ -26,12 +27,6 @@ typedef struct { SurvivePose camera_params[2]; } sba_context; -typedef struct { - sba_context hdr; - int acode; - int lh; -} sba_context_single_sweep; - typedef struct SBAData { GeneralOptimizerData opt; @@ -44,7 +39,9 @@ typedef struct SBAData { int use_jacobian_function; int required_meas; + survive_kpose_t kpose; SurviveIMUTracker tracker; + bool useIMU; struct { @@ -89,6 +86,8 @@ static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, Surviv size_t rtn = 0; SurviveObject *so = d->opt.so; + // fprintf(stderr, "#"); + for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { for (size_t lh = 0; lh < 2; lh++) { if (SurviveSensorActivations_isPairValid(scene, d->sensor_time_window, pdl->timecode, sensor, lh)) { @@ -96,24 +95,26 @@ static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, Surviv // FLT a[2]; // survive_apply_bsd_calibration(so->ctx, lh, _a, a); vmask[sensor * NUM_LIGHTHOUSES + lh] = 1; - if (cov) { *(cov++) = d->sensor_variance + - abs(pdl->timecode - scene->timecode[sensor][lh][0]) * d->sensor_variance_per_second / - (double)so->timebase_hz; + abs((int32_t)pdl->timecode - (int32_t)scene->timecode[sensor][lh][0]) * + d->sensor_variance_per_second / (double)so->timebase_hz; *(cov++) = 0; *(cov++) = 0; *(cov++) = d->sensor_variance + - abs(pdl->timecode - scene->timecode[sensor][lh][1]) * d->sensor_variance_per_second / - (double)so->timebase_hz; + abs((int32_t)pdl->timecode - (int32_t)scene->timecode[sensor][lh][1]) * + d->sensor_variance_per_second / (double)so->timebase_hz; } meas[rtn++] = a[0]; meas[rtn++] = a[1]; + // fprintf(stderr, "%.04f %.04f ", a[0], a[1]); } else { vmask[sensor * NUM_LIGHTHOUSES + lh] = 0; + // fprintf(stderr, "%.06f %.06f ", sensor, lh, -2,-2); } } } + // fprintf(stderr, "\n"); return rtn; } @@ -126,43 +127,6 @@ void sba_set_cameras(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, S ctx->obj_pose = LinmathPose_Identity; } -typedef struct { - bool hasInfo; - SurvivePose poses; -} sba_set_position_t; - -static void sba_set_position(SurviveObject *so, uint32_t timecode, SurvivePose *new_pose, void *_user) { - sba_set_position_t *user = _user; - assert(user->hasInfo == false); - user->hasInfo = 1; - user->poses = *new_pose; -} -void *GetDriver(const char *name); - -static void str_metric_function_single_sweep(int j, int i, double *bi, double *xij, void *adata) { - SurvivePose obj = *(SurvivePose *)bi; - int sensor_idx = j >> 1; - - sba_context_single_sweep *ctx = (sba_context_single_sweep *)(adata); - SurviveObject *so = ctx->hdr.so; - int lh = ctx->lh; - int acode = ctx->acode; - - assert(lh < 2); - assert(sensor_idx < so->sensor_ct); - - quatnormalize(obj.Rot, obj.Rot); - FLT xyz[3]; - ApplyPoseToPoint(xyz, &obj, &so->sensor_locations[sensor_idx * 3]); - - // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; - SurvivePose *camera = &so->ctx->bsd[lh].Pose; - - FLT out[2]; - survive_reproject_from_pose(so->ctx, lh, camera, xyz, out); - *xij = out[acode]; -} - static void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { SurvivePose obj = *(SurvivePose *)bi; int sensor_idx = j >> 1; @@ -174,7 +138,7 @@ static void str_metric_function(int j, int i, double *bi, double *xij, void *ada assert(lh < 2); assert(sensor_idx < so->sensor_ct); - // quatnormalize(obj.Rot, obj.Rot); + quatnormalize(obj.Rot, obj.Rot); // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; SurvivePose *camera = &so->ctx->bsd[lh].Pose; @@ -193,7 +157,7 @@ static void str_metric_function_jac(int j, int i, double *bi, double *xij, void assert(lh < 2); assert(sensor_idx < so->sensor_ct); - // quatnormalize(obj.Rot, obj.Rot); + quatnormalize(obj.Rot, obj.Rot); SurvivePose *camera = &so->ctx->bsd[lh].Pose; survive_reproject_full_jac_obj_pose(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh], @@ -208,8 +172,9 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); - double *cov = - d->sensor_variance_per_second > 0. ? alloca(sizeof(double) * 2 * 2 * so->sensor_ct * NUM_LIGHTHOUSES) : 0; + double *cov = (d->sensor_variance_per_second > 0. && d->sensor_variance) + ? alloca(sizeof(double) * 2 * 2 * so->sensor_ct * NUM_LIGHTHOUSES) + : 0; size_t meas_size = construct_input_from_scene(d, pdl, scene, vmask, meas, cov); static int failure_count = 500; @@ -271,9 +236,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive quatnormalize(soLocation.Rot, soLocation.Rot); *out = soLocation; rtn = info[1] / meas_size * 2; - } else - - { + } else { SurviveContext *ctx = so->ctx; // Docs say info[0] should be divided by meas; I don't buy it really... if (error_failure) { @@ -418,6 +381,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { if (error < 0) { } else { + quatnormalize(estimate.Rot, estimate.Rot); + if (d->useIMU) { FLT var_meters = 0.5; FLT var_quat = error + .05; @@ -428,6 +393,11 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { estimate = d->tracker.pose; } + LinmathVec3d pvar = {.1, .1, .1}; + FLT rvar = .01; + survive_kpose_integrate_pose(&d->kpose, lightData->timecode, &estimate, pvar, rvar); + estimate = d->kpose.state.pose; + PoserData_poser_pose_func(&lightData->hdr, so, &estimate); } } diff --git a/src/survive.c b/src/survive.c index 58566c1..ce17295 100644 --- a/src/survive.c +++ b/src/survive.c @@ -581,7 +581,7 @@ int survive_simple_inflate(struct SurviveContext *ctx, const char *input, int in #include -int survive_simple_inflate(struct SurviveContext *ctx, const char *input, int inlen, char *output, int outlen) { +int survive_simple_inflate(struct SurviveContext *ctx, const uint8_t *input, int inlen, uint8_t *output, int outlen) { z_stream zs; // Zlib stream. May only be used by configuration at beginning and by USB thread periodically. memset(&zs, 0, sizeof(zs)); inflateInit(&zs); /// Consider checking error @@ -614,4 +614,4 @@ const SurvivePose *survive_object_pose(SurviveObject *so) { return &so->OutPose; int8_t survive_object_sensor_ct(SurviveObject *so) { return so->sensor_ct; } const FLT *survive_object_sensor_locations(SurviveObject *so) { return so->sensor_locations; } -const FLT *survive_object_sensor_normals(SurviveObject *so) { return so->sensor_normals; } \ No newline at end of file +const FLT *survive_object_sensor_normals(SurviveObject *so) { return so->sensor_normals; } diff --git a/src/survive_cal.c b/src/survive_cal.c index faf2ac7..ad62d22 100755 --- a/src/survive_cal.c +++ b/src/survive_cal.c @@ -417,7 +417,6 @@ static void handle_calibration( struct SurviveCalData *cd ) mkdir( "calinfo", 0755 ); #endif int sen, axis, lh; - FLT temp_syncs[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES]; //Just to get it out of the way early, we'll calculate the sync-pulse-lengths here. FILE * sync_time_info = fopen( "calinfo/synctime.csv", "w" ); @@ -620,7 +619,6 @@ static void handle_calibration( struct SurviveCalData *cd ) fsd.lengths[i][j][1] = cd->avglens[dataindex+1]; fsd.angles[i][j][0] = cd->avgsweeps[dataindex + 0]; fsd.angles[i][j][1] = cd->avgsweeps[dataindex + 1]; - fsd.synctimes[i][j] = temp_syncs[i][j]; } int r = cd->ConfigPoserFn( cd->poseobjects[obj], (PoserData*)&fsd ); diff --git a/src/survive_imu.c b/src/survive_imu.c index 3622b2d..f866caf 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -66,18 +66,6 @@ static inline uint32_t tick_difference(uint32_t most_recent, uint32_t least_rece return diff; } -void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose) { - tracker->pose = *pose; - - for (int i = 0; i < 3; i++) { - tracker->current_velocity[i] = 0; - } - //(pose->Pos[i] - tracker->lastGT.Pos[i]) / tick_difference(timecode, tracker->lastGTTime) * 48000000.; - - tracker->lastGTTime = timecode; - tracker->lastGT = *pose; -} - static const int imu_calibration_iterations = 100; static void RotateAccel(LinmathVec3d rAcc, const SurvivePose *pose, const LinmathVec3d accel) { diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c index dc5c0d4..e83b2fc 100644 --- a/src/survive_sensor_activations.c +++ b/src/survive_sensor_activations.c @@ -4,6 +4,9 @@ bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, uint32_t tolerance, uint32_t timecode_now, uint32_t idx, int lh) { const uint32_t *data_timecode = self->timecode[idx][lh]; + if (self->lengths[idx][lh][0] == 0 || self->lengths[idx][lh][1] == 0) + return false; + return !(timecode_now - data_timecode[0] > tolerance || timecode_now - data_timecode[1] > tolerance); } @@ -26,7 +29,7 @@ void SurviveSensorActivations_add(SurviveSensorActivations *self, struct PoserDa *angle = lightData->angle; *data_timecode = lightData->timecode; - *length = lightData->length * 48000000; + *length = (uint32_t)(lightData->length * 48000000); } uint32_t SurviveSensorActivations_default_tolerance = (uint32_t)(48000000 /*mhz*/ * (16.7 * 2 /*ms*/) / 1000) + 5000; diff --git a/src/survive_turveybiguator.c b/src/survive_turveybiguator.c index 8dbce46..5b10e63 100644 --- a/src/survive_turveybiguator.c +++ b/src/survive_turveybiguator.c @@ -155,8 +155,8 @@ static uint8_t remove_outliers(SurviveObject *so) { // uint8_t removed_outliers = 0; - uint32_t d1 = abs(*min - mean); - uint32_t d2 = abs(*max - mean); + uint32_t d1 = mean - *min; + uint32_t d2 = *max - mean; if (d1 > d2) { if (d1 > tau_test) { @@ -199,6 +199,7 @@ static void handle_lightcap2_process_sweep_data(SurviveObject *so) { { unsigned int longest_pulse = 0; unsigned int timestamp_of_longest_pulse = 0; + (void)timestamp_of_longest_pulse; for (int i = 0; i < SENSORS_PER_OBJECT; i++) { if (lcd->sweep.sweep_len[i] > longest_pulse) { longest_pulse = lcd->sweep.sweep_len[i]; @@ -216,6 +217,7 @@ static void handle_lightcap2_process_sweep_data(SurviveObject *so) { { static int counts[SENSORS_PER_OBJECT][2] = {0}; + (void)counts; // if (lcd->per_sweep.activeLighthouse == 0 && !allZero) if (lcd->per_sweep.activeLighthouse > -1 && !allZero) { diff --git a/src/survive_vive.c b/src/survive_vive.c index d9cbc3e..88bc065 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -961,6 +961,8 @@ void calibrate_acc(SurviveObject* so, FLT* agm) { agm[1] *= so->acc_scale[1]; agm[2] *= so->acc_scale[2]; } + + fprintf(stderr, "%f\t%f\t%f\n", agm[0], agm[1], agm[2]); } void calibrate_gyro(SurviveObject* so, FLT* agm) { diff --git a/test.c b/test.c index 9fda5b1..7785b37 100644 --- a/test.c +++ b/test.c @@ -164,7 +164,6 @@ int main( int argc, char ** argv ) dump_iface( survive_get_so_by_name( ctx, "TR0" ), "TR0" ); dump_iface( survive_get_so_by_name( ctx, "WW0" ), "WW0" ); survive_startup(ctx); - survive_cal_install(ctx); while(survive_poll(ctx) == 0) { diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js index 26f77a6..56ef47f 100644 --- a/tools/viz/survive_viewer.js +++ b/tools/viz/survive_viewer.js @@ -143,7 +143,7 @@ function redrawCanvas(when) { ang[0][1] < when[key] - visible_tolerance) continue; - var half_fov = 1.0472 * 2; + var half_fov = 1.0472 * 2 / 5.; 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