The open source OpenXR runtime
t/slam: Add configuration options to tracker
Specifically:
- SLAM_PREDICTION_TYPE
- SLAM_CSV_PATH
+22
-4
+22
-4
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
+22
-4
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
···
25
25
#include <opencv2/core/mat.hpp>
26
26
#include <opencv2/core/version.hpp>
27
27
28
+
#include <filesystem>
28
29
#include <fstream>
29
30
#include <iomanip>
30
31
#include <map>
···
58
59
DEBUG_GET_ONCE_LOG_OPTION(slam_log, "SLAM_LOG", U_LOGGING_WARN)
59
60
60
61
//! Config file path, format is specific to the SLAM implementation in use
61
-
DEBUG_GET_ONCE_OPTION(slam_config, "SLAM_CONFIG", NULL)
62
+
DEBUG_GET_OPTION(slam_config, "SLAM_CONFIG", NULL)
62
63
63
64
//! Whether to submit data to the SLAM tracker without user action
64
65
DEBUG_GET_ONCE_BOOL_OPTION(slam_submit_from_start, "SLAM_SUBMIT_FROM_START", false)
65
66
67
+
//! Which level of prediction to use, @see prediction_type.
68
+
DEBUG_GET_ONCE_NUM_OPTION(slam_prediction_type, "SLAM_PREDICTION_TYPE", 2)
69
+
66
70
//! Whether to enable CSV writers from the start for later analysis
67
71
DEBUG_GET_ONCE_BOOL_OPTION(slam_write_csvs, "SLAM_WRITE_CSVS", false)
72
+
73
+
//! Path to write CSVs to
74
+
DEBUG_GET_OPTION(slam_csv_path, "SLAM_CSV_PATH", "evaluation/")
68
75
69
76
//! Namespace for the interface to the external SLAM tracking system
70
77
namespace xrt::auxiliary::tracking::slam {
···
79
86
using std::string;
80
87
using std::unique_ptr;
81
88
using std::vector;
89
+
using std::filesystem::create_directories;
82
90
using Trajectory = map<timepoint_ns, xrt_pose>;
83
91
84
92
using xrt::auxiliary::math::RelationHistory;
···
192
200
void
193
201
create()
194
202
{
195
-
file = ofstream{filename};
203
+
string csv_path = debug_get_option_slam_csv_path();
204
+
create_directories(csv_path);
205
+
file = ofstream{csv_path + "/" + filename};
196
206
file << "#timestamp [ns], p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], "
197
207
"q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z []" CSV_EOL;
198
208
file << std::fixed << std::setprecision(CSV_PRECISION);
···
237
247
void
238
248
create()
239
249
{
240
-
file = ofstream{filename};
250
+
string csv_path = debug_get_option_slam_csv_path();
251
+
create_directories(csv_path);
252
+
file = ofstream{csv_path + "/" + filename};
241
253
file << "#";
242
254
for (const string &col : column_names) {
243
255
string delimiter = &col != &column_names.back() ? "," : CSV_EOL;
···
317
329
// Prediction
318
330
319
331
//!< Type of prediction to use
320
-
prediction_type pred_type = PREDICTION_SP_SO_IA_SL;
332
+
prediction_type pred_type;
321
333
u_var_combo pred_combo; //!< UI combo box to select @ref pred_type
322
334
RelationHistory slam_rels{}; //!< A history of relations produced purely from external SLAM tracker data
323
335
struct m_ff_vec3_f32 *gyro_ff; //!< Last gyroscope samples
···
800
812
{
801
813
auto &t = *container_of(xts, TrackerSlam, base);
802
814
815
+
//! @todo This should not be cached, the same timestamp can be requested at a
816
+
//! later time on the frame for a better prediction.
803
817
if (when_ns == t.last_ts) {
804
818
*out_relation = t.last_rel;
805
819
return;
···
871
885
static void
872
886
push_frame(TrackerSlam &t, struct xrt_frame *frame, bool is_left)
873
887
{
888
+
SLAM_DASSERT(t.last_left_ts != INT64_MIN || is_left, "First frame was a right frame");
889
+
874
890
// Construct and send the image sample
875
891
cv::Mat img = t.cv_wrapper->wrap(frame);
876
892
SLAM_DASSERT_(frame->timestamp < INT64_MAX);
···
1020
1036
xrt_frame_context_add(xfctx, &t.node);
1021
1037
1022
1038
t.euroc_recorder = euroc_recorder_create(xfctx, NULL);
1039
+
1040
+
t.pred_type = (prediction_type)debug_get_num_option_slam_prediction_type();
1023
1041
1024
1042
m_filter_euro_vec3_init(&t.filter.pos_oe, t.filter.min_cutoff, t.filter.beta, t.filter.min_dcutoff);
1025
1043
m_filter_euro_quat_init(&t.filter.rot_oe, t.filter.min_cutoff, t.filter.beta, t.filter.min_dcutoff);