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 --- 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 + 8 files changed, 41 insertions(+), 74 deletions(-) (limited to 'src') 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) { -- cgit v1.2.3