diff options
Diffstat (limited to 'src/poser_charlesrefine.c')
-rw-r--r-- | src/poser_charlesrefine.c | 335 |
1 files changed, 166 insertions, 169 deletions
diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index f9c60e2..388ba77 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -1,4 +1,4 @@ -//Driver works, but you _must_ start it near the origin looking in +Z. +// Driver works, but you _must_ start it near the origin looking in +Z. #include <poser.h> #include <survive.h> @@ -7,18 +7,16 @@ #include "epnp/epnp.h" #include "linmath.h" #include <math.h> +#include <stdint.h> #include <stdio.h> #include <string.h> -#include <stdint.h> #define MAX_PT_PER_SWEEP 32 - -typedef struct -{ +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 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 angles_at_pts[MAX_PT_PER_SWEEP]; SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP]; @@ -26,11 +24,10 @@ typedef struct int ptsweep; } CharlesPoserData; - - int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { - CharlesPoserData * dd = so->PoserData; - if( !dd ) so->PoserData = dd = calloc( sizeof(CharlesPoserData), 1 ); + CharlesPoserData *dd = so->PoserData; + if (!dd) + so->PoserData = dd = calloc(sizeof(CharlesPoserData), 1); SurviveSensorActivations *scene = &so->activations; switch (pd->pt) { @@ -38,18 +35,17 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { // Really should use this... PoserDataIMU *imuData = (PoserDataIMU *)pd; - - //TODO: Actually do Madgwick's algorithm - LinmathQuat applymotion; - const SurvivePose * object_pose = &so->OutPose; + // TODO: Actually do Madgwick's algorithm + LinmathQuat applymotion; + const SurvivePose *object_pose = &so->OutPose; imuData->gyro[0] *= -0.0005; imuData->gyro[1] *= -0.0005; imuData->gyro[2] *= 0.0005; - quatfromeuler( applymotion, imuData->gyro ); - //printf( "%f %f %f\n", imuData->gyro [0], imuData->gyro [1], imuData->gyro [2] ); + 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 ); + 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); return 0; @@ -59,70 +55,67 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { PoserDataLight *ld = (PoserDataLight *)pd; int lhid = ld->lh; int senid = ld->sensor_id; - BaseStationData * bsd = &so->ctx->bsd[ld->lh]; - if( !bsd->PositionSet ) break; - SurvivePose * lhp = &bsd->Pose; + BaseStationData *bsd = &so->ctx->bsd[ld->lh]; + if (!bsd->PositionSet) + break; + SurvivePose *lhp = &bsd->Pose; 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. - //if( lhid == 1 ) break; + // 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_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]; + 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. + // 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); - - //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] ); + // 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 ); + 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. + // 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]; - //If 1, the "y" axis. //XXX Check me. - if( axis ) //XXX Just FYI this should include account for skew + // If 1, the "y" axis. //XXX Check me. + if (axis) // XXX Just FYI this should include account for skew { sweep_normal[0] = 0; - sweep_normal[1] = cos(angle ); - sweep_normal[2] = sin( angle ); - //printf( "+" ); - } - else - { - sweep_normal[0] = cos( angle ); + sweep_normal[1] = cos(angle); + sweep_normal[2] = sin(angle); + // printf( "+" ); + } else { + sweep_normal[0] = cos(angle); sweep_normal[1] = 0; - sweep_normal[2] = -sin( angle ); - //printf( "-" ); + sweep_normal[2] = -sin(angle); + // printf( "-" ); } - //Need to apply the lighthouse's transformation to the sweep's normal. - quatrotatevector( sweep_normal, lhp->Rot, sweep_normal); + // Need to apply the lighthouse's transformation to the sweep's normal. + quatrotatevector(sweep_normal, lhp->Rot, sweep_normal); - //Compute point-line distance between sensorpos_rel_lh and the plane defined by sweep_normal. - //Do this by projecting sensorpos_rel_lh (w) onto sweep_normal (v). - //You can do this by |v dot w| / |v| ... But we know |v| is 1. So... - FLT dist = dot3d( sensorpos_rel_lh, sweep_normal ); + // Compute point-line distance between sensorpos_rel_lh and the plane defined by sweep_normal. + // Do this by projecting sensorpos_rel_lh (w) onto sweep_normal (v). + // You can do this by |v dot w| / |v| ... But we know |v| is 1. So... + FLT dist = dot3d(sensorpos_rel_lh, sweep_normal); - if( (i = dd->ptsweep) < MAX_PT_PER_SWEEP ) - { - memcpy( dd->normal_at_errors[i], sweep_normal, sizeof(FLT)*3 ); + if ((i = dd->ptsweep) < MAX_PT_PER_SWEEP) { + memcpy(dd->normal_at_errors[i], sweep_normal, sizeof(FLT) * 3); 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], object_pose, sizeof(SurvivePose)); dd->ptsweep++; } @@ -133,197 +126,201 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { PoserDataLight *l = (PoserDataLight *)pd; int lhid = l->lh; - - //you can get sweepaxis and sweeplh. - if( dd->ptsweep ) - { + // you can get sweepaxis and sweeplh. + if (dd->ptsweep) { int i; 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; + BaseStationData *bsd = &so->ctx->bsd[lhid]; + SurvivePose *lh_pose = &bsd->Pose; int validpoints = 0; int ptvalid[MAX_PT_PER_SWEEP]; FLT avgerr = 0.0; - FLT vec_correct[3] = { 0., 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 +// 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.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_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 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_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 - //Step 1: Determine standard of deviation, and average in order to + // Step 1: Determine standard of deviation, and average in order to // drop points that are likely in error. { - //Calculate average + // Calculate average FLT avgerr_orig = 0.0; FLT stddevsq = 0.0; - for( i = 0; i < pts; i++ ) + for (i = 0; i < pts; i++) avgerr_orig += dd->quantity_errors[i]; - avgerr_orig/=pts; + avgerr_orig /= pts; - //Calculate standard of deviation. - for( i = 0; i < pts; i++ ) - { - FLT diff = dd->quantity_errors[i]-avgerr_orig; - stddevsq += diff*diff; + // Calculate standard of deviation. + for (i = 0; i < pts; i++) { + FLT diff = dd->quantity_errors[i] - avgerr_orig; + stddevsq += diff * diff; } - stddevsq/=pts; + stddevsq /= pts; - for( i = 0; i < pts; i++ ) - { + for (i = 0; i < pts; i++) { FLT err = dd->quantity_errors[i]; - FLT diff = err-avgerr_orig; + FLT diff = err - avgerr_orig; diff *= diff; - int isptvalid = (diff * MIN_HIT_QUALITY <= stddevsq + HIT_QUALITY_BASELINE)?1:0; + int isptvalid = (diff * MIN_HIT_QUALITY <= stddevsq + HIT_QUALITY_BASELINE) ? 1 : 0; ptvalid[i] = isptvalid; - if( isptvalid ) - { + if (isptvalid) { avgang += dd->angles_at_pts[i]; avgerr += err; - validpoints ++; + validpoints++; } } avgang /= validpoints; avgerr /= validpoints; } - //Step 2: Determine average lateral error. - //We can actually always perform this operation. Even with only one point. + // 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 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. - scale3d( avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT ); - add3d( vec_correct, vec_correct, avg_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); } - //Step 3: Control telecoption from lighthouse. + // 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. + 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; + 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. + 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; FLT veccamalong[3]; - sub3d( veccamalong, lh_pose->Pos, object_pose->Pos ); - normalize3d( veccamalong, veccamalong ); - scale3d( veccamalong, veccamalong, zoom ); - add3d( vec_correct, veccamalong, vec_correct ); + sub3d(veccamalong, lh_pose->Pos, object_pose->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 ); + quatcopy(object_pose_out.Rot, object_pose->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 - //we would actually be able to perform this on a per-hit basis. - if( 1 ) { + // 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 + // we would actually be able to perform this on a per-hit basis. + if (1) { LinmathQuat correction; - quatcopy( correction, LinmathQuat_Identity ); - for( i = 0; i < pts; i++ ) - { - if( !ptvalid[i] ) continue; - FLT dist = dd->quantity_errors[i]-avgerr; + quatcopy(correction, LinmathQuat_Identity); + for (i = 0; i < pts; i++) { + if (!ptvalid[i]) + continue; + FLT dist = dd->quantity_errors[i] - avgerr; 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]; + 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. + 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 ); + 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; + 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. + // 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); - + 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 ); + // 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 ); + 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]); + 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]); } - //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 ); + // 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); } - //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 ) ); + // 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; } @@ -332,11 +329,11 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { } dd->sweepaxis = l->acode & 1; - //printf( "SYNC %d %p\n", l->acode, dd ); + // printf( "SYNC %d %p\n", l->acode, dd ); break; } case POSERDATA_FULL_SCENE: { - //return opencv_solver_fullscene(so, (PoserDataFullScene *)(pd)); + // return opencv_solver_fullscene(so, (PoserDataFullScene *)(pd)); } } return -1; |