Merge branch 'master' into imu

10 files changed, 395 insertions, 29 deletions

diff --git a/src/poser.c b/src/poser.c
index 9a0de24..499e0ff 100644
--- a/src/poser.c
+++ b/src/poser.c
@@ -7,11 +7,29 @@
 #define _USE_MATH_DEFINES // for C
 #include <math.h>
 
-void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
-	if (poser_data->rawposeproc) {
-		poser_data->rawposeproc(so, lighthouse, pose, poser_data->userdata);
+static uint32_t PoserData_timecode(PoserData *poser_data) {
+	switch (poser_data->pt) {
+	case POSERDATA_LIGHT: {
+		PoserDataLight *lightData = (PoserDataLight *)poser_data;
+		return lightData->timecode;
+	}
+	case POSERDATA_FULL_SCENE: {
+		PoserDataFullScene *pdfs = (PoserDataFullScene *)(poser_data);
+		return -1;
+	}
+	case POSERDATA_IMU: {
+		PoserDataIMU *imuData = (PoserDataIMU *)poser_data;
+		return imuData->timecode;
+	}
+	}
+	return -1;
+}
+
+void PoserData_poser_pose_func(PoserData *poser_data, SurviveObject *so, SurvivePose *pose) {
+	if (poser_data->poseproc) {
+		poser_data->poseproc(so, PoserData_timecode(poser_data), pose, poser_data->userdata);
 	} else {
-		so->ctx->rawposeproc(so, lighthouse, pose);
+		so->ctx->poseproc(so, PoserData_timecode(poser_data), pose);
 	}
 }
 
diff --git a/src/poser_charlesrefine.c b/src/poser_charlesrefine.c
new file mode 100644
index 0000000..388ba77
--- /dev/null
+++ b/src/poser_charlesrefine.c
@@ -0,0 +1,342 @@
+// Driver works, but you _must_ start it near the origin looking in +Z.
+
+#include <poser.h>
+#include <survive.h>
+#include <survive_reproject.h>
+
+#include "epnp/epnp.h"
+#include "linmath.h"
+#include <math.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+#define MAX_PT_PER_SWEEP 32
+
+typedef struct {
+	int sweepaxis;
+	int sweeplh;
+	FLT normal_at_errors[MAX_PT_PER_SWEEP][3]; // Value is actually normalized, not just normal to sweep plane.
+	FLT quantity_errors[MAX_PT_PER_SWEEP];
+	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];
+	int ptsweep;
+} CharlesPoserData;
+
+int PoserCharlesRefine(SurviveObject *so, PoserData *pd) {
+	CharlesPoserData *dd = so->PoserData;
+	if (!dd)
+		so->PoserData = dd = calloc(sizeof(CharlesPoserData), 1);
+
+	SurviveSensorActivations *scene = &so->activations;
+	switch (pd->pt) {
+	case POSERDATA_IMU: {
+		// 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] *= -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);
+
+		return 0;
+	}
+	case POSERDATA_LIGHT: {
+		int i;
+		PoserDataLight *ld = (PoserDataLight *)pd;
+		int lhid = ld->lh;
+		int senid = ld->sensor_id;
+		BaseStationData *bsd = &so->ctx->bsd[ld->lh];
+		if (!bsd->PositionSet)
+			break;
+		SurvivePose *lhp = &bsd->Pose;
+		FLT angle = ld->angle;
+		int sensor_id = ld->sensor_id;
+		int axis = dd->sweepaxis;
+		const SurvivePose *object_pose = &so->OutPose;
+		dd->sweeplh = lhid;
+
+		// FOR NOW, drop LH1.
+		// if( lhid == 1 ) break;
+
+		//		const FLT * sensor_normal = &so->sensor_normals[senid*3];
+		//		FLT sensor_normal_worldspace[3];
+		//		ApplyPoseToPoint(sensor_normal_worldspace,   object_pose, sensor_inpos);
+
+		const FLT *sensor_inpos = &so->sensor_locations[senid * 3];
+		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);
+
+		// 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] );
+		// = sensor position, relative to lighthouse center.
+		FLT sensorpos_rel_lh[3];
+		sub3d(sensorpos_rel_lh, sensor_position_worldspace, lhp->Pos);
+
+		// Next, define a normal in global space of the plane created by the sweep hit.
+		// Careful that this must be normalized.
+		FLT sweep_normal[3];
+
+		// If 1, the "y" axis. //XXX Check me.
+		if (axis) // XXX Just FYI this should include account for skew
+		{
+			sweep_normal[0] = 0;
+			sweep_normal[1] = cos(angle);
+			sweep_normal[2] = sin(angle);
+			// printf( "+" );
+		} else {
+			sweep_normal[0] = cos(angle);
+			sweep_normal[1] = 0;
+			sweep_normal[2] = -sin(angle);
+			// printf( "-" );
+		}
+
+		// Need to apply the lighthouse's transformation to the sweep's normal.
+		quatrotatevector(sweep_normal, lhp->Rot, sweep_normal);
+
+		// Compute point-line distance between sensorpos_rel_lh and the plane defined by sweep_normal.
+		// Do this by projecting sensorpos_rel_lh (w) onto sweep_normal (v).
+		// You can do this by |v dot w| / |v| ... But we know |v| is 1. So...
+		FLT dist = dot3d(sensorpos_rel_lh, sweep_normal);
+
+		if ((i = dd->ptsweep) < MAX_PT_PER_SWEEP) {
+			memcpy(dd->normal_at_errors[i], sweep_normal, sizeof(FLT) * 3);
+			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));
+			dd->ptsweep++;
+		}
+
+		return 0;
+	}
+
+	case POSERDATA_SYNC: {
+		PoserDataLight *l = (PoserDataLight *)pd;
+		int lhid = l->lh;
+
+		// you can get sweepaxis and sweeplh.
+		if (dd->ptsweep) {
+			int i;
+			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.
+
+			BaseStationData *bsd = &so->ctx->bsd[lhid];
+			SurvivePose *lh_pose = &bsd->Pose;
+
+			int validpoints = 0;
+			int ptvalid[MAX_PT_PER_SWEEP];
+			FLT avgerr = 0.0;
+			FLT vec_correct[3] = {0., 0., 0.};
+			FLT avgang = 0.0;
+
+// Tunable parameters:
+#define MIN_HIT_QUALITY 0.5 // Determines which hits to cull.
+#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_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
+
+			// Step 1: Determine standard of deviation, and average in order to
+			// drop points that are likely in error.
+			{
+				// Calculate average
+				FLT avgerr_orig = 0.0;
+				FLT stddevsq = 0.0;
+				for (i = 0; i < pts; i++)
+					avgerr_orig += dd->quantity_errors[i];
+				avgerr_orig /= pts;
+
+				// Calculate standard of deviation.
+				for (i = 0; i < pts; i++) {
+					FLT diff = dd->quantity_errors[i] - avgerr_orig;
+					stddevsq += diff * diff;
+				}
+				stddevsq /= pts;
+
+				for (i = 0; i < pts; i++) {
+					FLT err = dd->quantity_errors[i];
+					FLT diff = err - avgerr_orig;
+					diff *= diff;
+					int isptvalid = (diff * MIN_HIT_QUALITY <= stddevsq + HIT_QUALITY_BASELINE) ? 1 : 0;
+					ptvalid[i] = isptvalid;
+					if (isptvalid) {
+						avgang += dd->angles_at_pts[i];
+						avgerr += err;
+						validpoints++;
+					}
+				}
+				avgang /= validpoints;
+				avgerr /= validpoints;
+			}
+
+			// Step 2: Determine average lateral error.
+			// We can actually always perform this operation.  Even with only one point.
+			{
+				FLT avg_err[3] = {0, 0, 0}; // Positional error.
+				for (i = 0; i < pts; i++) {
+					if (!ptvalid[i])
+						continue;
+					FLT *nrm = dd->normal_at_errors[i];
+					FLT err = dd->quantity_errors[i];
+					avg_err[0] = avg_err[0] + nrm[0] * err;
+					avg_err[1] = avg_err[1] + nrm[1] * err;
+					avg_err[2] = avg_err[2] + nrm[2] * err;
+				}
+
+				// NOTE: The "avg_err" is not geometrically centered.  This is actually
+				// probably okay, since if you have sevearl data points to one side, you
+				// can probably trust that more.
+				scale3d(avg_err, avg_err, 1. / validpoints);
+
+				// We have "Average error" now.  A vector in worldspace.
+				// This can correct for lateral error, but not distance from camera.
+
+				// XXX TODO: Should we check to see if we only have one or
+				// two points to make sure the error on this isn't unusually high?
+				// If calculated error is unexpectedly high, then we should probably
+				// Not apply the transform.
+				scale3d(avg_err, avg_err, -CORRECT_LATERAL_POSITION_COEFFICIENT);
+				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.
+			{
+				FLT zoom = 0.0;
+				FLT rmsang = 0.0;
+				for (i = 0; i < pts; i++) {
+					if (!ptvalid[i])
+						continue;
+					FLT delang = dd->angles_at_pts[i] - avgang;
+					FLT delerr = dd->quantity_errors[i] - avgerr;
+					if (axis)
+						delang *= -1; // Flip sign on alternate axis because it's measured backwards.
+					zoom += delerr * delang;
+					rmsang += delang * delang;
+				}
+
+				// Control into or outof lighthouse.
+				// XXX Check to see if we need to sqrt( the rmsang), need to check convergance behavior close to
+				// lighthouse.
+				// This is a questionable step.
+				zoom /= sqrt(rmsang);
+
+				zoom *= CORRECT_TELESCOPTION_COEFFICIENT;
+
+				FLT veccamalong[3];
+				sub3d(veccamalong, lh_pose->Pos, object_pose->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);
+
+			// 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
+			// we would actually be able to perform this on a per-hit basis.
+			if (1) {
+				LinmathQuat correction;
+				quatcopy(correction, LinmathQuat_Identity);
+				for (i = 0; i < pts; i++) {
+					if (!ptvalid[i])
+						continue;
+					FLT dist = dd->quantity_errors[i] - avgerr;
+					FLT angle = dd->angles_at_pts[i];
+					int sensor_id = dd->sensor_ids[i];
+					FLT *normal = dd->normal_at_errors[i];
+					const SurvivePose *object_pose_at_hit = &dd->object_pose_at_hit[i];
+					const FLT *sensor_inpos = &so->sensor_locations[sensor_id * 3];
+
+					LinmathQuat world_to_object_space;
+					quatgetreciprocal(world_to_object_space, object_pose_at_hit->Rot);
+					FLT correction_in_object_space[3]; // The amount across the surface of the object the rotation
+													   // should happen.
+
+					quatrotatevector(correction_in_object_space, world_to_object_space, normal);
+					dist *= CORRECT_ROTATION_COEFFICIENT;
+					if (dist > ROTATIONAL_CORRECTION_MAXFORCE)
+						dist = ROTATIONAL_CORRECTION_MAXFORCE;
+					if (dist < -ROTATIONAL_CORRECTION_MAXFORCE)
+						dist = -ROTATIONAL_CORRECTION_MAXFORCE;
+
+					// Now, we have a "tug" vector in object-local space.  Need to apply the torque.
+					FLT vector_from_center_of_object[3];
+					normalize3d(vector_from_center_of_object, sensor_inpos);
+					// scale3d(vector_from_center_of_object, sensor_inpos, 10.0 );
+					//			vector_from_center_of_object[2]*=-1;
+					//				vector_from_center_of_object[1]*=-1;
+					//					vector_from_center_of_object[0]*=-1;
+					// vector_from_center_of_object
+					scale3d(vector_from_center_of_object, vector_from_center_of_object, 1);
+
+					FLT new_vector_in_object_space[3];
+					// printf( "%f %f %f %f\n", object_pose_at_hit->Rot[0], object_pose_at_hit->Rot[1],
+					// object_pose_at_hit->Rot[2], object_pose_at_hit->Rot[3] );
+					// printf( "%f %f %f  // %f %f %f // %f\n", vector_from_center_of_object[0],
+					// vector_from_center_of_object[1], vector_from_center_of_object[2], correction_in_object_space[0],
+					// correction_in_object_space[1], correction_in_object_space[2], dist );
+					scale3d(correction_in_object_space, correction_in_object_space, -dist);
+					add3d(new_vector_in_object_space, vector_from_center_of_object, correction_in_object_space);
+
+					normalize3d(new_vector_in_object_space, new_vector_in_object_space);
+
+					LinmathQuat corrective_quaternion;
+					quatfrom2vectors(corrective_quaternion, vector_from_center_of_object, new_vector_in_object_space);
+					quatrotateabout(correction, correction, corrective_quaternion);
+					// printf( "%f -> %f %f %f => %f %f %f [%f %f %f %f]\n", dist, vector_from_center_of_object[0],
+					// vector_from_center_of_object[1], vector_from_center_of_object[2],
+					// correction_in_object_space[0], correction_in_object_space[1], correction_in_object_space[2],
+					// corrective_quaternion[0],corrective_quaternion[1],corrective_quaternion[1],corrective_quaternion[3]);
+				}
+				// 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);
+			}
+
+			// 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;
+			}
+
+			PoserData_poser_pose_func(pd, so, &object_pose_out);
+			dd->ptsweep = 0;
+		}
+
+		dd->sweepaxis = l->acode & 1;
+		// printf( "SYNC %d %p\n", l->acode, dd );
+		break;
+	}
+	case POSERDATA_FULL_SCENE: {
+		// return opencv_solver_fullscene(so, (PoserDataFullScene *)(pd));
+	}
+	}
+	return -1;
+}
+
+REGISTER_LINKTIME(PoserCharlesRefine);
diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c
index b44225e..5cac973 100644
--- a/src/poser_charlesslow.c
+++ b/src/poser_charlesslow.c
@@ -256,7 +256,7 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr
 			int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2;
 			if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue;
 
-			FLT out[2] = {};
+			FLT out[2] = {0};
 			survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out);
 
 			//Find out where our ray shoots forth from.
@@ -343,7 +343,7 @@ static FLT RunOpti( SurviveObject * hmd, PoserDataFullScene * fs, int lh, int pr
 		if( fs->lengths[p][lh][0] < 0 || fs->lengths[p][lh][1] < 0 ) continue;
 
 		//Find out where our ray shoots forth from.
-		FLT out[2] = {};
+		FLT out[2] = {0};
 		survive_apply_bsd_calibration(hmd->ctx, lh, fs->angles[p][lh], out);
 
 		// Find out where our ray shoots forth from.
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
index 9cdab45..330e7e8 100644
--- a/src/poser_daveortho.c
+++ b/src/poser_daveortho.c
@@ -107,7 +107,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
 
 				SurvivePose obj2world;
 				ApplyPoseToPose(&obj2world, &lh2world, &objpose);
-				PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world);
+				PoserData_poser_pose_func(pd, so, &obj2world);
 
 				if (0) {
 					fprintf(stderr,"INQUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2],
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index c05450a..eaa1659 100644
--- a/src/poser_epnp.c
+++ b/src/poser_epnp.c
@@ -164,7 +164,7 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) {
 
 			if (winnerTakesAll) {
 				int winner = meas[0] > meas[1] ? 0 : 1;
-				PoserData_poser_raw_pose_func(pd, so, winner, &posers[winner]);
+				PoserData_poser_pose_func(pd, so, &posers[winner]);
 			} else {
 				double a, b;
 				a = meas[0] * meas[0];
@@ -175,11 +175,11 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) {
 					interpolate.Pos[i] = (posers[0].Pos[i] * a + posers[1].Pos[i] * b) / (t);
 				}
 				quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, b / (t));
-				PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate);
+				PoserData_poser_pose_func(pd, so, &interpolate);
 			}
 		} else {
 			if (meas[lightData->lh])
-				PoserData_poser_raw_pose_func(pd, so, lightData->lh, &posers[lightData->lh]);
+				PoserData_poser_pose_func(pd, so, &posers[lightData->lh]);
 		}
 		return 0;
 	}
diff --git a/src/poser_imu.c b/src/poser_imu.c
index 02ff8e9..7615170 100644
--- a/src/poser_imu.c
+++ b/src/poser_imu.c
@@ -20,7 +20,7 @@ int PoserIMU(SurviveObject *so, PoserData *pd) {
 
 		survive_imu_tracker_integrate(so, dd, imu);
 
-		PoserData_poser_raw_pose_func(pd, so, -1, &dd->pose);
+		PoserData_poser_pose_func(pd, so, &dd->pose);
 
 		// if(magnitude3d(dd->pose.Pos) > 1)
 		// SV_ERROR("IMU drift");
diff --git a/src/poser_sba.c b/src/poser_sba.c
index f6b1131..f0d5645 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -133,7 +133,7 @@ typedef struct {
 	SurvivePose poses;
 } sba_set_position_t;
 
-static void sba_set_position(SurviveObject *so, uint8_t lighthouse, SurvivePose *new_pose, void *_user) {
+static void sba_set_position(SurviveObject *so, uint32_t timecode, SurvivePose *new_pose, void *_user) {
 	sba_set_position_t *user = _user;
 	assert(user->hasInfo == false);
 	user->hasInfo = 1;
@@ -224,7 +224,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
 			PoserData hdr = pdl->hdr;
 			memset(&pdl->hdr, 0, sizeof(pdl->hdr)); // Clear callback functions
 			pdl->hdr.pt = hdr.pt;
-			pdl->hdr.rawposeproc = sba_set_position;
+			pdl->hdr.poseproc = sba_set_position;
 
 			sba_set_position_t locations = {0};
 			pdl->hdr.userdata = &locations;
@@ -282,7 +282,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
 	if (status > 0 && (info[1] / meas_size * 2) < d->max_error) {
 		d->failures_to_reset_cntr = d->failures_to_reset;
 		quatnormalize(soLocation.Rot, soLocation.Rot);
-		PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation);
+		PoserData_poser_pose_func(&pdl->hdr, so, &soLocation);
 	}
 
 	{
@@ -456,8 +456,8 @@ int PoserSBA(SurviveObject *so, PoserData *pd) {
 	  PoserDataIMU * imu = (PoserDataIMU*)pd;
 	  if (ctx->calptr && ctx->calptr->stage < 5) {
 	  } else if(d->useIMU){
-	    survive_imu_tracker_integrate(so, &d->tracker, imu);	    
-	    PoserData_poser_raw_pose_func(pd, so, 1, &d->tracker.pose);			
+	    survive_imu_tracker_integrate(so, &d->tracker, imu);
+		PoserData_poser_pose_func(pd, so, &d->tracker.pose);
 	  }
 	} // INTENTIONAL FALLTHROUGH
 	default: {
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 4628207..b2eed7f 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1631,9 +1631,8 @@ static void QuickPose(SurviveObject *so, PoserData *pd, SurvivePose *additionalT
 			SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, pd, 0, additionalTx, lh, 0);
 
 			//printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
-			if (so->ctx->rawposeproc)
-			{
-				so->ctx->rawposeproc(so, lh, &pose);
+			if (so->ctx->poseproc) {
+				so->ctx->poseproc(so, lh, &pose);
 			}
 
 			if (ttDebug) printf("!\n");
diff --git a/src/survive.c b/src/survive.c
index 1a782a2..b024bbb 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -216,7 +216,7 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) {
 	ctx->angleproc = survive_default_angle_process;
 	ctx->lighthouseposeproc = survive_default_lighthouse_pose_process;
 	ctx->configfunction = survive_default_htc_config_process;
-	ctx->rawposeproc = survive_default_raw_pose_process;
+	ctx->poseproc = survive_default_raw_pose_process;
 
 	ctx->calibration_config = survive_calibration_config_ctor();
 	ctx->calibration_config.use_flag = (enum SurviveCalFlag)survive_configi(ctx, "bsd-cal", SC_GET, SVCal_All);
@@ -380,11 +380,11 @@ void survive_install_button_fn(SurviveContext *ctx, button_process_func fbp) {
 		ctx->buttonproc = survive_default_button_process;
 }
 
-void survive_install_raw_pose_fn(SurviveContext *ctx, raw_pose_func fbp) {
+void survive_install_pose_fn(SurviveContext *ctx, pose_func fbp) {
 	if (fbp)
-		ctx->rawposeproc = fbp;
+		ctx->poseproc = fbp;
 	else
-		ctx->rawposeproc = survive_default_raw_pose_process;
+		ctx->poseproc = survive_default_raw_pose_process;
 }
 
 void survive_install_lighthouse_pose_fn(SurviveContext *ctx, lighthouse_pose_func fbp) {
@@ -558,9 +558,16 @@ int survive_simple_inflate(struct SurviveContext *ctx, const char *input, int in
 	return len;
 }
 
+#endif
+
 const char *survive_object_codename(SurviveObject *so) { return so->codename; }
+
+const char *survive_object_drivername(SurviveObject *so) { return so->drivername; }
+const int8_t survive_object_charge(SurviveObject *so) { return so->charge; }
+const bool survive_object_charging(SurviveObject *so) { return so->charging; }
+
+const SurvivePose *survive_object_pose(SurviveObject *so) { return &so->OutPose; }
+
 int8_t survive_object_sensor_ct(SurviveObject *so) { return so->sensor_ct; }
 const FLT *survive_object_sensor_locations(SurviveObject *so) { return so->sensor_locations; }
-const FLT *survive_object_sensor_normals(SurviveObject *so) { return so->sensor_normals; }
-
-#endif
+const FLT *survive_object_sensor_normals(SurviveObject *so) { return so->sensor_normals; }
\ No newline at end of file
diff --git a/src/survive_process.c b/src/survive_process.c
index e05e809..abde02d 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -139,12 +139,12 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8
 	//}
 }
 
-void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
+void survive_default_raw_pose_process(SurviveObject *so, uint32_t timecode, SurvivePose *pose) {
 	// print the pose;
 	//printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
 	so->OutPose = *pose;
-	so->FromLHPose[lighthouse] = *pose;
-	survive_recording_raw_pose_process(so, lighthouse, pose);
+	so->OutPose_timecode = timecode;
+	survive_recording_raw_pose_process(so, timecode, pose);
 }
 
 void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose,