The open source OpenXR runtime
d/rs: predict pose
d/rs: predict pose
+10
-2
+10
-2
src/xrt/drivers/realsense/rs_6dof.c
+10
-2
src/xrt/drivers/realsense/rs_6dof.c
···
13
13
#include "xrt/xrt_device.h"
14
14
#include "math/m_api.h"
15
15
#include "math/m_space.h"
16
+
#include "math/m_predict.h"
16
17
17
18
#include "os/os_time.h"
18
19
#include "os/os_threading.h"
···
333
334
}
334
335
335
336
os_thread_helper_lock(&rs->oth);
336
-
struct xrt_space_relation relation = rs->relation;
337
+
struct xrt_space_relation relation_not_predicted = rs->relation;
338
+
uint64_t relation_timestamp_ns = rs->relation_timestamp_ns;
337
339
os_thread_helper_unlock(&rs->oth);
338
340
339
-
struct xrt_space_graph xsg = {0};
341
+
int64_t diff_prediction_ns = 0;
342
+
diff_prediction_ns = at_timestamp_ns - relation_timestamp_ns;
343
+
struct xrt_space_relation relation;
344
+
345
+
double delta_s = time_ns_to_s(diff_prediction_ns);
346
+
m_predict_relation(&relation_not_predicted, delta_s, &relation);
340
347
348
+
struct xrt_space_graph xsg = {0};
341
349
m_space_graph_add_pose(&xsg, &rs->offset);
342
350
m_space_graph_add_relation(&xsg, &relation);
343
351
m_space_graph_resolve(&xsg, out_relation);