The open source OpenXR runtime
t/slam: Implement feature count metric UI and recording
authored by
Mateo de Mayo
and committed by
Moses Turner
f55ac055
789111da
+209
-4
+208
-4
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
+208
-4
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
···
30
30
#include <opencv2/core/mat.hpp>
31
31
#include <opencv2/core/version.hpp>
32
32
33
+
#include <deque>
33
34
#include <filesystem>
34
35
#include <fstream>
35
36
#include <iomanip>
···
69
70
DEBUG_GET_ONCE_BOOL_OPTION(slam_write_csvs, "SLAM_WRITE_CSVS", false)
70
71
DEBUG_GET_ONCE_OPTION(slam_csv_path, "SLAM_CSV_PATH", "evaluation/")
71
72
DEBUG_GET_ONCE_BOOL_OPTION(slam_timing_stat, "SLAM_TIMING_STAT", true)
73
+
DEBUG_GET_ONCE_BOOL_OPTION(slam_features_stat, "SLAM_FEATURES_STAT", true)
72
74
73
75
//! Namespace for the interface to the external SLAM tracking system
74
76
namespace xrt::auxiliary::tracking::slam {
75
77
constexpr int UI_TIMING_POSE_COUNT = 192;
78
+
constexpr int UI_FEATURES_POSE_COUNT = 192;
76
79
constexpr int UI_GTDIFF_POSE_COUNT = 192;
77
80
constexpr int NUM_CAMS = 2; //!< This should be used as little as possible to allow setups that are not stereo
78
81
82
+
using std::deque;
79
83
using std::ifstream;
80
84
using std::make_shared;
81
85
using std::map;
82
86
using std::ofstream;
87
+
using std::pair;
83
88
using std::shared_ptr;
84
89
using std::string;
85
90
using std::to_string;
···
279
284
}
280
285
};
281
286
287
+
//! Writes feature information specific to a particular estimated pose
288
+
class FeaturesWriter
289
+
{
290
+
public:
291
+
bool enabled; // Modified through UI
292
+
293
+
private:
294
+
string directory;
295
+
string filename;
296
+
ofstream file;
297
+
bool created = false;
298
+
299
+
void
300
+
create()
301
+
{
302
+
create_directories(directory);
303
+
file = ofstream{directory + "/" + filename};
304
+
file << "#timestamp, cam0 feature count, cam1 feature count" CSV_EOL;
305
+
file << std::fixed << std::setprecision(CSV_PRECISION);
306
+
}
307
+
308
+
309
+
public:
310
+
FeaturesWriter(const string &dir, const string &fn, bool e) : enabled(e), directory(dir), filename(fn) {}
311
+
312
+
void
313
+
push(timepoint_ns ts, const vector<int> &counts)
314
+
{
315
+
if (!enabled) {
316
+
return;
317
+
}
318
+
319
+
if (!created) {
320
+
created = true;
321
+
create();
322
+
}
323
+
324
+
file << ts;
325
+
for (int count : counts) {
326
+
file << "," << count;
327
+
}
328
+
file << CSV_EOL;
329
+
}
330
+
};
282
331
/*!
283
332
* Main implementation of @ref xrt_tracked_slam. This is an adapter class for
284
333
* SLAM tracking that wraps an external SLAM implementation.
···
359
408
// Stats and metrics
360
409
361
410
// CSV writers for offline analysis (using pointers because of container_of)
362
-
TimingWriter *slam_times_writer; //!< Timestamps of the pipeline for performance analysis
363
-
TrajectoryWriter *slam_traj_writer; //!< Estimated poses from the SLAM system
364
-
TrajectoryWriter *pred_traj_writer; //!< Predicted poses
365
-
TrajectoryWriter *filt_traj_writer; //!< Predicted and filtered poses
411
+
TimingWriter *slam_times_writer; //!< Timestamps of the pipeline for performance analysis
412
+
FeaturesWriter *slam_features_writer; //!< Feature tracking information for analysis
413
+
TrajectoryWriter *slam_traj_writer; //!< Estimated poses from the SLAM system
414
+
TrajectoryWriter *pred_traj_writer; //!< Predicted poses
415
+
TrajectoryWriter *filt_traj_writer; //!< Predicted and filtered poses
366
416
367
417
//! Tracker timing info for performance evaluation
368
418
struct
···
381
431
struct u_var_button enable_btn; //!< Toggle tracker timing reports
382
432
} timing;
383
433
434
+
//! Tracker feature tracking info
435
+
struct Features
436
+
{
437
+
struct FeatureCounter
438
+
{
439
+
//! Feature count for each frame timestamp for this camera.
440
+
//! @note Harmless race condition over this as the UI might read this while it's being written
441
+
deque<pair<timepoint_ns, int>> entries{};
442
+
443
+
//! Persitently stored camera name for display in the UI
444
+
string cam_name;
445
+
446
+
void
447
+
addFeatureCount(timepoint_ns ts, int count)
448
+
{
449
+
entries.emplace_back(ts, count);
450
+
if (entries.size() > UI_FEATURES_POSE_COUNT) {
451
+
entries.pop_front();
452
+
}
453
+
}
454
+
};
455
+
456
+
vector<FeatureCounter> fcs; //!< Store feature count info for each camera
457
+
u_var_curves fcs_ui; //!< Display of `fcs` in UI
458
+
459
+
bool ext_available = false; //!< Whether the SLAM system supports the features extension
460
+
bool ext_enabled = false; //!< Whether the features extension is enabled
461
+
struct u_var_button enable_btn; //!< Toggle extension
462
+
} features;
463
+
384
464
//! Ground truth related fields
385
465
struct
386
466
{
···
403
483
static void
404
484
timing_ui_setup(TrackerSlam &t)
405
485
{
486
+
u_var_add_ro_ftext(&t, "\n%s", "Tracker timing");
487
+
406
488
// Setup toggle button
407
489
static const char *msg[2] = {"[OFF] Enable timing", "[ON] Disable timing"};
408
490
u_var_button_cb cb = [](void *t_ptr) {
···
484
566
485
567
/*
486
568
*
569
+
* Feature information functionality
570
+
*
571
+
*/
572
+
573
+
static void
574
+
features_ui_setup(TrackerSlam &t)
575
+
{
576
+
// We can't do anything useful if the system doesn't implement the feature
577
+
if (!t.features.ext_available) {
578
+
return;
579
+
}
580
+
581
+
u_var_add_ro_ftext(&t, "\n%s", "Tracker features");
582
+
583
+
// Setup toggle button
584
+
static const char *msg[2] = {"[OFF] Enable features info", "[ON] Disable features info"};
585
+
u_var_button_cb cb = [](void *t_ptr) {
586
+
TrackerSlam *t = (TrackerSlam *)t_ptr;
587
+
u_var_button &btn = t->features.enable_btn;
588
+
bool &e = t->features.ext_enabled;
589
+
e = !e;
590
+
snprintf(btn.label, sizeof(btn.label), "%s", msg[e]);
591
+
const auto params = make_shared<FPARAMS_EPEF>(e);
592
+
shared_ptr<void> _;
593
+
t->slam->use_feature(F_ENABLE_POSE_EXT_FEATURES, params, _);
594
+
};
595
+
t.features.enable_btn.cb = cb;
596
+
t.features.enable_btn.disabled = !t.features.ext_available;
597
+
t.features.enable_btn.ptr = &t;
598
+
u_var_add_button(&t, &t.features.enable_btn, msg[t.features.ext_enabled]);
599
+
600
+
// Setup graph
601
+
602
+
u_var_curve_getter getter = [](void *fs_ptr, int i) -> u_var_curve_point {
603
+
auto *fs = (TrackerSlam::Features::FeatureCounter *)fs_ptr;
604
+
timepoint_ns now = os_monotonic_get_ns();
605
+
606
+
size_t size = fs->entries.size();
607
+
if (size == 0) {
608
+
return {0, 0};
609
+
}
610
+
611
+
int last_idx = size - 1;
612
+
if (i > last_idx) {
613
+
i = last_idx;
614
+
}
615
+
616
+
auto [ts, count] = fs->entries.at(last_idx - i);
617
+
return {time_ns_to_s(now - ts), double(count)};
618
+
};
619
+
620
+
t.features.fcs_ui.curve_count = NUM_CAMS;
621
+
t.features.fcs_ui.xlabel = "Last seconds";
622
+
t.features.fcs_ui.ylabel = "Number of features";
623
+
624
+
t.features.fcs.resize(NUM_CAMS);
625
+
for (int i = 0; i < NUM_CAMS; i++) {
626
+
auto &fc = t.features.fcs[i];
627
+
fc.cam_name = "Cam" + to_string(i);
628
+
629
+
auto &fc_ui = t.features.fcs_ui.curves[i];
630
+
fc_ui.count = UI_FEATURES_POSE_COUNT;
631
+
fc_ui.data = &fc;
632
+
fc_ui.getter = getter;
633
+
fc_ui.label = fc.cam_name.c_str();
634
+
}
635
+
636
+
u_var_add_curves(&t, &t.features.fcs_ui, "Feature count");
637
+
}
638
+
639
+
static vector<int>
640
+
features_ui_push(TrackerSlam &t, const pose &ppp)
641
+
{
642
+
if (!t.features.ext_available) {
643
+
return {};
644
+
}
645
+
646
+
shared_ptr<pose_extension> ext = ppp.find_pose_extension(pose_ext_type::FEATURES);
647
+
if (!ext) {
648
+
return {};
649
+
}
650
+
651
+
pose_ext_features pef = *std::static_pointer_cast<pose_ext_features>(ext);
652
+
653
+
// Push to the UI graph
654
+
vector<int> fcs{};
655
+
for (size_t i = 0; i < pef.features_per_cam.size(); i++) {
656
+
int count = pef.features_per_cam.at(i).size();
657
+
t.features.fcs.at(i).addFeatureCount(ppp.timestamp, count);
658
+
fcs.push_back(count);
659
+
}
660
+
661
+
return fcs;
662
+
}
663
+
664
+
/*
665
+
*
487
666
* Ground truth functionality
488
667
*
489
668
*/
···
562
741
static void
563
742
gt_ui_setup(TrackerSlam &t)
564
743
{
744
+
u_var_add_ro_ftext(&t, "\n%s", "Tracker groundtruth");
565
745
t.gt.diff_ui.values.data = t.gt.diffs_mm;
566
746
t.gt.diff_ui.values.length = UI_GTDIFF_POSE_COUNT;
567
747
t.gt.diff_ui.values.index_ptr = &t.gt.diff_idx;
···
638
818
if (t.timing.ext_enabled) {
639
819
auto tss = timing_ui_push(t, np);
640
820
t.slam_times_writer->push(tss);
821
+
}
822
+
823
+
if (t.features.ext_enabled) {
824
+
vector feat_count = features_ui_push(t, np);
825
+
t.slam_features_writer->push(nts, feat_count);
641
826
}
642
827
643
828
dequeued = t.slam->try_dequeue_pose(tracked_pose);
···
797
982
u_var_add_f32(&t, &t.gravity_correction.z, "Gravity Correction");
798
983
799
984
u_var_add_gui_header(&t, NULL, "Stats");
985
+
u_var_add_ro_ftext(&t, "\n%s", "Record to CSV files");
800
986
u_var_add_bool(&t, &t.slam_traj_writer->enabled, "Record tracked trajectory");
801
987
u_var_add_bool(&t, &t.pred_traj_writer->enabled, "Record predicted trajectory");
802
988
u_var_add_bool(&t, &t.filt_traj_writer->enabled, "Record filtered trajectory");
803
989
u_var_add_bool(&t, &t.slam_times_writer->enabled, "Record tracker times");
990
+
u_var_add_bool(&t, &t.slam_features_writer->enabled, "Record feature count");
804
991
timing_ui_setup(t);
992
+
features_ui_setup(t);
805
993
// Later, gt_ui_setup will setup the tracking error UI if ground truth becomes available
806
994
}
807
995
···
1029
1217
os_thread_helper_destroy(&t_ptr->oth);
1030
1218
delete t.gt.trajectory;
1031
1219
delete t.slam_times_writer;
1220
+
delete t.slam_features_writer;
1032
1221
delete t.slam_traj_writer;
1033
1222
delete t.pred_traj_writer;
1034
1223
delete t.filt_traj_writer;
···
1073
1262
config->write_csvs = debug_get_bool_option_slam_write_csvs();
1074
1263
config->csv_path = debug_get_option_slam_csv_path();
1075
1264
config->timing_stat = debug_get_bool_option_slam_timing_stat();
1265
+
config->features_stat = debug_get_bool_option_slam_features_stat();
1076
1266
config->stereo_calib = NULL;
1077
1267
config->imu_calib = NULL;
1078
1268
config->extra_calib = NULL;
···
1182
1372
t.timing.ext_enabled = enable_timing_extension;
1183
1373
}
1184
1374
1375
+
// Setup features extension
1376
+
bool has_features_extension = t.slam->supports_feature(F_ENABLE_POSE_EXT_FEATURES);
1377
+
t.features.ext_available = has_features_extension;
1378
+
if (has_features_extension) {
1379
+
bool enable_features_extension = config->features_stat;
1380
+
1381
+
const auto params = make_shared<FPARAMS_EPET>(enable_features_extension);
1382
+
shared_ptr<void> _;
1383
+
t.slam->use_feature(F_ENABLE_POSE_EXT_FEATURES, params, _);
1384
+
1385
+
t.features.ext_enabled = enable_features_extension;
1386
+
}
1387
+
1185
1388
// Setup CSV files
1186
1389
bool write_csvs = config->write_csvs;
1187
1390
string dir = config->csv_path;
1188
1391
t.slam_times_writer = new TimingWriter{dir, "timing.csv", write_csvs, t.timing.columns};
1392
+
t.slam_features_writer = new FeaturesWriter{dir, "features.csv", write_csvs};
1189
1393
t.slam_traj_writer = new TrajectoryWriter{dir, "tracking.csv", write_csvs};
1190
1394
t.pred_traj_writer = new TrajectoryWriter{dir, "prediction.csv", write_csvs};
1191
1395
t.filt_traj_writer = new TrajectoryWriter{dir, "filtering.csv", write_csvs};
+1
src/xrt/auxiliary/tracking/t_tracking.h
+1
src/xrt/auxiliary/tracking/t_tracking.h
···
470
470
bool write_csvs; //!< Whether to enable CSV writers from the start for later analysis
471
471
const char *csv_path; //!< Path to write CSVs to
472
472
bool timing_stat; //!< Enable timing metric in external system
473
+
bool features_stat; //!< Enable feature metric in external system
473
474
474
475
// Instead of a slam_config file you can set custom calibration data
475
476
const struct t_stereo_camera_calibration *stereo_calib; //!< Camera calibration data