1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
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;
}
|
