From 8ae8f5aa50ef7472a486ec6f3688e7dfb0c9dcab Mon Sep 17 00:00:00 2001 From: cnlohr Date: Sun, 24 Jun 2018 20:23:34 -0400 Subject: Refactor --- src/poser_charlesrefine.c | 342 +++++++++++++++++++++++++++------------------- src/survive_vive.c | 3 +- 2 files changed, 206 insertions(+), 139 deletions(-) diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index dbc7087..3e09925 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -21,7 +21,7 @@ 0.0001 // Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm #define CORRECT_LATERAL_POSITION_COEFFICIENT 0.7 // Explodes if you exceed 1.0 (Normally 0.7 for snappy non-IMU response) -#define CORRECT_TELESCOPTION_COEFFICIENT 7.0 // Converges even as high as 10.0 and doesn't explode. (Normally 7.0 for non-IMU respone) +#define CORRECT_TELESCOPTION_COEFFICIENT 7.0f // Converges even as high as 10.0 and doesn't explode. (Normally 7.0 for non-IMU respone) #define CORRECT_ROTATION_COEFFICIENT \ 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 @@ -43,9 +43,10 @@ typedef struct { SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP]; uint8_t sensor_ids[MAX_PT_PER_SWEEP]; - LinmathPoint3d imuvel; - LinmathPoint3d imu_accel_zero; - LinmathQuat imu_up_correct; +// LinmathPoint3d imuvel; +// LinmathPoint3d imu_accel_zero; +// int did_zero_imu; +// LinmathPoint3d imu_up_correct; LinmathPoint3d MixingPositions[NUM_LIGHTHOUSES][2]; LinmathPoint3d mixed_output; @@ -64,6 +65,43 @@ typedef struct { SurvivePose * lastlhp; } CharlesPoserData; + +void AdjustRotation( SurviveObject *so, LinmathQuat adjustment, int is_imu, int is_coarse ) +{ + CharlesPoserData *dd = so->PoserData; + + quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, adjustment ); + quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot); + + //XXX TODO: Calibrate the gyroscope using the lightcap. + + //Always update the accelerometer zero based on the gyro. + //quatrotatevector( dd->imu_accel_zero, applygyro, dd->imu_accel_zero ); + //Always update the system-corrective quat based on the gyro. + //quatrotateabout( dd->imu_up_correct, dd->imu_up_correct, applygyro ); +} + +void AdjustPosition( SurviveObject * so, LinmathPoint3d adjustment, int is_imu ) +{ + CharlesPoserData *dd = so->PoserData; + + add3d( dd->InteralPoseUsedForCalc.Pos, adjustment, dd->InteralPoseUsedForCalc.Pos); + add3d( dd->mixed_output, adjustment, dd->mixed_output); +} + +//Emits "dd->mixed_output" for position and dd->InteralPoseUsedForCalc.Rot for rotation. +void EmitPose( SurviveObject *so, PoserData *pd ) +{ + CharlesPoserData *dd = so->PoserData; + + SurvivePose object_pose_out; + copy3d( object_pose_out.Pos, dd->mixed_output ); + quatcopy( object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot ); + PoserData_poser_pose_func(pd, so, &object_pose_out); +} + + + int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { CharlesPoserData *dd = so->PoserData; if (!dd) @@ -72,7 +110,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { SurvivePose object_pose_out; memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); memcpy(&dd->InteralPoseUsedForCalc, &LinmathPose_Identity, sizeof(LinmathPose_Identity)); - memcpy(&dd->imu_up_correct, &LinmathQuat_Identity, sizeof(LinmathQuat_Identity) ); + //memcpy(&dd->imu_up_correct, &LinmathQuat_Identity, sizeof(LinmathQuat_Identity) ); so->PoseConfidence = 0.0; PoserData_poser_pose_func(pd, so, &object_pose_out); } @@ -87,26 +125,26 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { LinmathQuat object_to_world_with_imu_up_correct; LinmathQuat world_space_to_object_space_quat; - quatrotateabout( object_to_world_with_imu_up_correct, dd->InteralPoseUsedForCalc.Rot, dd->imu_up_correct ); - quatgetreciprocal(world_space_to_object_space_quat, object_to_world_with_imu_up_correct ); - + //quatrotateabout( object_to_world_with_imu_up_correct, dd->InteralPoseUsedForCalc.Rot, dd->imu_up_correct ); + //quatgetreciprocal(world_space_to_object_space_quat, object_to_world_with_imu_up_correct ); { - LinmathQuat applymotion; + LinmathQuat applygyro; imuData->gyro[0] *= imu_time; imuData->gyro[1] *= imu_time; imuData->gyro[2] *= imu_time; - quatfromeuler( applymotion, imuData->gyro ); - - quatrotateabout(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot, applymotion ); //Contribution from Gyro - quatnormalize(object_pose_out.Rot, object_pose_out.Rot); - quatcopy( dd->InteralPoseUsedForCalc.Rot, object_pose_out.Rot); + quatfromeuler( applygyro, imuData->gyro ); + AdjustRotation( so, applygyro, 1, 0 ); } + EmitPose( so, pd ); + +#if 0 //This will be overwritten by the accelerometer updates. //We do this here in case we want to use it later. copy3d( object_pose_out.Pos, dd->InteralPoseUsedForCalc.Pos ); + // Do accelerometer based stuff. if( so->PoseConfidence > POSE_CONFIDENCE_FOR_HANDLING_LINEAR_IMU ) { @@ -119,22 +157,43 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { LinmathVec3d correct_diff; quatrotatevector(reright, world_space_to_object_space_quat, reright); sub3d( correct_diff, normup, reright ); - scale3d( correct_diff, correct_diff, -0.01 ); //This is the coefficient applying the drag. XXX THIS MUST CHANGE. + scale3d( correct_diff, correct_diff, -0.001 ); //This is the coefficient applying the drag. XXX THIS MUST CHANGE. add3d( correct_diff, correct_diff, reright ); normalize3d( correct_diff, correct_diff ); LinmathQuat reright_quat; quatfrom2vectors( reright_quat, reright, correct_diff ); //Push to correct "Up" a little bit. - - quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, reright_quat); + //Do this if we want to use the IMU's up to help out the libsurvive estimate of "up" + // quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, reright_quat); - //quatrotateabout(dd->imu_up_correct, dd->imu_up_correct, reright_quat); - //quatnormalize(dd->imu_up_correct, dd->imu_up_correct); + // quatrotateabout(dd->imu_up_correct, dd->imu_up_correct, reright_quat); + // quatnormalize(dd->imu_up_correct, dd->imu_up_correct); } + //Update position as a function from the IMU... - if(0 ) { + if( 1 ) { + LinmathVec3d acceleration; + scale3d( acceleration, imuData->accel, 1.0 ); + + if( !dd->did_zero_imu ) + { + copy3d( dd->imu_accel_zero, acceleration ); + dd->did_zero_imu = 1; + } + else + { + LinmathVec3d recalingval; + scale3d( recalingval, acceleration, 0.0001 ); + scale3d( dd->imu_accel_zero, dd->imu_accel_zero, 0.9999 ); + add3d( dd->imu_accel_zero, dd->imu_accel_zero, recalingval ); + } + + sub3d( acceleration, acceleration, dd->imu_accel_zero ); + scale3d( acceleration, acceleration, 9.8 * imu_time ); + printf( "ACCEL %f %f %f\n", PFTHREE( acceleration ) ); +#if 0 LinmathVec3d expected_up = { 0, 0, 1 }; LinmathVec3d acceleration; scale3d( acceleration, imuData->accel, 1.0 ); @@ -169,6 +228,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { //Update actual location. add3d( dd->InteralPoseUsedForCalc.Pos, dd->InteralPoseUsedForCalc.Pos, updatepos ); add3d( dd->mixed_output, dd->mixed_output, updatepos ); +#endif } } @@ -178,7 +238,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, dd->imu_up_correct ); quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); PoserData_poser_pose_func(pd, so, &object_pose_out); - +#endif return 0; } case POSERDATA_LIGHT: { @@ -298,127 +358,131 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { int validpoints = 0; int ptvalid[MAX_PT_PER_SWEEP]; FLT avgerr = 0.0; - FLT vec_correct[3] = {0., 0., 0.}; FLT avgang = 0.0; - // Step 1: Determine standard of deviation, and average in order to - // drop points that are likely in error. { - // Calculate average - FLT avgerr_orig = 0.0; - FLT stddevsq = 0.0; - for (i = 0; i < pts; i++) - avgerr_orig += dd->quantity_errors[i]; - avgerr_orig /= pts; - - // Calculate standard of deviation. - for (i = 0; i < pts; i++) { - FLT diff = dd->quantity_errors[i] - avgerr_orig; - stddevsq += diff * diff; - } - stddevsq /= pts; - - for (i = 0; i < pts; i++) { - FLT err = dd->quantity_errors[i]; - FLT diff = err - avgerr_orig; - diff *= diff; - int isptvalid = (diff * MIN_HIT_QUALITY <= stddevsq + HIT_QUALITY_BASELINE) ? 1 : 0; - ptvalid[i] = isptvalid; - if (isptvalid) { - avgang += dd->angles_at_pts[i]; - avgerr += err; - validpoints++; + FLT vec_correct[3] = {0., 0., 0.}; + // Step 1: Determine standard of deviation, and average in order to + // drop points that are likely in error. + { + // Calculate average + FLT avgerr_orig = 0.0; + FLT stddevsq = 0.0; + for (i = 0; i < pts; i++) + avgerr_orig += dd->quantity_errors[i]; + avgerr_orig /= pts; + + // Calculate standard of deviation. + for (i = 0; i < pts; i++) { + FLT diff = dd->quantity_errors[i] - avgerr_orig; + stddevsq += diff * diff; + } + stddevsq /= pts; + + for (i = 0; i < pts; i++) { + FLT err = dd->quantity_errors[i]; + FLT diff = err - avgerr_orig; + diff *= diff; + int isptvalid = (diff * MIN_HIT_QUALITY <= stddevsq + HIT_QUALITY_BASELINE) ? 1 : 0; + ptvalid[i] = isptvalid; + if (isptvalid) { + avgang += dd->angles_at_pts[i]; + avgerr += err; + validpoints++; + } } + avgang /= validpoints; + avgerr /= validpoints; } - avgang /= validpoints; - avgerr /= validpoints; - } - // Step 2: Determine average lateral error. - // We can actually always perform this operation. Even with only one point. - if ( so->PoseConfidence > MINIMUM_CONFIDENCE_TO_CORRECT_POSITION ) - { - FLT avg_err[3] = {0, 0, 0}; // Positional error. - for (i = 0; i < pts; i++) { - if (!ptvalid[i]) - continue; - FLT *nrm = dd->normal_at_errors[i]; - FLT err = dd->quantity_errors[i]; - avg_err[0] = avg_err[0] + nrm[0] * err; - avg_err[1] = avg_err[1] + nrm[1] * err; - avg_err[2] = avg_err[2] + nrm[2] * err; - } + // Step 2: Determine average lateral error. + // We can actually always perform this operation. Even with only one point. + if ( so->PoseConfidence > MINIMUM_CONFIDENCE_TO_CORRECT_POSITION ) + { + FLT avg_err[3] = {0, 0, 0}; // Positional error. + for (i = 0; i < pts; i++) { + if (!ptvalid[i]) + continue; + FLT *nrm = dd->normal_at_errors[i]; + FLT err = dd->quantity_errors[i]; + avg_err[0] = avg_err[0] + nrm[0] * err; + avg_err[1] = avg_err[1] + nrm[1] * err; + avg_err[2] = avg_err[2] + nrm[2] * err; + } - // NOTE: The "avg_err" is not geometrically centered. This is actually - // probably okay, since if you have sevearl data points to one side, you - // can probably trust that more. - scale3d(avg_err, avg_err, 1. / validpoints); + // NOTE: The "avg_err" is not geometrically centered. This is actually + // probably okay, since if you have sevearl data points to one side, you + // can probably trust that more. + scale3d(avg_err, avg_err, 1. / validpoints); - // We have "Average error" now. A vector in worldspace. - // This can correct for lateral error, but not distance from camera. + // We have "Average error" now. A vector in worldspace. + // This can correct for lateral error, but not distance from camera. - // XXX TODO: Should we check to see if we only have one or - // two points to make sure the error on this isn't unusually high? - // If calculated error is unexpectedly high, then we should probably - // Not apply the transform. + // XXX TODO: Should we check to see if we only have one or + // two points to make sure the error on this isn't unusually high? + // If calculated error is unexpectedly high, then we should probably + // Not apply the transform. - if( ( magnitude3d( avg_err ) < MAX_JUMP_DISTANCE || so->PoseConfidence < 0.8 ) ) - { - scale3d(avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT); - add3d(vec_correct, vec_correct, avg_err); - applied_corrections++; - } - else - { - so->PoseConfidence *= 0.9; + if( ( magnitude3d( avg_err ) < MAX_JUMP_DISTANCE || so->PoseConfidence < 0.8 ) ) + { + scale3d(avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT); + add3d(vec_correct, vec_correct, avg_err); + applied_corrections++; + } + else + { + so->PoseConfidence *= 0.9; + } } - } - // Step 3: Control telecoption from lighthouse. - // we need to find out what the weighting is to determine "zoom" - if (validpoints > 1 && so->PoseConfidence > MINIMUM_CONFIDENCE_TO_CORRECT_POSITION ) // Can't correct "zoom" with only one point. - { - FLT zoom = 0.0; - FLT rmsang = 0.0; - for (i = 0; i < pts; i++) { - if (!ptvalid[i]) - continue; - FLT delang = dd->angles_at_pts[i] - avgang; - FLT delerr = dd->quantity_errors[i] - avgerr; - if (axis) - delang *= -1; // Flip sign on alternate axis because it's measured backwards. - zoom += delerr * delang; - rmsang += delang * delang; - } + // Step 3: Control telecoption from lighthouse. + // we need to find out what the weighting is to determine "zoom" + if (validpoints > 1 && so->PoseConfidence > MINIMUM_CONFIDENCE_TO_CORRECT_POSITION ) // Can't correct "zoom" with only one point. + { + FLT zoom = 0.0; + FLT rmsang = 0.0; + for (i = 0; i < pts; i++) { + if (!ptvalid[i]) + continue; + FLT delang = dd->angles_at_pts[i] - avgang; + FLT delerr = dd->quantity_errors[i] - avgerr; + if (axis) + delang *= -1; // Flip sign on alternate axis because it's measured backwards. + zoom += delerr * delang; + rmsang += delang * delang; + } - // Control into or outof lighthouse. - // XXX Check to see if we need to sqrt( the rmsang), need to check convergance behavior close to - // lighthouse. - // This is a questionable step. - zoom /= sqrt(rmsang); + // Control into or outof lighthouse. + // XXX Check to see if we need to sqrt( the rmsang), need to check convergance behavior close to + // lighthouse. + // This is a questionable step. + zoom /= sqrt(rmsang); - zoom *= CORRECT_TELESCOPTION_COEFFICIENT; + zoom *= CORRECT_TELESCOPTION_COEFFICIENT; - //Don't apply completely wild zoom's unless our confidence is awful. - if( ( zoom < MAX_JUMP_DISTANCE || so->PoseConfidence < 0.8 ) ) - { - FLT veccamalong[3]; - sub3d(veccamalong, lh_pose->Pos, dd->InteralPoseUsedForCalc.Pos); - normalize3d(veccamalong, veccamalong); - scale3d(veccamalong, veccamalong, zoom); - add3d(vec_correct, veccamalong, vec_correct); - applied_corrections++; - } - else - { - so->PoseConfidence *= 0.9; + //Don't apply completely wild zoom's unless our confidence is awful. + if( ( zoom < MAX_JUMP_DISTANCE || so->PoseConfidence < 0.8 ) ) + { + FLT veccamalong[3]; + sub3d(veccamalong, lh_pose->Pos, dd->InteralPoseUsedForCalc.Pos); + normalize3d(veccamalong, veccamalong); + scale3d(veccamalong, veccamalong, zoom); + add3d(vec_correct, veccamalong, vec_correct); + applied_corrections++; + } + else + { + so->PoseConfidence *= 0.9; + } } + AdjustPosition( so, vec_correct, 0 ); } +#if 0 //Tricky: Update position here, and back-correct imuvel based on correction. if( 0 ) { //XXX XXX TODO Position update LinmathPoint3d vecc; @@ -431,12 +495,12 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { LinmathQuat world_to_object_space; quatgetreciprocal(world_to_object_space, dd->InteralPoseUsedForCalc.Rot); quatrotatevector( vecc, world_to_object_space, vecc ); - add3d( dd->imu_accel_zero, dd->imu_accel_zero, vecc ); - printf( "ACCELV: %f %f %f %f %f %f\n", vecc[0], vecc[1], vecc[2], dd->imu_accel_zero[0], dd->imu_accel_zero[1], dd->imu_accel_zero[2] ); + //add3d( dd->imu_accel_zero, dd->imu_accel_zero, vecc ); + //printf( "ACCELV: %f %f %f %f %f %f\n", vecc[0], vecc[1], vecc[2], dd->imu_accel_zero[0], dd->imu_accel_zero[1], dd->imu_accel_zero[2] ); } } +#endif - add3d( dd->InteralPoseUsedForCalc.Pos, vec_correct, dd->InteralPoseUsedForCalc.Pos); // 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 @@ -460,7 +524,9 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { LinmathQuat world_to_object_space; quatgetreciprocal(world_to_object_space, object_pose_at_hit->Rot); - //First, check to see if this hit is a sensor that is facing the lighthouse. + //4A: First, check to see if this hit is a sensor that is facing the lighthouse. + //This is for coarse corrections early on in the calibration. + //If one of these happens it means the orientation/pose is totally impossible. if( so->PoseConfidence < 0.9 ) { LinmathPoint3d vector_to_lighthouse; sub3d( vector_to_lighthouse, lhp->Pos, object_pose_at_hit->Pos ); //Get vector in world space. @@ -479,9 +545,8 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { cross3d( rotateaxis, vector_to_lighthouse, sensornormal ); LinmathQuat correction; quatfromaxisangle(correction, rotateaxis, facingness*.2 ); - quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, correction); - quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot); normal_faults ++; + AdjustRotation( so, correction, 0, 1 ); } } } @@ -528,14 +593,12 @@ 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(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, correction); - quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot); + AdjustRotation( so, correction, 0, 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; - //Box filter all of the guesstimations, and emit the new guess. { FLT MixedAmount = 0; @@ -552,7 +615,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { MixedAmount += Confidence; //printf( "%f ", Confidence ); } - scale3d( MixedPosition, MixedPosition, 1./MixedAmount ); + scale3d( dd->mixed_output, MixedPosition, 1./MixedAmount ); #if 0 printf( "Reprojection disagreements:" ); for( l = 0; l < NUM_LIGHTHOUSES; l++ ) for( a = 0; a < 2; a++ ) @@ -561,16 +624,19 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { } printf( "\n" ); #endif + EmitPose( so, pd ); //printf( "%f\n", MixedAmount ); - SurvivePose object_pose_out; - quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot ); - copy3d( object_pose_out.Pos, MixedPosition ); - - copy3d( dd->mixed_output, object_pose_out.Pos ); - quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, dd->imu_up_correct ); - quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); - PoserData_poser_pose_func(pd, so, &object_pose_out); + + // SurvivePose object_pose_out; + // quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot ); + // copy3d( object_pose_out.Pos, MixedPosition ); + + // copy3d( dd->mixed_output, object_pose_out.Pos ); + // quatrotateabout( object_pose_out.Rot, object_pose_out.Rot, dd->imu_up_correct ); + // quatnormalize( object_pose_out.Rot, object_pose_out.Rot ); + // PoserData_poser_pose_func(pd, so, &object_pose_out); + } // FLT var_meters = 0.5; // FLT error = 0.00001; diff --git a/src/survive_vive.c b/src/survive_vive.c index a83c6a1..0c8f583 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -491,10 +491,11 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject * SV_ERROR( "Could not claim interface %d of %s", j, devnames[i] ); return -9; } - usleep(20000); } SV_INFO( "Successfully enumerated %s (%d, %d)", devnames[i], did, conf->bNumInterfaces ); + + usleep(100000); } libusb_free_device_list( devs, 1 ); -- cgit v1.2.3