From 55f498db296ff353a0cf870c51bffd92cc360484 Mon Sep 17 00:00:00 2001 From: cnlohr Date: Fri, 6 Apr 2018 19:39:27 -0400 Subject: Unify the location of the updates to the IMU to use G's and rads/sec --- src/survive_default_devices.c | 42 ++++++++++++++++++++++++++++++++++++++---- src/survive_vive.c | 29 +---------------------------- 2 files changed, 39 insertions(+), 32 deletions(-) (limited to 'src') diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c index 2e47b9e..3c102b2 100644 --- a/src/survive_default_devices.c +++ b/src/survive_default_devices.c @@ -144,10 +144,6 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) { FLT *values = NULL; if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) { so->acc_bias = values; - const FLT bias_units = 1. / 1000.; // I deeply suspect bias is in milligravities -JB - so->acc_bias[0] *= bias_units; - so->acc_bias[1] *= bias_units; - so->acc_bias[2] *= bias_units; } } if (jsoneq(ct0conf, tk, "acc_scale") == 0) { @@ -174,6 +170,44 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) { } } + + //Handle device-specific sacling. + if( strcmp( so->codename, "HMD" ) == 0 ) + { + if( so->acc_scale ) + { + so->acc_scale[0] *= -1./8192.0; + so->acc_scale[1] *= -1./8192.0; + so->acc_scale[2] *= 1./8192.0; + } + if( so->acc_bias ) scale3d( so->acc_bias, so->acc_bias, 2./1000. ); //Odd but seems right. + if( so->gyro_scale ) + { + so->gyro_scale[0] *= -0.000065665; + so->gyro_scale[1] *= -0.000065665; + so->gyro_scale[2] *= 0.000065665; + } + } + else if( memcmp( so->codename, "WM", 2 ) == 0 ) + { + //??!!?? No one has yet decoded the watchman accelerometer data. + } + else //Verified on WW, Need to verify on Tracker. + { + //1G for accelerometer, from MPU6500 datasheet + //this can change if the firmware changes the sensitivity. + // When coming off of USB, these values are in units of .5g -JB + if( so->acc_scale ) scale3d( so->acc_scale, so->acc_scale, 2./8192.0 ); + + //If any other device, we know we at least need this. + // I deeply suspect bias is in milligravities -JB + if( so->acc_bias ) scale3d( so->acc_bias, so->acc_bias, 1./1000. ); + + // From datasheet, can be 250, 500, 1000, 2000 deg/s range over 16 bits + // FLT deg_per_sec = 250; + if( so->gyro_scale )scale3d( so->gyro_scale, so->gyro_scale, 3.14159 / 1800. ); + } + char fname[64]; sprintf(fname, "calinfo/%s_points.csv", so->codename); diff --git a/src/survive_vive.c b/src/survive_vive.c index 1ffb737..60b3dc9 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1239,20 +1239,9 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) 0,0,0 }; // if (w->acc_scale) printf("%f %f %f\n",w->acc_scale[0],w->acc_scale[1],w->acc_scale[2]); - calibrate_acc(w, agm); - //I don't understand where these numbers come from but the data from the WMD seems to max out at 255... - agm[0]*=(1.0f/255.0f); - agm[1]*=(1.0f/255.0f); - agm[2]*=(1.0f/255.0f); - + calibrate_acc(w, agm); calibrate_gyro(w, agm+3); - - //I don't understand where these numbers come from but the data from the WMD seems to max out at 255... - agm[3]*=(1.0f/255.0f); - agm[4]*=(1.0f/255.0f); - agm[5]*=(1.0f/255.0f); - w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 ); int16_t * k = (int16_t *)readdata+1; @@ -1495,24 +1484,8 @@ void survive_data_cb( SurviveUSBInterface * si ) 0, 0}; - //1G for accelerometer, from MPU6500 datasheet - //this can change if the firmware changes the sensitivity. - // When coming off of USB, these values are in units of .5g -JB - agm[0] *= (float)(2. / 8192.0); - agm[1] *= (float)(2. / 8192.0); - agm[2] *= (float)(2. / 8192.0); calibrate_acc(obj, agm); - - // From datasheet, can be 250, 500, 1000, 2000 deg/s range over 16 bits - // FLT deg_per_sec = 250; - // FLT conv = (float)((1./deg_per_sec)*(3.14159/180.)) / 8192.; - FLT DEGREES_TO_RADS = 3.14159 / 180.; - FLT conv = 1. / 10. * DEGREES_TO_RADS; calibrate_gyro(obj, agm + 3); - agm[3] *= conv; - agm[4] *= conv; - agm[5] *= conv; - ctx->imuproc( obj, 3, agm, timecode, code ); } } -- cgit v1.2.3 From 9dc7a7ed589db16cc040c7eeeae0343977f3f885 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 7 Apr 2018 07:35:41 -0600 Subject: Added proper initialization to imu tracker --- src/survive_imu.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/survive_imu.c b/src/survive_imu.c index 36d1aeb..8f4266a 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -174,7 +174,7 @@ static void iterate_velocity(LinmathVec3d result, SurviveIMUTracker *tracker, do } void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data) { - if (tracker->last_data.timecode == 0) { + if (!tracker->is_initialized) { tracker->pose.Rot[0] = 1.; if (tracker->last_data.datamask == imu_calibration_iterations) { tracker->last_data = *data; @@ -182,7 +182,7 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker const FLT up[3] = {0, 0, 1}; quatfrom2vectors(tracker->pose.Rot, tracker->updir, up); tracker->accel_scale_bias = 1. / magnitude3d(tracker->updir); - + tracker->is_initialized = true; return; } @@ -233,6 +233,11 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timecode, SurviveIMUTracker *tracker, SurvivePose *pose, const FLT *R) { + if (!tracker->is_initialized) { + tracker->pose = *pose; + return; + } + // Kalman filter assuming: // F -> Identity // H -> Identity -- cgit v1.2.3 From 5d43c7f5541b91f29aa3138a8a460f48198e11d0 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 7 Apr 2018 07:36:23 -0600 Subject: Fixed terrible bug in SBA which slowed it way down --- src/poser_sba.c | 37 +++++++++++++++++++------------------ 1 file changed, 19 insertions(+), 18 deletions(-) (limited to 'src') diff --git a/src/poser_sba.c b/src/poser_sba.c index fcf4f2e..23e03fc 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -399,7 +399,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->failures_to_reset_cntr = 0; d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1); d->successes_to_reset_cntr = 0; - d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 100); + d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, -1); d->useIMU = survive_configi(ctx, "sba-use-imu", SC_GET, 1); d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8); d->max_error = survive_configf(ctx, "sba-max-error", SC_GET, .0001); @@ -435,27 +435,28 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->last_lh = lightData->lh; d->last_acode = lightData->acode; - } - if (error < 0) { - if (d->failures_to_reset_cntr > 0) - d->failures_to_reset_cntr--; - } else { - if (d->useIMU) { - FLT var_meters = 0.5; - FLT var_quat = error + .05; - FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat, - error * var_quat, error * var_quat, error * var_quat}; - - survive_imu_tracker_integrate_observation(so, lightData->timecode, &d->tracker, &estimate, var); - estimate = d->tracker.pose; + + if (error < 0) { + if (d->failures_to_reset_cntr > 0) + d->failures_to_reset_cntr--; } + else { + if (d->useIMU) { + FLT var_meters = 0.5; + FLT var_quat = error + .05; + FLT var[7] = { error * var_meters, error * var_meters, error * var_meters, error * var_quat, + error * var_quat, error * var_quat, error * var_quat }; + + survive_imu_tracker_integrate_observation(so, lightData->timecode, &d->tracker, &estimate, var); + estimate = d->tracker.pose; + } - PoserData_poser_pose_func(&lightData->hdr, so, &estimate); - if (d->successes_to_reset_cntr > 0) - d->successes_to_reset_cntr--; + PoserData_poser_pose_func(&lightData->hdr, so, &estimate); + if (d->successes_to_reset_cntr > 0) + d->successes_to_reset_cntr--; + } } - return 0; } case POSERDATA_FULL_SCENE: { -- cgit v1.2.3 From b792a65dbfbb304927400ca6c754195d92089b61 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 7 Apr 2018 08:46:50 -0600 Subject: Fixed 32 bit linker issue --- src/survive.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/survive.c b/src/survive.c index b024bbb..9e750f9 100644 --- a/src/survive.c +++ b/src/survive.c @@ -106,7 +106,7 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { static int did_manual_driver_registration = 0; if (did_manual_driver_registration == 0) { #define MANUAL_DRIVER_REGISTRATION(func) \ - int func(SurviveObject *so, PoserData *pd); \ + int func(SurviveObject *so, PoserData *pd); \ RegisterDriver(#func, &func); MANUAL_DRIVER_REGISTRATION(PoserCharlesSlow) -- cgit v1.2.3 From 795f7802faa8d612e13d5853a0da98ad96a2746e Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sat, 7 Apr 2018 12:40:59 -0400 Subject: BOX FILTERS FTW!!! Temporarily remove use of survive_IMU but we should be able to add that back in. --- src/poser_charlesrefine.c | 134 +++++++++++++++++++++++++++++++++++----------- 1 file changed, 102 insertions(+), 32 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 52a5f54..cbc47c8 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -13,7 +13,7 @@ #include #include -#define MAX_PT_PER_SWEEP 32 +#define MAX_PT_PER_SWEEP SENSORS_PER_OBJECT typedef struct { int sweepaxis; @@ -23,6 +23,11 @@ typedef struct { FLT angles_at_pts[MAX_PT_PER_SWEEP]; SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP]; uint8_t sensor_ids[MAX_PT_PER_SWEEP]; + + LinmathPoint3d MixingPositions[NUM_LIGHTHOUSES][2]; + SurvivePose InteralPoseUsedForCalc; //Super high speed vibratey and terrible. + float MixingConfidence[NUM_LIGHTHOUSES][2]; + int ptsweep; SurviveIMUTracker tracker; @@ -31,16 +36,37 @@ typedef struct { int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { CharlesPoserData *dd = so->PoserData; if (!dd) + { so->PoserData = dd = calloc(sizeof(CharlesPoserData), 1); + SurvivePose object_pose_out; + memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); + so->PoseConfidence = 1.0; + PoserData_poser_pose_func(pd, so, &object_pose_out); + } SurviveSensorActivations *scene = &so->activations; switch (pd->pt) { case POSERDATA_IMU: { // Really should use this... - PoserDataIMU *imu = (PoserDataIMU *)pd; - - survive_imu_tracker_integrate(so, &dd->tracker, imu); - PoserData_poser_pose_func(pd, so, &dd->tracker.pose); + PoserDataIMU *imuData = (PoserDataIMU *)pd; + + //TODO: Actually do Madgwick's algorithm + LinmathQuat applymotion; + const SurvivePose * object_pose = &so->OutPose; + imuData->gyro[0] *= 1./240.; + imuData->gyro[1] *= 1./240.; + imuData->gyro[2] *= 1./240.; + quatfromeuler( applymotion, imuData->gyro ); + //printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); + SurvivePose object_pose_out; + quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion ); + copy3d( object_pose_out.Pos, object_pose->Pos ); + PoserData_poser_pose_func(pd, so, &object_pose_out); + quatcopy( dd->InteralPoseUsedForCalc.Rot, object_pose_out.Rot); + + //PoserDataIMU *imu = (PoserDataIMU *)pd; + //survive_imu_tracker_integrate(so, &dd->tracker, imu); + //PoserData_poser_pose_func(pd, so, &dd->tracker.pose); return 0; } @@ -56,7 +82,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { FLT angle = ld->angle; int sensor_id = ld->sensor_id; int axis = dd->sweepaxis; - const SurvivePose *object_pose = &so->OutPose; + //const SurvivePose *object_pose = &so->OutPose; + dd->sweeplh = lhid; // FOR NOW, drop LH1. @@ -70,7 +97,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { FLT sensor_position_worldspace[3]; // XXX Once I saw this get pretty wild (When in playback) // I had to invert the values of sensor_inpos. Not sure why. - ApplyPoseToPoint(sensor_position_worldspace, object_pose, sensor_inpos); + ApplyPoseToPoint(sensor_position_worldspace, &dd->InteralPoseUsedForCalc, sensor_inpos); // printf( "%f %f %f == > %f %f %f\n", sensor_inpos[0], sensor_inpos[1], sensor_inpos[2], // sensor_position_worldspace[0], sensor_position_worldspace[1], sensor_position_worldspace[2] ); @@ -109,7 +136,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { dd->quantity_errors[i] = dist; dd->angles_at_pts[i] = angle; dd->sensor_ids[i] = sensor_id; - memcpy(&dd->object_pose_at_hit[i], object_pose, sizeof(SurvivePose)); + memcpy(&dd->object_pose_at_hit[i], &dd->InteralPoseUsedForCalc, sizeof(SurvivePose)); dd->ptsweep++; } @@ -126,8 +153,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { int lhid = dd->sweeplh; int axis = dd->sweepaxis; int pts = dd->ptsweep; - const SurvivePose *object_pose = - &so->OutPose; // XXX TODO Should pull pose from approximate time when LHs were scanning it. + //const SurvivePose *object_pose = + // &so->OutPose; // XXX TODO Should pull pose from approximate time when LHs were scanning it. BaseStationData *bsd = &so->ctx->bsd[lhid]; SurvivePose *lh_pose = &bsd->Pose; @@ -143,11 +170,11 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { #define HIT_QUALITY_BASELINE \ 0.0001 // Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm -#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.8 // Explodes if you exceed 1.0 -#define CORRECT_TELESCOPTION_COEFFICIENT 8.0 // Converges even as high as 10.0 and doesn't explode. +#define CORRECT_LATERAL_POSITION_COEFFICIENT 0.7 // Explodes if you exceed 1.0 +#define CORRECT_TELESCOPTION_COEFFICIENT 7.00 // Converges even as high as 10.0 and doesn't explode. #define CORRECT_ROTATION_COEFFICIENT \ 1.0 // This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. -#define ROTATIONAL_CORRECTION_MAXFORCE 0.10 +#define ROTATIONAL_CORRECTION_MAXFORCE 0.01 // Step 1: Determine standard of deviation, and average in order to // drop points that are likely in error. @@ -212,6 +239,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { add3d(vec_correct, vec_correct, avg_err); } + + // Step 3: Control telecoption from lighthouse. // we need to find out what the weighting is to determine "zoom" if (validpoints > 1) // Can't correct "zoom" with only one point. @@ -238,16 +267,36 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { zoom *= CORRECT_TELESCOPTION_COEFFICIENT; FLT veccamalong[3]; - sub3d(veccamalong, lh_pose->Pos, object_pose->Pos); + sub3d(veccamalong, lh_pose->Pos, dd->InteralPoseUsedForCalc.Pos); normalize3d(veccamalong, veccamalong); scale3d(veccamalong, veccamalong, zoom); add3d(vec_correct, veccamalong, vec_correct); } - SurvivePose object_pose_out; - add3d(object_pose_out.Pos, vec_correct, object_pose->Pos); - quatcopy(object_pose_out.Rot, object_pose->Rot); +#if 0 + LinmathPoint3d LastDelta; + sub3d( LastDelta, dd->MixingPositions[lhid][axis], dd->InteralPoseUsedForCalc.Pos ); + //Compare with "vec_correct" + + LinmathPoint3d DeltaDelta; + sub3d( DeltaDelta, vec_correct, LastDelta ); + + + //SurvivePose object_pose_out; + + memcpy( dd->MixingPositions[lhid][axis], vec_correct, sizeof( vec_correct ) ); + + LinmathPoint3d system_average_adjust = { 0, 0, 0 }; + + printf( "%f %f %f + %f %f %f\n", vec_correct[0], vec_correct[1], vec_correct[2], dd->InteralPoseUsedForCalc.Pos[0], dd->InteralPoseUsedForCalc.Pos[1], dd->InteralPoseUsedForCalc.Pos[2] ); + +#endif + add3d(dd->InteralPoseUsedForCalc.Pos, vec_correct, dd->InteralPoseUsedForCalc.Pos); + + + + //quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot); // Stage 4: "Tug" on the rotation of the object, from all of the sensor's pov. // If we were able to determine likliehood of a hit in the sweep instead of afterward @@ -308,25 +357,46 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { } // printf( "Applying: %f %f %f %f\n", correction[0], correction[1], correction[2], correction[3] ); // Apply our corrective quaternion to the output. - quatrotateabout(object_pose_out.Rot, object_pose_out.Rot, correction); - quatnormalize(object_pose_out.Rot, object_pose_out.Rot); + quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, correction); + quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot); } - // Janky need to do this somewhere else... This initializes the pose estimator. - if (so->PoseConfidence < .01) { - memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); - so->PoseConfidence = 1.0; - } + memcpy( dd->MixingPositions[lhid][axis], dd->InteralPoseUsedForCalc.Pos, sizeof( dd->InteralPoseUsedForCalc.Pos ) ); + dd->MixingConfidence[lhid][axis] = (validpoints)?((validpoints>1)?1.0:0.5):0; - // PoserData_poser_pose_func(pd, so, &object_pose_out); - FLT var_meters = 0.5; - FLT error = 0.0001; - FLT var_quat = error + .05; - FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat, - error * var_quat, error * var_quat, error * var_quat}; - survive_imu_tracker_integrate_observation(so, l->timecode, &dd->tracker, &object_pose_out, var); - PoserData_poser_pose_func(pd, so, &dd->tracker.pose); + //Box filter all of the guesstimations, and emit the new guess. + { + FLT MixedAmount = 0; + LinmathPoint3d MixedPosition = { 0, 0, 0 }; + int l = 0, a = 0; + if( lhid == 0 && axis == 0 ) for( l = 0; l < NUM_LIGHTHOUSES; l++ ) for( a = 0; a < 2; a++ ) dd->MixingConfidence[l][a] -= 0.1; + + for( l = 0; l < NUM_LIGHTHOUSES; l++ ) for( a = 0; a < 2; a++ ) + { + LinmathPoint3d MixThis = { 0, 0, 0 }; + FLT Confidence = dd->MixingConfidence[l][a]; + if(Confidence < 0 ) Confidence = 0; + scale3d( MixThis, dd->MixingPositions[l][a], Confidence ); + add3d( MixedPosition, MixedPosition, MixThis ); + MixedAmount += Confidence; + //printf( "%f ", Confidence ); + } + scale3d( MixedPosition, MixedPosition, 1./MixedAmount ); + //printf( "%f\n", MixedAmount ); + SurvivePose object_pose_out; + quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot ); + copy3d( object_pose_out.Pos, MixedPosition ); + PoserData_poser_pose_func(pd, so, &object_pose_out); + } + // FLT var_meters = 0.5; + // FLT error = 0.00001; + // FLT var_quat = error + .05; + // FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat, + // error * var_quat, error * var_quat, error * var_quat}; + // + // survive_imu_tracker_integrate_observation(so, l->timecode, &dd->tracker, &object_pose_out, var); + // PoserData_poser_pose_func(pd, so, &dd->tracker.pose); dd->ptsweep = 0; } -- cgit v1.2.3 From 6445b2067a0f0dead657a0b54c0765b4edb4d5b8 Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Sat, 7 Apr 2018 18:33:30 +0000 Subject: apply a small drag from the accelerometer to correct "up" vector. --- src/poser_charlesrefine.c | 26 +++++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index cbc47c8..ce14f70 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -40,6 +40,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { so->PoserData = dd = calloc(sizeof(CharlesPoserData), 1); SurvivePose object_pose_out; memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); + memcpy(&dd->InteralPoseUsedForCalc, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); so->PoseConfidence = 1.0; PoserData_poser_pose_func(pd, so, &object_pose_out); } @@ -58,8 +59,31 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { imuData->gyro[2] *= 1./240.; quatfromeuler( applymotion, imuData->gyro ); //printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); + + LinmathQuat InvertQuat; + quatgetreciprocal(InvertQuat, object_pose->Rot); + + //Apply a tiny tug to re-right headset based on the gravity vector. + LinmathVec3d reright = { 0, 0, 1 }; + LinmathVec3d normup; + normalize3d( normup, imuData->accel ); + LinmathVec3d correct_diff; + quatrotatevector(reright, InvertQuat, reright); + sub3d( correct_diff, normup, reright ); + scale3d( correct_diff, correct_diff, -0.001 ); //This is the coefficient applying the drag. + add3d( correct_diff, correct_diff, reright ); + LinmathQuat reright_quat; + normalize3d( correct_diff, correct_diff ); + quatfrom2vectors( reright_quat, reright, correct_diff ); + + + SurvivePose object_pose_out; - quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion ); + quatrotateabout(object_pose_out.Rot, object_pose->Rot, applymotion ); //Contribution from Gyro + quatrotateabout(object_pose_out.Rot, object_pose_out.Rot, reright_quat); //Contribution from Accelerometer + quatnormalize(object_pose_out.Rot, object_pose_out.Rot); + + copy3d( object_pose_out.Pos, object_pose->Pos ); PoserData_poser_pose_func(pd, so, &object_pose_out); quatcopy( dd->InteralPoseUsedForCalc.Rot, object_pose_out.Rot); -- cgit v1.2.3 From 7a9782b4069cce0c1f263a5d4dc0dc4f817fcbfe Mon Sep 17 00:00:00 2001 From: Charles Lohr Date: Sat, 7 Apr 2018 19:55:57 +0000 Subject: Update code to permit wireless watchmen use. --- src/poser_charlesrefine.c | 2 +- src/survive_default_devices.c | 9 ++++++++- src/survive_vive.c | 15 +++++---------- 3 files changed, 14 insertions(+), 12 deletions(-) (limited to 'src') diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index ce14f70..cc188fb 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -197,7 +197,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { #define CORRECT_LATERAL_POSITION_COEFFICIENT 0.7 // Explodes if you exceed 1.0 #define CORRECT_TELESCOPTION_COEFFICIENT 7.00 // Converges even as high as 10.0 and doesn't explode. #define CORRECT_ROTATION_COEFFICIENT \ - 1.0 // This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. + 0.2 // This starts to fall apart above 5.0, but for good reason. It is amplified by the number of points seen. #define ROTATIONAL_CORRECTION_MAXFORCE 0.01 // Step 1: Determine standard of deviation, and average in order to diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c index 3c102b2..954c47a 100644 --- a/src/survive_default_devices.c +++ b/src/survive_default_devices.c @@ -190,7 +190,14 @@ int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) { } else if( memcmp( so->codename, "WM", 2 ) == 0 ) { - //??!!?? No one has yet decoded the watchman accelerometer data. + if( so->acc_scale ) scale3d( so->acc_scale, so->acc_scale, 2./8192.0 ); + if( so->acc_bias ) scale3d( so->acc_bias, so->acc_bias, 2./1000. ); //Need to verify. + if( so->gyro_scale ) scale3d( so->gyro_scale, so->gyro_scale, 3.14159 / 1800. / 1.8 ); //??! 1.8 feels right but why?! + int j; + for (j = 0; j < so->sensor_ct; j++) { + so->sensor_locations[j * 3 + 0] *= 1.0; + } + } else //Verified on WW, Need to verify on Tracker. { diff --git a/src/survive_vive.c b/src/survive_vive.c index 60b3dc9..83bc977 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1233,20 +1233,15 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata ) if( ( ( type & 0xe8 ) == 0xe8 ) || doimu ) //Hmm, this looks kind of yucky... we can get e8's that are accelgyro's but, cleared by first propset. { propset |= 2; - //XXX XXX BIG TODO!!! Actually recal gyro data. - FLT agm[9] = { readdata[1], readdata[2], readdata[3], - readdata[4], readdata[5], readdata[6], - 0,0,0 }; - -// if (w->acc_scale) printf("%f %f %f\n",w->acc_scale[0],w->acc_scale[1],w->acc_scale[2]); - //I don't understand where these numbers come from but the data from the WMD seems to max out at 255... + FLT agm[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + int j; + for( j = 0; j < 6; j++ ) + agm[j] = (int16_t)(readdata[j*2+1] | (readdata[j*2+2]<<8)); calibrate_acc(w, agm); calibrate_gyro(w, agm+3); w->ctx->imuproc( w, 3, agm, (time1<<24)|(time2<<16)|readdata[0], 0 ); - - int16_t * k = (int16_t *)readdata+1; - //printf( "Match8 %d %d %d %d %d %3d %3d\n", qty, k[0], k[1], k[2], k[3], k[4], k[5] ); readdata += 13; qty -= 13; + type &= ~0xe8; if( qty ) { -- cgit v1.2.3 From 5b07e56094f2f765d679ac06bc07d4081338bb29 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 7 Apr 2018 09:53:26 -0600 Subject: Added statistics to sba --- src/poser_sba.c | 29 ++++++++++++++++++++++++++--- 1 file changed, 26 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/poser_sba.c b/src/poser_sba.c index 23e03fc..827e420 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -51,6 +51,13 @@ typedef struct SBAData { SurviveIMUTracker tracker; bool useIMU; + struct { + int runs; + int poser_seed_runs; + int meas_failures; + int error_failures; + } stats; + SurviveObject *so; } SBAData; @@ -208,6 +215,9 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size); failure_count = 0; } + if (meas_size < d->required_meas) { + d->stats.meas_failures++; + } return -1; } failure_count = 0; @@ -231,6 +241,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive pdl->hdr.userdata = &locations; driver(so, &pdl->hdr); pdl->hdr = hdr; + d->stats.poser_seed_runs++; if (locations.hasInfo == false) { return -1; @@ -256,6 +267,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive opts[3] = SBA_STOP_THRESH; // max_reproj_error * meas.size(); opts[4] = 0.0; + d->stats.runs++; int status = sba_str_levmar(1, // Number of 3d points 0, // Number of 3d points to fix in spot NUM_LIGHTHOUSES * so->sensor_ct, vmask, @@ -287,6 +299,8 @@ 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 if ((info[1] / meas_size * 2) >= d->max_error) { + d->stats.error_failures++; } { @@ -466,13 +480,22 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { // std::cerr << "Average reproj error: " << error << std::endl; return 0; } + case POSERDATA_DISASSOCIATE: { + SV_INFO("SBA stats:"); + SV_INFO("\tseed runs %d / %d", d->stats.poser_seed_runs, d->stats.runs); + SV_INFO("\tmeas failures %d", d->stats.meas_failures); + SV_INFO("\terror failures %d", d->stats.error_failures); + free(d); + so->PoserData = 0; + return 0; + } case POSERDATA_IMU: { PoserDataIMU * imu = (PoserDataIMU*)pd; if (ctx->calptr && ctx->calptr->stage < 5) { - } else if(d->useIMU){ - survive_imu_tracker_integrate(so, &d->tracker, imu); - PoserData_poser_pose_func(pd, so, &d->tracker.pose); + } else if (d->useIMU) { + survive_imu_tracker_integrate(so, &d->tracker, imu); + PoserData_poser_pose_func(pd, so, &d->tracker.pose); } } // INTENTIONAL FALLTHROUGH default: { -- cgit v1.2.3 From bb0d186e967421c48ba781759063dca165f69df2 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sat, 7 Apr 2018 10:15:23 -0600 Subject: Detailed messages about sba failures --- src/poser_sba.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/poser_sba.c b/src/poser_sba.c index 827e420..1eefc80 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -294,23 +294,23 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } double rtn = -1; - if (status > 0 && (info[1] / meas_size * 2) < d->max_error) { + bool status_failure = status <= 0; + bool error_failure = (info[1] / meas_size * 2) >= d->max_error; + if (!status_failure && !error_failure) { d->failures_to_reset_cntr = d->failures_to_reset; quatnormalize(soLocation.Rot, soLocation.Rot); *out = soLocation; rtn = info[1] / meas_size * 2; - } else if ((info[1] / meas_size * 2) >= d->max_error) { + } else if (error_failure) { d->stats.error_failures++; } { SurviveContext *ctx = so->ctx; // Docs say info[0] should be divided by meas; I don't buy it really... - static int cnt = 0; - if (cnt++ > 1000 || meas_size < d->required_meas || (info[1] / meas_size * 2) > d->max_error) { - // SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size); - // SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); - cnt = 0; + if (error_failure) { + SV_INFO("%f original reproj error for %u meas", (info[0] / meas_size * 2), (int)meas_size); + SV_INFO("%f cur reproj error", (info[1] / meas_size * 2)); } } -- cgit v1.2.3 From d46271513e6f789af0e82d4ed6628abe21e96a92 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 15:54:07 -0600 Subject: Added jacobian to sba, ~2x speed improvement --- src/poser_sba.c | 41 ++++++-- src/survive_reproject.c | 70 ++++++++++++- src/survive_reproject.generated.h | 213 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 310 insertions(+), 14 deletions(-) create mode 100644 src/survive_reproject.generated.h (limited to 'src') diff --git a/src/poser_sba.c b/src/poser_sba.c index 1eefc80..f101018 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -45,7 +45,7 @@ typedef struct SBAData { FLT sensor_variance; FLT sensor_variance_per_second; int sensor_time_window; - + int use_jacobian_function; int required_meas; SurviveIMUTracker tracker; @@ -183,13 +183,30 @@ 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); - FLT xyz[3]; - ApplyPoseToPoint(xyz, &obj, &so->sensor_locations[sensor_idx * 3]); + // quatnormalize(obj.Rot, obj.Rot); // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; SurvivePose *camera = &so->ctx->bsd[lh].Pose; - survive_reproject_from_pose(so->ctx, lh, camera, xyz, xij); + survive_reproject_full(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh], + &so->ctx->calibration_config); +} + +static void str_metric_function_jac(int j, int i, double *bi, double *xij, void *adata) { + SurvivePose obj = *(SurvivePose *)bi; + int sensor_idx = j >> 1; + int lh = j & 1; + + sba_context *ctx = (sba_context *)(adata); + SurviveObject *so = ctx->so; + + assert(lh < 2); + assert(sensor_idx < so->sensor_ct); + + // 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], + &so->ctx->calibration_config); } static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, @@ -277,12 +294,12 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive cov, // cov data 2, // mnp -- 2 points per image str_metric_function, - 0, // jacobia of metric_func - &ctx, // user data - max_iterations, // Max iterations - 0, // verbosity - opts, // options - info); // info + d->use_jacobian_function ? str_metric_function_jac : 0, // jacobia of metric_func + &ctx, // user data + max_iterations, // Max iterations + 0, // verbosity + opts, // options + info); // info if (currentPositionValid) { // FLT distp[3]; @@ -421,6 +438,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { survive_configi(ctx, "sba-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2); d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 10.0); d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0); + d->use_jacobian_function = survive_configi(ctx, "sba-use-jacobian-function", SC_GET, 1.0); d->so = so; SV_INFO("Initializing SBA:"); @@ -431,6 +449,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { SV_INFO("\tsba-max-error: %f", d->max_error); SV_INFO("\tsba-successes-to-reset: %d", d->successes_to_reset); SV_INFO("\tsba-use-imu: %d", d->useIMU); + SV_INFO("\tsba-use-jacobian-function: %d", d->use_jacobian_function); } SBAData *d = so->PoserData; switch (pd->pt) { diff --git a/src/survive_reproject.c b/src/survive_reproject.c index d6ba70b..921386b 100644 --- a/src/survive_reproject.c +++ b/src/survive_reproject.c @@ -1,8 +1,72 @@ #include "survive_reproject.h" -#include <../redist/linmath.h> +#include "survive_reproject.generated.h" #include -#include -#include + +void survive_reproject_full_jac_obj_pose(FLT *out, const SurvivePose *obj_pose, const LinmathVec3d obj_pt, + const SurvivePose *lh2world, const BaseStationData *bsd, + const survive_calibration_config *config) { + FLT phase_scale = config->use_flag & SVCal_Phase ? config->phase_scale : 0.; + FLT phase_0 = bsd->fcal.phase[0]; + FLT phase_1 = bsd->fcal.phase[1]; + + FLT tilt_scale = config->use_flag & SVCal_Tilt ? config->tilt_scale : 0.; + FLT tilt_0 = bsd->fcal.tilt[0]; + FLT tilt_1 = bsd->fcal.tilt[1]; + + FLT curve_scale = config->use_flag & SVCal_Curve ? config->curve_scale : 0.; + FLT curve_0 = bsd->fcal.curve[0]; + FLT curve_1 = bsd->fcal.curve[1]; + + FLT gib_scale = config->use_flag & SVCal_Gib ? config->gib_scale : 0; + FLT gibPhase_0 = bsd->fcal.gibpha[0]; + FLT gibPhase_1 = bsd->fcal.gibpha[1]; + FLT gibMag_0 = bsd->fcal.gibmag[0]; + FLT gibMag_1 = bsd->fcal.gibmag[1]; + + gen_reproject_jac_obj_p(out, obj_pose->Pos, obj_pt, lh2world->Pos, phase_scale, phase_0, phase_1, tilt_scale, + tilt_0, tilt_1, curve_scale, curve_0, curve_1, gib_scale, gibPhase_0, gibPhase_1, gibMag_0, + gibMag_1); +} + +void survive_reproject_full(FLT *out, const SurvivePose *obj_pose, const LinmathVec3d obj_pt, + const SurvivePose *lh2world, const BaseStationData *bsd, + const survive_calibration_config *config) { + LinmathVec3d world_pt; + ApplyPoseToPoint(world_pt, obj_pose, obj_pt); + + SurvivePose world2lh; + InvertPose(&world2lh, lh2world); + + LinmathPoint3d t_pt; + ApplyPoseToPoint(t_pt, &world2lh, world_pt); + + FLT x = -t_pt[0] / -t_pt[2]; + FLT y = t_pt[1] / -t_pt[2]; + double xy[] = {x, y}; + double ang[] = {atan(x), atan(y)}; + + const FLT *phase = bsd->fcal.phase; + const FLT *curve = bsd->fcal.curve; + const FLT *tilt = bsd->fcal.tilt; + const FLT *gibPhase = bsd->fcal.gibpha; + const FLT *gibMag = bsd->fcal.gibmag; + enum SurviveCalFlag f = config->use_flag; + + for (int axis = 0; axis < 2; axis++) { + int opp_axis = axis == 0 ? 1 : 0; + + out[axis] = ang[axis]; + + if (f & SVCal_Phase) + out[axis] -= config->phase_scale * phase[axis]; + if (f & SVCal_Tilt) + out[axis] -= tan(config->tilt_scale * tilt[axis]) * xy[opp_axis]; + if (f & SVCal_Curve) + out[axis] -= config->curve_scale * curve[axis] * xy[opp_axis] * xy[opp_axis]; + if (f & SVCal_Gib) + out[axis] -= config->gib_scale * sin(gibPhase[axis] + ang[axis]) * gibMag[axis]; + } +} void survive_reproject_from_pose_with_bsd(const BaseStationData *bsd, const survive_calibration_config *config, const SurvivePose *pose, const FLT *pt, FLT *out) { diff --git a/src/survive_reproject.generated.h b/src/survive_reproject.generated.h new file mode 100644 index 0000000..f4b499a --- /dev/null +++ b/src/survive_reproject.generated.h @@ -0,0 +1,213 @@ + // NOTE: Auto-generated code; see tools/generate_reprojection_functions +#include +static inline void gen_reproject_jac_obj_p(FLT* out, const FLT *obj, const FLT *sensor, const FLT *lh, FLT phase_scale, FLT phase_0, FLT phase_1, FLT tilt_scale, FLT tilt_0, FLT tilt_1, FLT curve_scale, FLT curve_0, FLT curve_1, FLT gib_scale, FLT gibPhase_0, FLT gibPhase_1, FLT gibMag_0, FLT gibMag_1) { + FLT obj_px = *(obj++); + FLT obj_py = *(obj++); + FLT obj_pz = *(obj++); + FLT obj_qw = *(obj++); + FLT obj_qi = *(obj++); + FLT obj_qj = *(obj++); + FLT obj_qk = *(obj++); + FLT sensor_x = *(sensor++); + FLT sensor_y = *(sensor++); + FLT sensor_z = *(sensor++); + FLT lh_px = *(lh++); + FLT lh_py = *(lh++); + FLT lh_pz = *(lh++); + FLT lh_qw = *(lh++); + FLT lh_qi = *(lh++); + FLT lh_qj = *(lh++); + FLT lh_qk = *(lh++); + FLT x0 = tan(tilt_0*tilt_scale); + FLT x1 = lh_qi*lh_qj; + FLT x2 = lh_qk*lh_qw; + FLT x3 = x1 - x2; + FLT x4 = pow(lh_qi, 2); + FLT x5 = pow(lh_qj, 2); + FLT x6 = x4 + x5; + FLT x7 = pow(lh_qk, 2); + FLT x8 = sqrt(pow(lh_qw, 2) + x6 + x7); + FLT x9 = lh_qi*lh_qk; + FLT x10 = lh_qj*lh_qw; + FLT x11 = x10 + x9; + FLT x12 = 2*lh_px*x8; + FLT x13 = lh_qj*lh_qk; + FLT x14 = lh_qi*lh_qw; + FLT x15 = x13 - x14; + FLT x16 = 2*lh_py*x8; + FLT x17 = 2*x8; + FLT x18 = x17*x6 - 1; + FLT x19 = obj_qi*obj_qk; + FLT x20 = obj_qj*obj_qw; + FLT x21 = sensor_z*(x19 + x20); + FLT x22 = pow(obj_qi, 2); + FLT x23 = pow(obj_qj, 2); + FLT x24 = x22 + x23; + FLT x25 = pow(obj_qk, 2); + FLT x26 = sqrt(pow(obj_qw, 2) + x24 + x25); + FLT x27 = 2*x26; + FLT x28 = obj_qi*obj_qj; + FLT x29 = obj_qk*obj_qw; + FLT x30 = sensor_y*(x28 - x29); + FLT x31 = x23 + x25; + FLT x32 = obj_px - sensor_x*(x27*x31 - 1) + x21*x27 + x27*x30; + FLT x33 = 2*x32*x8; + FLT x34 = sensor_x*(x28 + x29); + FLT x35 = obj_qj*obj_qk; + FLT x36 = obj_qi*obj_qw; + FLT x37 = sensor_z*(x35 - x36); + FLT x38 = x22 + x25; + FLT x39 = obj_py - sensor_y*(x27*x38 - 1) + x27*x34 + x27*x37; + FLT x40 = 2*x39*x8; + FLT x41 = sensor_y*(x35 + x36); + FLT x42 = sensor_x*(x19 - x20); + FLT x43 = obj_pz - sensor_z*(x24*x27 - 1) + x27*x41 + x27*x42; + FLT x44 = lh_pz*x18 - x11*x12 + x11*x33 - x15*x16 + x15*x40 - x18*x43; + FLT x45 = 1.0/x44; + FLT x46 = 2*x45*x8; + FLT x47 = x13 + x14; + FLT x48 = 2*lh_pz*x8; + FLT x49 = x17*(x4 + x7) - 1; + FLT x50 = 2*x43*x8; + FLT x51 = lh_py*x49 - x12*x3 + x3*x33 - x39*x49 - x47*x48 + x47*x50; + FLT x52 = pow(x44, -2); + FLT x53 = 4*curve_0*curve_scale*x51*x52*x8; + FLT x54 = x11*x52; + FLT x55 = pow(x51, 2); + FLT x56 = curve_0*x55; + FLT x57 = pow(x44, -3); + FLT x58 = 4*curve_scale*x11*x57*x8; + FLT x59 = x1 + x2; + FLT x60 = -x10 + x9; + FLT x61 = x17*(x5 + x7) - 1; + FLT x62 = lh_px*x61 - x16*x59 - x32*x61 + x40*x59 - x48*x60 + x50*x60; + FLT x63 = pow(x62, 2); + FLT x64 = 1.0/(x52*x63 + 1); + FLT x65 = x45*x61; + FLT x66 = 2*x54*x62*x8; + FLT x67 = x64*(x65 + x66); + FLT x68 = gibMag_0*gib_scale*cos(gibPhase_0 + atan(x45*x62)); + FLT x69 = x45*x49; + FLT x70 = 2*x15*x8; + FLT x71 = x51*x52; + FLT x72 = x70*x71; + FLT x73 = 4*curve_scale*x15*x57*x8; + FLT x74 = 2*x64; + FLT x75 = x45*x8; + FLT x76 = x74*(-x15*x52*x62*x8 + x59*x75); + FLT x77 = x46*x47; + FLT x78 = x18*x71; + FLT x79 = 2*curve_scale*x18*x57; + FLT x80 = x46*x60; + FLT x81 = x52*x62; + FLT x82 = x18*x81; + FLT x83 = x64*(x80 + x82); + FLT x84 = 2*x0; + FLT x85 = obj_qj*x26; + FLT x86 = sensor_x*x85; + FLT x87 = obj_qi*x26; + FLT x88 = sensor_y*x87; + FLT x89 = sensor_z*x24; + FLT x90 = 1.0/x26; + FLT x91 = obj_qw*x90; + FLT x92 = -x41*x91 - x42*x91 + x86 - x88 + x89*x91; + FLT x93 = 2*x8*x92; + FLT x94 = obj_qk*x26; + FLT x95 = sensor_y*x94; + FLT x96 = sensor_z*x85; + FLT x97 = sensor_x*x31; + FLT x98 = -x21*x91 - x30*x91 + x91*x97 + x95 - x96; + FLT x99 = 2*x8*x98; + FLT x100 = sensor_x*x94; + FLT x101 = sensor_z*x87; + FLT x102 = sensor_y*x38; + FLT x103 = x100 - x101 - x102*x91 + x34*x91 + x37*x91; + FLT x104 = x103*x49 + x3*x99 + x47*x93; + FLT x105 = x104*x45; + FLT x106 = 4*curve_0*curve_scale*x51*x52; + FLT x107 = -x103*x70 + x11*x99 - x18*x92; + FLT x108 = x107*x71; + FLT x109 = 4*curve_scale*x107*x57; + FLT x110 = 2*x59*x8; + FLT x111 = x103*x110 - x60*x93 + x61*x98; + FLT x112 = x74*(x107*x81 + x111*x45); + FLT x113 = sensor_y*x85; + FLT x114 = sensor_z*x94; + FLT x115 = obj_qi*x90; + FLT x116 = -x113 - x114 - x115*x21 - x115*x30 + x115*x97; + FLT x117 = 2*x116*x8; + FLT x118 = obj_qw*x26; + FLT x119 = sensor_y*x118; + FLT x120 = obj_qi*x27; + FLT x121 = -sensor_z*(x115*x24 + x120) + x100 + x115*x41 + x115*x42 + x119; + FLT x122 = 2*x121*x8; + FLT x123 = sensor_z*x118; + FLT x124 = -sensor_y*(x115*x38 + x120) + x115*x34 + x115*x37 - x123 + x86; + FLT x125 = x117*x3 - x122*x47 + x124*x49; + FLT x126 = x125*x45; + FLT x127 = x11*x117 + x121*x18 - x124*x70; + FLT x128 = x127*x71; + FLT x129 = 4*curve_0*curve_scale*x55*x57; + FLT x130 = x110*x124 + x116*x61 + x122*x60; + FLT x131 = x74*(x127*x81 + x130*x45); + FLT x132 = sensor_x*x87; + FLT x133 = obj_qj*x90; + FLT x134 = -x102*x133 + x114 + x132 + x133*x34 + x133*x37; + FLT x135 = obj_qj*x27; + FLT x136 = -sensor_x*(x133*x31 + x135) + x123 + x133*x21 + x133*x30 + x88; + FLT x137 = 2*x136*x8; + FLT x138 = sensor_x*x118; + FLT x139 = -sensor_z*(x133*x24 + x135) + x133*x41 + x133*x42 - x138 + x95; + FLT x140 = 2*x139*x8; + FLT x141 = -x134*x49 + x137*x3 + x140*x47; + FLT x142 = x141*x45; + FLT x143 = x11*x137 + x134*x70 - x139*x18; + FLT x144 = x143*x71; + FLT x145 = x110*x134 - x136*x61 + x140*x60; + FLT x146 = x74*(-x143*x81 + x145*x45); + FLT x147 = obj_qk*x90; + FLT x148 = x113 + x132 + x147*x41 + x147*x42 - x147*x89; + FLT x149 = 2*x148*x8; + FLT x150 = obj_qk*x27; + FLT x151 = -sensor_x*(x147*x31 + x150) + x101 - x119 + x147*x21 + x147*x30; + FLT x152 = 2*x151*x8; + FLT x153 = -sensor_y*(x147*x38 + x150) + x138 + x147*x34 + x147*x37 + x96; + FLT x154 = x149*x47 + x152*x3 - x153*x49; + FLT x155 = x154*x45; + FLT x156 = x11*x152 - x148*x18 + x153*x70; + FLT x157 = x156*x71; + FLT x158 = x110*x153 + x149*x60 - x151*x61; + FLT x159 = x74*(-x156*x81 + x158*x45); + FLT x160 = tan(tilt_1*tilt_scale); + FLT x161 = curve_1*x63; + FLT x162 = 1.0/(x52*x55 + 1); + FLT x163 = 2*x162; + FLT x164 = x163*(x3*x75 - x51*x54*x8); + FLT x165 = gibMag_1*gib_scale*cos(gibPhase_1 - atan(x45*x51)); + FLT x166 = 4*curve_1*curve_scale*x52*x62*x8; + FLT x167 = x162*(x69 + x72); + FLT x168 = x162*(x77 + x78); + FLT x169 = 2*x160*x45; + FLT x170 = 4*curve_1*curve_scale*x52*x62; + FLT x171 = 2*x160*x52*x62; + FLT x172 = x163*(-x105 + x108); + FLT x173 = 4*curve_1*curve_scale*x57*x63; + FLT x174 = x163*(-x126 + x128); + FLT x175 = x163*(x142 - x144); + FLT x176 = x163*(x155 - x157); + *(out++) = x0*x3*x46 - 2*x0*x51*x54*x8 - x3*x53 + x56*x58 + x67*x68 - x67; + *(out++) = 2*curve_0*curve_scale*x49*x51*x52 - x0*x69 - x0*x72 + x56*x73 - x68*x76 + x76; + *(out++) = x0*x77 + x0*x78 - x47*x53 - x56*x79 - x68*x83 + x83; + *(out++) = x104*x106 - x105*x84 + x108*x84 - x109*x56 - x112*x68 + x112; + *(out++) = x106*x125 - x126*x84 - x127*x129 + x128*x84 - x131*x68 + x131; + *(out++) = -x106*x141 + x129*x143 + x142*x84 - x144*x84 - x146*x68 + x146; + *(out++) = -x106*x154 + x129*x156 + x155*x84 - x157*x84 - x159*x68 + x159; + *(out++) = 2*curve_1*curve_scale*x52*x61*x62 + x160*x65 + x160*x66 + x161*x58 + x164*x165 - x164; + *(out++) = -x110*x160*x45 + x160*x52*x62*x70 + x161*x73 - x165*x167 - x166*x59 + x167; + *(out++) = -x160*x80 - x160*x82 - x161*x79 + x165*x168 - x166*x60 - x168; + *(out++) = -x107*x171 - x109*x161 - x111*x169 - x111*x170 + x165*x172 - x172; + *(out++) = -x127*x171 - x127*x173 - x130*x169 - x130*x170 + x165*x174 - x174; + *(out++) = x143*x171 + x143*x173 - x145*x169 - x145*x170 + x165*x175 - x175; + *(out++) = x156*x171 + x156*x173 - x158*x169 - x158*x170 + x165*x176 - x176; +} + -- cgit v1.2.3 From 538b7c32bfbd6460c61a080c2697365934b81938 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 16:16:14 -0600 Subject: Made picking the seed poser use cmd line --- src/poser_sba.c | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) (limited to 'src') diff --git a/src/poser_sba.c b/src/poser_sba.c index f101018..1f79986 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -59,6 +59,8 @@ typedef struct SBAData { } stats; SurviveObject *so; + + PoserCB seed_poser; } SBAData; static void metric_function(int j, int i, double *aj, double *xij, void *adata) { @@ -241,12 +243,11 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive SurvivePose soLocation = so->OutPose; bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0; - + static bool seed_warning = false; if (d->successes_to_reset_cntr == 0 || d->failures_to_reset_cntr == 0 || currentPositionValid == 0) { SurviveContext *ctx = so->ctx; // SV_INFO("Must rerun seed poser"); - const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); - PoserCB driver = (PoserCB)GetDriver(subposer); + PoserCB driver = d->seed_poser; if (driver) { PoserData hdr = pdl->hdr; @@ -267,7 +268,8 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } d->successes_to_reset_cntr = d->successes_to_reset; - } else { + } else if (seed_warning == false) { + seed_warning = true; SV_INFO("Not using a seed poser for SBA; results will likely be way off"); } } @@ -335,7 +337,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } // Optimizes for LH position assuming object is posed at 0 -static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, +static double run_sba(SBAData *d, PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { double *covx = 0; @@ -347,11 +349,9 @@ static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_itera .obj_pose = so->OutPose}; { - const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); - PoserCB driver = (PoserCB)GetDriver(subposer); + PoserCB driver = d->seed_poser; SurviveContext *ctx = so->ctx; if (driver) { - SV_INFO("Using %s seed poser for SBA", subposer); PoserData hdr = pdfs->hdr; memset(&pdfs->hdr, 0, sizeof(pdfs->hdr)); // Clear callback functions pdfs->hdr.pt = hdr.pt; @@ -441,6 +441,9 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->use_jacobian_function = survive_configi(ctx, "sba-use-jacobian-function", SC_GET, 1.0); d->so = so; + const char *subposer = survive_configs(ctx, "sba-seed-poser", SC_GET, "PoserEPNP"); + d->seed_poser = (PoserCB)GetDriver(subposer); + SV_INFO("Initializing SBA:"); SV_INFO("\tsba-required-meas: %d", d->required_meas); SV_INFO("\tsba-sensor-variance: %f", d->sensor_variance); @@ -450,6 +453,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { SV_INFO("\tsba-successes-to-reset: %d", d->successes_to_reset); SV_INFO("\tsba-use-imu: %d", d->useIMU); SV_INFO("\tsba-use-jacobian-function: %d", d->use_jacobian_function); + SV_INFO("\tsba-seed-poser: %s(%p)", subposer, d->seed_poser); } SBAData *d = so->PoserData; switch (pd->pt) { @@ -495,7 +499,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { case POSERDATA_FULL_SCENE: { SurviveContext *ctx = so->ctx; PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd); - double error = run_sba(pdfs, so, 100, .005); + double error = run_sba(d, pdfs, so, 100, .005); // std::cerr << "Average reproj error: " << error << std::endl; return 0; } @@ -518,10 +522,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { } } // INTENTIONAL FALLTHROUGH default: { - const char *subposer = config_read_str(so->ctx->global_config_values, "SBASeedPoser", "PoserEPNP"); - PoserCB driver = (PoserCB)GetDriver(subposer); - if (driver) { - return driver(so, pd); + if (d->seed_poser) { + return d->seed_poser(so, pd); } break; } -- cgit v1.2.3 From 9601f596fe545692ed39f939cf846f9883ea188c Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 16:26:41 -0600 Subject: Revered what config poser was doing for SBA --- src/poser_sba.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/poser_sba.c b/src/poser_sba.c index 1f79986..347b600 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -337,7 +337,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } // Optimizes for LH position assuming object is posed at 0 -static double run_sba(SBAData *d, PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, +static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/) { double *covx = 0; @@ -349,7 +349,9 @@ static double run_sba(SBAData *d, PoserDataFullScene *pdfs, SurviveObject *so, i .obj_pose = so->OutPose}; { - PoserCB driver = d->seed_poser; + const char *subposer = survive_configs(so->ctx, "sba-seed-poser", SC_GET, "PoserEPNP"); + + PoserCB driver = (PoserCB)GetDriver(subposer); SurviveContext *ctx = so->ctx; if (driver) { PoserData hdr = pdfs->hdr; @@ -499,7 +501,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { case POSERDATA_FULL_SCENE: { SurviveContext *ctx = so->ctx; PoserDataFullScene *pdfs = (PoserDataFullScene *)(pd); - double error = run_sba(d, pdfs, so, 100, .005); + double error = run_sba(pdfs, so, 100, .005); // std::cerr << "Average reproj error: " << error << std::endl; return 0; } -- cgit v1.2.3 From 6363d21d6bfd60564830c410ea75f96b3bc1f129 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 16:46:22 -0600 Subject: Optimizations because vc++ is dumb --- src/survive_reproject.generated.h | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) (limited to 'src') diff --git a/src/survive_reproject.generated.h b/src/survive_reproject.generated.h index f4b499a..e184775 100644 --- a/src/survive_reproject.generated.h +++ b/src/survive_reproject.generated.h @@ -22,11 +22,11 @@ static inline void gen_reproject_jac_obj_p(FLT* out, const FLT *obj, const FLT * FLT x1 = lh_qi*lh_qj; FLT x2 = lh_qk*lh_qw; FLT x3 = x1 - x2; - FLT x4 = pow(lh_qi, 2); - FLT x5 = pow(lh_qj, 2); + FLT x4 = (lh_qi * lh_qi); + FLT x5 = (lh_qj * lh_qj); FLT x6 = x4 + x5; - FLT x7 = pow(lh_qk, 2); - FLT x8 = sqrt(pow(lh_qw, 2) + x6 + x7); + FLT x7 = (lh_qk * lh_qk); + FLT x8 = sqrt((lh_qw * lh_qw) + x6 + x7); FLT x9 = lh_qi*lh_qk; FLT x10 = lh_qj*lh_qw; FLT x11 = x10 + x9; @@ -40,11 +40,11 @@ static inline void gen_reproject_jac_obj_p(FLT* out, const FLT *obj, const FLT * FLT x19 = obj_qi*obj_qk; FLT x20 = obj_qj*obj_qw; FLT x21 = sensor_z*(x19 + x20); - FLT x22 = pow(obj_qi, 2); - FLT x23 = pow(obj_qj, 2); + FLT x22 = (obj_qi * obj_qi); + FLT x23 = (obj_qj * obj_qj); FLT x24 = x22 + x23; - FLT x25 = pow(obj_qk, 2); - FLT x26 = sqrt(pow(obj_qw, 2) + x24 + x25); + FLT x25 = (obj_qk * obj_qk); + FLT x26 = sqrt((obj_qw * obj_qw) + x24 + x25); FLT x27 = 2*x26; FLT x28 = obj_qi*obj_qj; FLT x29 = obj_qk*obj_qw; @@ -70,18 +70,18 @@ static inline void gen_reproject_jac_obj_p(FLT* out, const FLT *obj, const FLT * FLT x49 = x17*(x4 + x7) - 1; FLT x50 = 2*x43*x8; FLT x51 = lh_py*x49 - x12*x3 + x3*x33 - x39*x49 - x47*x48 + x47*x50; - FLT x52 = pow(x44, -2); + FLT x52 = 1. / (x44 * x44); FLT x53 = 4*curve_0*curve_scale*x51*x52*x8; FLT x54 = x11*x52; - FLT x55 = pow(x51, 2); + FLT x55 = (x51*x51); FLT x56 = curve_0*x55; - FLT x57 = pow(x44, -3); + FLT x57 = 1. / (x44*x44*x44); FLT x58 = 4*curve_scale*x11*x57*x8; FLT x59 = x1 + x2; FLT x60 = -x10 + x9; FLT x61 = x17*(x5 + x7) - 1; FLT x62 = lh_px*x61 - x16*x59 - x32*x61 + x40*x59 - x48*x60 + x50*x60; - FLT x63 = pow(x62, 2); + FLT x63 = (x62 * x62); FLT x64 = 1.0/(x52*x63 + 1); FLT x65 = x45*x61; FLT x66 = 2*x54*x62*x8; -- cgit v1.2.3 From e06e452445bdd3d61f393bde4206cd848a54444b Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 8 Apr 2018 22:51:57 -0600 Subject: Created common base for optimizer based posers --- src/poser_epnp.c | 2 +- src/poser_general_optimizer.c | 100 ++++++++++++++++++++++++++++++++++++++++ src/poser_general_optimizer.h | 29 ++++++++++++ src/poser_sba.c | 103 ++++++++---------------------------------- 4 files changed, 148 insertions(+), 86 deletions(-) create mode 100644 src/poser_general_optimizer.c create mode 100644 src/poser_general_optimizer.h (limited to 'src') diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 7e922e7..615c01c 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -129,7 +129,7 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { case POSERDATA_LIGHT: { PoserDataLight *lightData = (PoserDataLight *)pd; - SurvivePose posers[2]; + SurvivePose posers[2] = {}; int meas[2] = {0, 0}; for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) { if (so->ctx->bsd[lh].PositionSet) { diff --git a/src/poser_general_optimizer.c b/src/poser_general_optimizer.c new file mode 100644 index 0000000..313e378 --- /dev/null +++ b/src/poser_general_optimizer.c @@ -0,0 +1,100 @@ +#include "poser_general_optimizer.h" +#include "string.h" +#include +#include +#include + +void *GetDriver(const char *name); +void general_optimizer_data_init(GeneralOptimizerData *d, SurviveObject *so) { + memset(d, 0, sizeof(*d)); + d->so = so; + + SurviveContext *ctx = so->ctx; + + d->failures_to_reset = survive_configi(ctx, "failures-to-reset", SC_GET, 1); + d->successes_to_reset = survive_configi(ctx, "successes-to-reset", SC_GET, -1); + d->max_error = survive_configf(ctx, "max-error", SC_GET, .0001); + + const char *subposer = survive_configs(ctx, "seed-poser", SC_GET, "PoserEPNP"); + d->seed_poser = (PoserCB)GetDriver(subposer); + + SV_INFO("Initializing general optimizer:"); + SV_INFO("\tmax-error: %f", d->max_error); + SV_INFO("\tsuccesses-to-reset: %d", d->successes_to_reset); + SV_INFO("\tfailures-to-reset: %d", d->failures_to_reset); + SV_INFO("\tseed-poser: %s(%p)", subposer, d->seed_poser); +} +void general_optimizer_data_record_failure(GeneralOptimizerData *d) { + if (d->failures_to_reset_cntr > 0) + d->failures_to_reset_cntr--; +} +bool general_optimizer_data_record_success(GeneralOptimizerData *d, FLT error) { + d->stats.runs++; + if (d->max_error > error) { + if (d->successes_to_reset_cntr > 0) + d->successes_to_reset_cntr--; + d->failures_to_reset_cntr = d->failures_to_reset; + return true; + } + return false; +} + +typedef struct { + bool hasInfo; + SurvivePose pose; +} set_position_t; + +static void set_position(SurviveObject *so, uint32_t timecode, SurvivePose *new_pose, void *_user) { + set_position_t *user = _user; + assert(user->hasInfo == false); + user->hasInfo = true; + user->pose = *new_pose; +} + +bool general_optimizer_data_record_current_pose(GeneralOptimizerData *d, PoserData *_hdr, size_t len_hdr, + SurvivePose *soLocation) { + *soLocation = d->so->OutPose; + bool currentPositionValid = quatmagnitude(soLocation->Rot) != 0; + static bool seed_warning = false; + if (d->successes_to_reset_cntr == 0 || d->failures_to_reset_cntr == 0 || currentPositionValid == 0) { + PoserCB driver = d->seed_poser; + SurviveContext *ctx = d->so->ctx; + if (driver) { + + PoserData *hdr = alloca(len_hdr); + memcpy(hdr, _hdr, len_hdr); + memset(hdr, 0, sizeof(PoserData)); // Clear callback functions + hdr->pt = _hdr->pt; + hdr->poseproc = set_position; + + set_position_t locations = {0}; + hdr->userdata = &locations; + driver(d->so, hdr); + d->stats.poser_seed_runs++; + + if (locations.hasInfo == false) { + return false; + } else if (locations.hasInfo) { + *soLocation = locations.pose; + } + + d->successes_to_reset_cntr = d->successes_to_reset; + } else if (seed_warning == false) { + seed_warning = true; + SV_INFO("Not using a seed poser for SBA; results will likely be way off"); + } + } + return true; +} + +void general_optimizer_data_record_imu(GeneralOptimizerData *d, PoserDataIMU *imu) { + if (d->seed_poser) { + d->seed_poser(d->so, &imu->hdr); + } +} + +void general_optimizer_data_dtor(GeneralOptimizerData *d) { + SurviveContext *ctx = d->so->ctx; + SV_INFO("\tseed runs %d / %d", d->stats.poser_seed_runs, d->stats.runs); + SV_INFO("\terror failures %d", d->stats.error_failures); +} diff --git a/src/poser_general_optimizer.h b/src/poser_general_optimizer.h new file mode 100644 index 0000000..81d94bf --- /dev/null +++ b/src/poser_general_optimizer.h @@ -0,0 +1,29 @@ +#include +#include + +typedef struct GeneralOptimizerData { + int failures_to_reset; + int failures_to_reset_cntr; + int successes_to_reset; + int successes_to_reset_cntr; + + FLT max_error; + + struct { + int runs; + int poser_seed_runs; + int error_failures; + } stats; + + PoserCB seed_poser; + SurviveObject *so; +} GeneralOptimizerData; + +void general_optimizer_data_init(GeneralOptimizerData *d, SurviveObject *so); +void general_optimizer_data_dtor(GeneralOptimizerData *d); + +void general_optimizer_data_record_failure(GeneralOptimizerData *d); +bool general_optimizer_data_record_success(GeneralOptimizerData *d, FLT error); +void general_optimizer_data_record_imu(GeneralOptimizerData *d, PoserDataIMU *imu); +bool general_optimizer_data_record_current_pose(GeneralOptimizerData *d, PoserData *hdr, size_t len_hdr, + SurvivePose *p); diff --git a/src/poser_sba.c b/src/poser_sba.c index 347b600..4c7fcd1 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -13,6 +13,7 @@ #include "assert.h" #include "linmath.h" #include "math.h" +#include "poser_general_optimizer.h" #include "string.h" #include "survive_cal.h" #include "survive_config.h" @@ -32,16 +33,11 @@ typedef struct { } sba_context_single_sweep; typedef struct SBAData { + GeneralOptimizerData opt; + int last_acode; int last_lh; - int failures_to_reset; - int failures_to_reset_cntr; - int successes_to_reset; - int successes_to_reset_cntr; - - FLT max_error; - FLT sensor_variance; FLT sensor_variance_per_second; int sensor_time_window; @@ -52,15 +48,8 @@ typedef struct SBAData { bool useIMU; struct { - int runs; - int poser_seed_runs; int meas_failures; - int error_failures; } stats; - - SurviveObject *so; - - PoserCB seed_poser; } SBAData; static void metric_function(int j, int i, double *aj, double *xij, void *adata) { @@ -98,7 +87,7 @@ static size_t construct_input(const SurviveObject *so, PoserDataFullScene *pdfs, static size_t construct_input_from_scene(SBAData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, char *vmask, double *meas, double *cov) { size_t rtn = 0; - SurviveObject *so = d->so; + SurviveObject *so = d->opt.so; for (size_t sensor = 0; sensor < so->sensor_ct; sensor++) { for (size_t lh = 0; lh < 2; lh++) { @@ -215,7 +204,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/, SurvivePose *out) { double *covx = 0; - SurviveObject *so = d->so; + SurviveObject *so = d->opt.so; char *vmask = alloca(sizeof(char) * so->sensor_ct * NUM_LIGHTHOUSES); double *meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES); @@ -241,37 +230,10 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } failure_count = 0; - SurvivePose soLocation = so->OutPose; - bool currentPositionValid = quatmagnitude(&soLocation.Rot[0]) != 0; - static bool seed_warning = false; - if (d->successes_to_reset_cntr == 0 || d->failures_to_reset_cntr == 0 || currentPositionValid == 0) { - SurviveContext *ctx = so->ctx; - // SV_INFO("Must rerun seed poser"); - PoserCB driver = d->seed_poser; + SurvivePose soLocation = {}; - if (driver) { - PoserData hdr = pdl->hdr; - memset(&pdl->hdr, 0, sizeof(pdl->hdr)); // Clear callback functions - pdl->hdr.pt = hdr.pt; - pdl->hdr.poseproc = sba_set_position; - - sba_set_position_t locations = {0}; - pdl->hdr.userdata = &locations; - driver(so, &pdl->hdr); - pdl->hdr = hdr; - d->stats.poser_seed_runs++; - - if (locations.hasInfo == false) { - return -1; - } else if (locations.hasInfo) { - soLocation = locations.poses; - } - - d->successes_to_reset_cntr = d->successes_to_reset; - } else if (seed_warning == false) { - seed_warning = true; - SV_INFO("Not using a seed poser for SBA; results will likely be way off"); - } + if (!general_optimizer_data_record_current_pose(&d->opt, &pdl->hdr, sizeof(*pdl), &soLocation)) { + return -1; } double opts[SBA_OPTSSZ] = {0}; @@ -286,7 +248,6 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive opts[3] = SBA_STOP_THRESH; // max_reproj_error * meas.size(); opts[4] = 0.0; - d->stats.runs++; int status = sba_str_levmar(1, // Number of 3d points 0, // Number of 3d points to fix in spot NUM_LIGHTHOUSES * so->sensor_ct, vmask, @@ -303,26 +264,14 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive opts, // options info); // info - if (currentPositionValid) { - // FLT distp[3]; - // sub3d(distp, so->OutPose.Pos, soLocation.Pos); - // FLT distance = magnitude3d(distp); - - // if (distance > 1.) - // status = -1; - } - double rtn = -1; bool status_failure = status <= 0; - bool error_failure = (info[1] / meas_size * 2) >= d->max_error; + bool error_failure = !general_optimizer_data_record_success(&d->opt, (info[1] / meas_size * 2)); if (!status_failure && !error_failure) { - d->failures_to_reset_cntr = d->failures_to_reset; quatnormalize(soLocation.Rot, soLocation.Rot); *out = soLocation; rtn = info[1] / meas_size * 2; - } else if (error_failure) { - d->stats.error_failures++; - } + } else { SurviveContext *ctx = so->ctx; @@ -349,7 +298,7 @@ static double run_sba(PoserDataFullScene *pdfs, SurviveObject *so, int max_itera .obj_pose = so->OutPose}; { - const char *subposer = survive_configs(so->ctx, "sba-seed-poser", SC_GET, "PoserEPNP"); + const char *subposer = survive_configs(so->ctx, "seed-poser", SC_GET, "PoserEPNP"); PoserCB driver = (PoserCB)GetDriver(subposer); SurviveContext *ctx = so->ctx; @@ -429,33 +378,25 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { if (so->PoserData == 0) { so->PoserData = calloc(1, sizeof(SBAData)); SBAData *d = so->PoserData; - d->failures_to_reset_cntr = 0; - d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1); - d->successes_to_reset_cntr = 0; - d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, -1); + + general_optimizer_data_init(&d->opt, so); d->useIMU = survive_configi(ctx, "sba-use-imu", SC_GET, 1); d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8); - d->max_error = survive_configf(ctx, "sba-max-error", SC_GET, .0001); + d->sensor_time_window = survive_configi(ctx, "sba-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2); d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 10.0); d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0); d->use_jacobian_function = survive_configi(ctx, "sba-use-jacobian-function", SC_GET, 1.0); - d->so = so; - const char *subposer = survive_configs(ctx, "sba-seed-poser", SC_GET, "PoserEPNP"); - d->seed_poser = (PoserCB)GetDriver(subposer); SV_INFO("Initializing SBA:"); SV_INFO("\tsba-required-meas: %d", d->required_meas); SV_INFO("\tsba-sensor-variance: %f", d->sensor_variance); SV_INFO("\tsba-sensor-variance-per-sec: %f", d->sensor_variance_per_second); SV_INFO("\tsba-time-window: %d", d->sensor_time_window); - SV_INFO("\tsba-max-error: %f", d->max_error); - SV_INFO("\tsba-successes-to-reset: %d", d->successes_to_reset); SV_INFO("\tsba-use-imu: %d", d->useIMU); SV_INFO("\tsba-use-jacobian-function: %d", d->use_jacobian_function); - SV_INFO("\tsba-seed-poser: %s(%p)", subposer, d->seed_poser); } SBAData *d = so->PoserData; switch (pd->pt) { @@ -477,8 +418,7 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { if (error < 0) { - if (d->failures_to_reset_cntr > 0) - d->failures_to_reset_cntr--; + } else { if (d->useIMU) { @@ -492,8 +432,6 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { } PoserData_poser_pose_func(&lightData->hdr, so, &estimate); - if (d->successes_to_reset_cntr > 0) - d->successes_to_reset_cntr--; } } return 0; @@ -507,9 +445,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { } case POSERDATA_DISASSOCIATE: { SV_INFO("SBA stats:"); - SV_INFO("\tseed runs %d / %d", d->stats.poser_seed_runs, d->stats.runs); SV_INFO("\tmeas failures %d", d->stats.meas_failures); - SV_INFO("\terror failures %d", d->stats.error_failures); + general_optimizer_data_dtor(&d->opt); free(d); so->PoserData = 0; return 0; @@ -522,12 +459,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { survive_imu_tracker_integrate(so, &d->tracker, imu); PoserData_poser_pose_func(pd, so, &d->tracker.pose); } - } // INTENTIONAL FALLTHROUGH - default: { - if (d->seed_poser) { - return d->seed_poser(so, pd); - } - break; + + general_optimizer_data_record_imu(&d->opt, imu); } } return -1; -- cgit v1.2.3 From 5985e89c8d7460f17ede727615bad795b3ab2c87 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 9 Apr 2018 09:14:06 -0600 Subject: Added MPFIT based poser --- src/poser_mpfit.c | 290 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 290 insertions(+) create mode 100644 src/poser_mpfit.c (limited to 'src') diff --git a/src/poser_mpfit.c b/src/poser_mpfit.c new file mode 100644 index 0000000..03ef522 --- /dev/null +++ b/src/poser_mpfit.c @@ -0,0 +1,290 @@ +#ifndef USE_DOUBLE +#define FLT double +#define USE_DOUBLE +#endif + +#include + +#include "mpfit/mpfit.h" +#include "poser.h" +#include +#include + +#include "assert.h" +#include "linmath.h" +#include "math.h" +#include "poser_general_optimizer.h" +#include "string.h" +#include "survive_cal.h" +#include "survive_config.h" +#include "survive_reproject.h" + +typedef struct { + SurviveObject *so; + FLT *pts3d; + int *lh; + FLT *meas; + SurvivePose camera_params[2]; +} mpfit_context; + +typedef struct MPFITData { + GeneralOptimizerData opt; + + int last_acode; + int last_lh; + + int sensor_time_window; + int use_jacobian_function; + int required_meas; + + SurviveIMUTracker tracker; + bool useIMU; + + struct { + int meas_failures; + } stats; +} MPFITData; + +static size_t construct_input_from_scene(MPFITData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, + double *meas, double *sensors, int *lhs) { + size_t rtn = 0; + SurviveObject *so = d->opt.so; + + 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)) { + const double *a = scene->angles[sensor][lh]; + + sensors[rtn * 3 + 0] = so->sensor_locations[sensor * 3 + 0]; + sensors[rtn * 3 + 1] = so->sensor_locations[sensor * 3 + 1]; + sensors[rtn * 3 + 2] = so->sensor_locations[sensor * 3 + 2]; + meas[rtn * 2 + 0] = a[0]; + meas[rtn * 2 + 1] = a[1]; + lhs[rtn] = lh; + rtn++; + } + } + } + return rtn; +} + +static void str_metric_function(int j, int i, double *bi, double *xij, void *adata) { + SurvivePose obj = *(SurvivePose *)bi; + int sensor_idx = j >> 1; + int lh = j & 1; + + mpfit_context *ctx = (mpfit_context *)(adata); + SurviveObject *so = ctx->so; + + assert(lh < 2); + assert(sensor_idx < so->sensor_ct); + + // quatnormalize(obj.Rot, obj.Rot); + + // std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl; + SurvivePose *camera = &so->ctx->bsd[lh].Pose; + survive_reproject_full(xij, &obj, &so->sensor_locations[sensor_idx * 3], camera, &so->ctx->bsd[lh], + &so->ctx->calibration_config); +} + +static void str_metric_function_jac(int j, int i, double *bi, double *xij, void *adata) { + SurvivePose obj = *(SurvivePose *)bi; + int sensor_idx = j >> 1; + int lh = j & 1; + + mpfit_context *ctx = (mpfit_context *)(adata); + SurviveObject *so = ctx->so; + + assert(lh < 2); + assert(sensor_idx < so->sensor_ct); + + // 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], + &so->ctx->calibration_config); +} + +int mpfunc(int m, int n, double *p, double *deviates, double **derivs, void *private) { + mpfit_context *mpfunc_ctx = private; + + SurvivePose *pose = (SurvivePose *)p; + + for (int i = 0; i < m / 2; i++) { + FLT out[2]; + survive_reproject_full(out, pose, mpfunc_ctx->pts3d + i * 3, &mpfunc_ctx->camera_params[mpfunc_ctx->lh[i]], + &mpfunc_ctx->so->ctx->bsd[mpfunc_ctx->lh[i]], &mpfunc_ctx->so->ctx->calibration_config); + assert(!isnan(out[0])); + assert(!isnan(out[1])); + deviates[i * 2 + 0] = out[0] - mpfunc_ctx->meas[i * 2 + 0]; + deviates[i * 2 + 1] = out[1] - mpfunc_ctx->meas[i * 2 + 1]; + + if (derivs) { + FLT out[7 * 2]; + survive_reproject_full_jac_obj_pose( + out, pose, mpfunc_ctx->pts3d + i * 3, &mpfunc_ctx->camera_params[mpfunc_ctx->lh[i]], + &mpfunc_ctx->so->ctx->bsd[mpfunc_ctx->lh[i]], &mpfunc_ctx->so->ctx->calibration_config); + + for (int j = 0; j < n; j++) { + derivs[j][i * 2 + 0] = out[j]; + derivs[j][i * 2 + 1] = out[j + 7]; + } + } + } + + return 0; +} + +static double run_mpfit_find_3d_structure(MPFITData *d, PoserDataLight *pdl, SurviveSensorActivations *scene, + int max_iterations /* = 50*/, double max_reproj_error /* = 0.005*/, + SurvivePose *out) { + double *covx = 0; + SurviveObject *so = d->opt.so; + + mpfit_context mpfitctx = {.so = so, + .camera_params = {so->ctx->bsd[0].Pose, so->ctx->bsd[1].Pose}, + .meas = alloca(sizeof(double) * 2 * so->sensor_ct * NUM_LIGHTHOUSES), + .lh = alloca(sizeof(int) * so->sensor_ct * NUM_LIGHTHOUSES), + .pts3d = alloca(sizeof(double) * 3 * so->sensor_ct * NUM_LIGHTHOUSES)}; + + size_t meas_size = 2 * construct_input_from_scene(d, pdl, scene, mpfitctx.meas, mpfitctx.pts3d, mpfitctx.lh); + + static int failure_count = 500; + bool hasAllBSDs = true; + for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) + hasAllBSDs &= so->ctx->bsd[lh].PositionSet; + + if (!hasAllBSDs || meas_size < d->required_meas) { + if (hasAllBSDs && failure_count++ == 500) { + SurviveContext *ctx = so->ctx; + SV_INFO("Can't solve for position with just %u measurements", (unsigned int)meas_size); + failure_count = 0; + } + if (meas_size < d->required_meas) { + d->stats.meas_failures++; + } + return -1; + } + failure_count = 0; + + SurvivePose soLocation = {}; + + if (!general_optimizer_data_record_current_pose(&d->opt, &pdl->hdr, sizeof(*pdl), &soLocation)) { + return -1; + } + + mp_result result = {}; + mp_par pars[7] = {}; + pars[0].parname = "X"; + pars[1].parname = "Y"; + pars[2].parname = "Z"; + pars[3].parname = "w"; + pars[4].parname = "i"; + pars[5].parname = "j"; + pars[6].parname = "k"; + + const bool debug_jacobian = false; + if (d->use_jacobian_function) { + for (int i = 0; i < 7; i++) { + if (debug_jacobian) { + pars[i].side = 1; + pars[i].deriv_debug = 1; + } else { + pars[i].side = 3; + } + } + } + + int res = mpfit(mpfunc, meas_size, 7, soLocation.Pos, pars, 0, &mpfitctx, &result); + + double rtn = -1; + bool status_failure = res <= 0; + bool error_failure = !general_optimizer_data_record_success(&d->opt, result.bestnorm); + if (!status_failure && !error_failure) { + quatnormalize(soLocation.Rot, soLocation.Rot); + *out = soLocation; + rtn = result.bestnorm; + } + + return rtn; +} + +int PoserMPFIT(SurviveObject *so, PoserData *pd) { + SurviveContext *ctx = so->ctx; + if (so->PoserData == 0) { + so->PoserData = calloc(1, sizeof(MPFITData)); + MPFITData *d = so->PoserData; + + general_optimizer_data_init(&d->opt, so); + d->useIMU = (bool)survive_configi(ctx, "mpfit-use-imu", SC_GET, 1); + d->required_meas = survive_configi(ctx, "mpfit-required-meas", SC_GET, 8); + + d->sensor_time_window = + survive_configi(ctx, "mpfit-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2); + d->use_jacobian_function = survive_configi(ctx, "mpfit-use-jacobian-function", SC_GET, 1.0); + + SV_INFO("Initializing MPFIT:"); + SV_INFO("\tmpfit-required-meas: %d", d->required_meas); + SV_INFO("\tmpfit-time-window: %d", d->sensor_time_window); + SV_INFO("\tmpfit-use-imu: %d", d->useIMU); + SV_INFO("\tmpfit-use-jacobian-function: %d", d->use_jacobian_function); + } + MPFITData *d = so->PoserData; + switch (pd->pt) { + case POSERDATA_LIGHT: { + // No poses if calibration is ongoing + if (ctx->calptr && ctx->calptr->stage < 5) + return 0; + SurviveSensorActivations *scene = &so->activations; + PoserDataLight *lightData = (PoserDataLight *)pd; + SurvivePose estimate; + + // only process sweeps + FLT error = -1; + if (d->last_lh != lightData->lh || d->last_acode != lightData->acode) { + error = run_mpfit_find_3d_structure(d, lightData, scene, 100, .5, &estimate); + + d->last_lh = lightData->lh; + d->last_acode = lightData->acode; + + if (error > 0) { + if (d->useIMU) { + FLT var_meters = 0.5; + FLT var_quat = error + .05; + FLT var[7] = {error * var_meters, error * var_meters, error * var_meters, error * var_quat, + error * var_quat, error * var_quat, error * var_quat}; + + survive_imu_tracker_integrate_observation(so, lightData->timecode, &d->tracker, &estimate, var); + estimate = d->tracker.pose; + } + + PoserData_poser_pose_func(&lightData->hdr, so, &estimate); + } + } + return 0; + } + + case POSERDATA_DISASSOCIATE: { + SV_INFO("MPFIT stats:"); + SV_INFO("\tmeas failures %d", d->stats.meas_failures); + general_optimizer_data_dtor(&d->opt); + free(d); + so->PoserData = 0; + return 0; + } + case POSERDATA_IMU: { + + PoserDataIMU *imu = (PoserDataIMU *)pd; + if (ctx->calptr && ctx->calptr->stage < 5) { + } else if (d->useIMU) { + survive_imu_tracker_integrate(so, &d->tracker, imu); + PoserData_poser_pose_func(pd, so, &d->tracker.pose); + } + + general_optimizer_data_record_imu(&d->opt, imu); + } + } + return -1; +} + +REGISTER_LINKTIME(PoserMPFIT); -- cgit v1.2.3 From 7f4385538d6b19ea5dd3f00c4a0eb49385798031 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Mon, 9 Apr 2018 09:33:40 -0600 Subject: Made new posers work in windows --- src/poser_epnp.c | 2 +- src/poser_general_optimizer.c | 3 ++- src/poser_mpfit.c | 15 ++++----------- src/poser_sba.c | 2 +- src/survive.c | 2 ++ 5 files changed, 10 insertions(+), 14 deletions(-) (limited to 'src') diff --git a/src/poser_epnp.c b/src/poser_epnp.c index 615c01c..851ce29 100644 --- a/src/poser_epnp.c +++ b/src/poser_epnp.c @@ -129,7 +129,7 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) { case POSERDATA_LIGHT: { PoserDataLight *lightData = (PoserDataLight *)pd; - SurvivePose posers[2] = {}; + SurvivePose posers[2] = { 0 }; int meas[2] = {0, 0}; for (int lh = 0; lh < so->ctx->activeLighthouses; lh++) { if (so->ctx->bsd[lh].PositionSet) { diff --git a/src/poser_general_optimizer.c b/src/poser_general_optimizer.c index 313e378..b3c4e65 100644 --- a/src/poser_general_optimizer.c +++ b/src/poser_general_optimizer.c @@ -1,6 +1,7 @@ #include "poser_general_optimizer.h" #include "string.h" -#include + +#include #include #include diff --git a/src/poser_mpfit.c b/src/poser_mpfit.c index 03ef522..439d151 100644 --- a/src/poser_mpfit.c +++ b/src/poser_mpfit.c @@ -167,22 +167,15 @@ static double run_mpfit_find_3d_structure(MPFITData *d, PoserDataLight *pdl, Sur } failure_count = 0; - SurvivePose soLocation = {}; + SurvivePose soLocation = { 0 }; if (!general_optimizer_data_record_current_pose(&d->opt, &pdl->hdr, sizeof(*pdl), &soLocation)) { return -1; } - mp_result result = {}; - mp_par pars[7] = {}; - pars[0].parname = "X"; - pars[1].parname = "Y"; - pars[2].parname = "Z"; - pars[3].parname = "w"; - pars[4].parname = "i"; - pars[5].parname = "j"; - pars[6].parname = "k"; - + mp_result result = { 0 }; + mp_par pars[7] = { 0 }; + const bool debug_jacobian = false; if (d->use_jacobian_function) { for (int i = 0; i < 7; i++) { diff --git a/src/poser_sba.c b/src/poser_sba.c index 4c7fcd1..5166951 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -230,7 +230,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } failure_count = 0; - SurvivePose soLocation = {}; + SurvivePose soLocation = { 0 }; if (!general_optimizer_data_record_current_pose(&d->opt, &pdl->hdr, sizeof(*pdl), &soLocation)) { return -1; diff --git a/src/survive.c b/src/survive.c index 9e750f9..1ab09da 100644 --- a/src/survive.c +++ b/src/survive.c @@ -114,6 +114,8 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) { MANUAL_DRIVER_REGISTRATION(PoserDummy) MANUAL_DRIVER_REGISTRATION(PoserEPNP) MANUAL_DRIVER_REGISTRATION(PoserSBA) + MANUAL_DRIVER_REGISTRATION(PoserCharlesRefine) + MANUAL_DRIVER_REGISTRATION(PoserMPFIT) MANUAL_DRIVER_REGISTRATION(DriverRegHTCVive) MANUAL_DRIVER_REGISTRATION(DriverRegPlayback) -- cgit v1.2.3 From d7c88592a5450a65f5359e23d87122a04d019503 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 10 Apr 2018 21:58:25 -0600 Subject: New impl of mahoney imu filter --- src/survive_imu.c | 141 +++++++++++++++++------------------------------------- 1 file changed, 43 insertions(+), 98 deletions(-) (limited to 'src') diff --git a/src/survive_imu.c b/src/survive_imu.c index 8f4266a..9cd0322 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -1,107 +1,56 @@ #include "survive_imu.h" #include "linmath.h" +#include "math.h" #include "survive_internal.h" +#include #include -//--------------------------------------------------------------------------------------------------- -// Definitions +// Mahoney is due to https://hal.archives-ouvertes.fr/hal-00488376/document +// See also http://www.olliw.eu/2013/imu-data-fusing/#chapter41 and +// http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ +static void mahony_ahrs(SurviveIMUTracker *tracker, LinmathVec3d _gyro, LinmathVec3d _accel) { + LinmathVec3d gyro; + memcpy(gyro, _gyro, 3 * sizeof(FLT)); -#define sampleFreq 240.0f // sample frequency in Hz -#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain -#define twoKiDef (2.0f * 0.0f) // 2 * integral gain + LinmathVec3d accel; + memcpy(accel, _accel, 3 * sizeof(FLT)); -//--------------------------------------------------------------------------------------------------- -// Function declarations + const FLT sample_f = 240; + const FLT prop_gain = .5; + const FLT int_gain = 0; -float invSqrt(float x) { - float halfx = 0.5f * x; - float y = x; - long i = *(long *)&y; - i = 0x5f3759df - (i >> 1); - y = *(float *)&i; - y = y * (1.5f - (halfx * y * y)); - return y; -} -//--------------------------------------------------------------------------------------------------- -// IMU algorithm update -// From http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ -static void MahonyAHRSupdateIMU(SurviveIMUTracker *tracker, float gx, float gy, float gz, float ax, float ay, - float az) { - float recipNorm; - float halfvx, halfvy, halfvz; - float halfex, halfey, halfez; - float qa, qb, qc; - - const float twoKp = twoKpDef; // 2 * proportional gain (Kp) - const float twoKi = twoKiDef; // 2 * integral gain (Ki) - - float q0 = tracker->pose.Rot[0]; - float q1 = tracker->pose.Rot[1]; - float q2 = tracker->pose.Rot[2]; - float q3 = tracker->pose.Rot[3]; - - // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) - if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - - // Normalise accelerometer measurement - recipNorm = invSqrt(ax * ax + ay * ay + az * az); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // Estimated direction of gravity and vector perpendicular to magnetic flux - halfvx = q1 * q3 - q0 * q2; - halfvy = q0 * q1 + q2 * q3; - halfvz = q0 * q0 - 0.5f + q3 * q3; - - // Error is sum of cross product between estimated and measured direction of gravity - halfex = (ay * halfvz - az * halfvy); - halfey = (az * halfvx - ax * halfvz); - halfez = (ax * halfvy - ay * halfvx); - - // Compute and apply integral feedback if enabled - if (twoKi > 0.0f) { - tracker->integralFBx += twoKi * halfex * (1.0f / sampleFreq); // tracker->integral error scaled by Ki - tracker->integralFBy += twoKi * halfey * (1.0f / sampleFreq); - tracker->integralFBz += twoKi * halfez * (1.0f / sampleFreq); - gx += tracker->integralFBx; // apply tracker->integral feedback - gy += tracker->integralFBy; - gz += tracker->integralFBz; - } else { - tracker->integralFBx = 0.0f; // prevent tracker->integral windup - tracker->integralFBy = 0.0f; - tracker->integralFBz = 0.0f; + FLT *q = tracker->pose.Rot; + + FLT mag_accel = magnitude3d(accel); + + if (mag_accel != 0.0) { + scale3d(accel, accel, 1. / mag_accel); + + // Equiv of q^-1 * G + LinmathVec3d v = {q[1] * q[3] - q[0] * q[2], q[0] * q[1] + q[2] * q[3], q[0] * q[0] - 0.5 + q[3] * q[3]}; + + LinmathVec3d error; + cross3d(error, accel, v); + + if (int_gain > 0.0f) { + LinmathVec3d fb_correction; + scale3d(fb_correction, error, int_gain * 2. / sample_f); + add3d(tracker->integralFB, tracker->integralFB, fb_correction); + add3d(gyro, gyro, tracker->integralFB); } - // Apply proportional feedback - gx += twoKp * halfex; - gy += twoKp * halfey; - gz += twoKp * halfez; + scale3d(error, error, prop_gain * 2.); + add3d(gyro, gyro, error); } - // Integrate rate of change of quaternion - gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors - gy *= (0.5f * (1.0f / sampleFreq)); - gz *= (0.5f * (1.0f / sampleFreq)); - qa = q0; - qb = q1; - qc = q2; - q0 += (-qb * gx - qc * gy - q3 * gz); - q1 += (qa * gx + qc * gz - q3 * gy); - q2 += (qa * gy - qb * gz + q3 * gx); - q3 += (qa * gz + qb * gy - qc * gx); - - // Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - - tracker->pose.Rot[0] = q0; - tracker->pose.Rot[1] = q1; - tracker->pose.Rot[2] = q2; - tracker->pose.Rot[3] = q3; + scale3d(gyro, gyro, 0.5 / sample_f); + + LinmathQuat correction = { + (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]), (q[0] * gyro[0] + q[2] * gyro[2] - q[3] * gyro[1]), + (q[0] * gyro[1] - q[1] * gyro[2] + q[3] * gyro[0]), (q[0] * gyro[2] + q[1] * gyro[1] - q[2] * gyro[0])}; + + quatadd(q, q, correction); + quatnormalize(q, q); } static inline uint32_t tick_difference(uint32_t most_recent, uint32_t least_recent) { @@ -125,7 +74,6 @@ void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, } //(pose->Pos[i] - tracker->lastGT.Pos[i]) / tick_difference(timecode, tracker->lastGTTime) * 48000000.; - tracker->integralFBx = tracker->integralFBy = tracker->integralFBz = 0.0; tracker->lastGTTime = timecode; tracker->lastGT = *pose; } @@ -198,10 +146,7 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker tracker->updir[i] = data->accel[i] * .10 + tracker->updir[i] * .90; } - float gx = data->gyro[0], gy = data->gyro[1], gz = data->gyro[2]; - float ax = data->accel[0], ay = data->accel[1], az = data->accel[2]; - - MahonyAHRSupdateIMU(tracker, gx, gy, gz, ax, ay, az); + mahony_ahrs(tracker, data->gyro, data->accel); FLT time_diff = tick_difference(data->timecode, tracker->last_data.timecode) / (FLT)so->timebase_hz; @@ -270,4 +215,4 @@ void survive_imu_tracker_integrate_observation(SurviveObject *so, uint32_t timec tracker->lastGTTime = timecode; tracker->lastGT = tracker->pose; -} \ No newline at end of file +} -- cgit v1.2.3