The open source OpenXR runtime
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
+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