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: use new HistoryBuf api

Browse source
This commit is contained in:
Moses Turner 2021-11-23 18:58:13 -06:00 committed by Jakob Bornecrantz
parent 90c74578d1
commit 614e0e58ae
3 changed files with 60 additions and 51 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -47,11 +47,12 @@ 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[0] - *past->middle_unfiltered[0]) +
m_vec2_len(present->kps[WRIST_7KP] - present->kps[MIDDLE_7KP]);
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_distances = (m_vec2_len(*past->wrist_unfiltered[0] - present->kps[WRIST_7KP]) +
m_vec2_len(*past->middle_unfiltered[0] - 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 final = sum_of_distances / sum_of_lengths;
@ -112,10 +113,10 @@ htImageToKeypoints(struct ht_view *htv)
// Do the things for each active bbox history!
for (size_t i = 0; i < history_indices.size(); i++) {
HandHistory2DBBox *hist_of_interest = &htv->bbox_histories[history_indices[i]];
hist_of_interest->wrist_unfiltered.push(hand_detections[detection_indices[i]].kps[WRIST_7KP]);
hist_of_interest->index_unfiltered.push(hand_detections[detection_indices[i]].kps[INDEX_7KP]);
hist_of_interest->middle_unfiltered.push(hand_detections[detection_indices[i]].kps[MIDDLE_7KP]);
hist_of_interest->pinky_unfiltered.push(hand_detections[detection_indices[i]].kps[LITTLE_7KP]);
hist_of_interest->wrist_unfiltered.push_back(hand_detections[detection_indices[i]].kps[WRIST_7KP]);
hist_of_interest->index_unfiltered.push_back(hand_detections[detection_indices[i]].kps[INDEX_7KP]);
hist_of_interest->middle_unfiltered.push_back(hand_detections[detection_indices[i]].kps[MIDDLE_7KP]);
hist_of_interest->pinky_unfiltered.push_back(hand_detections[detection_indices[i]].kps[LITTLE_7KP]);
// Eh do the rest later
}
@ -150,9 +151,10 @@ htImageToKeypoints(struct ht_view *htv)
xrt_vec2 unfiltered_middle;
xrt_vec2 unfiltered_direction;
centerAndRotationFromJoints(htv, entry->wrist_unfiltered[0], entry->index_unfiltered[0],
entry->middle_unfiltered[0], entry->pinky_unfiltered[0], &unfiltered_middle,
&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);
xrt_vec2 filtered_middle;
xrt_vec2 filtered_direction;
@ -285,18 +287,21 @@ 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[0]->kps[idx_joint].x));
cJSON_AddItemToArray(
j_vec3,
cJSON_CreateNumber(
htd->histories_3d[idx_hand].last_hands_unfiltered[0]->kps[idx_joint].y));
cJSON_AddItemToArray(
j_vec3,
cJSON_CreateNumber(
htd->histories_3d[idx_hand].last_hands_unfiltered[0]->kps[idx_joint].z));
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));
}
@ -566,7 +571,8 @@ htRunAlgorithm(struct ht_device *htd)
for (size_t i = 0; i < past_indices.size(); i++) {
htd->histories_3d[past_indices[i]].last_hands_unfiltered.push(hands_unfiltered[present_indices[i]]);
htd->histories_3d[past_indices[i]].last_hands_unfiltered.push_back(
hands_unfiltered[present_indices[i]]);
}
// The above may not do anything, because we'll start out with no hand histories! All the numbers of elements
// should be zero.
@ -579,7 +585,7 @@ htRunAlgorithm(struct ht_device *htd)
history_new.uuid = rand(); // Not a great uuid, huh? Good enough for us, this only has to be
// unique across say an hour period max.
handEuroFiltersInit(&history_new, FCMIN_HAND, FCMIN_D_HAND, BETA_HAND);
history_new.last_hands_unfiltered.push(hands_unfiltered[i]);
history_new.last_hands_unfiltered.push_back(hands_unfiltered[i]);
// history_new.
htd->histories_3d.push_back(
history_new); // Add something to the end - don't initialize any of it.
@ -616,8 +622,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[0]->idx_l);
valid_2d_idxs[1].push_back(htd->histories_3d[i].last_hands_unfiltered[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);
handednessHandHistory3D(&htd->histories_3d[i]);
}
@ -645,7 +651,7 @@ htRunAlgorithm(struct ht_device *htd)
for (size_t hand_index = 0; hand_index < num_hands; hand_index++) {
handEuroFiltersRun(htd, &htd->histories_3d[hand_index], &filtered_hands[hand_index]);
htd->histories_3d[hand_index].last_hands_filtered.push(filtered_hands[hand_index]);
htd->histories_3d[hand_index].last_hands_filtered.push_back(filtered_hands[hand_index]);
applyThumbIndexDrag(&filtered_hands[hand_index]);
filtered_hands[hand_index].handedness = htd->histories_3d[hand_index].handedness;
}

2
src/xrt/drivers/ht/ht_driver.hpp
View file

@ -169,7 +169,7 @@ struct HandHistory3D
float handedness;
bool have_prev_hand = false;
double prev_dy;
uint64_t prev_ts_for_alpha; // also in last_hands_unfiltered[0] but go away.
uint64_t prev_ts_for_alpha; // also in last_hands_unfiltered.back() but go away.
uint64_t first_ts;
uint64_t prev_filtered_ts;

47
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[0], present_hand);
return sumOfHandJointDistances(*history_hand->last_hands_unfiltered.get_at_age(0), present_hand);
}
float
@ -262,7 +262,7 @@ void
handednessHandHistory3D(HandHistory3D *history)
{
float inter = handednessJointSet(history->last_hands_unfiltered[0]);
float inter = handednessJointSet(history->last_hands_unfiltered.get_at_age(0));
if ((fabsf(inter) > 0.3f) || (fabsf(history->handedness) < 0.3f)) {
history->handedness += inter;
@ -305,16 +305,17 @@ handEuroFiltersRun(struct ht_device *htd, HandHistory3D *f, Hand3D *out_hand)
#if 0
// float vals[4] = {0.5, 0.33, 0.1, 0.07};
float vals[4] = {0.9, 0.09, 0.009, 0.001};
int m = f->last_hands_unfiltered.length-1;
double ts_out = (vals[0] * (double)f->last_hands_unfiltered[std::min(m,0)]->timestamp) +
(vals[1] * (double)f->last_hands_unfiltered[std::min(m,1)]->timestamp) +
(vals[2] * (double)f->last_hands_unfiltered[std::min(m,2)]->timestamp) +
(vals[3] * (double)f->last_hands_unfiltered[std::min(m,3)]->timestamp);
auto m = f->last_hands_unfiltered.size() - 1;
double ts_out = (vals[0] * (double)f->last_hands_unfiltered.get_at_age(std::min(m, 0))->timestamp) +
(vals[1] * (double)f->last_hands_unfiltered.get_at_age(std::min(m, 1))->timestamp) +
(vals[2] * (double)f->last_hands_unfiltered.get_at_age(std::min(m, 2))->timestamp) +
(vals[3] * (double)f->last_hands_unfiltered.get_at_age(std::min(m, 3))->timestamp);
out_hand->timestamp = (uint64_t)ts_out;
for (int kp_idx = 0; kp_idx < 21; kp_idx++) {
for (int hist_idx = 0; hist_idx < 4; hist_idx++) {
float *in_y_arr = (float *)&f->last_hands_unfiltered[std::min(m,hist_idx)]->kps[kp_idx];
float *in_y_arr =
(float *)&f->last_hands_unfiltered.get_at_age(std::min(m, hist_idx))->kps[kp_idx];
float *out_y_arr = (float *)&out_hand->kps[kp_idx];
for (int i = 0; i < 3; i++) {
out_y_arr[i] += in_y_arr[i] * vals[hist_idx];
@ -323,29 +324,31 @@ 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[0]->timestamp,
&f->last_hands_unfiltered[0]->kps[i], &out_hand->kps[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]);
}
// conspicuously wrong!
out_hand->timestamp = f->last_hands_unfiltered[0]->timestamp;
out_hand->timestamp = f->last_hands_unfiltered.get_at_age(0)->timestamp;
#else
if (!f->have_prev_hand) {
f->last_hands_filtered.push(*f->last_hands_unfiltered[0]);
uint64_t ts = f->last_hands_unfiltered[0]->timestamp;
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->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[0];
*out_hand = *f->last_hands_unfiltered.get_at_age(0);
}
uint64_t ts = f->last_hands_unfiltered[0]->timestamp;
uint64_t ts = f->last_hands_unfiltered.get_at_age(0)->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[0], f->last_hands_filtered[0]) / 21.0f) * 0.7f;
(sumOfHandJointDistances(f->last_hands_unfiltered.get_at_age(0), f->last_hands_filtered.get_at_age(0)) /
21.0f) *
0.7f;
alpha_d = calc_smoothing_alpha(htd->dynamic_config.hand_fc_min_d.val, dt);
double alpha, fc_cutoff;
@ -356,12 +359,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[0]->kps[i].x, f->last_hands_filtered[0]->kps[i].x);
out_hand->kps[i].y =
exp_smooth(alpha, f->last_hands_unfiltered[0]->kps[i].y, f->last_hands_filtered[0]->kps[i].y);
out_hand->kps[i].z =
exp_smooth(alpha, f->last_hands_unfiltered[0]->kps[i].z, f->last_hands_filtered[0]->kps[i].z);
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);
}
double prev_ts_offset = (double)(f->prev_filtered_ts - f->first_ts);
double current_ts_offset = (double)(ts - f->first_ts);