The open source OpenXR runtime
t/slam: Move pose corrections into the tracked devices
This discards the pose correction logic used that was for the D455 not for
an euroc dataset. It will be reintroduced in a later commit with an
appropriate device.
authored by
Mateo de Mayo
and committed by
Jakob Bornecrantz
1a5af031
1c99e8ed
+19
-36
+4
-35
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
+4
-35
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
···
186
186
*
187
187
* @todo This function should do pose prediction, currently it is not using @p
188
188
* when_ns and just returning the latest tracked pose instead.
189
-
*
190
-
* @todo This function is correcting the returned pose axes and orientation but
191
-
* it is yet unclear if that should be done in here at all.
192
189
*/
193
190
extern "C" void
194
191
t_slam_get_tracked_pose(struct xrt_tracked_slam *xts, timepoint_ns when_ns, struct xrt_space_relation *out_relation)
···
199
196
if (dequeued) {
200
197
SLAM_TRACE("pose p=[%f,%f,%f] r=[%f,%f,%f,%f]", p.px, p.py, p.pz, p.rx, p.ry, p.rz, p.rw);
201
198
202
-
// TODO: IMUs, Monado, and Kimera coordinate systems usually differ, so the
203
-
// produced pose needs to be corrected. This correction could happen in the
204
-
// slam_tracker implementation inside the SLAM system, but here in Monado
205
-
// there are tools to facilitate the process, so let's just do it here for
206
-
// now. Another possibility could be to do it in the device driver.
207
-
208
-
#ifdef XRT_HAVE_KIMERA_SLAM
209
-
// TODO: These corrections are specific for a D455 camera; generalize.
210
-
211
-
// Correct swapped axes
212
-
xrt_pose located_pose{{-p.ry, -p.rz, p.rx, p.rw}, {-p.py, -p.pz, p.px}};
213
-
214
-
// Correct orientation
215
-
struct xrt_space_graph space_graph = {};
216
-
xrt_pose pre_correction{xrt_quat{-0.5, -0.5, -0.5, 0.5}, xrt_vec3{0, 0, 0}}; // euler(90, 90, 0)
217
-
constexpr float sin45 = 0.7071067811865475;
218
-
xrt_pose pos_correction{xrt_quat{sin45, 0, sin45, 0}, xrt_vec3{0, 0, 0}}; // euler(180, 90, 0)
219
-
m_space_graph_add_pose(&space_graph, &pre_correction);
220
-
m_space_graph_add_pose(&space_graph, &located_pose);
221
-
m_space_graph_add_pose(&space_graph, &pos_correction);
222
-
m_space_graph_resolve(&space_graph, out_relation);
223
-
224
-
#else
225
-
// Do not correct, use the returned pose directly
199
+
// Note that any pose correction should happen in the device consuming the tracking
226
200
out_relation->pose = {{p.rx, p.ry, p.rz, p.rw}, {p.px, p.py, p.pz}};
227
-
#endif
228
-
229
201
out_relation->relation_flags = (enum xrt_space_relation_flags)(
230
202
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT |
231
203
XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
···
245
217
SLAM_ASSERT_(frame->timestamp < INT64_MAX);
246
218
img_sample sample{(int64_t)frame->timestamp, img, is_left};
247
219
t.slam->push_frame(sample);
248
-
SLAM_TRACE("frame t=%lu", frame->timestamp);
220
+
SLAM_TRACE("%s frame t=%lu", is_left ? " left" : "right", frame->timestamp);
249
221
}
250
222
251
223
extern "C" void
···
288
260
t_slam_run(void *ptr)
289
261
{
290
262
auto &t = *(TrackerSlam *)ptr;
263
+
SLAM_DEBUG("SLAM tracker starting");
291
264
t.slam->start();
292
-
SLAM_DEBUG("SLAM tracker running");
293
265
return NULL;
294
266
}
295
267
···
314
286
t.base.get_tracked_pose = t_slam_get_tracked_pose;
315
287
316
288
const char *config_file = debug_get_option_slam_config();
317
-
if (!config_file) {
318
-
SLAM_ERROR("SLAM tracker requires a config file set with the SLAM_CONFIG environment variable");
319
-
return 0;
320
-
}
289
+
SLAM_ASSERT(config_file, "SLAM tracker requires a config file set with the SLAM_CONFIG environment variable");
321
290
322
291
std::string config_file_string = std::string(config_file);
323
292
t.slam = new slam_tracker{config_file_string};
+15
-1
src/xrt/drivers/euroc/euroc_device.c
+15
-1
src/xrt/drivers/euroc/euroc_device.c
···
16
16
#include "math/m_mathinclude.h"
17
17
#include "xrt/xrt_prober.h"
18
18
#include "xrt/xrt_tracking.h"
19
+
#include "xrt/xrt_config_have.h"
19
20
20
21
#include "euroc_driver.h"
21
22
···
104
105
return;
105
106
}
106
107
108
+
//! Corrections specific for original euroc datasets and Kimera.
109
+
//! If your datasets comes from a different camera you should probably
110
+
//! use a different pose correction function.
111
+
static inline struct xrt_pose
112
+
euroc_device_correct_pose_from_kimera(struct xrt_pose pose)
113
+
{
114
+
//! @todo Implement proper pose corrections for the original euroc datasets
115
+
//! @todo Allow to use different pose corrections depending on the device used to record
116
+
return pose;
117
+
}
118
+
107
119
static void
108
120
euroc_device_get_tracked_pose(struct xrt_device *xdev,
109
121
enum xrt_input_name name,
···
119
131
int pose_bits = XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT;
120
132
bool pose_tracked = out_relation->relation_flags & pose_bits;
121
133
if (pose_tracked) {
122
-
ed->pose = out_relation->pose;
134
+
#ifdef XRT_HAVE_KIMERA_SLAM
135
+
ed->pose = euroc_device_correct_pose_from_kimera(out_relation->pose);
136
+
#endif
123
137
}
124
138
}
125
139