aboutsummaryrefslogtreecommitdiff
path: root/src/poser_charlesrefine.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/poser_charlesrefine.c')
-rw-r--r--src/poser_charlesrefine.c133
1 files changed, 64 insertions, 69 deletions
diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c
index cc188fb..1749d8e 100644
--- a/src/poser_charlesrefine.c
+++ b/src/poser_charlesrefine.c
@@ -25,7 +25,7 @@ typedef struct {
uint8_t sensor_ids[MAX_PT_PER_SWEEP];
LinmathPoint3d MixingPositions[NUM_LIGHTHOUSES][2];
- SurvivePose InteralPoseUsedForCalc; //Super high speed vibratey and terrible.
+ SurvivePose InteralPoseUsedForCalc; // Super high speed vibratey and terrible.
float MixingConfidence[NUM_LIGHTHOUSES][2];
int ptsweep;
@@ -35,8 +35,7 @@ typedef struct {
int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
CharlesPoserData *dd = so->PoserData;
- if (!dd)
- {
+ if (!dd) {
so->PoserData = dd = calloc(sizeof(CharlesPoserData), 1);
SurvivePose object_pose_out;
memcpy(&object_pose_out, &LinmathPose_Identity, sizeof(LinmathPose_Identity));
@@ -51,46 +50,43 @@ 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;
- 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] );
+ // 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] );
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 };
+ // Apply a tiny tug to re-right headset based on the gravity vector.
+ LinmathVec3d reright = {0, 0, 1};
LinmathVec3d normup;
- normalize3d( normup, imuData->accel );
+ 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 );
-
-
+ 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 ); //Contribution from Gyro
- quatrotateabout(object_pose_out.Rot, object_pose_out.Rot, reright_quat); //Contribution from Accelerometer
+ 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 );
+ 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);
+ 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);
+ // PoserDataIMU *imu = (PoserDataIMU *)pd;
+ // survive_imu_tracker_integrate(so, &dd->tracker, imu);
+ // PoserData_poser_pose_func(pd, so, &dd->tracker.pose);
return 0;
}
@@ -106,8 +102,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.
@@ -177,7 +173,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
int lhid = dd->sweeplh;
int axis = dd->sweepaxis;
int pts = dd->ptsweep;
- //const SurvivePose *object_pose =
+ // 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];
@@ -195,7 +191,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
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_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
@@ -263,8 +259,6 @@ 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.
@@ -297,7 +291,6 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
add3d(vec_correct, veccamalong, vec_correct);
}
-
#if 0
LinmathPoint3d LastDelta;
sub3d( LastDelta, dd->MixingPositions[lhid][axis], dd->InteralPoseUsedForCalc.Pos );
@@ -318,9 +311,7 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
#endif
add3d(dd->InteralPoseUsedForCalc.Pos, vec_correct, dd->InteralPoseUsedForCalc.Pos);
-
-
- //quatcopy(object_pose_out.Rot, dd->InteralPoseUsedForCalc.Rot);
+ // 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
@@ -385,42 +376,46 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
quatnormalize(dd->InteralPoseUsedForCalc.Rot, dd->InteralPoseUsedForCalc.Rot);
}
- memcpy( dd->MixingPositions[lhid][axis], dd->InteralPoseUsedForCalc.Pos, sizeof( dd->InteralPoseUsedForCalc.Pos ) );
- dd->MixingConfidence[lhid][axis] = (validpoints)?((validpoints>1)?1.0:0.5):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.
+ // Box filter all of the guesstimations, and emit the new guess.
{
FLT MixedAmount = 0;
- LinmathPoint3d MixedPosition = { 0, 0, 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 );
+ 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 );
+ 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);
+ // 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;
}