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/slam: Enable some tracing

Browse source
This commit is contained in:
Jakob Bornecrantz 2023-04-30 00:32:58 +01:00
parent c0cc22e101
commit 2b921ab673
1 changed files with 14 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

14
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
View file

@ -16,6 +16,7 @@
#include "util/u_misc.h" #include "util/u_misc.h"
#include "util/u_sink.h" #include "util/u_sink.h"
#include "util/u_var.h" #include "util/u_var.h"
#include "util/u_trace_marker.h"
#include "os/os_threading.h" #include "os/os_threading.h"
#include "math/m_api.h" #include "math/m_api.h"
#include "math/m_filter_fifo.h" #include "math/m_filter_fifo.h"
@ -844,6 +845,8 @@ flush_poses(TrackerSlam &t)
static void static void
predict_pose(TrackerSlam &t, timepoint_ns when_ns, struct xrt_space_relation *out_relation) predict_pose(TrackerSlam &t, timepoint_ns when_ns, struct xrt_space_relation *out_relation)
{ {
XRT_TRACE_MARKER();
bool valid_pred_type = t.pred_type >= SLAM_PRED_NONE && t.pred_type <= SLAM_PRED_SP_SO_IA_IL; bool valid_pred_type = t.pred_type >= SLAM_PRED_NONE && t.pred_type <= SLAM_PRED_SP_SO_IA_IL;
SLAM_DASSERT(valid_pred_type, "Invalid prediction type (%d)", t.pred_type); SLAM_DASSERT(valid_pred_type, "Invalid prediction type (%d)", t.pred_type);
@ -901,6 +904,8 @@ predict_pose(TrackerSlam &t, timepoint_ns when_ns, struct xrt_space_relation *ou
static void static void
filter_pose(TrackerSlam &t, timepoint_ns when_ns, struct xrt_space_relation *out_relation) filter_pose(TrackerSlam &t, timepoint_ns when_ns, struct xrt_space_relation *out_relation)
{ {
XRT_TRACE_MARKER();
if (t.filter.use_moving_average_filter) { if (t.filter.use_moving_average_filter) {
if (out_relation->relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) { if (out_relation->relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
xrt_vec3 pos = out_relation->pose.position; xrt_vec3 pos = out_relation->pose.position;
@ -1131,6 +1136,8 @@ using namespace xrt::auxiliary::tracking::slam;
extern "C" void extern "C" void
t_slam_get_tracked_pose(struct xrt_tracked_slam *xts, timepoint_ns when_ns, struct xrt_space_relation *out_relation) t_slam_get_tracked_pose(struct xrt_tracked_slam *xts, timepoint_ns when_ns, struct xrt_space_relation *out_relation)
{ {
XRT_TRACE_MARKER();
auto &t = *container_of(xts, TrackerSlam, base); auto &t = *container_of(xts, TrackerSlam, base);
//! @todo This should not be cached, the same timestamp can be requested at a //! @todo This should not be cached, the same timestamp can be requested at a
@ -1160,6 +1167,8 @@ t_slam_get_tracked_pose(struct xrt_tracked_slam *xts, timepoint_ns when_ns, stru
extern "C" void extern "C" void
t_slam_gt_sink_push(struct xrt_pose_sink *sink, xrt_pose_sample *sample) t_slam_gt_sink_push(struct xrt_pose_sink *sink, xrt_pose_sample *sample)
{ {
XRT_TRACE_MARKER();
auto &t = *container_of(sink, TrackerSlam, gt_sink); auto &t = *container_of(sink, TrackerSlam, gt_sink);
if (t.gt.trajectory->empty()) { if (t.gt.trajectory->empty()) {
@ -1175,6 +1184,8 @@ t_slam_gt_sink_push(struct xrt_pose_sink *sink, xrt_pose_sample *sample)
extern "C" void extern "C" void
t_slam_receive_imu(struct xrt_imu_sink *sink, struct xrt_imu_sample *s) t_slam_receive_imu(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
{ {
XRT_TRACE_MARKER();
auto &t = *container_of(sink, TrackerSlam, imu_sink); auto &t = *container_of(sink, TrackerSlam, imu_sink);
timepoint_ns ts = s->timestamp_ns; timepoint_ns ts = s->timestamp_ns;
@ -1205,6 +1216,8 @@ t_slam_receive_imu(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
static void static void
receive_frame(TrackerSlam &t, struct xrt_frame *frame, int cam_index) receive_frame(TrackerSlam &t, struct xrt_frame *frame, int cam_index)
{ {
XRT_TRACE_MARKER();
if (cam_index == 1) { if (cam_index == 1) {
flush_poses(t); // Useful to flush SLAM poses when no openxr app is open flush_poses(t); // Useful to flush SLAM poses when no openxr app is open
} }
@ -1215,6 +1228,7 @@ receive_frame(TrackerSlam &t, struct xrt_frame *frame, int cam_index)
SLAM_DASSERT_(frame->timestamp < INT64_MAX); SLAM_DASSERT_(frame->timestamp < INT64_MAX);
img_sample sample{(int64_t)frame->timestamp, img, cam_index}; img_sample sample{(int64_t)frame->timestamp, img, cam_index};
if (t.submit) { if (t.submit) {
XRT_TRACE_IDENT(slam_push);
t.slam->push_frame(sample); t.slam->push_frame(sample);
} }
SLAM_TRACE("cam%d frame t=%lu", cam_index, frame->timestamp); SLAM_TRACE("cam%d frame t=%lu", cam_index, frame->timestamp);