Cleaned up debug code

5 files changed, 108 insertions, 5 deletions

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 <stdio.h>
+#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 <linmath.h>
+#include <survive_types.h>
+
+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_<cv::Vec3b> err[2];
 
 cv::Vec3f flow2rgb(float x, float y, float scale = 1) {
 	cv::Mat_<cv::Vec3f> 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_<cv::Vec3f> bgr(1, 1);
 	cv::cvtColor(hsv, bgr, CV_HSV2RGB_FULL);
 	return bgr(0, 0) * 255;