mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-01 12:46:12 +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!"); // U_LOG_E("Returning big number because htAlgorithm told me to!");
return 100000000000000000000000000000.0f; return 100000000000000000000000000000.0f;
} }
float sum_of_lengths = float sum_of_lengths = m_vec2_len(past->wrist_unfiltered.back() - past->middle_unfiltered.back()) +
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]);
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]) + float sum_of_distances = (m_vec2_len(past->wrist_unfiltered.back() - present->kps[WRIST_7KP]) +
m_vec2_len(*past->middle_unfiltered.get_at_age(0) - present->kps[MIDDLE_7KP])); m_vec2_len(past->middle_unfiltered.back() - present->kps[MIDDLE_7KP]));
float final = sum_of_distances / sum_of_lengths; float final = sum_of_distances / sum_of_lengths;
@ -151,10 +150,9 @@ htImageToKeypoints(struct ht_view *htv)
xrt_vec2 unfiltered_middle; xrt_vec2 unfiltered_middle;
xrt_vec2 unfiltered_direction; xrt_vec2 unfiltered_direction;
centerAndRotationFromJoints( centerAndRotationFromJoints(htv, &entry->wrist_unfiltered.back(), &entry->index_unfiltered.back(),
htv, entry->wrist_unfiltered.get_at_age(0), entry->index_unfiltered.get_at_age(0), &entry->middle_unfiltered.back(), &entry->pinky_unfiltered.back(),
entry->middle_unfiltered.get_at_age(0), entry->pinky_unfiltered.get_at_age(0), &unfiltered_middle, &unfiltered_middle, &unfiltered_direction);
&unfiltered_direction);
xrt_vec2 filtered_middle; xrt_vec2 filtered_middle;
xrt_vec2 filtered_direction; 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++) { for (int idx_joint = 0; idx_joint < 21; idx_joint++) {
// const char* key = keys[idx_joint]; // const char* key = keys[idx_joint];
cJSON *j_vec3 = cJSON_AddArrayToObject(j_hand_in_frame, keys[idx_joint]); cJSON *j_vec3 = cJSON_AddArrayToObject(j_hand_in_frame, keys[idx_joint]);
cJSON_AddItemToArray(j_vec3, cJSON_AddItemToArray(
cJSON_CreateNumber(htd->histories_3d[idx_hand] j_vec3,
.last_hands_unfiltered.get_at_age(0) cJSON_CreateNumber(
->kps[idx_joint] htd->histories_3d[idx_hand].last_hands_unfiltered.back().kps[idx_joint].x));
.x)); cJSON_AddItemToArray(
cJSON_AddItemToArray(j_vec3, j_vec3,
cJSON_CreateNumber(htd->histories_3d[idx_hand] cJSON_CreateNumber(
.last_hands_unfiltered.get_at_age(0) htd->histories_3d[idx_hand].last_hands_unfiltered.back().kps[idx_joint].y));
->kps[idx_joint] cJSON_AddItemToArray(
.y)); j_vec3,
cJSON_AddItemToArray(j_vec3, cJSON_CreateNumber(
cJSON_CreateNumber(htd->histories_3d[idx_hand] htd->histories_3d[idx_hand].last_hands_unfiltered.back().kps[idx_joint].z));
.last_hands_unfiltered.get_at_age(0)
->kps[idx_joint]
.z));
} }
@ -622,8 +617,8 @@ htRunAlgorithm(struct ht_device *htd)
for (size_t i = 0; i < htd->histories_3d.size(); i++) { 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, // 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); // 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[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.get_at_age(0)->idx_r); valid_2d_idxs[1].push_back(htd->histories_3d[i].last_hands_unfiltered.back().idx_r);
handednessHandHistory3D(&htd->histories_3d[i]); 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) 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. // 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 float
@ -262,7 +262,7 @@ void
handednessHandHistory3D(HandHistory3D *history) 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)) { if ((fabsf(inter) > 0.3f) || (fabsf(history->handedness) < 0.3f)) {
history->handedness += inter; history->handedness += inter;
@ -324,31 +324,29 @@ handEuroFiltersRun(struct ht_device *htd, HandHistory3D *f, Hand3D *out_hand)
} }
#elif 0 #elif 0
for (int i = 0; i < 21; i++) { for (int i = 0; i < 21; i++) {
m_filter_euro_vec3_run(&f->filters[i], f->last_hands_unfiltered.get_at_age(0)->timestamp, m_filter_euro_vec3_run(&f->filters[i], f->last_hands_unfiltered.back().timestamp,
&f->last_hands_unfiltered.get_at_age(0)->kps[i], &out_hand->kps[i]); &f->last_hands_unfiltered.back().kps[i], &out_hand->kps[i]);
} }
// conspicuously wrong! // conspicuously wrong!
out_hand->timestamp = f->last_hands_unfiltered.get_at_age(0)->timestamp; out_hand->timestamp = f->last_hands_unfiltered.back().timestamp;
#else #else
if (!f->have_prev_hand) { if (!f->have_prev_hand) {
f->last_hands_filtered.push_back(*f->last_hands_unfiltered.get_at_age(0)); f->last_hands_filtered.push_back(*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;
f->prev_ts_for_alpha = ts; f->prev_ts_for_alpha = ts;
f->first_ts = ts; f->first_ts = ts;
f->prev_filtered_ts = ts; f->prev_filtered_ts = ts;
f->prev_dy = 0; f->prev_dy = 0;
f->have_prev_hand = true; 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; double dt, alpha_d;
dt = (double)(ts - f->prev_ts_for_alpha) / U_TIME_1S_IN_NS; dt = (double)(ts - f->prev_ts_for_alpha) / U_TIME_1S_IN_NS;
double abs_dy = double abs_dy =
(sumOfHandJointDistances(f->last_hands_unfiltered.get_at_age(0), f->last_hands_filtered.get_at_age(0)) / (sumOfHandJointDistances(&f->last_hands_unfiltered.back(), &f->last_hands_filtered.back()) / 21.0f) * 0.7f;
21.0f) *
0.7f;
alpha_d = calc_smoothing_alpha(htd->dynamic_config.hand_fc_min_d.val, dt); alpha_d = calc_smoothing_alpha(htd->dynamic_config.hand_fc_min_d.val, dt);
double alpha, fc_cutoff; 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); HT_DEBUG(htd, "dt is %f, abs_dy is %f, alpha is %f", dt, abs_dy, alpha);
for (int i = 0; i < 21; i++) { 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, out_hand->kps[i].x =
f->last_hands_filtered.get_at_age(0)->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.get_at_age(0)->kps[i].y, out_hand->kps[i].y =
f->last_hands_filtered.get_at_age(0)->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.get_at_age(0)->kps[i].z, out_hand->kps[i].z =
f->last_hands_filtered.get_at_age(0)->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 prev_ts_offset = (double)(f->prev_filtered_ts - f->first_ts);
double current_ts_offset = (double)(ts - f->first_ts); double current_ts_offset = (double)(ts - f->first_ts);