The open source OpenXR runtime
external/slam: Update slam_tracker interface to support dynamic features
+148
-30
+145
-30
src/external/slam_tracker/slam_tracker.hpp
+145
-30
src/external/slam_tracker/slam_tracker.hpp
···
6
6
* @author Mateo de Mayo <mateo.demayo@collabora.com>
7
7
* @ingroup aux_tracking
8
8
*
9
-
* This header file contains the declaration of the @ref slam_tracker class to
10
-
* implement a SLAM system for usage in Monado.
11
-
*
12
-
* A copy of this file is present in both Monado and any SLAM system that
13
-
* intends to be used by Monado. The SLAM system should provide the
14
-
* `slam_tracker` implementation so that Monado can use it.
15
-
*
16
-
* This file also declares additional data types to be used between Monado and
9
+
* This file contains the declaration of the @ref slam_tracker class. This
10
+
* header is intended to appear in both Monado and an external SLAM system. The
11
+
* implementation of `slam_tracker` is provided by the external system.
12
+
* Additional data types are declared for the communication between Monado and
17
13
* the system.
18
14
*
19
-
* @todo The interface is preliminary and should be improved to avoid
20
-
* unnecessary copies.
21
15
*/
22
16
23
17
#pragma once
24
18
25
19
#include <opencv2/core/mat.hpp>
20
+
21
+
#include <cstdint>
22
+
#include <memory>
23
+
#include <string>
24
+
#include <vector>
26
25
27
26
namespace xrt::auxiliary::tracking::slam {
28
27
···
41
40
* @brief IMU Sample type to pass around between programs
42
41
*/
43
42
struct imu_sample {
44
-
std::int64_t timestamp; // In nanoseconds
45
-
double ax, ay, az; // In meters per second squared (m / s^2)
46
-
double wx, wy, wz; // In radians per second (rad / s)
43
+
std::int64_t timestamp; //!< In nanoseconds
44
+
double ax, ay, az; //!< Accel in meters per second squared (m / s^2)
45
+
double wx, wy, wz; //!< Gyro in radians per second (rad / s)
47
46
imu_sample() = default;
48
47
imu_sample(std::int64_t timestamp, double ax, double ay, double az, double wx,
49
48
double wy, double wz)
···
59
58
cv::Mat img;
60
59
bool is_left;
61
60
img_sample() = default;
62
-
img_sample(std::int64_t timestamp, cv::Mat img, bool is_left)
61
+
img_sample(std::int64_t timestamp, const cv::Mat &img, bool is_left)
63
62
: timestamp(timestamp), img(img), is_left(is_left) {}
64
63
};
65
64
···
79
78
* them up. Therefore, this constructor receives a path to a
80
79
* implementation-specific configuration file.
81
80
*/
82
-
slam_tracker(std::string config_file);
81
+
slam_tracker(const std::string &config_file);
83
82
~slam_tracker();
84
83
85
84
slam_tracker(const slam_tracker &) = delete;
86
85
slam_tracker &operator=(const slam_tracker &) = delete;
87
86
87
+
void initialize();
88
88
void start();
89
-
void stop();
90
89
bool is_running();
90
+
void stop();
91
+
void finalize();
91
92
92
-
//! There must be a single producer thread pushing samples.
93
-
//! Samples must have monotonically increasing timestamps.
94
-
//! The implementation must be non-blocking.
95
-
//! A separate consumer thread should process the samples.
96
-
void push_imu_sample(imu_sample sample);
93
+
/*!
94
+
* @brief Push an IMU sample into the tracker.
95
+
*
96
+
* There must be a single producer thread pushing samples.
97
+
* Samples must have monotonically increasing timestamps.
98
+
* The implementation must be non-blocking.
99
+
* Thus, a separate consumer thread should process the samples.
100
+
*/
101
+
void push_imu_sample(const imu_sample &sample);
97
102
98
-
//! Same conditions as `push_imu_sample` apply.
99
-
//! When using stereo frames, they must be pushed in a left-right order.
100
-
//! The consecutive left-right pair must have the same timestamps.
101
-
void push_frame(img_sample sample);
103
+
/*!
104
+
* @brief Push an image sample into the tracker.
105
+
*
106
+
* Same conditions as @ref push_imu_sample apply.
107
+
* When using stereo frames, they must be pushed in a left-right order.
108
+
* The consecutive left-right pair must have the same timestamps.
109
+
*/
110
+
void push_frame(const img_sample &sample);
102
111
103
-
//! There must be a single thread accessing the tracked pose.
104
-
bool try_dequeue_pose(pose &pose);
112
+
/*!
113
+
* @brief Get the latest tracked pose from the SLAM system.
114
+
*
115
+
* There must be a single thread consuming this method.
116
+
*
117
+
* @param[out] out_pose Dequeued pose.
118
+
* @return true If a new pose was dequeued into @p out_pose.
119
+
* @return false If there was no pose to dequeue.
120
+
*/
121
+
bool try_dequeue_pose(pose &out_pose);
122
+
123
+
//! Asks the SLAM system whether it supports a specific feature.
124
+
bool supports_feature(int feature_id);
125
+
126
+
/*!
127
+
* @brief Use a special feature of the SLAM tracker.
128
+
*
129
+
* This method uses heap allocated objects for passing parameters and
130
+
* obtaining the results. Use `std::static_pointer_cast` to shared pointers to
131
+
* the expected types.
132
+
*
133
+
* @param feature_id Id of the special feature.
134
+
* @param params Pointer to the parameter object for this feature.
135
+
* @param result Pointer to the result produced by the feature call.
136
+
* @return false if the feature was not supported, true otherwise.
137
+
*/
138
+
bool use_feature(int feature_id, const std::shared_ptr<void> ¶ms,
139
+
std::shared_ptr<void> &result);
105
140
106
-
private:
141
+
private:
107
142
struct implementation;
108
-
implementation *impl;
143
+
std::unique_ptr<implementation> impl;
144
+
};
145
+
146
+
/*
147
+
* Special features
148
+
*
149
+
* A special feature is comprised of an ID, a PARAMS type and a RESULT type. It
150
+
* can be defined using DEFINE_FEATURE. Once defined, the definition should not
151
+
* suffer future changes.
152
+
*
153
+
* One of the main concerns in the features interface is the ability to add new
154
+
* features without being required to update the SLAM systems that are not
155
+
* interested in implementing the feature.
156
+
*
157
+
*/
158
+
159
+
#define DEFINE_FEATURE(NAME, SHORT_NAME, ID, PARAMS_TYPE, RESULT_TYPE) \
160
+
using FPARAMS_##SHORT_NAME = PARAMS_TYPE; \
161
+
using FRESULT_##SHORT_NAME = RESULT_TYPE; \
162
+
constexpr int FID_##SHORT_NAME = ID; \
163
+
constexpr int F_##NAME = ID;
164
+
165
+
struct cam_calibration {
166
+
enum class cam_model { pinhole, fisheye };
167
+
168
+
int cam_index; //!< For multi-camera setups. For stereo 0 ~ left, 1 ~ right.
169
+
int width, height; //<! Resolution
170
+
double frequency; //<! Frames per second
171
+
double fx, fy; //<! Focal point
172
+
double cx, cy; //<! Principal point
173
+
cam_model model;
174
+
std::vector<double> model_params;
175
+
cv::Matx<double, 4, 4> T_cam_imu; //!< Transformation from camera to imu space
176
+
};
177
+
178
+
struct inertial_calibration {
179
+
// Calibration intrinsics to apply to each raw measurement.
180
+
181
+
//! This transform will be applied to raw measurements.
182
+
cv::Matx<double, 3, 3> transform;
183
+
184
+
//! Offset to apply to raw measurements to; called bias in other contexts.
185
+
cv::Matx<double, 3, 1> offset;
186
+
187
+
// Parameters for the random processes that model this IMU. See section "2.1
188
+
// Gyro Noise Model" of N. Trawny and S. I. Roumeliotis, "Indirect Kalman
189
+
// Filter for 3D Attitude Estimation". Analogous for accelerometers.
190
+
// http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf#page=15
191
+
192
+
//! IMU internal bias ~ wiener process with steps N(0, σ²); this field is σ;
193
+
//! [σ] = U / sqrt(sec³) with U = rad if gyroscope, U = m/s if accelerometer.
194
+
cv::Matx<double, 3, 1> bias_std;
195
+
196
+
//! IMU measurement noise ~ N(0, σ²); this field is σ.
197
+
//! [σ] = U / sqrt(sec) with U = rad if gyroscope, U = m/s if accelerometer.
198
+
cv::Matx<double, 3, 1> noise_std;
199
+
200
+
inertial_calibration() : transform(cv::Matx<double, 3, 3>::eye()) {}
201
+
};
202
+
203
+
struct imu_calibration {
204
+
int imu_index; //!< For multi-imu setups. Usually just 0.
205
+
double frequency; //!< Samples per second
206
+
inertial_calibration accel;
207
+
inertial_calibration gyro;
109
208
};
110
209
111
-
} // namespace xrt::auxiliary::tracking::slam
210
+
/*!
211
+
* Feature ADD_CAMERA_CALIBRATION
212
+
*
213
+
* Use it after constructor but before `start()` to write or overwrite camera
214
+
* calibration data that might come from the system-specific config file.
215
+
*/
216
+
DEFINE_FEATURE(ADD_CAMERA_CALIBRATION, ACC, 1, cam_calibration, void)
217
+
218
+
/*!
219
+
* Feature ADD_IMU_CALIBRATION
220
+
*
221
+
* Use it after constructor but before `start()` to write or overwrite IMU
222
+
* calibration data that might come from the system-specific config file.
223
+
*/
224
+
DEFINE_FEATURE(ADD_IMU_CALIBRATION, AIC, 2, imu_calibration, void)
225
+
226
+
} // namespace xrt::auxiliary::tracking::slam
+3
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
+3
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
···
265
265
t_slam_node_break_apart(struct xrt_frame_node *node)
266
266
{
267
267
auto &t = *container_of(node, TrackerSlam, node);
268
+
t.slam->finalize();
268
269
t.slam->stop();
269
270
os_thread_helper_stop(&t.oth);
270
271
SLAM_DEBUG("SLAM tracker dismantled");
···
322
323
323
324
std::string config_file_string = std::string(config_file);
324
325
t.slam = new slam_tracker{config_file_string};
326
+
327
+
t.slam->initialize();
325
328
326
329
t.left_sink.push_frame = t_slam_frame_sink_push_left;
327
330
t.right_sink.push_frame = t_slam_frame_sink_push_right;