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
|
#include "poser_general_optimizer.h"
#include "string.h"
#include <assert.h>
#include <malloc.h>
#include <stdio.h>
void *GetDriver(const char *name);
void general_optimizer_data_init(GeneralOptimizerData *d, SurviveObject *so) {
memset(d, 0, sizeof(*d));
d->so = so;
SurviveContext *ctx = so->ctx;
d->failures_to_reset = survive_configi(ctx, "failures-to-reset", SC_GET, 1);
d->successes_to_reset = survive_configi(ctx, "successes-to-reset", SC_GET, -1);
d->max_error = survive_configf(ctx, "max-error", SC_GET, .0001);
const char *subposer = survive_configs(ctx, "seed-poser", SC_GET, "PoserEPNP");
d->seed_poser = (PoserCB)GetDriver(subposer);
SV_INFO("Initializing general optimizer:");
SV_INFO("\tmax-error: %f", d->max_error);
SV_INFO("\tsuccesses-to-reset: %d", d->successes_to_reset);
SV_INFO("\tfailures-to-reset: %d", d->failures_to_reset);
SV_INFO("\tseed-poser: %s(%p)", subposer, d->seed_poser);
}
void general_optimizer_data_record_failure(GeneralOptimizerData *d) {
if (d->failures_to_reset_cntr > 0)
d->failures_to_reset_cntr--;
}
bool general_optimizer_data_record_success(GeneralOptimizerData *d, FLT error) {
d->stats.runs++;
if (d->max_error > error) {
if (d->successes_to_reset_cntr > 0)
d->successes_to_reset_cntr--;
d->failures_to_reset_cntr = d->failures_to_reset;
return true;
}
return false;
}
typedef struct {
bool hasInfo;
SurvivePose pose;
} set_position_t;
static void set_position(SurviveObject *so, uint32_t timecode, SurvivePose *new_pose, void *_user) {
set_position_t *user = _user;
assert(user->hasInfo == false);
user->hasInfo = true;
user->pose = *new_pose;
}
bool general_optimizer_data_record_current_pose(GeneralOptimizerData *d, PoserData *_hdr, size_t len_hdr,
SurvivePose *soLocation) {
*soLocation = d->so->OutPose;
bool currentPositionValid = quatmagnitude(soLocation->Rot) != 0;
static bool seed_warning = false;
if (d->successes_to_reset_cntr == 0 || d->failures_to_reset_cntr == 0 || currentPositionValid == 0) {
PoserCB driver = d->seed_poser;
SurviveContext *ctx = d->so->ctx;
if (driver) {
PoserData *hdr = alloca(len_hdr);
memcpy(hdr, _hdr, len_hdr);
memset(hdr, 0, sizeof(PoserData)); // Clear callback functions
hdr->pt = _hdr->pt;
hdr->poseproc = set_position;
set_position_t locations = {0};
hdr->userdata = &locations;
driver(d->so, hdr);
d->stats.poser_seed_runs++;
if (locations.hasInfo == false) {
return false;
} else if (locations.hasInfo) {
*soLocation = locations.pose;
}
d->successes_to_reset_cntr = d->successes_to_reset;
} else if (seed_warning == false) {
seed_warning = true;
SV_INFO("Not using a seed poser for SBA; results will likely be way off");
}
}
return true;
}
void general_optimizer_data_record_imu(GeneralOptimizerData *d, PoserDataIMU *imu) {
if (d->seed_poser) {
d->seed_poser(d->so, &imu->hdr);
}
}
void general_optimizer_data_dtor(GeneralOptimizerData *d) {
SurviveContext *ctx = d->so->ctx;
SV_INFO("\tseed runs %d / %d", d->stats.poser_seed_runs, d->stats.runs);
SV_INFO("\terror failures %d", d->stats.error_failures);
}
|
