From 135904406d50bc2199698c3177307e470151e7f9 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Fri, 20 Apr 2018 23:06:50 -0600 Subject: Cleaned up debug code --- src/poser_sba.c | 4 +- src/survive_kalman.c | 72 ++++++++++++++++++++++++++++++++++++ src/survive_kalman.h | 33 +++++++++++++++++ src/survive_vive.c | 2 - tools/showreproject/showreproject.cc | 2 +- 5 files changed, 108 insertions(+), 5 deletions(-) create mode 100644 src/survive_kalman.c create mode 100644 src/survive_kalman.h diff --git a/src/poser_sba.c b/src/poser_sba.c index 07d8f25..ed19c1d 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -395,8 +395,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { LinmathVec3d pvar = {.1, .1, .1}; FLT rvar = .01; - survive_kpose_integrate_pose(&d->kpose, lightData->timecode, &estimate, pvar, rvar); - estimate = d->kpose.state.pose; + //survive_kpose_integrate_pose(&d->kpose, lightData->timecode, &estimate, pvar, rvar); + //estimate = d->kpose.state.pose; PoserData_poser_pose_func(&lightData->hdr, so, &estimate); } diff --git a/src/survive_kalman.c b/src/survive_kalman.c new file mode 100644 index 0000000..5a528a8 --- /dev/null +++ b/src/survive_kalman.c @@ -0,0 +1,72 @@ + +#include +#include "survive_kalman.h" + +void survive_kpose_predict(const survive_kpose_t *kpose, survive_timecode when, SurvivePose *out) { + for(int i = 0;i < 3;i++) + out->Pos[i] = kpose->state.pose.Pos[i] + + kpose->state.velocity[i] * (survive_timecode_difference(when, kpose->timecode) / 48000000.); +} + +void survive_kpose_init(survive_kpose_t *kpose, survive_timecode timecode, const SurvivePose *pose, + const double *pos_variance, FLT rot_variance) { + kpose->variance.rot_variance = rot_variance; + copy3d(kpose->variance.pos_variance, pos_variance); + kpose->state.pose = *pose; + + kpose->process_variance_per_second.rot_variance = .01; + + FLT avg_acc_per_sec = 0.5; + FLT avg_vel_per_sec = 0.5; + for(int i = 0;i < 3;i++) { + kpose->process_variance_per_second.vel_variance[i] = avg_acc_per_sec; + kpose->process_variance_per_second.pos_variance[i] = avg_vel_per_sec; + } + + kpose->timecode = timecode; + + fprintf(stderr, "Init!\n"); + +} + +static inline void kalman_update(FLT* R, FLT* X, FLT P, FLT X_p) { + FLT gain = *R / (*R + P); + *X = *X * (1. - gain) + X_p * gain; + *R = *R * (1. - gain);// + P * gain; + + //fprintf(stderr, "Gain %f\n", gain); +} + +static inline void update(survive_kpose_t *kpose, survive_timecode when) { + survive_kpose_predict(kpose, when, &kpose->state.pose); + FLT diff_seconds = (survive_timecode_difference(when, kpose->timecode) / 48000000.); + for(int i = 0;i < 3;i++) { + kpose->variance.pos_variance[i] += kpose->process_variance_per_second.pos_variance[i] * diff_seconds; + } + kpose->variance.rot_variance += kpose->process_variance_per_second.rot_variance * diff_seconds; + kpose->timecode = when; +} + +void survive_kpose_integrate_pose(survive_kpose_t *kpose, survive_timecode timecode, const SurvivePose *pose, + const double *pos_variance, FLT rot_variance) { + if (kpose->variance.rot_variance == 0.0) { + survive_kpose_init(kpose, timecode, pose, pos_variance, rot_variance); + return; + } + + update(kpose, timecode); + + FLT rot_gain = kpose->variance.rot_variance / (kpose->variance.rot_variance + rot_variance); + LinmathQuat new_rot; + //quatslerp(new_rot, pose->Rot, kpose->state.pose.Rot, 1. - rot_gain); + for(int i = 0;i < 4;i++) + kpose->state.pose.Rot[i] = (1. - rot_gain) * kpose->state.pose.Rot[i] + rot_gain * pose->Rot[i]; + quatnormalize(kpose->state.pose.Rot,kpose->state.pose.Rot); + //quatcopy(kpose->state.pose.Rot, new_rot); + kpose->variance.rot_variance = kpose->variance.rot_variance * (1. - rot_gain);// + rot_variance * rot_gain; + + for(int i = 0;i < 3;i++) { + kalman_update(kpose->variance.pos_variance + i, kpose->state.pose.Pos + i, pos_variance[i], pose->Pos[i]); + } + +} diff --git a/src/survive_kalman.h b/src/survive_kalman.h new file mode 100644 index 0000000..405b369 --- /dev/null +++ b/src/survive_kalman.h @@ -0,0 +1,33 @@ + +#include +#include + +typedef struct { + SurvivePose pose; + LinmathVec3d velocity; +} survive_kalman_pose_state_t ; + +typedef struct { + LinmathVec3d pos_variance; + FLT rot_variance; + LinmathVec3d vel_variance; +} survive_kalman_pose_var_t ; + +typedef struct { + survive_kalman_pose_state_t state; + survive_kalman_pose_var_t variance; + + survive_timecode timecode; + survive_kalman_pose_var_t process_variance_per_second; +} survive_kpose_t; + + +void survive_kpose_predict(const survive_kpose_t* kpose, survive_timecode when, SurvivePose* out); + +void survive_kpose_integrate_pose(survive_kpose_t* kpose, survive_timecode timecode, + const SurvivePose* pose, + const LinmathVec3d pos_variance, FLT rot_variance); + +void survive_kpose_integrate_pose_velocity(survive_kpose_t* kpose, survive_timecode timecode, + const SurvivePose* pose, const LinmathVec3d vel, + const LinmathVec3d pos_variance, FLT rot_variance, const LinmathVec3d vel_var); diff --git a/src/survive_vive.c b/src/survive_vive.c index 88bc065..d9cbc3e 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -961,8 +961,6 @@ void calibrate_acc(SurviveObject* so, FLT* agm) { agm[1] *= so->acc_scale[1]; agm[2] *= so->acc_scale[2]; } - - fprintf(stderr, "%f\t%f\t%f\n", agm[0], agm[1], agm[2]); } void calibrate_gyro(SurviveObject* so, FLT* agm) { diff --git a/tools/showreproject/showreproject.cc b/tools/showreproject/showreproject.cc index 3eea83d..12654ec 100644 --- a/tools/showreproject/showreproject.cc +++ b/tools/showreproject/showreproject.cc @@ -20,7 +20,7 @@ cv::Mat_ err[2]; cv::Vec3f flow2rgb(float x, float y, float scale = 1) { cv::Mat_ hsv(1, 1); - hsv(0, 0) = {atan2(y, x) * 180. / M_PI, 1.0, std::min(1.0f, sqrt(x * x + y * y) * scale)}; + hsv(0, 0) = {(float)(atan2(y, x) * 180. / M_PI), 1.0, std::min(1.0f, sqrt(x * x + y * y) * scale)}; cv::Mat_ bgr(1, 1); cv::cvtColor(hsv, bgr, CV_HSV2RGB_FULL); return bgr(0, 0) * 255; -- cgit v1.2.3