mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-01-04 06:06:17 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

t/psvr: Various fixes and changes

Browse source 
- Fix out of array bounds crash, curr_y can be out of bounds.
- Set tracked bits appropriately.
- Use m_imu_3dof for orientation.
This commit is contained in:
iVRy VR 2021-04-12 15:02:16 +00:00 committed by Jakob Bornecrantz
parent c54a6bef0a
commit 0064989e8b
1 changed files with 25 additions and 15 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

40
src/xrt/auxiliary/tracking/t_tracker_psvr.cpp
View file

@ -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);