diff --git a/src/xrt/drivers/ht/ht_algorithm.cpp b/src/xrt/drivers/ht/ht_algorithm.cpp index 77b13bd88..c8acdbb6f 100644 --- a/src/xrt/drivers/ht/ht_algorithm.cpp +++ b/src/xrt/drivers/ht/ht_algorithm.cpp @@ -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; } diff --git a/src/xrt/drivers/ht/ht_driver.hpp b/src/xrt/drivers/ht/ht_driver.hpp index 7c840c0b3..6058c77e8 100644 --- a/src/xrt/drivers/ht/ht_driver.hpp +++ b/src/xrt/drivers/ht/ht_driver.hpp @@ -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; diff --git a/src/xrt/drivers/ht/ht_hand_math.cpp b/src/xrt/drivers/ht/ht_hand_math.cpp index 68a7e70e6..eba0f4e12 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[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);