+37 -33

src/xrt/auxiliary/math/m_relation_history.cpp

··· 183 183 } 184 184 } 185 185 186 186 - bool 187 187 - m_relation_history_estimate_motion(struct m_relation_history *rh, 188 188 - const struct xrt_space_relation *in_relation, 189 189 - int64_t timestamp, 190 190 - struct xrt_space_relation *out_relation) 186 186 + static void 187 187 + m_relation_history_estimate_motion(struct xrt_space_relation const &old_relation, 188 188 + struct xrt_space_relation const &new_relation, 189 189 + float dt, 190 190 + struct xrt_vec3 &out_linear_velocity, 191 191 + struct xrt_vec3 &out_angular_velocity, 192 192 + enum xrt_space_relation_flags &out_flags) 191 193 { 194 194 + assert(dt != 0.0f); 192 195 193 193 - int64_t last_time_ns; 194 194 - struct xrt_space_relation last_relation; 195 195 - if (!m_relation_history_get_latest(rh, &last_time_ns, &last_relation)) { 196 196 - return false; 197 197 - }; 196 196 + enum xrt_space_relation_flags shared_flags = 197 197 + (enum xrt_space_relation_flags)(old_relation.relation_flags & new_relation.relation_flags); 198 198 199 199 - float dt = (float)time_ns_to_s(timestamp - last_time_ns); 199 199 + // If both relations have position data, estimate linear velocity 200 200 + if (shared_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) { 201 201 + out_flags = (enum xrt_space_relation_flags)(out_flags | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); 200 202 201 201 - // Used to find out what values are valid in both the old relation and the new relation 202 202 - enum xrt_space_relation_flags tmp_flags = 203 203 - (enum xrt_space_relation_flags)(last_relation.relation_flags & in_relation->relation_flags); 204 204 - 205 205 - // Brevity 206 206 - enum xrt_space_relation_flags &outf = out_relation->relation_flags; 207 207 - 208 208 - 209 209 - if (tmp_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) { 210 210 - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_VALID_BIT); 211 211 - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); 203 203 + out_linear_velocity = (new_relation.pose.position - old_relation.pose.position) / dt; 204 204 + } 212 205 213 213 - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); 206 206 + // If both relations have orientation data, estimate angular velocity 207 207 + if (shared_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) { 208 208 + out_flags = (enum xrt_space_relation_flags)(out_flags | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT); 214 209 215 215 - out_relation->linear_velocity = (in_relation->pose.position - last_relation.pose.position) / dt; 210 210 + math_quat_finite_difference(&old_relation.pose.orientation, &new_relation.pose.orientation, dt, 211 211 + &out_angular_velocity); 216 212 } 213 213 + } 214 214 + 215 215 + bool 216 216 + m_relation_history_push_with_motion_estimation(struct m_relation_history *rh, 217 217 + struct xrt_space_relation const *in_relation, 218 218 + int64_t timestamp) 219 219 + { 220 220 + assert((in_relation->relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) == 0); 221 221 + assert((in_relation->relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) == 0); 217 222 218 218 - if (tmp_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) { 219 219 - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT); 220 220 - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); 223 223 + struct xrt_space_relation final_relation = *in_relation; 221 224 222 222 - outf = (enum xrt_space_relation_flags)(outf | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT); 225 225 + int64_t last_time_ns; 226 226 + struct xrt_space_relation last_relation; 227 227 + if (m_relation_history_get_latest(rh, &last_time_ns, &last_relation) && timestamp > last_time_ns) { 228 228 + float dt = (float)time_ns_to_s(timestamp - last_time_ns); 223 229 224 224 - math_quat_finite_difference(&last_relation.pose.orientation, &in_relation->pose.orientation, dt, 225 225 - &out_relation->angular_velocity); 230 230 + m_relation_history_estimate_motion(last_relation, *in_relation, dt, final_relation.linear_velocity, 231 231 + final_relation.angular_velocity, final_relation.relation_flags); 226 232 } 227 233 228 228 - out_relation->pose = in_relation->pose; 229 229 - 230 230 - return true; 234 234 + return m_relation_history_push(rh, &final_relation, timestamp); 231 235 } 232 236 233 237 bool

+15 -17

src/xrt/auxiliary/math/m_relation_history.h

··· 63 63 m_relation_history_push(struct m_relation_history *rh, struct xrt_space_relation const *in_relation, int64_t timestamp); 64 64 65 65 /*! 66 66 - * Interpolates or extrapolates to the desired timestamp. 66 66 + * Pushes a new pose to the history, estimating linear and angular velocity based on the previous entry. 67 67 * 68 68 - * Read-only operation - doesn't remove anything from the buffer or anything like that - you can call this as often as 69 69 - * you want. 68 68 + * If the history is full, it will also pop a pose out of the other side of the buffer. 69 69 + * 70 70 + * @return false if the timestamp is earlier than the most recent timestamp already recorded 70 71 * 71 72 * @public @memberof m_relation_history 72 73 */ 73 73 - enum m_relation_history_result 74 74 - m_relation_history_get(const struct m_relation_history *rh, 75 75 - int64_t at_timestamp_ns, 76 76 - struct xrt_space_relation *out_relation); 74 74 + bool 75 75 + m_relation_history_push_with_motion_estimation(struct m_relation_history *rh, 76 76 + struct xrt_space_relation const *in_relation, 77 77 + int64_t timestamp); 77 78 78 79 /*! 79 79 - * Estimates the movement (velocity and angular velocity) of a new relation based on 80 80 - * the latest relation found in the buffer (as returned by m_relation_history_get_latest). 80 80 + * Interpolates or extrapolates to the desired timestamp. 81 81 * 82 82 - * Read-only on m_relation_history and in_relation. 83 83 - * Copies in_relation->pose to out_relation->pose, and writes new flags and linear/angular velocities to 84 84 - * out_relation->pose. OK to alias in_relation and out_relation. 82 82 + * Read-only operation - doesn't remove anything from the buffer or anything like that - you can call this as often 83 83 + * as you want. 85 84 * 86 85 * @public @memberof m_relation_history 87 86 */ 88 88 - bool 89 89 - m_relation_history_estimate_motion(struct m_relation_history *rh, 90 90 - const struct xrt_space_relation *in_relation, 91 91 - int64_t timestamp, 92 92 - struct xrt_space_relation *out_relation); 87 87 + enum m_relation_history_result 88 88 + m_relation_history_get(const struct m_relation_history *rh, 89 89 + int64_t at_timestamp_ns, 90 90 + struct xrt_space_relation *out_relation); 93 91 94 92 /*! 95 93 * Get the latest report in the buffer, if any.

+1 -3

src/xrt/drivers/hydra/hydra_driver.c

··· 345 345 (XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) | 346 346 (XRT_SPACE_RELATION_POSITION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT); 347 347 348 348 - m_relation_history_estimate_motion(state->relation_history, &space_relation, now, &space_relation); 349 349 - 350 350 - m_relation_history_push(state->relation_history, &space_relation, now); 348 348 + m_relation_history_push_with_motion_estimation(state->relation_history, &space_relation, now); 351 349 352 350 state->buttons = hydra_read_uint8(&buf); 353 351

+4 -10

src/xrt/tracking/hand/t_hand_tracking_async.c

··· 137 137 struct xrt_space_relation wrist_rel = 138 138 hta->working.hands[i].values.hand_joint_set_default[XRT_HAND_JOINT_WRIST].relation; 139 139 140 140 - m_relation_history_estimate_motion( // 141 141 - hta->present.relation_hist[i], // 142 142 - &wrist_rel, // 143 143 - hta->working.timestamp, // 144 144 - &wrist_rel); // 145 145 - 146 146 - m_relation_history_push( // 147 147 - hta->present.relation_hist[i], // 148 148 - &wrist_rel, // 149 149 - hta->working.timestamp); // 140 140 + m_relation_history_push_with_motion_estimation( // 141 141 + hta->present.relation_hist[i], // 142 142 + &wrist_rel, // 143 143 + hta->working.timestamp); // 150 144 } 151 145 152 146 hta->hand_tracking_work_active = false;