From 0064989e8bec437d84535554fc7611fb69097439 Mon Sep 17 00:00:00 2001 From: iVRy VR Date: Mon, 12 Apr 2021 15:02:16 +0000 Subject: [PATCH] t/psvr: Various fixes and changes - Fix out of array bounds crash, curr_y can be out of bounds. - Set tracked bits appropriately. - Use m_imu_3dof for orientation. --- src/xrt/auxiliary/tracking/t_tracker_psvr.cpp | 40 ++++++++++++------- 1 file changed, 25 insertions(+), 15 deletions(-) diff --git a/src/xrt/auxiliary/tracking/t_tracker_psvr.cpp b/src/xrt/auxiliary/tracking/t_tracker_psvr.cpp index b59aa2a88..008eeb3ea 100644 --- a/src/xrt/auxiliary/tracking/t_tracker_psvr.cpp +++ b/src/xrt/auxiliary/tracking/t_tracker_psvr.cpp @@ -24,6 +24,7 @@ #include "math/m_mathinclude.h" #include "math/m_api.h" #include "math/m_permutation.h" +#include "math/m_imu_3dof.h" #include "os/os_threading.h" @@ -230,7 +231,7 @@ public: struct { struct xrt_vec3 pos = {}; - struct xrt_quat rot = {}; + struct m_imu_3dof imu_3dof; } fusion; struct @@ -1281,7 +1282,7 @@ sample_line(cv::Mat &src, cv::Point2i start, cv::Point2i end, int *inside_length while (1) { // sample our pixel and see if it is in the interior - if (curr_x > 0 && curr_y > 0) { + if (curr_x > 0 && curr_y > 0 && curr_x < src.cols && curr_y < src.rows) { // cv is row, column uint8_t *val = src.ptr(curr_y, curr_x); @@ -1677,7 +1678,9 @@ process(TrackerPSVR &t, struct xrt_frame *xf) // leds. if (t.merged_points.size() >= PSVR_OPTICAL_SOLVE_THRESH) { Eigen::Quaternionf correction = - rot * Eigen::Quaternionf(t.fusion.rot.w, t.fusion.rot.x, t.fusion.rot.y, t.fusion.rot.z).inverse(); + rot * Eigen::Quaternionf(t.fusion.imu_3dof.rot.w, t.fusion.imu_3dof.rot.x, t.fusion.imu_3dof.rot.y, + t.fusion.imu_3dof.rot.z) + .inverse(); float correction_magnitude = t.target_optical_rotation_correction.angularDistance(correction); @@ -1856,10 +1859,14 @@ get_pose(TrackerPSVR &t, timepoint_ns when_ns, struct xrt_space_relation *out_re //! @todo assuming that orientation is actually //! currently tracked. - out_relation->relation_flags = (enum xrt_space_relation_flags)( - XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT | - XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); + out_relation->relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_POSITION_VALID_BIT | + XRT_SPACE_RELATION_POSITION_TRACKED_BIT | + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT); + if (t.done_correction) { + out_relation->relation_flags = (enum xrt_space_relation_flags)( + out_relation->relation_flags | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); + } os_thread_helper_unlock(&t.oth); } @@ -1876,16 +1883,16 @@ imu_data(TrackerPSVR &t, timepoint_ns timestamp_ns, struct xrt_tracking_sample * if (t.last_imu != 0) { time_duration_ns delta_ns = timestamp_ns - t.last_imu; float dt = time_ns_to_s(delta_ns); - // Super simple fusion. - math_quat_integrate_velocity(&t.fusion.rot, &sample->gyro_rad_secs, dt, &t.fusion.rot); + // Update 3DOF fusion + m_imu_3dof_update(&t.fusion.imu_3dof, timestamp_ns, &sample->accel_m_s2, &sample->gyro_rad_secs); } // apply our optical correction to imu rotation // data Eigen::Quaternionf corrected_rot_q = - t.optical_rotation_correction * - Eigen::Quaternionf(t.fusion.rot.w, t.fusion.rot.x, t.fusion.rot.y, t.fusion.rot.z); + t.optical_rotation_correction * Eigen::Quaternionf(t.fusion.imu_3dof.rot.w, t.fusion.imu_3dof.rot.x, + t.fusion.imu_3dof.rot.y, t.fusion.imu_3dof.rot.z); Eigen::Matrix4f corrected_rot = Eigen::Matrix4f::Identity(); corrected_rot.block(0, 0, 3, 3) = corrected_rot_q.toRotationMatrix(); @@ -1990,6 +1997,8 @@ t_psvr_node_destroy(struct xrt_frame_node *node) os_thread_helper_destroy(&t_ptr->oth); + m_imu_3dof_close(&t_ptr->fusion.imu_3dof); + delete t_ptr; } @@ -2096,7 +2105,6 @@ t_psvr_create(struct xrt_frame_context *xfctx, t.sink.push_frame = t_psvr_sink_push_frame; t.node.break_apart = t_psvr_node_break_apart; t.node.destroy = t_psvr_node_destroy; - t.fusion.rot.w = 1.0f; ret = os_thread_helper_init(&t.oth); if (ret != 0) { @@ -2108,10 +2116,12 @@ t_psvr_create(struct xrt_frame_context *xfctx, t.fusion.pos.y = 0.0f; t.fusion.pos.z = 0.0f; - t.fusion.rot.x = 0.0f; - t.fusion.rot.y = 0.0f; - t.fusion.rot.z = 0.0f; - t.fusion.rot.w = 1.0f; + m_imu_3dof_init(&t.fusion.imu_3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); + + t.fusion.imu_3dof.rot.x = 0.0f; + t.fusion.imu_3dof.rot.y = 0.0f; + t.fusion.imu_3dof.rot.z = 0.0f; + t.fusion.imu_3dof.rot.w = 1.0f; xrt_frame_context_add(xfctx, &t.node);