diff options
Diffstat (limited to 'src/poser_charlesrefine.c')
| -rw-r--r-- | src/poser_charlesrefine.c | 565 |
1 files changed, 321 insertions, 244 deletions
diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index b14d882..e95b621 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -1,5 +1,3 @@ -// Driver works, but you _must_ start it near the origin looking in +Z. - #include <poser.h> #include <survive.h> #include <survive_reproject.h> @@ -15,25 +13,118 @@ #define MAX_PT_PER_SWEEP SENSORS_PER_OBJECT +/* + More notes to self: + try updating expected lighthouse position against each lighthouse with imu data and don't tie them to each other. + +*/ + + +// Tunable parameters: +#define MIN_HIT_QUALITY 0.5 // Determines which hits to cull. +#define HIT_QUALITY_BASELINE \ + 0.0001 // Determines which hits to cull. Actually SQRT(baseline) if 0.0001, it is really 1cm + +#define LIGHTCAP_DESCALE 0.2 //DO NOT EXCEED 0.7 + +#define CORRECT_LATERAL_POSITION_COEFFICIENT LIGHTCAP_DESCALE // Explodes if you exceed 1.0 (Normally 0.7 for snappy non-IMU response) +#define CORRECT_TELESCOPTION_COEFFICIENT (10.f*LIGHTCAP_DESCALE) // 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 +#define MINIMUM_CONFIDENCE_TO_CORRECT_POSITION 0.02 // 0.2 +#define POSE_CONFIDENCE_FOR_HANDLING_LINEAR_IMU .9 //0.9 +#define MAX_JUMP_DISTANCE 0.5 //0.4 + + +//Grand todo: +// Update global "Up" vector from LH's PoV based on "Up" from the HMD. + typedef struct { int sweepaxis; int sweeplh; FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; // Value is actually normalized, not just normal to sweep plane. - FLT quantity_errors[MAX_PT_PER_SWEEP]; + FLT quantity_errors[MAX_PT_PER_SWEEP]; //Dot product of error offset. 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. + LinmathPoint3d mixed_output; + + //Super high speed vibratey and terrible. + //Also, big deal: this does NOT have the "re-righting" vector + //from the accelerometer applied to it. + SurvivePose InteralPoseUsedForCalc; + FLT MixingConfidence[NUM_LIGHTHOUSES][2]; FLT last_angle_lh_axis[NUM_LIGHTHOUSES][2]; int ptsweep; SurviveIMUTracker tracker; + SurvivePose * lastlhp; + + + //Additional flags, used when we start to try to use the accelerometer + LinmathPoint3d average_accelerometer_up_vector_in_world_space; + LinmathPoint3d velocity_according_to_accelerometer; } CharlesPoserData; + +void AdjustRotation( SurviveObject *so, LinmathQuat adjustment, int is_imu, int is_coarse ) +{ + CharlesPoserData *dd = so->PoserData; + //LinmathQuat invert_adjust; + //quatgetreciprocal( invert_adjust, adjustment ); + quatrotateabout(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot, adjustment ); + quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot); + +} + +void AdjustPosition( SurviveObject * so, LinmathPoint3d adjustment, int is_imu, float descale ) +{ + CharlesPoserData *dd = so->PoserData; + + add3d( dd->InteralPoseUsedForCalc.Pos, adjustment, dd->InteralPoseUsedForCalc.Pos); + add3d( dd->mixed_output, adjustment, dd->mixed_output); + + if( descale > 0.0001 ) //Coming from lightcap. + { + LinmathPoint3d backflow; + scale3d( backflow, adjustment, 1.0/descale ); + CharlesPoserData *dd = so->PoserData; + //XXX TODO figure out how to dampen velocity. + add3d( dd->velocity_according_to_accelerometer, dd->velocity_according_to_accelerometer, backflow ); + scale3d( backflow, backflow, .001 ); + add3d( dd->average_accelerometer_up_vector_in_world_space, dd->average_accelerometer_up_vector_in_world_space, backflow ); + } + +} + +//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 ); + + //average_accelerometer_up_vector_in_world_space should be "up" + LinmathVec3d true_up = { 0, 0, 1 }; + LinmathVec3d dist_up; + normalize3d( dist_up, dd->average_accelerometer_up_vector_in_world_space ); //NOTE: Average vector probably won't be normalized. + LinmathQuat adjustment_from_rerighting_up; + quatfrom2vectors( adjustment_from_rerighting_up, dist_up, true_up ); + + quatcopy( object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot ); + quatrotateabout( object_pose_out.Rot, adjustment_from_rerighting_up, dd->InteralPoseUsedForCalc.Rot ); + PoserData_poser_pose_func(pd, so, &object_pose_out); +} + + + int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { CharlesPoserData *dd = so->PoserData; if (!dd) @@ -42,57 +133,63 @@ 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)); - so->PoseConfidence = 1.0; + so->PoseConfidence = 0.0; PoserData_poser_pose_func(pd, so, &object_pose_out); } SurviveSensorActivations *scene = &so->activations; switch (pd->pt) { case POSERDATA_IMU: { + float imu_time = 1./ so->imu_freq; // Really should use this... PoserDataIMU *imuData = (PoserDataIMU *)pd; - //TODO: Actually do Madgwick's algorithm - LinmathQuat applymotion; - const SurvivePose * object_pose = &so->OutPose; - imuData->gyro[0] *= 1./1000.; - imuData->gyro[1] *= 1./1000.; - imuData->gyro[2] *= 1./1000.; - 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 ); + { + LinmathQuat applygyro; + imuData->gyro[0] *= imu_time; + imuData->gyro[1] *= imu_time; + imuData->gyro[2] *= imu_time; + quatfromeuler( applygyro, imuData->gyro ); + AdjustRotation( so, applygyro, 1, 0 ); + } + //printf( "ACCEL %f %f %f\n", PFTHREE( imuData->accel ) ); - SurvivePose object_pose_out; - 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); + { + LinmathPoint3d rotated_out; + quatrotatevector( rotated_out, dd->InteralPoseUsedForCalc.Rot, imuData->accel ); + + if( so->PoseConfidence > 0.6 ) + { + LinmathPoint3d correction; + //XXX Danger, will robinson. + //We are doing an IIR on the acceleration. Tests have shown THIS IS BAD. We should try to correct based on the lightcap data. + scale3d( dd->average_accelerometer_up_vector_in_world_space, dd->average_accelerometer_up_vector_in_world_space, .999 ); + scale3d( correction, rotated_out, .001 ); + add3d( dd->average_accelerometer_up_vector_in_world_space, dd->average_accelerometer_up_vector_in_world_space, correction ); - 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); + LinmathPoint3d deviation; + sub3d( deviation, rotated_out, dd->average_accelerometer_up_vector_in_world_space ); - //PoserDataIMU *imu = (PoserDataIMU *)pd; - //survive_imu_tracker_integrate(so, &dd->tracker, imu); - //PoserData_poser_pose_func(pd, so, &dd->tracker.pose); + LinmathPoint3d acc; + scale3d( acc, deviation, 9.8*imu_time ); + add3d( dd->velocity_according_to_accelerometer, acc, dd->velocity_according_to_accelerometer ); + scale3d( dd->velocity_according_to_accelerometer, dd->velocity_according_to_accelerometer, .999 ); //XXX Danger! We are doing an IIR on velocity. This is dangerous. + LinmathPoint3d posdiff; + scale3d( posdiff, dd->velocity_according_to_accelerometer, imu_time ); + AdjustPosition( so, posdiff, 1, 0 ); + } + else + { + copy3d( dd->average_accelerometer_up_vector_in_world_space, rotated_out ); + scale3d( dd->velocity_according_to_accelerometer, dd->velocity_according_to_accelerometer, 0 ); + } + } + + EmitPose( so, pd ); return 0; } case POSERDATA_LIGHT: { @@ -103,38 +200,25 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { BaseStationData *bsd = &so->ctx->bsd[ld->lh]; if (!bsd->PositionSet) break; - SurvivePose *lhp = &bsd->Pose; + SurvivePose *lhp = dd->lastlhp = &bsd->Pose; FLT inangle = ld->angle; int sensor_id = ld->sensor_id; int axis = dd->sweepaxis; - //const SurvivePose *object_pose = &so->OutPose; dd->sweeplh = lhid; - // FOR NOW, drop LH1. - // if( lhid == 1 ) break; - - // const FLT * sensor_normal = &so->sensor_normals[senid*3]; - // FLT sensor_normal_worldspace[3]; - // ApplyPoseToPoint(sensor_normal_worldspace, object_pose, sensor_inpos); - const FLT *sensor_inpos = &so->sensor_locations[senid * 3]; 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, &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] ); - // = sensor position, relative to lighthouse center. FLT sensorpos_rel_lh[3]; sub3d(sensorpos_rel_lh, sensor_position_worldspace, lhp->Pos); // Next, define a normal in global space of the plane created by the sweep hit. // Careful that this must be normalized. FLT sweep_normal[3]; - - FLT inangles[2]; FLT outangles[2]; inangles[axis] = inangle; @@ -195,15 +279,14 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { case POSERDATA_SYNC: { PoserDataLight *l = (PoserDataLight *)pd; int lhid = l->lh; - // you can get sweepaxis and sweeplh. if (dd->ptsweep) { int i; + int applied_corrections = 0; + int normal_faults = 0; 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. BaseStationData *bsd = &so->ctx->bsd[lhid]; SurvivePose *lh_pose = &bsd->Pose; @@ -211,141 +294,128 @@ 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; -// Tunable parameters: -#define MIN_HIT_QUALITY 0.5 // Determines which hits to cull. -#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.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 \ - 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 - // 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; } - } - avgang /= validpoints; - avgerr /= validpoints; - } - - // Step 2: Determine average lateral error. - // We can actually always perform this operation. Even with only one point. - { - 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; + 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; } - // 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. + // 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; + } - // 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. - scale3d(avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT); - add3d(vec_correct, vec_correct, avg_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); + // 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. - // 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. - { - 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; + 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; + } } - // 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; - - FLT veccamalong[3]; - sub3d(veccamalong, lh_pose->Pos, dd->InteralPoseUsedForCalc.Pos); - normalize3d(veccamalong, veccamalong); - scale3d(veccamalong, veccamalong, zoom); - add3d(vec_correct, veccamalong, vec_correct); - } - - -#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); + // 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; + } - //quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot); + // 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; + + //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, (applied_corrections==2)?LIGHTCAP_DESCALE:0 ); + } // 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 @@ -356,71 +426,88 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { for (i = 0; i < pts; i++) { if (!ptvalid[i]) continue; - FLT dist = dd->quantity_errors[i] - avgerr; + + FLT dist = dd->quantity_errors[i] - avgerr; //Relative dot-product error. FLT angle = dd->angles_at_pts[i]; int sensor_id = dd->sensor_ids[i]; - FLT *normal = dd->normal_at_errors[i]; - const SurvivePose *object_pose_at_hit = &dd->object_pose_at_hit[i]; - const FLT *sensor_inpos = &so->sensor_locations[sensor_id * 3]; + FLT * normal = dd->normal_at_errors[i]; + FLT * sensornormal = &so->sensor_normals[sensor_id*3]; + SurvivePose * lhp = dd->lastlhp; + const SurvivePose * object_pose_at_hit = &dd->object_pose_at_hit[i]; + const FLT * sensor_inpos = &so->sensor_locations[sensor_id * 3]; LinmathQuat world_to_object_space; quatgetreciprocal(world_to_object_space, object_pose_at_hit->Rot); - FLT correction_in_object_space[3]; // The amount across the surface of the object the rotation - // should happen. - - quatrotatevector(correction_in_object_space, world_to_object_space, normal); - dist *= CORRECT_ROTATION_COEFFICIENT; - if (dist > ROTATIONAL_CORRECTION_MAXFORCE) - dist = ROTATIONAL_CORRECTION_MAXFORCE; - if (dist < -ROTATIONAL_CORRECTION_MAXFORCE) - dist = -ROTATIONAL_CORRECTION_MAXFORCE; - - // Now, we have a "tug" vector in object-local space. Need to apply the torque. - FLT vector_from_center_of_object[3]; - normalize3d(vector_from_center_of_object, sensor_inpos); - // scale3d(vector_from_center_of_object, sensor_inpos, 10.0 ); - // vector_from_center_of_object[2]*=-1; - // vector_from_center_of_object[1]*=-1; - // vector_from_center_of_object[0]*=-1; - // vector_from_center_of_object - scale3d(vector_from_center_of_object, vector_from_center_of_object, 1); - - FLT new_vector_in_object_space[3]; - // printf( "%f %f %f %f\n", object_pose_at_hit->Rot[0], object_pose_at_hit->Rot[1], - // object_pose_at_hit->Rot[2], object_pose_at_hit->Rot[3] ); - // printf( "%f %f %f // %f %f %f // %f\n", vector_from_center_of_object[0], - // vector_from_center_of_object[1], vector_from_center_of_object[2], correction_in_object_space[0], - // correction_in_object_space[1], correction_in_object_space[2], dist ); - scale3d(correction_in_object_space, correction_in_object_space, -dist); - add3d(new_vector_in_object_space, vector_from_center_of_object, correction_in_object_space); - - normalize3d(new_vector_in_object_space, new_vector_in_object_space); - - LinmathQuat corrective_quaternion; - quatfrom2vectors(corrective_quaternion, vector_from_center_of_object, new_vector_in_object_space); - quatrotateabout(correction, correction, corrective_quaternion); - // printf( "%f -> %f %f %f => %f %f %f [%f %f %f %f]\n", dist, vector_from_center_of_object[0], - // vector_from_center_of_object[1], vector_from_center_of_object[2], - // correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2], - // corrective_quaternion[0],corrective_quaternion[1],corrective_quaternion[1],corrective_quaternion[3]); + + //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. + normalize3d( vector_to_lighthouse, vector_to_lighthouse ); + quatrotatevector( vector_to_lighthouse, world_to_object_space, vector_to_lighthouse ); + float facingness = dot3d( sensornormal, vector_to_lighthouse ); + if( facingness < -.1 ) + { + //This is an impossible sensor hit. + so->PoseConfidence *= 0.8; + + //If our pose confidence is low, apply a torque. + if( so->PoseConfidence < 0.8 ) + { + LinmathPoint3d rotateaxis; + cross3d( rotateaxis, vector_to_lighthouse, sensornormal ); + LinmathQuat correction; + quatfromaxisangle(correction, rotateaxis, facingness*.2 ); + normal_faults ++; + AdjustRotation( so, correction, 0, 1 ); + } + } + } + + //Apply the normal tug. + { + + FLT correction_in_object_space[3]; // The amount across the surface of the object the rotation + // should happen. + + quatrotatevector(correction_in_object_space, world_to_object_space, normal); + dist *= CORRECT_ROTATION_COEFFICIENT; + if (dist > ROTATIONAL_CORRECTION_MAXFORCE) + dist = ROTATIONAL_CORRECTION_MAXFORCE; + if (dist < -ROTATIONAL_CORRECTION_MAXFORCE) + dist = -ROTATIONAL_CORRECTION_MAXFORCE; + + // Now, we have a "tug" vector in object-local space. Need to apply the torque. + FLT vector_from_center_of_object[3]; + normalize3d(vector_from_center_of_object, sensor_inpos); + scale3d(vector_from_center_of_object, vector_from_center_of_object, 1); + + FLT new_vector_in_object_space[3]; + scale3d(correction_in_object_space, correction_in_object_space, -dist); + add3d(new_vector_in_object_space, vector_from_center_of_object, correction_in_object_space); + + normalize3d(new_vector_in_object_space, new_vector_in_object_space); + + LinmathQuat corrective_quaternion; + quatfrom2vectors(corrective_quaternion, vector_from_center_of_object, new_vector_in_object_space); + quatrotateabout(correction, correction, corrective_quaternion); + } + } - // 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; 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 }; @@ -431,31 +518,21 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { MixedAmount += Confidence; //printf( "%f ", Confidence ); } - scale3d( MixedPosition, MixedPosition, 1./MixedAmount ); - - printf( "Reprojection disagreements:" ); - for( l = 0; l < NUM_LIGHTHOUSES; l++ ) for( a = 0; a < 2; a++ ) - { - printf( "%f ", dist3d( dd->MixingPositions[l][a], MixedPosition ) ); - } - printf( "\n" ); + scale3d( dd->mixed_output, MixedPosition, 1./MixedAmount ); + 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 ); - 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; + + if( validpoints > 1 && applied_corrections > 1 && !normal_faults) + { + so->PoseConfidence += (1-so->PoseConfidence)*.04; + } + else if( validpoints > 1 && so->PoseConfidence < 0.5 && !normal_faults ) + { + so->PoseConfidence += (1-so->PoseConfidence)*.01; + } } dd->sweepaxis = l->acode & 1; |