diff options
author | Justin Berger <j.david.berger@gmail.com> | 2018-04-04 22:10:41 -0600 |
---|---|---|
committer | Justin Berger <j.david.berger@gmail.com> | 2018-04-04 22:10:50 -0600 |
commit | d83a2cd790bc390f7485b3e8b63166ee29151050 (patch) | |
tree | 02a4c437fb0a562feec0af01cf28961b3416a83d | |
parent | 9ce1758981b137f5304587009ad32768b09fd0a9 (diff) | |
download | libsurvive-d83a2cd790bc390f7485b3e8b63166ee29151050.tar.gz libsurvive-d83a2cd790bc390f7485b3e8b63166ee29151050.tar.bz2 |
Added imu tracker to charlesrefine
-rw-r--r-- | src/poser_charlesrefine.c | 31 | ||||
-rw-r--r-- | src/survive_imu.c | 3 |
2 files changed, 18 insertions, 16 deletions
diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c index 388ba77..52a5f54 100644 --- a/src/poser_charlesrefine.c +++ b/src/poser_charlesrefine.c @@ -6,10 +6,12 @@ #include "epnp/epnp.h" #include "linmath.h" +#include "survive_cal.h" #include <math.h> #include <stdint.h> #include <stdio.h> #include <string.h> +#include <survive_imu.h> #define MAX_PT_PER_SWEEP 32 @@ -22,6 +24,8 @@ typedef struct { SurvivePose object_pose_at_hit[MAX_PT_PER_SWEEP]; uint8_t sensor_ids[MAX_PT_PER_SWEEP]; int ptsweep; + + SurviveIMUTracker tracker; } CharlesPoserData; int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { @@ -33,20 +37,10 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { switch (pd->pt) { case POSERDATA_IMU: { // Really should use this... - PoserDataIMU *imuData = (PoserDataIMU *)pd; + PoserDataIMU *imu = (PoserDataIMU *)pd; - // 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] ); - 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); + survive_imu_tracker_integrate(so, &dd->tracker, imu); + PoserData_poser_pose_func(pd, so, &dd->tracker.pose); return 0; } @@ -324,7 +318,16 @@ int PoserCharlesRefine(SurviveObject *so, PoserData *pd) { so->PoseConfidence = 1.0; } - PoserData_poser_pose_func(pd, so, &object_pose_out); + // 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); + dd->ptsweep = 0; } diff --git a/src/survive_imu.c b/src/survive_imu.c index 205e2c2..36d1aeb 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -175,10 +175,9 @@ static void iterate_velocity(LinmathVec3d result, SurviveIMUTracker *tracker, do void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data) { if (tracker->last_data.timecode == 0) { - + tracker->pose.Rot[0] = 1.; if (tracker->last_data.datamask == imu_calibration_iterations) { tracker->last_data = *data; - tracker->pose.Rot[0] = 1.; const FLT up[3] = {0, 0, 1}; quatfrom2vectors(tracker->pose.Rot, tracker->updir, up); |