Added tool to calculate various things about length of received pulses

2 files changed, 127 insertions, 0 deletions

diff --git a/tools/lightlengthparams/Makefile b/tools/lightlengthparams/Makefile
new file mode 100644
index 0000000..b743f4f
--- /dev/null
+++ b/tools/lightlengthparams/Makefile
@@ -0,0 +1,15 @@
+all : lightlengthparams
+
+SRT:=../..
+
+LIBSURVIVE:=$(SRT)/lib/libsurvive.so
+
+CFLAGS:=-I$(SRT)/redist -I$(SRT)/include -O0 -g -DFLT=double -DUSE_DOUBLE
+LDFLAGS:=-lm -lpthread -llapacke  -lcblas 
+
+lightlengthparams : lightlengthparams.c $(LIBSURVIVE) 
+	gcc $(CFLAGS) -o $@ $^ $(LDFLAGS)
+
+clean :
+	rm -rf lightlengthparams
+
diff --git a/tools/lightlengthparams/lightlengthparams.c b/tools/lightlengthparams/lightlengthparams.c
new file mode 100644
index 0000000..3b28b3f
--- /dev/null
+++ b/tools/lightlengthparams/lightlengthparams.c
@@ -0,0 +1,112 @@
+#include <assert.h>
+#include <libsurvive/survive.h>
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+uint32_t current_timecode;
+
+void light_process(SurviveObject *so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length,
+				   uint32_t lighthouse) {
+	current_timecode = timecode;
+	survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse);
+}
+
+void raw_pose_process(SurviveObject *so, uint8_t _lh, SurvivePose *pose2w) {
+	survive_default_raw_pose_process(so, _lh, pose2w);
+
+	/*printf("Pose: ");
+	for(int i = 0;i < 7;i++) {
+	  printf("%f, ", pose2w->Pos[i]);
+	};
+	printf("\n");
+	*/
+	for (int lh = 0; lh < 2; lh++) {
+		SurvivePose pose2lh = {};
+		SurvivePose w2lh = {};
+		InvertPose(&w2lh, &so->ctx->bsd[lh].Pose);
+		ApplyPoseToPose(&pose2lh, &w2lh, pose2w);
+
+		SurviveSensorActivations *scene = &so->activations;
+		for (size_t sensor_idx = 0; sensor_idx < so->sensor_ct; sensor_idx++) {
+			if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance / 2,
+													 current_timecode, sensor_idx, lh)) {
+				uint32_t *lengths = scene->lengths[sensor_idx][lh];
+
+				const FLT *sensor_location = so->sensor_locations + 3 * sensor_idx;
+				const FLT *sensor_normals = so->sensor_normals + 3 * sensor_idx;
+
+				LinmathPoint3d sensor = {}, sensorN = {};
+
+				scale3d(sensorN, sensor_normals, .01);
+				add3d(sensorN, sensor_location, sensor_normals);
+
+				/*
+				printf("\n");
+				printf("%f, %f, %f\n", sensor_location[0], sensor_location[1], sensor_location[2]);
+				printf("%f, %f, %f\n", sensorN[0], sensorN[1], sensorN[2]);
+
+				FLT m[4][4];
+				PoseToMatrix((FLT*)m, &pose2lh);
+
+				for(int i = 0;i < 7;i++) {
+			  printf("%f, ", pose2lh.Pos[i]);
+				};
+				printf("\n");
+				for(int i = 0;i < 4;i++) {
+			  for(int j = 0;j < 4;j++) {
+				printf("%f, ", m[i][j]);
+			  }
+			  printf("\n");
+				}
+				printf("\n");
+				*/
+				ApplyPoseToPoint(sensor, &pose2lh, sensor_location);
+				ApplyPoseToPoint(sensorN, &pose2lh, sensorN);
+
+				// printf("%f, %f, %f\n", sensor[0], sensor[1], sensor[2]);
+				// printf("%f, %f, %f\n", sensorN[0], sensorN[1], sensorN[2]);
+				FLT dist = magnitude3d(sensor);
+				LinmathVec3d zout = {0, 0, -dist};
+				LinmathQuat r;
+				quatfrom2vectors(r, sensor, zout);
+				quatrotatevector(sensorN, r, sensorN);
+				sub3d(sensorN, sensorN, zout);
+
+				FLT dot = dot3d(sensor, sensorN);
+				if (dist > 20 || dist < 0.25)
+					continue;
+
+				FLT angs[2] = {};
+				for (int axis = 0; axis < 2; axis++) {
+					angs[axis] = cos(atan2(fabs(sensorN[axis ? 0 : 1]), sensorN[2]));
+				}
+				FLT area = angs[0] * angs[1];
+
+				printf("%u\t%u\t%lu\t%f\t%f\t%f\t%f\t%u\t%u\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n", current_timecode, lh,
+					   sensor_idx, dist, angs[0], angs[1], area, lengths[0], lengths[1], dot, sensor[0], sensor[1],
+					   sensor[2], sensorN[0], sensorN[1], sensorN[2]);
+
+				// assert(angs[0] >= 0);
+			}
+		}
+	}
+}
+
+int main(int argc, char **argv) {
+	SurviveContext *ctx = survive_init(argc, argv);
+	if (ctx == 0) // implies -help or similiar
+		return 0;
+
+	printf("timecode\tlh\tsensor_idx\tdistance\tnorm_x\tnorm_y\tarea\tlength_x\tlength_y\tdot\tx\ty\tz\tnx\tny\tnz\n");
+
+	survive_startup(ctx);
+
+	survive_install_raw_pose_fn(ctx, raw_pose_process);
+	survive_install_light_fn(ctx, light_process);
+	while (survive_poll(ctx) == 0) {
+	}
+
+	survive_close(ctx);
+	return 0;
+}