aboutsummaryrefslogtreecommitdiff
path: root/src/poser_charlesrefine.c
diff options
context:
space:
mode:
authorcnlohr <lohr85@gmail.com>2018-04-07 12:40:59 -0400
committercnlohr <lohr85@gmail.com>2018-04-07 12:40:59 -0400
commit795f7802faa8d612e13d5853a0da98ad96a2746e (patch)
treef945004245e41e758827b307546ef277ea3b9cce /src/poser_charlesrefine.c
parent55f498db296ff353a0cf870c51bffd92cc360484 (diff)
downloadlibsurvive-795f7802faa8d612e13d5853a0da98ad96a2746e.tar.gz
libsurvive-795f7802faa8d612e13d5853a0da98ad96a2746e.tar.bz2
BOX FILTERS FTW!!! Temporarily remove use of survive_IMU but we should be able to add that back in.
Diffstat (limited to 'src/poser_charlesrefine.c')
-rw-r--r--src/poser_charlesrefine.c134
1 files changed, 102 insertions, 32 deletions
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 <string.h>
#include <survive_imu.h>
-#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;
}