mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2024-12-29 11:06:18 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

d/ht: Cleanup usage of history buffer.

Browse source 
May wish to reconsider whether all these usages are required: they appear to only use the back (latest) element.
This commit is contained in:
Ryan Pavlik 2021-12-02 16:49:47 -06:00 committed by Jakob Bornecrantz
parent 94e053d0d4
commit 65ef8d58f1
2 changed files with 37 additions and 44 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

47
src/xrt/drivers/ht/ht_algorithm.cpp
View file

@ -47,12 +47,11 @@ errHistory2D(HandHistory2DBBox *past, Palm7KP *present)
// U_LOG_E("Returning big number because htAlgorithm told me to!");
return 100000000000000000000000000000.0f;
}
float sum_of_lengths =
m_vec2_len(*past->wrist_unfiltered.get_at_age(0) - *past->middle_unfiltered.get_at_age(0)) +
m_vec2_len(present->kps[WRIST_7KP] - present->kps[MIDDLE_7KP]);
float sum_of_lengths = m_vec2_len(past->wrist_unfiltered.back() - past->middle_unfiltered.back()) +
m_vec2_len(present->kps[WRIST_7KP] - present->kps[MIDDLE_7KP]);
float sum_of_distances = (m_vec2_len(*past->wrist_unfiltered.get_at_age(0) - present->kps[WRIST_7KP]) +
m_vec2_len(*past->middle_unfiltered.get_at_age(0) - present->kps[MIDDLE_7KP]));
float sum_of_distances = (m_vec2_len(past->wrist_unfiltered.back() - present->kps[WRIST_7KP]) +
m_vec2_len(past->middle_unfiltered.back() - present->kps[MIDDLE_7KP]));
float final = sum_of_distances / sum_of_lengths;
@ -151,10 +150,9 @@ htImageToKeypoints(struct ht_view *htv)
xrt_vec2 unfiltered_middle;
xrt_vec2 unfiltered_direction;
centerAndRotationFromJoints(
htv, entry->wrist_unfiltered.get_at_age(0), entry->index_unfiltered.get_at_age(0),
entry->middle_unfiltered.get_at_age(0), entry->pinky_unfiltered.get_at_age(0), &unfiltered_middle,
&unfiltered_direction);
centerAndRotationFromJoints(htv, &entry->wrist_unfiltered.back(), &entry->index_unfiltered.back(),
&entry->middle_unfiltered.back(), &entry->pinky_unfiltered.back(),
&unfiltered_middle, &unfiltered_direction);
xrt_vec2 filtered_middle;
xrt_vec2 filtered_direction;
@ -287,21 +285,18 @@ jsonMaybeAddSomeHands(struct ht_device *htd, bool err)
for (int idx_joint = 0; idx_joint < 21; idx_joint++) {
// const char* key = keys[idx_joint];
cJSON *j_vec3 = cJSON_AddArrayToObject(j_hand_in_frame, keys[idx_joint]);
cJSON_AddItemToArray(j_vec3,
cJSON_CreateNumber(htd->histories_3d[idx_hand]
.last_hands_unfiltered.get_at_age(0)
->kps[idx_joint]
.x));
cJSON_AddItemToArray(j_vec3,
cJSON_CreateNumber(htd->histories_3d[idx_hand]
.last_hands_unfiltered.get_at_age(0)
->kps[idx_joint]
.y));
cJSON_AddItemToArray(j_vec3,
cJSON_CreateNumber(htd->histories_3d[idx_hand]
.last_hands_unfiltered.get_at_age(0)
->kps[idx_joint]
.z));
cJSON_AddItemToArray(
j_vec3,
cJSON_CreateNumber(
htd->histories_3d[idx_hand].last_hands_unfiltered.back().kps[idx_joint].x));
cJSON_AddItemToArray(
j_vec3,
cJSON_CreateNumber(
htd->histories_3d[idx_hand].last_hands_unfiltered.back().kps[idx_joint].y));
cJSON_AddItemToArray(
j_vec3,
cJSON_CreateNumber(
htd->histories_3d[idx_hand].last_hands_unfiltered.back().kps[idx_joint].z));
}
@ -622,8 +617,8 @@ htRunAlgorithm(struct ht_device *htd)
for (size_t i = 0; i < htd->histories_3d.size(); i++) {
// U_LOG_E("Valid hand %zu l_idx %i r_idx %i", i, htd->histories_3d[i].last_hands[0]->idx_l,
// htd->histories_3d[i].last_hands[0]->idx_r);
valid_2d_idxs[0].push_back(htd->histories_3d[i].last_hands_unfiltered.get_at_age(0)->idx_l);
valid_2d_idxs[1].push_back(htd->histories_3d[i].last_hands_unfiltered.get_at_age(0)->idx_r);
valid_2d_idxs[0].push_back(htd->histories_3d[i].last_hands_unfiltered.back().idx_l);
valid_2d_idxs[1].push_back(htd->histories_3d[i].last_hands_unfiltered.back().idx_r);
handednessHandHistory3D(&htd->histories_3d[i]);
}

34
src/xrt/drivers/ht/ht_hand_math.cpp
View file

@ -32,7 +32,7 @@ float
errHandHistory(const HandHistory3D &history_hand, const Hand3D &present_hand)
{
// Remember we never have to deal with an empty hand. Can always access the last element.
return sumOfHandJointDistances(*history_hand->last_hands_unfiltered.get_at_age(0), present_hand);
return sumOfHandJointDistances(history_hand->last_hands_unfiltered.back(), present_hand);
}
float
@ -262,7 +262,7 @@ void
handednessHandHistory3D(HandHistory3D *history)
{
float inter = handednessJointSet(history->last_hands_unfiltered.get_at_age(0));
float inter = handednessJointSet(&history->last_hands_unfiltered.back());
if ((fabsf(inter) > 0.3f) || (fabsf(history->handedness) < 0.3f)) {
history->handedness += inter;
@ -324,31 +324,29 @@ handEuroFiltersRun(struct ht_device *htd, HandHistory3D *f, Hand3D *out_hand)
}
#elif 0
for (int i = 0; i < 21; i++) {
m_filter_euro_vec3_run(&f->filters[i], f->last_hands_unfiltered.get_at_age(0)->timestamp,
&f->last_hands_unfiltered.get_at_age(0)->kps[i], &out_hand->kps[i]);
m_filter_euro_vec3_run(&f->filters[i], f->last_hands_unfiltered.back().timestamp,
&f->last_hands_unfiltered.back().kps[i], &out_hand->kps[i]);
}
// conspicuously wrong!
out_hand->timestamp = f->last_hands_unfiltered.get_at_age(0)->timestamp;
out_hand->timestamp = f->last_hands_unfiltered.back().timestamp;
#else
if (!f->have_prev_hand) {
f->last_hands_filtered.push_back(*f->last_hands_unfiltered.get_at_age(0));
uint64_t ts = f->last_hands_unfiltered.get_at_age(0)->timestamp;
f->last_hands_filtered.push_back(*f->last_hands_unfiltered.back());
uint64_t ts = f->last_hands_unfiltered.back().timestamp;
f->prev_ts_for_alpha = ts;
f->first_ts = ts;
f->prev_filtered_ts = ts;
f->prev_dy = 0;
f->have_prev_hand = true;
*out_hand = *f->last_hands_unfiltered.get_at_age(0);
*out_hand = *f->last_hands_unfiltered.back();
}
uint64_t ts = f->last_hands_unfiltered.get_at_age(0)->timestamp;
uint64_t ts = f->last_hands_unfiltered.back().timestamp;
double dt, alpha_d;
dt = (double)(ts - f->prev_ts_for_alpha) / U_TIME_1S_IN_NS;
double abs_dy =
(sumOfHandJointDistances(f->last_hands_unfiltered.get_at_age(0), f->last_hands_filtered.get_at_age(0)) /
21.0f) *
0.7f;
(sumOfHandJointDistances(&f->last_hands_unfiltered.back(), &f->last_hands_filtered.back()) / 21.0f) * 0.7f;
alpha_d = calc_smoothing_alpha(htd->dynamic_config.hand_fc_min_d.val, dt);
double alpha, fc_cutoff;
@ -359,12 +357,12 @@ handEuroFiltersRun(struct ht_device *htd, HandHistory3D *f, Hand3D *out_hand)
HT_DEBUG(htd, "dt is %f, abs_dy is %f, alpha is %f", dt, abs_dy, alpha);
for (int i = 0; i < 21; i++) {
out_hand->kps[i].x = exp_smooth(alpha, f->last_hands_unfiltered.get_at_age(0)->kps[i].x,
f->last_hands_filtered.get_at_age(0)->kps[i].x);
out_hand->kps[i].y = exp_smooth(alpha, f->last_hands_unfiltered.get_at_age(0)->kps[i].y,
f->last_hands_filtered.get_at_age(0)->kps[i].y);
out_hand->kps[i].z = exp_smooth(alpha, f->last_hands_unfiltered.get_at_age(0)->kps[i].z,
f->last_hands_filtered.get_at_age(0)->kps[i].z);
out_hand->kps[i].x =
exp_smooth(alpha, f->last_hands_unfiltered.back().kps[i].x, f->last_hands_filtered.back().kps[i].x);
out_hand->kps[i].y =
exp_smooth(alpha, f->last_hands_unfiltered.back().kps[i].y, f->last_hands_filtered.back().kps[i].y);
out_hand->kps[i].z =
exp_smooth(alpha, f->last_hands_unfiltered.back().kps[i].z, f->last_hands_filtered.back().kps[i].z);
}
double prev_ts_offset = (double)(f->prev_filtered_ts - f->first_ts);
double current_ts_offset = (double)(ts - f->first_ts);