diff --git a/src/xrt/drivers/ht/ht_algorithm.cpp b/src/xrt/drivers/ht/ht_algorithm.cpp index c8acdbb6f..7202fac54 100644 --- a/src/xrt/drivers/ht/ht_algorithm.cpp +++ b/src/xrt/drivers/ht/ht_algorithm.cpp @@ -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]); } diff --git a/src/xrt/drivers/ht/ht_hand_math.cpp b/src/xrt/drivers/ht/ht_hand_math.cpp index eba0f4e12..584145fc4 100644 --- a/src/xrt/drivers/ht/ht_hand_math.cpp +++ b/src/xrt/drivers/ht/ht_hand_math.cpp @@ -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);