From 1a5af0311117620afd2163672a4c9a1146273941 Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Fri, 17 Sep 2021 09:21:07 -0300 Subject: [PATCH] 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. --- src/xrt/auxiliary/tracking/t_tracker_slam.cpp | 39 ++----------------- src/xrt/drivers/euroc/euroc_device.c | 16 +++++++- 2 files changed, 19 insertions(+), 36 deletions(-) diff --git a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp index f9e624f15..bdc913c6c 100644 --- a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp +++ b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp @@ -186,9 +186,6 @@ t_slam_imu_sink_push(struct xrt_imu_sink *sink, struct xrt_imu_sample *s) * * @todo This function should do pose prediction, currently it is not using @p * when_ns and just returning the latest tracked pose instead. - * - * @todo This function is correcting the returned pose axes and orientation but - * it is yet unclear if that should be done in here at all. */ extern "C" void t_slam_get_tracked_pose(struct xrt_tracked_slam *xts, timepoint_ns when_ns, struct xrt_space_relation *out_relation) @@ -199,33 +196,8 @@ t_slam_get_tracked_pose(struct xrt_tracked_slam *xts, timepoint_ns when_ns, stru if (dequeued) { 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); - // TODO: IMUs, Monado, and Kimera coordinate systems usually differ, so the - // produced pose needs to be corrected. This correction could happen in the - // slam_tracker implementation inside the SLAM system, but here in Monado - // there are tools to facilitate the process, so let's just do it here for - // now. Another possibility could be to do it in the device driver. - -#ifdef XRT_HAVE_KIMERA_SLAM - // TODO: These corrections are specific for a D455 camera; generalize. - - // Correct swapped axes - xrt_pose located_pose{{-p.ry, -p.rz, p.rx, p.rw}, {-p.py, -p.pz, p.px}}; - - // Correct orientation - struct xrt_space_graph space_graph = {}; - xrt_pose pre_correction{xrt_quat{-0.5, -0.5, -0.5, 0.5}, xrt_vec3{0, 0, 0}}; // euler(90, 90, 0) - constexpr float sin45 = 0.7071067811865475; - xrt_pose pos_correction{xrt_quat{sin45, 0, sin45, 0}, xrt_vec3{0, 0, 0}}; // euler(180, 90, 0) - m_space_graph_add_pose(&space_graph, &pre_correction); - m_space_graph_add_pose(&space_graph, &located_pose); - m_space_graph_add_pose(&space_graph, &pos_correction); - m_space_graph_resolve(&space_graph, out_relation); - -#else - // Do not correct, use the returned pose directly + // Note that any pose correction should happen in the device consuming the tracking out_relation->pose = {{p.rx, p.ry, p.rz, p.rw}, {p.px, p.py, p.pz}}; -#endif - out_relation->relation_flags = (enum xrt_space_relation_flags)( XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); @@ -245,7 +217,7 @@ push_frame(const TrackerSlam &t, struct xrt_frame *frame, bool is_left) SLAM_ASSERT_(frame->timestamp < INT64_MAX); img_sample sample{(int64_t)frame->timestamp, img, is_left}; t.slam->push_frame(sample); - SLAM_TRACE("frame t=%lu", frame->timestamp); + SLAM_TRACE("%s frame t=%lu", is_left ? " left" : "right", frame->timestamp); } extern "C" void @@ -288,8 +260,8 @@ extern "C" void * t_slam_run(void *ptr) { auto &t = *(TrackerSlam *)ptr; + SLAM_DEBUG("SLAM tracker starting"); t.slam->start(); - SLAM_DEBUG("SLAM tracker running"); return NULL; } @@ -314,10 +286,7 @@ t_slam_create(struct xrt_frame_context *xfctx, struct xrt_tracked_slam **out_xts t.base.get_tracked_pose = t_slam_get_tracked_pose; const char *config_file = debug_get_option_slam_config(); - if (!config_file) { - SLAM_ERROR("SLAM tracker requires a config file set with the SLAM_CONFIG environment variable"); - return 0; - } + SLAM_ASSERT(config_file, "SLAM tracker requires a config file set with the SLAM_CONFIG environment variable"); std::string config_file_string = std::string(config_file); t.slam = new slam_tracker{config_file_string}; diff --git a/src/xrt/drivers/euroc/euroc_device.c b/src/xrt/drivers/euroc/euroc_device.c index 1721611c7..4c9ff74d3 100644 --- a/src/xrt/drivers/euroc/euroc_device.c +++ b/src/xrt/drivers/euroc/euroc_device.c @@ -16,6 +16,7 @@ #include "math/m_mathinclude.h" #include "xrt/xrt_prober.h" #include "xrt/xrt_tracking.h" +#include "xrt/xrt_config_have.h" #include "euroc_driver.h" @@ -104,6 +105,17 @@ euroc_device_update_inputs(struct xrt_device *xdev) return; } +//! Corrections specific for original euroc datasets and Kimera. +//! If your datasets comes from a different camera you should probably +//! use a different pose correction function. +static inline struct xrt_pose +euroc_device_correct_pose_from_kimera(struct xrt_pose pose) +{ + //! @todo Implement proper pose corrections for the original euroc datasets + //! @todo Allow to use different pose corrections depending on the device used to record + return pose; +} + static void euroc_device_get_tracked_pose(struct xrt_device *xdev, enum xrt_input_name name, @@ -119,7 +131,9 @@ euroc_device_get_tracked_pose(struct xrt_device *xdev, int pose_bits = XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT; bool pose_tracked = out_relation->relation_flags & pose_bits; if (pose_tracked) { - ed->pose = out_relation->pose; +#ifdef XRT_HAVE_KIMERA_SLAM + ed->pose = euroc_device_correct_pose_from_kimera(out_relation->pose); +#endif } }