aboutsummaryrefslogtreecommitdiff
path: root/src/poser_general_optimizer.c
blob: ff82fd48a4d71ba47fdb3acd0f7f7b654b476583 (plain)
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);
}