The open source OpenXR runtime
0
fork

Configure Feed

Select the types of activity you want to include in your feed.

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.

authored by

iVRy VR and committed by
Jakob Bornecrantz
0064989e c54a6bef

+25 -15
+25 -15
src/xrt/auxiliary/tracking/t_tracker_psvr.cpp
··· 24 24 #include "math/m_mathinclude.h" 25 25 #include "math/m_api.h" 26 26 #include "math/m_permutation.h" 27 + #include "math/m_imu_3dof.h" 27 28 28 29 #include "os/os_threading.h" 29 30 ··· 230 231 struct 231 232 { 232 233 struct xrt_vec3 pos = {}; 233 - struct xrt_quat rot = {}; 234 + struct m_imu_3dof imu_3dof; 234 235 } fusion; 235 236 236 237 struct ··· 1281 1282 1282 1283 while (1) { 1283 1284 // sample our pixel and see if it is in the interior 1284 - if (curr_x > 0 && curr_y > 0) { 1285 + if (curr_x > 0 && curr_y > 0 && curr_x < src.cols && curr_y < src.rows) { 1285 1286 // cv is row, column 1286 1287 uint8_t *val = src.ptr(curr_y, curr_x); 1287 1288 ··· 1677 1678 // leds. 1678 1679 if (t.merged_points.size() >= PSVR_OPTICAL_SOLVE_THRESH) { 1679 1680 Eigen::Quaternionf correction = 1680 - rot * Eigen::Quaternionf(t.fusion.rot.w, t.fusion.rot.x, t.fusion.rot.y, t.fusion.rot.z).inverse(); 1681 + rot * Eigen::Quaternionf(t.fusion.imu_3dof.rot.w, t.fusion.imu_3dof.rot.x, t.fusion.imu_3dof.rot.y, 1682 + t.fusion.imu_3dof.rot.z) 1683 + .inverse(); 1681 1684 1682 1685 float correction_magnitude = t.target_optical_rotation_correction.angularDistance(correction); 1683 1686 ··· 1856 1859 1857 1860 //! @todo assuming that orientation is actually 1858 1861 //! currently tracked. 1859 - out_relation->relation_flags = (enum xrt_space_relation_flags)( 1860 - XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT | 1861 - XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); 1862 + out_relation->relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_POSITION_VALID_BIT | 1863 + XRT_SPACE_RELATION_POSITION_TRACKED_BIT | 1864 + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT); 1862 1865 1866 + if (t.done_correction) { 1867 + out_relation->relation_flags = (enum xrt_space_relation_flags)( 1868 + out_relation->relation_flags | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); 1869 + } 1863 1870 os_thread_helper_unlock(&t.oth); 1864 1871 } 1865 1872 ··· 1876 1883 if (t.last_imu != 0) { 1877 1884 time_duration_ns delta_ns = timestamp_ns - t.last_imu; 1878 1885 float dt = time_ns_to_s(delta_ns); 1879 - // Super simple fusion. 1880 - math_quat_integrate_velocity(&t.fusion.rot, &sample->gyro_rad_secs, dt, &t.fusion.rot); 1886 + // Update 3DOF fusion 1887 + m_imu_3dof_update(&t.fusion.imu_3dof, timestamp_ns, &sample->accel_m_s2, &sample->gyro_rad_secs); 1881 1888 } 1882 1889 1883 1890 // apply our optical correction to imu rotation 1884 1891 // data 1885 1892 1886 1893 Eigen::Quaternionf corrected_rot_q = 1887 - t.optical_rotation_correction * 1888 - Eigen::Quaternionf(t.fusion.rot.w, t.fusion.rot.x, t.fusion.rot.y, t.fusion.rot.z); 1894 + t.optical_rotation_correction * Eigen::Quaternionf(t.fusion.imu_3dof.rot.w, t.fusion.imu_3dof.rot.x, 1895 + t.fusion.imu_3dof.rot.y, t.fusion.imu_3dof.rot.z); 1889 1896 1890 1897 Eigen::Matrix4f corrected_rot = Eigen::Matrix4f::Identity(); 1891 1898 corrected_rot.block(0, 0, 3, 3) = corrected_rot_q.toRotationMatrix(); ··· 1990 1997 1991 1998 os_thread_helper_destroy(&t_ptr->oth); 1992 1999 2000 + m_imu_3dof_close(&t_ptr->fusion.imu_3dof); 2001 + 1993 2002 delete t_ptr; 1994 2003 } 1995 2004 ··· 2096 2105 t.sink.push_frame = t_psvr_sink_push_frame; 2097 2106 t.node.break_apart = t_psvr_node_break_apart; 2098 2107 t.node.destroy = t_psvr_node_destroy; 2099 - t.fusion.rot.w = 1.0f; 2100 2108 2101 2109 ret = os_thread_helper_init(&t.oth); 2102 2110 if (ret != 0) { ··· 2108 2116 t.fusion.pos.y = 0.0f; 2109 2117 t.fusion.pos.z = 0.0f; 2110 2118 2111 - t.fusion.rot.x = 0.0f; 2112 - t.fusion.rot.y = 0.0f; 2113 - t.fusion.rot.z = 0.0f; 2114 - t.fusion.rot.w = 1.0f; 2119 + m_imu_3dof_init(&t.fusion.imu_3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); 2120 + 2121 + t.fusion.imu_3dof.rot.x = 0.0f; 2122 + t.fusion.imu_3dof.rot.y = 0.0f; 2123 + t.fusion.imu_3dof.rot.z = 0.0f; 2124 + t.fusion.imu_3dof.rot.w = 1.0f; 2115 2125 2116 2126 xrt_frame_context_add(xfctx, &t.node); 2117 2127