diff options
Diffstat (limited to 'logic')
| -rw-r--r-- | logic/main-settings.hpp | 3 | ||||
| -rw-r--r-- | logic/tracker.cpp | 193 |
2 files changed, 111 insertions, 85 deletions
diff --git a/logic/main-settings.hpp b/logic/main-settings.hpp index 53c5fae1..ea1d93f3 100644 --- a/logic/main-settings.hpp +++ b/logic/main-settings.hpp @@ -74,6 +74,7 @@ struct main_settings value<bool> use_camera_offset_from_centering; value<bool> center_at_startup; value<int> center_method; + value<int> neck_y, neck_z; key_opts key_start_tracking, key_stop_tracking, key_toggle_tracking, key_restart_tracking; key_opts key_center, key_toggle, key_zero; key_opts key_toggle_press, key_zero_press; @@ -104,6 +105,8 @@ struct main_settings use_camera_offset_from_centering(b, "use-camera-offset-from-centering", false), center_at_startup(b, "center-at-startup", true), center_method(b, "centering-method", true), + neck_y(b, "neck-height", 0), + neck_z(b, "neck-depth", 0), key_start_tracking(b, "start-tracking"), key_stop_tracking(b, "stop-tracking"), key_toggle_tracking(b, "toggle-tracking"), diff --git a/logic/tracker.cpp b/logic/tracker.cpp index a5892dd0..25e067a7 100644 --- a/logic/tracker.cpp +++ b/logic/tracker.cpp @@ -156,8 +156,7 @@ void Tracker::logic() logger.write_pose(raw); // raw - if (is_nan(raw)) - raw = last_raw; + bool nanp = is_nan(raw) | is_nan(value); // TODO split this function, it's too big @@ -171,58 +170,57 @@ void Tracker::logic() scaled_rotation.camera = get_camera_offset_matrix(c_div); real_rotation.camera = get_camera_offset_matrix(1); - bool nanp = is_nan(value) || is_nan(scaled_rotation.rotation) || is_nan(real_rotation.rotation); + nanp |= is_nan(value) || is_nan(scaled_rotation.rotation) || is_nan(real_rotation.rotation); - if (!nanp) + if (!tracking_started) { - if (!tracking_started) - { - using std::fabs; + using std::fabs; - for (int i = 0; i < 6; i++) - if (fabs(newpose(i)) != 0) - { - tracking_started = true; - break; - } - } + for (int i = 0; i < 6; i++) + if (fabs(newpose(i)) != 0) + { + tracking_started = true; + break; + } - if (get(f_center) && tracking_started) - { - set(f_center, false); + tracking_started &= !nanp; + } - if (libs.pFilter) - libs.pFilter->center(); + if (get(f_center) && tracking_started) + { + set(f_center, false); - if (libs.pTracker->center()) - { - scaled_rotation.rotation = scaled_rotation.camera.t(); - real_rotation.rotation = real_rotation.camera.t(); - - scaled_rotation.rotation = rmat::eye(); - real_rotation.rotation = rmat::eye(); - scaled_rotation.center_roll = rmat::eye(); - scaled_rotation.center_yaw = rmat::eye(); - scaled_rotation.center_pitch = rmat::eye(); - } - else - { - euler::tait_bryan_to_matrices(rmat_to_euler(scaled_rotation.rotation), - scaled_rotation.center_roll, - scaled_rotation.center_pitch, - scaled_rotation.center_yaw); + if (libs.pFilter) + libs.pFilter->center(); + + if (libs.pTracker->center()) + { + scaled_rotation.rotation = scaled_rotation.camera.t(); + real_rotation.rotation = real_rotation.camera.t(); + + scaled_rotation.rotation = rmat::eye(); + real_rotation.rotation = rmat::eye(); + scaled_rotation.center_roll = rmat::eye(); + scaled_rotation.center_yaw = rmat::eye(); + scaled_rotation.center_pitch = rmat::eye(); + } + else + { + euler::tait_bryan_to_matrices(rmat_to_euler(scaled_rotation.rotation), + scaled_rotation.center_roll, + scaled_rotation.center_pitch, + scaled_rotation.center_yaw); #if 0 - euler::tait_bryan_to_matrices(rmat_to_euler(real_rotation.rotation), - real_rotation.center_roll, - real_rotation.center_pitch, - real_rotation.center_yaw); + euler::tait_bryan_to_matrices(rmat_to_euler(real_rotation.rotation), + real_rotation.center_roll, + real_rotation.center_pitch, + real_rotation.center_yaw); #endif - real_rotation.rot_center = real_rotation.rotation.t(); - scaled_rotation.rot_center = scaled_rotation.rotation.t(); - } - - t_center = euler_t(&value(TX)); + real_rotation.rot_center = real_rotation.rotation.t(); + scaled_rotation.rot_center = scaled_rotation.rotation.t(); } + + t_center = euler_t(&value(TX)); } { @@ -268,20 +266,37 @@ void Tracker::logic() logger.write_pose(value); // "corrected" - after various transformations to account for camera position - // whenever something can corrupt its internal state due to nan/inf, elide the call - if (is_nan(value)) - { - nanp = true; - logger.write_pose(value); // "filtered" - } - else + nanp |= is_nan(value); + { - Pose tmp(value); + { + Pose tmp(value); - if (libs.pFilter) - libs.pFilter->filter(tmp, value); + // nan/inf values will corrupt filter internal state + if (!nanp && libs.pFilter) + libs.pFilter->filter(tmp, value); + + logger.write_pose(value); // "filtered" + } + + euler_t neck, rel; - logger.write_pose(value); // "filtered" + { + double ny = s.neck_y, nz = -s.neck_z; + + if (ny != 0 || nz != 0) + { + const rmat R = euler_to_rmat( + euler_t(value(Yaw) * d2r, + value(Pitch) * d2r, + value(Roll) * d2r)); + euler_t xyz(0, ny, nz); + t_compensate(R, xyz, xyz, false, false, false); + neck(TX) = xyz(TX); + neck(TY) = xyz(TY) - ny; + neck(TZ) = xyz(TZ) - nz; + } + } // CAVEAT rotation only, due to tcomp for (int i = 3; i < 6; i++) @@ -294,38 +309,45 @@ void Tracker::logic() for (int i = 0; i < 6; i++) value(i) = 0; - if (is_nan(value)) - nanp = true; - } + const bool reltrans = !get(f_tcomp_disabled); - if (s.tcomp_p && !get(f_tcomp_disabled)) - { - const double tcomp_c[] = + if (s.tcomp_p && reltrans) { - double(!s.tcomp_disable_src_yaw), - double(!s.tcomp_disable_src_pitch), - double(!s.tcomp_disable_src_roll), - }; - euler_t value_(value(TX), value(TY), value(TZ)); - t_compensate(euler_to_rmat( - euler_t(value(Yaw) * d2r * tcomp_c[0], - value(Pitch) * d2r * tcomp_c[1], - value(Roll) * d2r * tcomp_c[2])), - value_, - value_, - s.tcomp_disable_tx, - s.tcomp_disable_ty, - s.tcomp_disable_tz); - if (is_nan(value_)) - nanp = true; + const double tcomp_c[] = + { + double(!s.tcomp_disable_src_yaw), + double(!s.tcomp_disable_src_pitch), + double(!s.tcomp_disable_src_roll), + }; + const rmat R = euler_to_rmat( + euler_t(value(Yaw) * d2r * tcomp_c[0], + value(Pitch) * d2r * tcomp_c[1], + value(Roll) * d2r * tcomp_c[2])); + euler_t ret; + t_compensate(R, + euler_t(value(TX), value(TY), value(TZ)), + ret, + s.tcomp_disable_tx, + s.tcomp_disable_ty, + s.tcomp_disable_tz); + + for (int i = 0; i < 3; i++) + rel(i) = ret(i) - value(i); + } + + // don't t_compensate existing compensated values for (int i = 0; i < 3; i++) - value(i) = value_(i); + value(i) += neck(i) + rel(i); + + nanp |= is_nan(neck) | is_nan(rel) | is_nan(value); } // CAVEAT translation only, due to tcomp for (int i = 0; i < 3; i++) value(i) = map(value(i), m(i)); + nanp |= is_nan(value); + for (int i = 0; i < 6; i++) if (m(i).opts.invert) value(i) = -value(i); @@ -334,17 +356,18 @@ void Tracker::logic() if (nanp) { - value = last_mapped; + QMutexLocker foo(&mtx); + + value = output_pose; + raw = raw_6dof; // for widget last value display for (int i = 0; i < 6; i++) - (void) map(value(i), m(i)); + (void) map(raw_6dof(i), m(i)); } - libs.pProtocol->pose(value); - - last_mapped = value; - last_raw = raw; + if (!nanp) + libs.pProtocol->pose(value); QMutexLocker foo(&mtx); output_pose = value; |