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

d/rs: predict pose

Browse source 
d/rs: predict pose
This commit is contained in:
Moses Turner 2021-01-12 17:15:40 -06:00
parent baf0e1acfb
commit a1ed0b293c
1 changed files with 10 additions and 2 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

12
src/xrt/drivers/realsense/rs_6dof.c
View file

@ -13,6 +13,7 @@
#include "xrt/xrt_device.h" #include "xrt/xrt_device.h"
#include "math/m_api.h" #include "math/m_api.h"
#include "math/m_space.h" #include "math/m_space.h"
#include "math/m_predict.h"
#include "os/os_time.h" #include "os/os_time.h"
#include "os/os_threading.h" #include "os/os_threading.h"
@ -333,11 +334,18 @@ rs_6dof_get_tracked_pose(struct xrt_device *xdev,
} }
os_thread_helper_lock(&rs->oth); os_thread_helper_lock(&rs->oth);
struct xrt_space_relation relation = rs->relation; struct xrt_space_relation relation_not_predicted = rs->relation;
uint64_t relation_timestamp_ns = rs->relation_timestamp_ns;
os_thread_helper_unlock(&rs->oth); os_thread_helper_unlock(&rs->oth);
struct xrt_space_graph xsg = {0}; int64_t diff_prediction_ns = 0;
diff_prediction_ns = at_timestamp_ns - relation_timestamp_ns;
struct xrt_space_relation relation;
double delta_s = time_ns_to_s(diff_prediction_ns);
m_predict_relation(&relation_not_predicted, delta_s, &relation);
struct xrt_space_graph xsg = {0};
m_space_graph_add_pose(&xsg, &rs->offset); m_space_graph_add_pose(&xsg, &rs->offset);
m_space_graph_add_relation(&xsg, &relation); m_space_graph_add_relation(&xsg, &relation);
m_space_graph_resolve(&xsg, out_relation); m_space_graph_resolve(&xsg, out_relation);