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
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
|
#include "survive_reproject.h"
#include <../redist/linmath.h>
#include <math.h>
#include <string.h>
static void survive_calibration_options_config_normalize(
survive_calibration_options_config *option) {
if (!option->enable[0])
option->invert[0] = false;
if (!option->enable[1])
option->invert[1] = false;
if (!option->enable[0] && !option->enable[1])
option->swap = false;
}
void survive_calibration_options_config_apply(
const survive_calibration_options_config *option, const FLT *input,
FLT *output) {
FLT tmp[2]; // In case they try to do in place
for (int i = 0; i < 2; i++) {
tmp[i] = option->enable[i] * (option->invert[i] ? -1 : 1) *
input[i ^ option->swap];
}
for (int i = 0; i < 2; i++) {
output[i] = tmp[i];
}
}
survive_calibration_config
survive_calibration_config_create_from_idx(size_t v) {
survive_calibration_config config;
memset(&config, 0, sizeof(config));
bool *_this = (bool *)&config;
for (size_t i = 0; i < sizeof(config); i++) {
_this[i] = (bool)(v & 1);
v = v >> 1;
}
survive_calibration_options_config_normalize(&config.phase);
survive_calibration_options_config_normalize(&config.tilt);
survive_calibration_options_config_normalize(&config.curve);
survive_calibration_options_config_normalize(&config.gibMag);
config.gibPhase.enable[0] = config.gibMag.enable[0];
config.gibPhase.enable[1] = config.gibMag.enable[1];
survive_calibration_options_config_normalize(&config.gibPhase);
if (!config.gibPhase.enable[0] && !config.gibPhase.enable[1])
config.gibUseSin = false;
return config;
}
size_t
survive_calibration_config_index(const survive_calibration_config *config) {
bool *_this = (bool *)config;
size_t v = 0;
for (size_t i = 0; i < sizeof(*config); i++) {
v = (v | _this[sizeof(*config) - i - 1]);
v = v << 1;
}
v = v >> 1;
return v;
}
static FLT gibf(bool useSin, FLT v) {
if (useSin)
return sin(v);
return cos(v);
}
void survive_reproject_from_pose_with_config(
const SurviveContext *ctx, const survive_calibration_config *config,
int lighthouse, const SurvivePose *pose, const FLT *pt, FLT *out) {
FLT invq[4];
quatgetreciprocal(invq, pose->Rot);
FLT tvec[3];
quatrotatevector(tvec, invq, pose->Pos);
FLT t_pt[3];
quatrotatevector(t_pt, invq, pt);
for (int i = 0; i < 3; i++)
t_pt[i] = t_pt[i] - tvec[i];
FLT x = -t_pt[0] / -t_pt[2];
FLT y = t_pt[1] / -t_pt[2];
double ang_x = atan(x);
double ang_y = atan(y);
const BaseStationData *bsd = &ctx->bsd[lighthouse];
double phase[2];
survive_calibration_options_config_apply(&config->phase, bsd->fcalphase,
phase);
double tilt[2];
survive_calibration_options_config_apply(&config->tilt, bsd->fcaltilt,
tilt);
double curve[2];
survive_calibration_options_config_apply(&config->curve, bsd->fcalcurve,
curve);
double gibPhase[2];
survive_calibration_options_config_apply(&config->gibPhase, bsd->fcalgibpha,
gibPhase);
double gibMag[2];
survive_calibration_options_config_apply(&config->gibMag, bsd->fcalgibmag,
gibMag);
out[0] = ang_x + phase[0] + tan(tilt[0]) * y + curve[0] * y * y +
gibf(config->gibUseSin, gibPhase[0] + ang_x) * gibMag[0];
out[1] = ang_y + phase[1] + tan(tilt[1]) * x + curve[1] * x * x +
gibf(config->gibUseSin, gibPhase[1] + ang_y) * gibMag[1];
}
void survive_reproject_from_pose(const SurviveContext *ctx, int lighthouse,
const SurvivePose *pose, FLT *pt, FLT *out) {
survive_reproject_from_pose_with_config(
ctx, survive_calibration_default_config(), lighthouse, pose, pt, out);
}
void survive_reproject(const SurviveContext *ctx, int lighthouse, FLT *point3d,
FLT *out) {
survive_reproject_from_pose(ctx, lighthouse, &ctx->bsd[lighthouse].Pose,
point3d, out);
}
const survive_calibration_config *survive_calibration_default_config() {
static survive_calibration_config *def = 0;
if (def == 0) {
def = malloc(sizeof(survive_calibration_config));
memset(def, 0, sizeof(survive_calibration_config));
*def = survive_calibration_config_create_from_idx(0);
}
return def;
}
size_t survive_calibration_config_max_idx() {
survive_calibration_config cfg;
memset(&cfg, 0x1, sizeof(survive_calibration_config));
return survive_calibration_config_index(&cfg);
}
|
