matrixfurry.com
The open source OpenXR runtime
1// Copyright 2021-2024, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief SLAM tracking code.
6 * @author Mateo de Mayo <mateo.demayo@collabora.com>
7 * @ingroup aux_tracking
8 */
9
10#include "xrt/xrt_config_have.h"
11#include "xrt/xrt_defines.h"
12#include "xrt/xrt_tracking.h"
13#include "xrt/xrt_frameserver.h"
14#include "util/u_debug.h"
15#include "util/u_logging.h"
16#include "util/u_misc.h"
17#include "util/u_sink.h"
18#include "util/u_var.h"
19#include "util/u_trace_marker.h"
20#include "os/os_threading.h"
21#include "math/m_api.h"
22#include "math/m_filter_fifo.h"
23#include "math/m_filter_one_euro.h"
24#include "math/m_predict.h"
25#include "math/m_relation_history.h"
26#include "math/m_space.h"
27#include "math/m_vec3.h"
28#include "tracking/t_euroc_recorder.h"
29#include "tracking/t_openvr_tracker.h"
30#include "tracking/t_tracking.h"
31#include "tracking/t_vit_loader.h"
32
33#include "vit/vit_interface.h"
34
35#include <opencv2/core/mat.hpp>
36#include <opencv2/core/version.hpp>
37
38#include <deque>
39#include <filesystem>
40#include <fstream>
41#include <iomanip>
42#include <map>
43#include <mutex>
44#include <string>
45#include <vector>
46
47//! @todo Get preferred system from systems found at build time
48#define PREFERRED_VIT_SYSTEM_LIBRARY "libbasalt.so"
49
50#define SLAM_TRACE(...) U_LOG_IFL_T(t.log_level, __VA_ARGS__)
51#define SLAM_DEBUG(...) U_LOG_IFL_D(t.log_level, __VA_ARGS__)
52#define SLAM_INFO(...) U_LOG_IFL_I(t.log_level, __VA_ARGS__)
53#define SLAM_WARN(...) U_LOG_IFL_W(t.log_level, __VA_ARGS__)
54#define SLAM_ERROR(...) U_LOG_IFL_E(t.log_level, __VA_ARGS__)
55#define SLAM_ASSERT(predicate, ...) \
56 do { \
57 bool p = predicate; \
58 if (!p) { \
59 U_LOG(U_LOGGING_ERROR, __VA_ARGS__); \
60 assert(false && "SLAM_ASSERT failed: " #predicate); \
61 exit(EXIT_FAILURE); \
62 } \
63 } while (false);
64#define SLAM_ASSERT_(predicate) SLAM_ASSERT(predicate, "Assertion failed " #predicate)
65
66// Debug assertions, not vital but useful for finding errors
67#ifdef NDEBUG
68#define SLAM_DASSERT(predicate, ...) (void)(predicate)
69#define SLAM_DASSERT_(predicate) (void)(predicate)
70#else
71#define SLAM_DASSERT(predicate, ...) SLAM_ASSERT(predicate, __VA_ARGS__)
72#define SLAM_DASSERT_(predicate) SLAM_ASSERT_(predicate)
73#endif
74
75//! @see t_slam_tracker_config
76DEBUG_GET_ONCE_LOG_OPTION(slam_log, "SLAM_LOG", U_LOGGING_INFO)
77DEBUG_GET_ONCE_OPTION(vit_system_library_path, "VIT_SYSTEM_LIBRARY_PATH", PREFERRED_VIT_SYSTEM_LIBRARY)
78DEBUG_GET_ONCE_OPTION(slam_config, "SLAM_CONFIG", nullptr)
79DEBUG_GET_ONCE_BOOL_OPTION(slam_ui, "SLAM_UI", false)
80DEBUG_GET_ONCE_BOOL_OPTION(slam_submit_from_start, "SLAM_SUBMIT_FROM_START", false)
81DEBUG_GET_ONCE_NUM_OPTION(slam_openvr_groundtruth_device, "SLAM_OPENVR_GROUNDTRUTH_DEVICE", 0)
82DEBUG_GET_ONCE_NUM_OPTION(slam_prediction_type, "SLAM_PREDICTION_TYPE", long(SLAM_PRED_IP_IO_IA_IL))
83DEBUG_GET_ONCE_BOOL_OPTION(slam_write_csvs, "SLAM_WRITE_CSVS", false)
84DEBUG_GET_ONCE_OPTION(slam_csv_path, "SLAM_CSV_PATH", "evaluation/")
85DEBUG_GET_ONCE_BOOL_OPTION(slam_timing_stat, "SLAM_TIMING_STAT", true)
86DEBUG_GET_ONCE_BOOL_OPTION(slam_features_stat, "SLAM_FEATURES_STAT", true)
87DEBUG_GET_ONCE_NUM_OPTION(slam_cam_count, "SLAM_CAM_COUNT", 2)
88
89//! Namespace for the interface to the external SLAM tracking system
90namespace xrt::auxiliary::tracking::slam {
91constexpr int UI_TIMING_POSE_COUNT = 192;
92constexpr int UI_FEATURES_POSE_COUNT = 192;
93constexpr int UI_GTDIFF_POSE_COUNT = 192;
94
95using os::Mutex;
96using std::deque;
97using std::ifstream;
98using std::make_shared;
99using std::map;
100using std::ofstream;
101using std::ostream;
102using std::pair;
103using std::shared_ptr;
104using std::string;
105using std::to_string;
106using std::unique_lock;
107using std::vector;
108using std::filesystem::create_directories;
109using Trajectory = map<timepoint_ns, xrt_pose>;
110using timing_sample = vector<timepoint_ns>;
111
112using xrt::auxiliary::math::RelationHistory;
113
114
115/*
116 *
117 * CSV Writers
118 *
119 */
120
121ostream &
122operator<<(ostream &os, const xrt_pose_sample &s)
123{
124 timepoint_ns ts = s.timestamp_ns;
125 xrt_vec3 p = s.pose.position;
126 xrt_quat r = s.pose.orientation;
127 os << ts << ",";
128 os << p.x << "," << p.y << "," << p.z << ",";
129 os << r.w << "," << r.x << "," << r.y << "," << r.z << CSV_EOL;
130 return os;
131}
132
133ostream &
134operator<<(ostream &os, const timing_sample ×tamps)
135{
136 for (const timepoint_ns &ts : timestamps) {
137 string delimiter = &ts != ×tamps.back() ? "," : CSV_EOL;
138 os << ts << delimiter;
139 }
140 return os;
141}
142
143struct feature_count_sample
144{
145 timepoint_ns ts;
146 vector<int> counts;
147};
148
149ostream &
150operator<<(ostream &os, const feature_count_sample &s)
151{
152 os << s.ts;
153 for (int count : s.counts) {
154 os << "," << count;
155 }
156 os << CSV_EOL;
157 return os;
158}
159
160//! Writes a CSV file for a particular row type
161template <typename RowType> class CSVWriter
162{
163public:
164 bool enabled; // Modified through UI
165
166protected:
167 vector<string> column_names;
168
169private:
170 string directory;
171 string filename;
172 ofstream file;
173 bool created = false;
174 Mutex mutex;
175
176 void
177 create()
178 {
179 create_directories(directory);
180 file = ofstream{directory + "/" + filename};
181 file << "#";
182 for (const string &col : column_names) {
183 string delimiter = &col != &column_names.back() ? "," : CSV_EOL;
184 file << col << delimiter;
185 }
186 file << std::fixed << std::setprecision(CSV_PRECISION);
187 }
188
189public:
190 CSVWriter(const string &dir, const string &fn, bool e, const vector<string> &cn = {})
191 : enabled(e), column_names(cn), directory(dir), filename(fn)
192 {}
193
194 virtual ~CSVWriter() {}
195
196 void
197 push(RowType row)
198 {
199 unique_lock lock(mutex);
200
201 if (!enabled) {
202 return;
203 }
204
205 if (!created) {
206 created = true;
207 create();
208 }
209
210 file << row;
211 }
212};
213
214//! Writes poses and their timestamps to a CSV file
215struct TrajectoryWriter : public CSVWriter<xrt_pose_sample>
216{
217 TrajectoryWriter(const string &dir, const string &fn, bool e) : CSVWriter<xrt_pose_sample>(dir, fn, e)
218 {
219 column_names = {"timestamp [ns]", "p_RS_R_x [m]", "p_RS_R_y [m]", "p_RS_R_z [m]",
220 "q_RS_w []", "q_RS_x []", "q_RS_y []", "q_RS_z []"};
221 }
222};
223
224//! Writes timestamps measured when estimating a new pose by the SLAM system
225struct TimingWriter : public CSVWriter<timing_sample>
226{
227 TimingWriter(const string &dir, const string &fn, bool e, const vector<string> &cn)
228 : CSVWriter<timing_sample>(dir, fn, e, cn)
229 {}
230};
231
232//! Writes feature information specific to a particular estimated pose
233struct FeaturesWriter : public CSVWriter<feature_count_sample>
234{
235 FeaturesWriter(const string &dir, const string &fn, bool e, size_t cam_count)
236 : CSVWriter<feature_count_sample>(dir, fn, e)
237 {
238 column_names.push_back("timestamp");
239 for (size_t i = 0; i < cam_count; i++) {
240 column_names.push_back("cam" + to_string(i) + " feature count");
241 }
242 }
243};
244
245/*!
246 * Main implementation of @ref xrt_tracked_slam. This is an adapter class for
247 * SLAM tracking that wraps an external SLAM implementation.
248 *
249 * @implements xrt_tracked_slam
250 * @implements xrt_frame_node
251 * @implements xrt_frame_sink
252 * @implements xrt_imu_sink
253 * @implements xrt_pose_sink
254 */
255struct TrackerSlam
256{
257 struct xrt_tracked_slam base = {};
258 struct xrt_frame_node node = {}; //!< Will be called on destruction
259 struct t_vit_bundle vit; //!< VIT system function pointers
260 struct vit_tracker_extension_set exts; //!< VIT tracker supported extensions
261 struct vit_tracker *tracker; //!< Pointer to the tracker created by the loaded VIT system;
262
263 struct xrt_slam_sinks sinks = {}; //!< Pointers to the sinks below
264 struct xrt_frame_sink cam_sinks[XRT_TRACKING_MAX_SLAM_CAMS]; //!< Sends camera frames to the SLAM system
265 struct xrt_imu_sink imu_sink = {}; //!< Sends imu samples to the SLAM system
266 struct xrt_pose_sink gt_sink = {}; //!< Register groundtruth trajectory for stats
267 struct xrt_hand_masks_sink hand_masks_sink = {}; //!< Register latest masks to ignore
268
269 bool submit; //!< Whether to submit data pushed to sinks to the SLAM tracker
270 uint32_t cam_count; //!< Number of cameras used for tracking
271
272 struct u_var_button reset_state_btn; //!< Reset tracker state button
273
274 enum u_logging_level log_level; //!< Logging level for the SLAM tracker, set by SLAM_LOG var
275
276 struct xrt_slam_sinks *euroc_recorder; //!< EuRoC dataset recording sinks
277 struct openvr_tracker *ovr_tracker; //!< OpenVR lighthouse tracker
278
279 // Used mainly for checking that the timestamps come in order
280 timepoint_ns last_imu_ts; //!< Last received IMU sample timestamp
281 vector<timepoint_ns> last_cam_ts; //!< Last received image timestamp per cam
282 struct xrt_hand_masks_sample last_hand_masks; //!< Last received hand masks info
283 Mutex last_hand_masks_mutex; //!< Mutex for @ref last_hand_masks
284
285 // Prediction
286
287 //! Type of prediction to use
288 t_slam_prediction_type pred_type;
289 u_var_combo pred_combo; //!< UI combo box to select @ref pred_type
290 RelationHistory slam_rels{}; //!< A history of relations produced purely from external SLAM tracker data
291 int dbg_pred_every = 1; //!< Skip X SLAM poses so that you get tracked mostly by the prediction algo
292 int dbg_pred_counter = 0; //!< SLAM pose counter for prediction debugging
293 struct os_mutex lock_ff; //!< Lock for gyro_ff and accel_ff.
294 struct m_ff_vec3_f32 *gyro_ff; //!< Last gyroscope samples
295 struct m_ff_vec3_f32 *accel_ff; //!< Last accelerometer samples
296 vector<u_sink_debug> ui_sink; //!< Sink to display frames in UI of each camera
297
298 //! Used to correct accelerometer measurements when integrating into the prediction.
299 //! @todo Should be automatically computed instead of required to be filled manually through the UI.
300 xrt_vec3 gravity_correction{0, 0, -MATH_GRAVITY_M_S2};
301
302 struct xrt_space_relation last_rel = XRT_SPACE_RELATION_ZERO; //!< Last reported/tracked pose
303 timepoint_ns last_ts; //!< Last reported/tracked pose timestamp
304
305 //! Filters are used to smooth out the resulting trajectory
306 struct
307 {
308 // Moving average filter
309 bool use_moving_average_filter = false;
310 //! Time window in ms take the average on.
311 //! Increasing it smooths out the tracking at the cost of adding delay.
312 double window = 66;
313 struct m_ff_vec3_f32 *pos_ff; //! Predicted positions fifo
314 struct m_ff_vec3_f32 *rot_ff; //! Predicted rotations fifo (only xyz components, w is inferred)
315
316 // Exponential smoothing filter
317 bool use_exponential_smoothing_filter = false;
318 float alpha = 0.1; //!< How much should we lerp towards the @ref target value on each update
319 struct xrt_space_relation last = XRT_SPACE_RELATION_ZERO; //!< Last filtered relation
320 struct xrt_space_relation target = XRT_SPACE_RELATION_ZERO; //!< Target relation
321
322 // One euro filter
323 bool use_one_euro_filter = false;
324 m_filter_euro_vec3 pos_oe; //!< One euro position filter
325 m_filter_euro_quat rot_oe; //!< One euro rotation filter
326 const float min_cutoff = M_PI; //!< Default minimum cutoff frequency
327 const float min_dcutoff = 1; //!< Default minimum cutoff frequency for the derivative
328 const float beta = 0.16; //!< Default speed coefficient
329
330 } filter;
331
332 // Stats and metrics
333
334 // CSV writers for offline analysis (using pointers because of container_of)
335 TimingWriter *slam_times_writer; //!< Timestamps of the pipeline for performance analysis
336 FeaturesWriter *slam_features_writer; //!< Feature tracking information for analysis
337 TrajectoryWriter *slam_traj_writer; //!< Estimated poses from the SLAM system
338 TrajectoryWriter *pred_traj_writer; //!< Predicted poses
339 TrajectoryWriter *filt_traj_writer; //!< Predicted and filtered poses
340
341 //! Tracker timing info for performance evaluation
342 struct
343 {
344 bool enabled = false; //!< Whether the timing extension is enabled
345 float dur_ms[UI_TIMING_POSE_COUNT]; //!< Timing durations in ms
346 int idx = 0; //!< Index of latest entry in @ref dur_ms
347 u_var_combo start_ts; //!< UI combo box to select initial timing measurement
348 u_var_combo end_ts; //!< UI combo box to select final timing measurement
349 int start_ts_idx; //!< Selected initial timing measurement in @ref start_ts
350 int end_ts_idx; //!< Selected final timing measurement in @ref end_ts
351 struct u_var_timing ui; //!< Realtime UI for tracker durations
352 vector<string> columns; //!< Column names of the measured timestamps
353 string joined_columns; //!< Column names as a null separated string
354 struct u_var_button enable_btn; //!< Toggle tracker timing reports
355 } timing;
356
357 //! Tracker feature tracking info
358 struct Features
359 {
360 struct FeatureCounter
361 {
362 //! Feature count for each frame timestamp for this camera.
363 //! @note Harmless race condition over this as the UI might read this while it's being written
364 deque<pair<timepoint_ns, int>> entries{};
365
366 //! Persistently stored camera name for display in the UI
367 string cam_name;
368
369 void
370 addFeatureCount(timepoint_ns ts, int count)
371 {
372 entries.emplace_back(ts, count);
373 if (entries.size() > UI_FEATURES_POSE_COUNT) {
374 entries.pop_front();
375 }
376 }
377 };
378
379 vector<FeatureCounter> fcs; //!< Store feature count info for each camera
380 u_var_curves fcs_ui; //!< Display of `fcs` in UI
381
382 bool enabled = false; //!< Whether the features extension is enabled
383 struct u_var_button enable_btn; //!< Toggle extension
384 } features;
385
386 //! Ground truth related fields
387 struct
388 {
389 Trajectory *trajectory; //!< Empty if we've not received groundtruth
390 xrt_pose origin; //!< First ground truth pose
391 float diffs_mm[UI_GTDIFF_POSE_COUNT]; //!< Positional error wrt ground truth
392 int diff_idx = 0; //!< Index of last error in @ref diffs_mm
393 struct u_var_timing diff_ui; //!< Realtime UI for positional error
394 bool override_tracking = false; //!< Force the tracker to report gt poses instead
395 } gt;
396};
397
398
399/*
400 *
401 * Timing functionality
402 *
403 */
404
405static void
406timing_ui_setup(TrackerSlam &t)
407{
408 t.timing.enabled = false;
409
410 u_var_add_ro_raw_text(&t, "\nTracker timing", "Tracker timing");
411
412 // Setup toggle button
413 static const char *msg[2] = {"[OFF] Enable timing", "[ON] Disable timing"};
414 u_var_button_cb cb = [](void *t_ptr) {
415 TrackerSlam *t = (TrackerSlam *)t_ptr;
416 u_var_button &btn = t->timing.enable_btn;
417 bool e = !t->timing.enabled;
418 snprintf(btn.label, sizeof(btn.label), "%s", msg[e]);
419 vit_result_t vres = t->vit.tracker_enable_extension(t->tracker, VIT_TRACKER_EXTENSION_POSE_TIMING, e);
420 if (vres != VIT_SUCCESS) {
421 U_LOG_IFL_E(t->log_level, "Failed to set tracker timing extension");
422 return;
423 }
424 t->timing.enabled = e;
425 };
426 t.timing.enable_btn.cb = cb;
427 t.timing.enable_btn.disabled = !t.exts.has_pose_timing;
428 t.timing.enable_btn.ptr = &t;
429 u_var_add_button(&t, &t.timing.enable_btn, msg[t.timing.enabled]);
430
431 // We provide two timing columns by default, even if there is no extension support
432 t.timing.columns = {"sampled", "received_by_monado"};
433
434 // Only fill the timing columns if the tracker supports pose timing
435 if (t.exts.has_pose_timing) {
436 vit_tracker_timing_titles titles = {};
437 vit_result_t vres = t.vit.tracker_get_timing_titles(t.tracker, &titles);
438 if (vres != VIT_SUCCESS) {
439 SLAM_ERROR("Failed to get timing titles from tracker");
440 return;
441 }
442
443 // Copies the titles locally.
444 std::vector<std::string> cols(titles.titles, titles.titles + titles.count);
445
446 t.timing.columns.insert(t.timing.columns.begin() + 1, cols.begin(), cols.end());
447 }
448
449 // Construct null-separated array of options for the combo box
450 using namespace std::string_literals;
451 t.timing.joined_columns = "";
452 for (const string &name : t.timing.columns) {
453 t.timing.joined_columns += name + "\0"s;
454 }
455 t.timing.joined_columns += "\0"s;
456
457 t.timing.start_ts.count = t.timing.columns.size();
458 t.timing.start_ts.options = t.timing.joined_columns.c_str();
459 t.timing.start_ts.value = &t.timing.start_ts_idx;
460 t.timing.start_ts_idx = 0;
461 u_var_add_combo(&t, &t.timing.start_ts, "Start timestamp");
462
463 t.timing.end_ts.count = t.timing.columns.size();
464 t.timing.end_ts.options = t.timing.joined_columns.c_str();
465 t.timing.end_ts.value = &t.timing.end_ts_idx;
466 t.timing.end_ts_idx = t.timing.columns.size() - 1;
467 u_var_add_combo(&t, &t.timing.end_ts, "End timestamp");
468
469 t.timing.ui.values.data = t.timing.dur_ms;
470 t.timing.ui.values.length = UI_TIMING_POSE_COUNT;
471 t.timing.ui.values.index_ptr = &t.timing.idx;
472 t.timing.ui.reference_timing = 16.6;
473 t.timing.ui.center_reference_timing = true;
474 t.timing.ui.range = t.timing.ui.reference_timing;
475 t.timing.ui.dynamic_rescale = true;
476 t.timing.ui.unit = "ms";
477 u_var_add_f32_timing(&t, &t.timing.ui, "External tracker times");
478}
479
480//! Updates timing UI with info from a computed pose and returns that info
481static vector<timepoint_ns>
482timing_ui_push(TrackerSlam &t, const vit_pose_t *pose, int64_t ts)
483{
484 timepoint_ns now = os_monotonic_get_ns();
485 vector<timepoint_ns> tss = {ts, now};
486
487 // Add extra timestamps if the SLAM tracker provides them
488 if (t.timing.enabled) {
489 vit_pose_timing timing;
490 vit_result_t vres = t.vit.pose_get_timing(pose, &timing);
491 if (vres != VIT_SUCCESS) {
492 // Even if the timing is enabled, some of the poses already in the queue won't have it enabled.
493 if (vres != VIT_ERROR_NOT_ENABLED) {
494 SLAM_ERROR("Failed to get pose timing");
495 }
496
497 return {};
498 }
499
500 std::vector<int64_t> data(timing.timestamps, timing.timestamps + timing.count);
501 tss.insert(tss.begin() + 1, data.begin(), data.end());
502
503 // The two timestamps to compare in the graph
504 timepoint_ns start = tss.at(t.timing.start_ts_idx);
505 timepoint_ns end = tss.at(t.timing.end_ts_idx);
506
507 // Push to the UI graph
508 float tss_ms = (end - start) / U_TIME_1MS_IN_NS;
509 t.timing.idx = (t.timing.idx + 1) % UI_TIMING_POSE_COUNT;
510 t.timing.dur_ms[t.timing.idx] = tss_ms;
511 constexpr float a = 1.0f / UI_TIMING_POSE_COUNT; // Exponential moving average
512 t.timing.ui.reference_timing = (1 - a) * t.timing.ui.reference_timing + a * tss_ms;
513 }
514
515 return tss;
516}
517
518
519/*
520 *
521 * Feature information functionality
522 *
523 */
524
525static void
526features_ui_setup(TrackerSlam &t)
527{
528 t.features.enabled = false;
529
530 u_var_add_ro_raw_text(&t, "\nTracker features", "Tracker features");
531
532 // Setup toggle button
533 static const char *msg[2] = {"[OFF] Enable features info", "[ON] Disable features info"};
534 u_var_button_cb cb = [](void *t_ptr) {
535 TrackerSlam *t = (TrackerSlam *)t_ptr;
536 u_var_button &btn = t->features.enable_btn;
537 bool e = !t->features.enabled;
538 snprintf(btn.label, sizeof(btn.label), "%s", msg[e]);
539 vit_result_t vres = t->vit.tracker_enable_extension(t->tracker, VIT_TRACKER_EXTENSION_POSE_FEATURES, e);
540 if (vres != VIT_SUCCESS) {
541 U_LOG_IFL_E(t->log_level, "Failed to set tracker features extension");
542 return;
543 }
544 t->features.enabled = e;
545 };
546 t.features.enable_btn.cb = cb;
547 t.features.enable_btn.disabled = !t.exts.has_pose_features;
548 t.features.enable_btn.ptr = &t;
549 u_var_add_button(&t, &t.features.enable_btn, msg[t.features.enabled]);
550
551 // Setup graph
552 u_var_curve_getter getter = [](void *fs_ptr, int i) -> u_var_curve_point {
553 auto *fs = (TrackerSlam::Features::FeatureCounter *)fs_ptr;
554 timepoint_ns now = os_monotonic_get_ns();
555
556 size_t size = fs->entries.size();
557 if (size == 0) {
558 return {0, 0};
559 }
560
561 int last_idx = size - 1;
562 if (i > last_idx) {
563 i = last_idx;
564 }
565
566 auto [ts, count] = fs->entries.at(last_idx - i);
567 return {time_ns_to_s(now - ts), double(count)};
568 };
569
570 t.features.fcs_ui.curve_count = t.cam_count;
571 t.features.fcs_ui.xlabel = "Last seconds";
572 t.features.fcs_ui.ylabel = "Number of features";
573
574 t.features.fcs.resize(t.cam_count);
575 for (uint32_t i = 0; i < t.cam_count; ++i) {
576 auto &fc = t.features.fcs[i];
577 fc.cam_name = "Cam" + to_string(i);
578
579 auto &fc_ui = t.features.fcs_ui.curves[i];
580 fc_ui.count = UI_FEATURES_POSE_COUNT;
581 fc_ui.data = &fc;
582 fc_ui.getter = getter;
583 fc_ui.label = fc.cam_name.c_str();
584 }
585
586 u_var_add_curves(&t, &t.features.fcs_ui, "Feature count");
587}
588
589static vector<int>
590features_ui_push(TrackerSlam &t, const vit_pose_t *pose, int64_t ts)
591{
592 if (!t.features.enabled) {
593 return {};
594 }
595
596 // Push to the UI graph
597 vector<int> fcs{};
598 for (uint32_t i = 0; i < t.cam_count; ++i) {
599 vit_pose_features features = {};
600 vit_result_t vres = t.vit.pose_get_features(pose, i, &features);
601 if (vres != VIT_SUCCESS) {
602 // Even if the features are enabled, some of the poses already in the queue won't have it
603 // enabled.
604 if (vres != VIT_ERROR_NOT_ENABLED) {
605 SLAM_ERROR("Failed to get pose features for camera %u", i);
606 }
607
608 return {};
609 }
610
611 t.features.fcs.at(i).addFeatureCount(ts, features.count);
612 fcs.push_back(features.count);
613 }
614
615 return fcs;
616}
617
618/*
619 *
620 * Ground truth functionality
621 *
622 */
623
624//! Gets an interpolated groundtruth pose (if available) at a specified timestamp
625static xrt_pose
626get_gt_pose_at(const Trajectory >, timepoint_ns ts)
627{
628 if (gt.empty()) {
629 return XRT_POSE_IDENTITY;
630 }
631
632 Trajectory::const_iterator rit = gt.upper_bound(ts);
633
634 if (rit == gt.begin()) { // Too far in the past, return first gt pose
635 return gt.begin()->second;
636 }
637
638 if (rit == gt.end()) { // Too far in the future, return last gt pose
639 return std::prev(gt.end())->second;
640 }
641
642 Trajectory::const_iterator lit = std::prev(rit);
643
644 const auto &[lts, lpose] = *lit;
645 const auto &[rts, rpose] = *rit;
646
647 float t = double(ts - lts) / double(rts - lts);
648 SLAM_DASSERT_(0 <= t && t <= 1);
649
650 xrt_pose res{};
651 math_quat_slerp(&lpose.orientation, &rpose.orientation, t, &res.orientation);
652 res.position = m_vec3_lerp(lpose.position, rpose.position, t);
653 return res;
654}
655
656//! Converts a pose from the tracker to ground truth
657static struct xrt_pose
658xr2gt_pose(const xrt_pose >_origin, const xrt_pose &xr_pose)
659{
660 //! @todo Right now this is hardcoded for Basalt and the EuRoC vicon datasets
661 //! groundtruth and ignores orientation. Applies a fixed transformation so
662 //! that the tracked and groundtruth trajectories origins and general motion
663 //! match. The usual way of evaluating trajectory errors in SLAM requires to
664 //! first align the trajectories through a non-linear optimization (e.g. gauss
665 //! newton) so that they are as similar as possible. For this you need the
666 //! entire tracked trajectory to be known beforehand, which makes it not
667 //! suitable for reporting an error metric in realtime. See this 2-page paper
668 //! for more info on trajectory alignment:
669 //! https://ylatif.github.io/movingsensors/cameraReady/paper07.pdf
670
671 xrt_vec3 pos = xr_pose.position;
672 xrt_quat z180{0, 0, 1, 0};
673 math_quat_rotate_vec3(&z180, &pos, &pos);
674 math_quat_rotate_vec3(>_origin.orientation, &pos, &pos);
675 pos += gt_origin.position;
676
677 return {XRT_QUAT_IDENTITY, pos};
678}
679
680//! The inverse of @ref xr2gt_pose.
681static struct xrt_pose
682gt2xr_pose(const xrt_pose >_origin, const xrt_pose >_pose)
683{
684 xrt_vec3 pos = gt_pose.position;
685 pos -= gt_origin.position;
686 xrt_quat gt_origin_orientation_inv = gt_origin.orientation;
687 math_quat_invert(>_origin_orientation_inv, >_origin_orientation_inv);
688 math_quat_rotate_vec3(>_origin_orientation_inv, &pos, &pos);
689 xrt_quat zn180{0, 0, -1, 0};
690 math_quat_rotate_vec3(&zn180, &pos, &pos);
691
692 return {XRT_QUAT_IDENTITY, pos};
693}
694
695static void
696gt_ui_setup(TrackerSlam &t)
697{
698 u_var_add_ro_raw_text(&t, "\nTracker groundtruth", "Tracker groundtruth");
699 t.gt.diff_ui.values.data = t.gt.diffs_mm;
700 t.gt.diff_ui.values.length = UI_GTDIFF_POSE_COUNT;
701 t.gt.diff_ui.values.index_ptr = &t.gt.diff_idx;
702 t.gt.diff_ui.reference_timing = 0;
703 t.gt.diff_ui.center_reference_timing = true;
704 t.gt.diff_ui.range = 100; // 10cm
705 t.gt.diff_ui.dynamic_rescale = true;
706 t.gt.diff_ui.unit = "mm";
707 u_var_add_f32_timing(&t, &t.gt.diff_ui, "Tracking absolute error");
708}
709
710static void
711gt_ui_push(TrackerSlam &t, timepoint_ns ts, xrt_pose tracked_pose)
712{
713 if (t.gt.trajectory->empty()) {
714 return;
715 }
716
717 xrt_pose gt_pose = get_gt_pose_at(*t.gt.trajectory, ts);
718 xrt_pose xr_pose = xr2gt_pose(t.gt.origin, tracked_pose);
719
720 float len_mm = m_vec3_len(xr_pose.position - gt_pose.position) * 1000;
721 t.gt.diff_idx = (t.gt.diff_idx + 1) % UI_GTDIFF_POSE_COUNT;
722 t.gt.diffs_mm[t.gt.diff_idx] = len_mm;
723 constexpr float a = 1.0f / UI_GTDIFF_POSE_COUNT; // Exponential moving average
724 t.gt.diff_ui.reference_timing = (1 - a) * t.gt.diff_ui.reference_timing + a * len_mm;
725}
726
727/*
728 *
729 * Tracker functionality
730 *
731 */
732
733//! Dequeue all tracked poses from the SLAM system and update prediction data with them.
734static bool
735flush_poses(TrackerSlam &t)
736{
737
738 vit_pose_t *pose = NULL;
739 vit_result_t vres = t.vit.tracker_pop_pose(t.tracker, &pose);
740 if (vres != VIT_SUCCESS) {
741 SLAM_ERROR("Failed to get pose from VIT tracker");
742 }
743
744 if (pose == NULL) {
745 SLAM_TRACE("No poses to flush");
746 return false;
747 }
748
749 do {
750 // New pose
751 vit_pose_data_t data;
752 vres = t.vit.pose_get_data(pose, &data);
753 if (vres != VIT_SUCCESS) {
754 SLAM_ERROR("Failed to get pose data from VIT tracker");
755 return false;
756 }
757
758 int64_t nts = data.timestamp;
759
760 xrt_vec3 npos{data.px, data.py, data.pz};
761 xrt_quat nrot{data.ox, data.oy, data.oz, data.ow};
762
763 // Last relation
764 xrt_space_relation lr = XRT_SPACE_RELATION_ZERO;
765 int64_t lts;
766 t.slam_rels.get_latest(<s, &lr);
767 xrt_vec3 lpos = lr.pose.position;
768 xrt_quat lrot = lr.pose.orientation;
769
770 double dt = time_ns_to_s(nts - lts);
771
772 SLAM_TRACE("Dequeued SLAM pose ts=%ld p=[%f,%f,%f] r=[%f,%f,%f,%f]", //
773 nts, data.px, data.py, data.pz, data.ox, data.oy, data.oz, data.ow);
774
775 // TODO linear velocity from the VIT system
776 // Compute new relation based on new pose and velocities since last pose
777 xrt_space_relation rel{};
778 rel.relation_flags = XRT_SPACE_RELATION_BITMASK_ALL;
779 rel.pose = {nrot, npos};
780 rel.linear_velocity = (npos - lpos) / dt;
781 math_quat_finite_difference(&lrot, &nrot, dt, &rel.angular_velocity);
782
783 // Push to relationship history unless we are debugging prediction
784 if (t.dbg_pred_counter % t.dbg_pred_every == 0) {
785 t.slam_rels.push(rel, nts);
786 }
787 t.dbg_pred_counter = (t.dbg_pred_counter + 1) % t.dbg_pred_every;
788
789 gt_ui_push(t, nts, rel.pose);
790 t.slam_traj_writer->push({nts, rel.pose});
791 xrt_pose_sample pose_sample = {nts, rel.pose};
792 xrt_sink_push_pose(t.euroc_recorder->gt, &pose_sample);
793
794 auto tss = timing_ui_push(t, pose, nts);
795 t.slam_times_writer->push(tss);
796
797 if (t.features.enabled) {
798 vector feat_count = features_ui_push(t, pose, nts);
799 t.slam_features_writer->push({nts, feat_count});
800 }
801
802 t.vit.pose_destroy(pose);
803 } while (t.vit.tracker_pop_pose(t.tracker, &pose) == VIT_SUCCESS && pose);
804
805 return true;
806}
807
808//! Integrates IMU samples on top of a base pose and predicts from that
809static void
810predict_pose_from_imu(TrackerSlam &t,
811 timepoint_ns when_ns,
812 xrt_space_relation base_rel, // Pose to integrate IMUs on top of
813 timepoint_ns base_rel_ts,
814 struct xrt_space_relation *out_relation)
815{
816 os_mutex_lock(&t.lock_ff);
817
818 // Find oldest imu index i that is newer than latest SLAM pose (or -1)
819 int i = 0;
820 uint64_t imu_ts = UINT64_MAX;
821 xrt_vec3 _;
822 while (m_ff_vec3_f32_get(t.gyro_ff, i, &_, &imu_ts)) {
823 if ((int64_t)imu_ts < base_rel_ts) {
824 i--; // Back to the oldest newer-than-SLAM IMU index (or -1)
825 break;
826 }
827 i++;
828 }
829
830 if (i == -1) {
831 SLAM_WARN("No IMU samples received after latest SLAM pose (and frame)");
832 }
833
834 xrt_space_relation integ_rel = base_rel;
835 timepoint_ns integ_rel_ts = base_rel_ts;
836 xrt_quat &o = integ_rel.pose.orientation;
837 xrt_vec3 &p = integ_rel.pose.position;
838 xrt_vec3 &w = integ_rel.angular_velocity;
839 xrt_vec3 &v = integ_rel.linear_velocity;
840 bool clamped = false; // If when_ns is older than the latest IMU ts
841
842 while (i >= 0) { // Decreasing i increases timestamp
843 // Get samples
844 xrt_vec3 g{};
845 xrt_vec3 a{};
846 uint64_t g_ts{};
847 uint64_t a_ts{};
848 bool got = true;
849 got &= m_ff_vec3_f32_get(t.gyro_ff, i, &g, &g_ts);
850 got &= m_ff_vec3_f32_get(t.accel_ff, i, &a, &a_ts);
851 timepoint_ns ts = g_ts;
852
853
854 // Checks
855 if (ts > when_ns) {
856 clamped = true;
857 //! @todo Instead of using same a and g values, do an interpolated sample like this:
858 // a = prev_a + ((when_ns - prev_ts) / (ts - prev_ts)) * (a - prev_a);
859 // g = prev_g + ((when_ns - prev_ts) / (ts - prev_ts)) * (g - prev_g);
860 ts = when_ns; // clamp ts to when_ns
861 }
862 SLAM_DASSERT(got && g_ts == a_ts, "Failure getting synced gyro and accel samples");
863 SLAM_DASSERT(ts >= base_rel_ts, "Accessing imu sample that is older than latest SLAM pose");
864
865 // Update time
866 float dt = (float)time_ns_to_s(ts - integ_rel_ts);
867 integ_rel_ts = ts;
868
869 // Integrate gyroscope
870 xrt_quat angvel_delta{};
871 xrt_vec3 scaled_half_g = g * dt * 0.5f;
872 math_quat_exp(&scaled_half_g, &angvel_delta); // Same as using math_quat_from_angle_vector(g/dt)
873 math_quat_rotate(&o, &angvel_delta, &o); // Orientation
874 math_quat_rotate_derivative(&o, &g, &w); // Angular velocity
875
876 // Integrate accelerometer
877 xrt_vec3 world_accel{};
878 math_quat_rotate_vec3(&o, &a, &world_accel);
879 world_accel += t.gravity_correction;
880 v += world_accel * dt; // Linear velocity
881 p += v * dt + world_accel * (dt * dt * 0.5f); // Position
882
883 if (clamped) {
884 break;
885 }
886 i--;
887 }
888
889 os_mutex_unlock(&t.lock_ff);
890
891 // Do the prediction based on the updated relation
892 double last_imu_to_now_dt = time_ns_to_s(when_ns - integ_rel_ts);
893 xrt_space_relation predicted_relation{};
894 m_predict_relation(&integ_rel, last_imu_to_now_dt, &predicted_relation);
895
896 *out_relation = predicted_relation;
897}
898
899//! Return our best guess of the relation at time @p when_ns using all the data the tracker has.
900static void
901predict_pose(TrackerSlam &t, timepoint_ns when_ns, struct xrt_space_relation *out_relation)
902{
903 XRT_TRACE_MARKER();
904
905 bool valid_pred_type = t.pred_type >= SLAM_PRED_NONE && t.pred_type < SLAM_PRED_COUNT;
906 SLAM_DASSERT(valid_pred_type, "Invalid prediction type (%d)", t.pred_type);
907
908 // Get last relation computed purely from SLAM data
909 xrt_space_relation rel{};
910 int64_t rel_ts;
911 bool empty = !t.slam_rels.get_latest(&rel_ts, &rel);
912
913 // Stop if there is no previous relation to use for prediction
914 if (empty) {
915 out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_NONE;
916 return;
917 }
918
919 // Use only last SLAM pose without prediction if PREDICTION_NONE
920 if (t.pred_type == SLAM_PRED_NONE) {
921 *out_relation = rel;
922 return;
923 }
924
925 // Use only SLAM data if asking for an old point in time or PREDICTION_SP_SO_SA_SL
926 SLAM_DASSERT_(rel_ts < INT64_MAX);
927 if (t.pred_type == SLAM_PRED_SP_SO_SA_SL || when_ns <= (int64_t)rel_ts) {
928 t.slam_rels.get(when_ns, out_relation);
929 return;
930 }
931
932
933 if (t.pred_type == SLAM_PRED_IP_IO_IA_IL) {
934 predict_pose_from_imu(t, when_ns, rel, (int64_t)rel_ts, out_relation);
935 return;
936 }
937
938 os_mutex_lock(&t.lock_ff);
939
940 // Update angular velocity with gyro data
941 if (t.pred_type >= SLAM_PRED_SP_SO_IA_SL) {
942 xrt_vec3 avg_gyro{};
943 m_ff_vec3_f32_filter(t.gyro_ff, rel_ts, when_ns, &avg_gyro);
944 math_quat_rotate_derivative(&rel.pose.orientation, &avg_gyro, &rel.angular_velocity);
945 }
946
947 // Update linear velocity with accel data
948 if (t.pred_type >= SLAM_PRED_SP_SO_IA_IL) {
949 xrt_vec3 avg_accel{};
950 m_ff_vec3_f32_filter(t.accel_ff, rel_ts, when_ns, &avg_accel);
951 xrt_vec3 world_accel{};
952 math_quat_rotate_vec3(&rel.pose.orientation, &avg_accel, &world_accel);
953 world_accel += t.gravity_correction;
954 double slam_to_imu_dt = time_ns_to_s(t.last_imu_ts - rel_ts);
955 rel.linear_velocity += world_accel * slam_to_imu_dt;
956 }
957
958 os_mutex_unlock(&t.lock_ff);
959
960 // Do the prediction based on the updated relation
961 double slam_to_now_dt = time_ns_to_s(when_ns - rel_ts);
962 xrt_space_relation predicted_relation{};
963 m_predict_relation(&rel, slam_to_now_dt, &predicted_relation);
964
965 *out_relation = predicted_relation;
966}
967
968//! Various filters to remove noise from the predicted trajectory.
969static void
970filter_pose(TrackerSlam &t, timepoint_ns when_ns, struct xrt_space_relation *out_relation)
971{
972 XRT_TRACE_MARKER();
973
974 if (t.filter.use_moving_average_filter) {
975 if (out_relation->relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
976 xrt_vec3 pos = out_relation->pose.position;
977 m_ff_vec3_f32_push(t.filter.pos_ff, &pos, when_ns);
978 }
979
980 if (out_relation->relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
981 // Don't save w component as we can retrieve it knowing these are (almost) unit quaternions
982 xrt_vec3 rot = {out_relation->pose.orientation.x, out_relation->pose.orientation.y,
983 out_relation->pose.orientation.z};
984 m_ff_vec3_f32_push(t.filter.rot_ff, &rot, when_ns);
985 }
986
987 // Get averages in time window
988 timepoint_ns window = t.filter.window * U_TIME_1MS_IN_NS;
989 xrt_vec3 avg_pos;
990 m_ff_vec3_f32_filter(t.filter.pos_ff, when_ns - window, when_ns, &avg_pos);
991 xrt_vec3 avg_rot; // Naive but good enough rotation average
992 m_ff_vec3_f32_filter(t.filter.rot_ff, when_ns - window, when_ns, &avg_rot);
993
994 // Considering the naive averaging this W is a bit wrong, but it feels reasonably well
995 float avg_rot_w = sqrtf(1 - (avg_rot.x * avg_rot.x + avg_rot.y * avg_rot.y + avg_rot.z * avg_rot.z));
996 out_relation->pose.orientation = xrt_quat{avg_rot.x, avg_rot.y, avg_rot.z, avg_rot_w};
997 out_relation->pose.position = avg_pos;
998
999 //! @todo Implement the quaternion averaging with a m_ff_vec4_f32 and
1000 //! normalization. Although it would be best to have a way of generalizing
1001 //! types before so as to not have redundant copies of ff logic.
1002 }
1003
1004 if (t.filter.use_exponential_smoothing_filter) {
1005 xrt_space_relation &last = t.filter.last;
1006 xrt_space_relation &target = t.filter.target;
1007 target = *out_relation;
1008 m_space_relation_interpolate(&last, &target, t.filter.alpha, target.relation_flags, &last);
1009 *out_relation = last;
1010 }
1011
1012 if (t.filter.use_one_euro_filter) {
1013 xrt_pose &p = out_relation->pose;
1014 if (out_relation->relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) {
1015 m_filter_euro_vec3_run(&t.filter.pos_oe, when_ns, &p.position, &p.position);
1016 }
1017 if (out_relation->relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) {
1018 m_filter_euro_quat_run(&t.filter.rot_oe, when_ns, &p.orientation, &p.orientation);
1019 }
1020 }
1021}
1022
1023static void
1024setup_ui(TrackerSlam &t)
1025{
1026 t.pred_combo.count = SLAM_PRED_COUNT;
1027 t.pred_combo.options = "None\0Interpolate SLAM poses\0Also gyro\0Also accel\0Latest IMU\0";
1028 t.pred_combo.value = (int *)&t.pred_type;
1029 t.ui_sink = vector<u_sink_debug>(t.cam_count);
1030 for (size_t i = 0; i < t.ui_sink.size(); i++) {
1031 u_sink_debug_init(&t.ui_sink[i]);
1032 }
1033 os_mutex_init(&t.lock_ff);
1034 m_ff_vec3_f32_alloc(&t.gyro_ff, 1000);
1035 m_ff_vec3_f32_alloc(&t.accel_ff, 1000);
1036 m_ff_vec3_f32_alloc(&t.filter.pos_ff, 1000);
1037 m_ff_vec3_f32_alloc(&t.filter.rot_ff, 1000);
1038
1039 u_var_add_root(&t, "SLAM Tracker", true);
1040 u_var_add_log_level(&t, &t.log_level, "Log Level");
1041 u_var_add_bool(&t, &t.submit, "Submit data to SLAM");
1042
1043 u_var_button_cb reset_state_cb = [](void *t_ptr) {
1044 TrackerSlam &t = *(TrackerSlam *)t_ptr;
1045
1046 vit_result_t vres = t.vit.tracker_reset(t.tracker);
1047 if (vres != VIT_SUCCESS) {
1048 SLAM_WARN("Failed to reset VIT tracker");
1049 }
1050 };
1051 t.reset_state_btn.cb = reset_state_cb;
1052 t.reset_state_btn.ptr = &t;
1053 u_var_add_button(&t, &t.reset_state_btn, "Reset tracker state");
1054
1055 u_var_add_bool(&t, &t.gt.override_tracking, "Track with ground truth (if available)");
1056 euroc_recorder_add_ui(t.euroc_recorder, &t, "");
1057
1058 u_var_add_gui_header(&t, NULL, "Trajectory Filter");
1059 u_var_add_bool(&t, &t.filter.use_moving_average_filter, "Enable moving average filter");
1060 u_var_add_f64(&t, &t.filter.window, "Window size (ms)");
1061 u_var_add_bool(&t, &t.filter.use_exponential_smoothing_filter, "Enable exponential smoothing filter");
1062 u_var_add_f32(&t, &t.filter.alpha, "Smoothing factor");
1063 u_var_add_bool(&t, &t.filter.use_one_euro_filter, "Enable one euro filter");
1064 u_var_add_f32(&t, &t.filter.pos_oe.base.fc_min, "Position minimum cutoff");
1065 u_var_add_f32(&t, &t.filter.pos_oe.base.beta, "Position beta speed");
1066 u_var_add_f32(&t, &t.filter.pos_oe.base.fc_min_d, "Position minimum delta cutoff");
1067 u_var_add_f32(&t, &t.filter.rot_oe.base.fc_min, "Orientation minimum cutoff");
1068 u_var_add_f32(&t, &t.filter.rot_oe.base.beta, "Orientation beta speed");
1069 u_var_add_f32(&t, &t.filter.rot_oe.base.fc_min_d, "Orientation minimum delta cutoff");
1070
1071 u_var_add_gui_header(&t, NULL, "Prediction");
1072 u_var_add_combo(&t, &t.pred_combo, "Prediction Type");
1073 u_var_add_i32(&t, &t.dbg_pred_every, "Debug prediction skips (try 30)");
1074 u_var_add_ro_ff_vec3_f32(&t, t.gyro_ff, "Gyroscope");
1075 u_var_add_ro_ff_vec3_f32(&t, t.accel_ff, "Accelerometer");
1076 u_var_add_f32(&t, &t.gravity_correction.z, "Gravity Correction");
1077 for (size_t i = 0; i < t.ui_sink.size(); i++) {
1078 char label[64] = {0};
1079 snprintf(label, sizeof(label), "Camera %zu", i);
1080 u_var_add_sink_debug(&t, &t.ui_sink[i], label);
1081 }
1082
1083 u_var_add_gui_header(&t, NULL, "Stats");
1084 u_var_add_ro_raw_text(&t, "\nRecord to CSV files", "Record to CSV files");
1085 u_var_add_bool(&t, &t.slam_traj_writer->enabled, "Record tracked trajectory");
1086 u_var_add_bool(&t, &t.pred_traj_writer->enabled, "Record predicted trajectory");
1087 u_var_add_bool(&t, &t.filt_traj_writer->enabled, "Record filtered trajectory");
1088 u_var_add_bool(&t, &t.slam_times_writer->enabled, "Record tracker times");
1089 u_var_add_bool(&t, &t.slam_features_writer->enabled, "Record feature count");
1090 timing_ui_setup(t);
1091 features_ui_setup(t);
1092 // Later, gt_ui_setup will setup the tracking error UI if ground truth becomes available
1093}
1094
1095static void
1096add_camera_calibration(const TrackerSlam &t, const t_slam_camera_calibration *calib, uint32_t cam_index)
1097{
1098 const t_camera_calibration &view = calib->base;
1099
1100 vit_camera_calibration params = {};
1101 params.camera_index = cam_index;
1102 params.width = view.image_size_pixels.w;
1103 params.height = view.image_size_pixels.h;
1104 params.frequency = calib->frequency;
1105
1106 params.fx = view.intrinsics[0][0];
1107 params.fy = view.intrinsics[1][1];
1108 params.cx = view.intrinsics[0][2];
1109 params.cy = view.intrinsics[1][2];
1110
1111 switch (view.distortion_model) {
1112 case T_DISTORTION_OPENCV_RADTAN_8: {
1113 params.model = VIT_CAMERA_DISTORTION_RT8;
1114 const size_t size = sizeof(struct t_camera_calibration_rt8_params) + sizeof(double);
1115 params.distortion_count = size / sizeof(double);
1116 SLAM_ASSERT_(params.distortion_count == 9);
1117
1118 memcpy(params.distortion, &view.rt8, size);
1119
1120 // -1 metric radius tells Basalt to estimate the metric radius on its own.
1121 params.distortion[8] = -1.f;
1122 break;
1123 }
1124 case T_DISTORTION_WMR: {
1125 params.model = VIT_CAMERA_DISTORTION_RT8;
1126 const size_t size = sizeof(struct t_camera_calibration_rt8_params) + sizeof(double);
1127 params.distortion_count = size / sizeof(double);
1128 SLAM_ASSERT_(params.distortion_count == 9);
1129
1130 memcpy(params.distortion, &view.wmr, size);
1131
1132 params.distortion[8] = view.wmr.rpmax;
1133
1134 break;
1135 }
1136 case T_DISTORTION_FISHEYE_KB4: {
1137 params.model = VIT_CAMERA_DISTORTION_KB4;
1138 const size_t size = sizeof(struct t_camera_calibration_kb4_params);
1139 params.distortion_count = size / sizeof(double);
1140 SLAM_ASSERT_(params.distortion_count == 4);
1141
1142 memcpy(params.distortion, &view.kb4, size);
1143 break;
1144 }
1145 default:
1146 SLAM_ASSERT(false, "SLAM doesn't support distortion type %s",
1147 t_stringify_camera_distortion_model(view.distortion_model));
1148 break;
1149 }
1150
1151 xrt_matrix_4x4 T; // Row major T_imu_cam
1152 math_matrix_4x4_transpose(&calib->T_imu_cam, &T);
1153
1154 // Converts the xrt_matrix_4x4 from float to double
1155 for (size_t i = 0; i < ARRAY_SIZE(params.transform); ++i)
1156 params.transform[i] = T.v[i];
1157
1158 vit_result_t vres = t.vit.tracker_add_camera_calibration(t.tracker, ¶ms);
1159 if (vres != VIT_SUCCESS) {
1160 SLAM_ERROR("Failed to add camera calibration for camera %u", cam_index);
1161 }
1162}
1163
1164static void
1165add_imu_calibration(const TrackerSlam &t, const t_slam_imu_calibration *imu_calib)
1166{
1167 vit_imu_calibration_t params = {};
1168 params.imu_index = 0;
1169 params.frequency = imu_calib->frequency;
1170
1171 // TODO improve memcpy size calculation
1172
1173 const t_inertial_calibration &accel = imu_calib->base.accel;
1174 memcpy(params.accel.transform, accel.transform, sizeof(double) * 9);
1175 memcpy(params.accel.offset, accel.offset, sizeof(double) * 3);
1176 memcpy(params.accel.bias_std, accel.bias_std, sizeof(double) * 3);
1177 memcpy(params.accel.noise_std, accel.noise_std, sizeof(double) * 3);
1178
1179 const t_inertial_calibration &gyro = imu_calib->base.gyro;
1180 memcpy(params.gyro.transform, gyro.transform, sizeof(double) * 9);
1181 memcpy(params.gyro.offset, gyro.offset, sizeof(double) * 3);
1182 memcpy(params.gyro.bias_std, gyro.bias_std, sizeof(double) * 3);
1183 memcpy(params.gyro.noise_std, gyro.noise_std, sizeof(double) * 3);
1184
1185 vit_result_t vres = t.vit.tracker_add_imu_calibration(t.tracker, ¶ms);
1186 if (vres != VIT_SUCCESS) {
1187 SLAM_ERROR("Failed to add imu calibration");
1188 }
1189}
1190
1191static void
1192send_calibration(const TrackerSlam &t, const t_slam_calibration &c)
1193{
1194 // Try to send camera calibration data to the SLAM system
1195 if (t.exts.has_add_camera_calibration) {
1196 for (int i = 0; i < c.cam_count; i++) {
1197 SLAM_INFO("Sending Camera %d calibration from Monado", i);
1198 add_camera_calibration(t, &c.cams[i], i);
1199 }
1200 } else {
1201 SLAM_WARN("Tracker doesn't support camera calibration");
1202 }
1203
1204 // Try to send IMU calibration data to the SLAM system
1205 if (t.exts.has_add_imu_calibration) {
1206 SLAM_INFO("Sending IMU calibration from Monado");
1207 add_imu_calibration(t, &c.imu);
1208 } else {
1209 SLAM_WARN("Tracker doesn't support IMU calibration");
1210 }
1211}
1212
1213} // namespace xrt::auxiliary::tracking::slam
1214
1215using namespace xrt::auxiliary::tracking::slam;
1216
1217/*
1218 *
1219 * External functions
1220 *
1221 */
1222
1223//! Get a filtered prediction from the SLAM tracked poses.
1224extern "C" void
1225t_slam_get_tracked_pose(struct xrt_tracked_slam *xts, timepoint_ns when_ns, struct xrt_space_relation *out_relation)
1226{
1227 XRT_TRACE_MARKER();
1228
1229 auto &t = *container_of(xts, TrackerSlam, base);
1230
1231 //! @todo This should not be cached, the same timestamp can be requested at a
1232 //! later time on the frame for a better prediction.
1233 if (when_ns == t.last_ts) {
1234 *out_relation = t.last_rel;
1235 return;
1236 }
1237
1238 flush_poses(t);
1239
1240 predict_pose(t, when_ns, out_relation);
1241 t.pred_traj_writer->push({when_ns, out_relation->pose});
1242
1243 filter_pose(t, when_ns, out_relation);
1244 t.filt_traj_writer->push({when_ns, out_relation->pose});
1245
1246 t.last_rel = *out_relation;
1247 t.last_ts = when_ns;
1248
1249 if (t.gt.override_tracking) {
1250 out_relation->pose = gt2xr_pose(t.gt.origin, get_gt_pose_at(*t.gt.trajectory, when_ns));
1251 }
1252}
1253
1254//! Receive and register ground truth to use for trajectory error metrics.
1255extern "C" void
1256t_slam_gt_sink_push(struct xrt_pose_sink *sink, xrt_pose_sample *sample)
1257{
1258 XRT_TRACE_MARKER();
1259
1260 auto &t = *container_of(sink, TrackerSlam, gt_sink);
1261
1262 if (t.gt.trajectory->empty()) {
1263 t.gt.origin = sample->pose;
1264 gt_ui_setup(t);
1265 }
1266
1267 t.gt.trajectory->insert_or_assign(sample->timestamp_ns, sample->pose);
1268 xrt_sink_push_pose(t.euroc_recorder->gt, sample);
1269}
1270
1271//! Receive and register masks to use in the next image
1272extern "C" void
1273t_slam_hand_mask_sink_push(struct xrt_hand_masks_sink *sink, struct xrt_hand_masks_sample *hand_masks)
1274{
1275 XRT_TRACE_MARKER();
1276
1277 auto &t = *container_of(sink, TrackerSlam, hand_masks_sink);
1278 unique_lock lock(t.last_hand_masks_mutex);
1279 t.last_hand_masks = *hand_masks;
1280}
1281
1282//! Receive and send IMU samples to the external SLAM system
1283extern "C" void
1284t_slam_receive_imu(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
1285{
1286 XRT_TRACE_MARKER();
1287
1288 auto &t = *container_of(sink, TrackerSlam, imu_sink);
1289
1290 timepoint_ns ts = s->timestamp_ns;
1291 xrt_vec3_f64 a = s->accel_m_s2;
1292 xrt_vec3_f64 w = s->gyro_rad_secs;
1293
1294 timepoint_ns now = (timepoint_ns)os_monotonic_get_ns();
1295 SLAM_TRACE("[%ld] imu t=%ld a=[%f,%f,%f] w=[%f,%f,%f]", now, ts, a.x, a.y, a.z, w.x, w.y, w.z);
1296 // Check monotonically increasing timestamps
1297 if (ts <= t.last_imu_ts) {
1298 SLAM_WARN("Sample (%" PRId64 ") is older than last (%" PRId64 ") by %" PRId64 " ns", ts, t.last_imu_ts,
1299 t.last_imu_ts - ts);
1300 return;
1301 }
1302 t.last_imu_ts = ts;
1303
1304 //! @todo There are many conversions like these between xrt and
1305 //! slam_tracker.hpp types. Implement a casting mechanism to avoid copies.
1306 vit_imu_sample_t sample = {};
1307 sample.timestamp = ts;
1308 sample.ax = a.x;
1309 sample.ay = a.y;
1310 sample.az = a.z;
1311 sample.wx = w.x;
1312 sample.wy = w.y;
1313 sample.wz = w.z;
1314
1315 if (t.submit) {
1316 t.vit.tracker_push_imu_sample(t.tracker, &sample);
1317 }
1318
1319 xrt_sink_push_imu(t.euroc_recorder->imu, s);
1320
1321 struct xrt_vec3 gyro = {(float)w.x, (float)w.y, (float)w.z};
1322 struct xrt_vec3 accel = {(float)a.x, (float)a.y, (float)a.z};
1323 os_mutex_lock(&t.lock_ff);
1324 m_ff_vec3_f32_push(t.gyro_ff, &gyro, ts);
1325 m_ff_vec3_f32_push(t.accel_ff, &accel, ts);
1326 os_mutex_unlock(&t.lock_ff);
1327}
1328
1329//! Push the frame to the external SLAM system
1330static void
1331receive_frame(TrackerSlam &t, struct xrt_frame *frame, uint32_t cam_index)
1332{
1333 XRT_TRACE_MARKER();
1334
1335 SLAM_DASSERT_(frame->timestamp < INT64_MAX);
1336
1337 // Return early if we don't submit
1338 if (!t.submit) {
1339 return;
1340 }
1341
1342 if (cam_index == t.cam_count - 1) {
1343 flush_poses(t); // Useful to flush SLAM poses when no openxr app is open
1344 }
1345
1346 SLAM_DASSERT(t.last_cam_ts[0] != INT64_MIN || cam_index == 0, "First frame was not a cam0 frame");
1347
1348 // Check monotonically increasing timestamps
1349 timepoint_ns &last_ts = t.last_cam_ts[cam_index];
1350 timepoint_ns ts = (int64_t)frame->timestamp;
1351 SLAM_TRACE("[%" PRId64 "] cam%d frame t=%" PRId64, os_monotonic_get_ns(), cam_index, ts);
1352 if (last_ts >= ts) {
1353 SLAM_WARN("Frame (%" PRId64 ") is older than last (%" PRId64 ") by %" PRId64 " ns", ts, last_ts,
1354 last_ts - ts);
1355 }
1356 last_ts = ts;
1357
1358 // Construct and send the image sample
1359 vit_img_sample sample = {};
1360 sample.cam_index = cam_index;
1361 sample.timestamp = ts;
1362
1363 sample.data = frame->data;
1364 sample.width = frame->width;
1365 sample.height = frame->height;
1366 sample.stride = frame->stride;
1367 sample.size = frame->size;
1368
1369 // TODO check format before
1370 switch (frame->format) {
1371 case XRT_FORMAT_L8: sample.format = VIT_IMAGE_FORMAT_L8; break;
1372 case XRT_FORMAT_R8G8B8: sample.format = VIT_IMAGE_FORMAT_R8G8B8; break;
1373 default: SLAM_ERROR("Unknown image format"); return;
1374 }
1375
1376 xrt_hand_masks_sample hand_masks{};
1377 {
1378 unique_lock lock(t.last_hand_masks_mutex);
1379 hand_masks = t.last_hand_masks;
1380 }
1381
1382 auto &view = hand_masks.views[cam_index];
1383 std::vector<vit_mask_t> masks;
1384 if (view.enabled) {
1385 for (auto &hand : view.hands) {
1386 if (!hand.enabled) {
1387 continue;
1388 }
1389 vit_mask_t mask{};
1390 mask.x = hand.rect.x;
1391 mask.y = hand.rect.y;
1392 mask.w = hand.rect.w;
1393 mask.h = hand.rect.h;
1394 masks.push_back(mask);
1395 }
1396
1397 sample.mask_count = masks.size();
1398 sample.masks = masks.empty() ? nullptr : masks.data();
1399 }
1400
1401 {
1402 XRT_TRACE_IDENT(slam_push);
1403 t.vit.tracker_push_img_sample(t.tracker, &sample);
1404 }
1405}
1406
1407#define DEFINE_RECEIVE_CAM(cam_id) \
1408 extern "C" void t_slam_receive_cam##cam_id(struct xrt_frame_sink *sink, struct xrt_frame *frame) \
1409 { \
1410 auto &t = *container_of(sink, TrackerSlam, cam_sinks[cam_id]); \
1411 receive_frame(t, frame, cam_id); \
1412 u_sink_debug_push_frame(&t.ui_sink[cam_id], frame); \
1413 xrt_sink_push_frame(t.euroc_recorder->cams[cam_id], frame); \
1414 }
1415
1416DEFINE_RECEIVE_CAM(0)
1417DEFINE_RECEIVE_CAM(1)
1418DEFINE_RECEIVE_CAM(2)
1419DEFINE_RECEIVE_CAM(3)
1420DEFINE_RECEIVE_CAM(4)
1421
1422//! Define a function for each XRT_TRACKING_MAX_SLAM_CAMS and reference it in this array
1423void (*t_slam_receive_cam[XRT_TRACKING_MAX_SLAM_CAMS])(xrt_frame_sink *, xrt_frame *) = {
1424 t_slam_receive_cam0, //
1425 t_slam_receive_cam1, //
1426 t_slam_receive_cam2, //
1427 t_slam_receive_cam3, //
1428 t_slam_receive_cam4, //
1429};
1430
1431
1432extern "C" void
1433t_slam_node_break_apart(struct xrt_frame_node *node)
1434{
1435 auto &t = *container_of(node, TrackerSlam, node);
1436 if (t.ovr_tracker != NULL) {
1437 t_openvr_tracker_stop(t.ovr_tracker);
1438 }
1439
1440 vit_result_t vres = t.vit.tracker_stop(t.tracker);
1441 if (vres != VIT_SUCCESS) {
1442 SLAM_ERROR("Failed to stop VIT tracker");
1443 return;
1444 }
1445
1446 SLAM_DEBUG("SLAM tracker dismantled");
1447}
1448
1449extern "C" void
1450t_slam_node_destroy(struct xrt_frame_node *node)
1451{
1452 auto t_ptr = container_of(node, TrackerSlam, node);
1453 auto &t = *t_ptr; // Needed by SLAM_DEBUG
1454 SLAM_DEBUG("Destroying SLAM tracker");
1455 if (t.ovr_tracker != NULL) {
1456 t_openvr_tracker_destroy(t.ovr_tracker);
1457 }
1458 delete t.gt.trajectory;
1459 delete t.slam_times_writer;
1460 delete t.slam_features_writer;
1461 delete t.slam_traj_writer;
1462 delete t.pred_traj_writer;
1463 delete t.filt_traj_writer;
1464 u_var_remove_root(t_ptr);
1465 for (size_t i = 0; i < t.ui_sink.size(); i++) {
1466 u_sink_debug_destroy(&t.ui_sink[i]);
1467 }
1468 m_ff_vec3_f32_free(&t.gyro_ff);
1469 m_ff_vec3_f32_free(&t.accel_ff);
1470 os_mutex_destroy(&t.lock_ff);
1471 m_ff_vec3_f32_free(&t.filter.pos_ff);
1472 m_ff_vec3_f32_free(&t.filter.rot_ff);
1473
1474 t_ptr->vit.tracker_destroy(t_ptr->tracker);
1475 t_vit_bundle_unload(&t_ptr->vit);
1476
1477 delete t_ptr;
1478}
1479
1480extern "C" int
1481t_slam_start(struct xrt_tracked_slam *xts)
1482{
1483 auto &t = *container_of(xts, TrackerSlam, base);
1484 vit_result_t vres = t.vit.tracker_start(t.tracker);
1485 if (vres != VIT_SUCCESS) {
1486 SLAM_ERROR("Failed to start VIT tracker");
1487 return -1;
1488 }
1489
1490 SLAM_DEBUG("SLAM tracker started");
1491 return 0;
1492}
1493
1494extern "C" void
1495t_slam_fill_default_config(struct t_slam_tracker_config *config)
1496{
1497 config->log_level = debug_get_log_option_slam_log();
1498 config->vit_system_library_path = debug_get_option_vit_system_library_path();
1499 config->slam_config = debug_get_option_slam_config();
1500 config->slam_ui = debug_get_bool_option_slam_ui();
1501 config->submit_from_start = debug_get_bool_option_slam_submit_from_start();
1502 config->openvr_groundtruth_device = int(debug_get_num_option_slam_openvr_groundtruth_device());
1503 config->prediction = t_slam_prediction_type(debug_get_num_option_slam_prediction_type());
1504 config->write_csvs = debug_get_bool_option_slam_write_csvs();
1505 config->csv_path = debug_get_option_slam_csv_path();
1506 config->timing_stat = debug_get_bool_option_slam_timing_stat();
1507 config->features_stat = debug_get_bool_option_slam_features_stat();
1508 config->cam_count = int(debug_get_num_option_slam_cam_count());
1509 config->slam_calib = NULL;
1510}
1511
1512extern "C" int
1513t_slam_create(struct xrt_frame_context *xfctx,
1514 struct t_slam_tracker_config *config,
1515 struct xrt_tracked_slam **out_xts,
1516 struct xrt_slam_sinks **out_sink)
1517{
1518 struct t_slam_tracker_config default_config = {};
1519 if (config == nullptr) {
1520 t_slam_fill_default_config(&default_config);
1521 config = &default_config;
1522 }
1523
1524 enum u_logging_level log_level = config->log_level;
1525
1526 std::unique_ptr<TrackerSlam> t_ptr = std::make_unique<TrackerSlam>();
1527 TrackerSlam &t = *t_ptr;
1528
1529 t.log_level = log_level;
1530
1531 SLAM_INFO("Loading VIT system library from VIT_SYSTEM_LIBRARY_PATH='%s'", config->vit_system_library_path);
1532
1533 if (!t_vit_bundle_load(&t.vit, config->vit_system_library_path)) {
1534 SLAM_ERROR("Failed to load VIT system library from '%s'", config->vit_system_library_path);
1535 return -1;
1536 }
1537
1538 // Check the user has provided a SLAM_CONFIG file
1539 const char *config_file = config->slam_config;
1540 bool some_calib = config->slam_calib != nullptr;
1541 if (!config_file && !some_calib) {
1542 SLAM_WARN("Unable to determine sensor calibration, did you forget to set SLAM_CONFIG?");
1543 return -1;
1544 }
1545
1546 struct vit_config system_config = {};
1547 system_config.file = config_file;
1548 system_config.cam_count = config->cam_count;
1549 system_config.show_ui = config->slam_ui;
1550
1551 vit_result_t vres = t.vit.tracker_create(&system_config, &t.tracker);
1552 if (vres != VIT_SUCCESS) {
1553 SLAM_ERROR("Failed to create VIT tracker (%d)", vres);
1554 return -1;
1555 }
1556
1557 vres = t.vit.tracker_get_supported_extensions(t.tracker, &t.exts);
1558 if (vres != VIT_SUCCESS) {
1559 SLAM_ERROR("Failed to get VIT tracker supported extensions (%d)", vres);
1560 return -1;
1561 }
1562
1563 t.base.get_tracked_pose = t_slam_get_tracked_pose;
1564
1565 if (!config_file) {
1566 SLAM_INFO("Using calibration from driver and default pipeline settings");
1567 send_calibration(t, *config->slam_calib); // Not null because of `some_calib`
1568 } else {
1569 SLAM_INFO("Using sensor calibration provided by the SLAM_CONFIG file");
1570 }
1571
1572 SLAM_ASSERT(t_slam_receive_cam[ARRAY_SIZE(t_slam_receive_cam) - 1] != nullptr, "See `cam_sink_push` docs");
1573 t.sinks.cam_count = config->cam_count;
1574 for (int i = 0; i < XRT_TRACKING_MAX_SLAM_CAMS; i++) {
1575 t.cam_sinks[i].push_frame = t_slam_receive_cam[i];
1576 t.sinks.cams[i] = &t.cam_sinks[i];
1577 }
1578
1579 t.imu_sink.push_imu = t_slam_receive_imu;
1580 t.sinks.imu = &t.imu_sink;
1581
1582 t.gt_sink.push_pose = t_slam_gt_sink_push;
1583 t.sinks.gt = &t.gt_sink;
1584
1585 t.hand_masks_sink.push_hand_masks = t_slam_hand_mask_sink_push;
1586 t.sinks.hand_masks = &t.hand_masks_sink;
1587
1588 t.submit = config->submit_from_start;
1589 t.cam_count = config->cam_count;
1590
1591 t.node.break_apart = t_slam_node_break_apart;
1592 t.node.destroy = t_slam_node_destroy;
1593
1594 xrt_frame_context_add(xfctx, &t.node);
1595
1596 t.euroc_recorder = euroc_recorder_create(xfctx, NULL, t.cam_count, false);
1597
1598 t.last_imu_ts = INT64_MIN;
1599 t.last_cam_ts = vector<timepoint_ns>(t.cam_count, INT64_MIN);
1600 t.last_hand_masks = xrt_hand_masks_sample{};
1601
1602 t.pred_type = config->prediction;
1603
1604 m_filter_euro_vec3_init(&t.filter.pos_oe, t.filter.min_cutoff, t.filter.min_dcutoff, t.filter.beta);
1605 m_filter_euro_quat_init(&t.filter.rot_oe, t.filter.min_cutoff, t.filter.min_dcutoff, t.filter.beta);
1606
1607 t.gt.trajectory = new Trajectory{};
1608
1609 // Setup CSV files
1610 bool write_csvs = config->write_csvs;
1611 string dir = config->csv_path;
1612 t.slam_times_writer = new TimingWriter(dir, "timing.csv", write_csvs, t.timing.columns);
1613 t.slam_features_writer = new FeaturesWriter(dir, "features.csv", write_csvs, t.cam_count);
1614 t.slam_traj_writer = new TrajectoryWriter(dir, "tracking.csv", write_csvs);
1615 t.pred_traj_writer = new TrajectoryWriter(dir, "prediction.csv", write_csvs);
1616 t.filt_traj_writer = new TrajectoryWriter(dir, "filtering.csv", write_csvs);
1617
1618 setup_ui(t);
1619
1620 // Setup OpenVR groundtruth tracker
1621 if (config->openvr_groundtruth_device > 0) {
1622 enum openvr_device dev_class = openvr_device(config->openvr_groundtruth_device);
1623 const double freq = 1000.0f;
1624 t.ovr_tracker = t_openvr_tracker_create(freq, &dev_class, &t.sinks.gt, 1);
1625 if (t.ovr_tracker != NULL) {
1626 t_openvr_tracker_start(t.ovr_tracker);
1627 }
1628 }
1629
1630 // Get ownership
1631 TrackerSlam *tracker = t_ptr.release();
1632
1633 *out_xts = &tracker->base;
1634 *out_sink = &tracker->sinks;
1635
1636 SLAM_DEBUG("SLAM tracker created");
1637 return 0;
1638}