diff --git a/CMakeLists.txt b/CMakeLists.txt index 3a62d4f..c192193 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -276,6 +276,10 @@ message(STATUS "Found {fmt} ${fmt_VERSION} in: ${fmt_DIR}") add_subdirectory(thirdparty) +# custom scoped monado target +add_library(basalt::monado INTERFACE IMPORTED) +set_property(TARGET basalt::monado PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/monado) + # custom scoped cli11 target add_library(basalt::cli11 INTERFACE IMPORTED) set_property(TARGET basalt::cli11 PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/CLI11/include) @@ -361,6 +365,7 @@ target_sources(basalt ${CMAKE_CURRENT_SOURCE_DIR}/src/linearization/linearization_abs_sc.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/linearization/linearization_base.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/linearization/linearization_rel_sc.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/monado/slam_tracker.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/optical_flow/optical_flow.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/utils/keypoints.cpp ${CMAKE_CURRENT_SOURCE_DIR}/src/utils/system_utils.cpp @@ -378,8 +383,8 @@ target_sources(basalt ) target_link_libraries(basalt - PUBLIC ${STD_CXX_FS} basalt::opencv basalt::basalt-headers TBB::tbb - PRIVATE basalt::magic_enum rosbag apriltag opengv nlohmann::json fmt::fmt) + PUBLIC ${STD_CXX_FS} basalt::opencv basalt::basalt-headers TBB::tbb pangolin basalt::cli11 + PRIVATE basalt::magic_enum rosbag apriltag opengv nlohmann::json fmt::fmt basalt::monado) target_include_directories(basalt PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) target_compile_definitions(basalt PUBLIC ${BASALT_COMPILE_DEFINITIONS}) #target_compile_definitions(basalt PUBLIC BASALT_DISABLE_ASSERTS) diff --git a/data/monado/euroc.toml b/data/monado/euroc.toml new file mode 100644 index 0000000..65e141c --- /dev/null +++ b/data/monado/euroc.toml @@ -0,0 +1,19 @@ +# Options for the Basalt SLAM Tracker + +# Show GUI +show-gui=1 + +# Ground-truth camera calibration used for simulation. +cam-calib="/home/mateo/Documents/apps/bsltdeps/basalt/data/euroc_ds_calib.json" + +# Path to config file. +config-path="/home/mateo/Documents/apps/bsltdeps/basalt/data/euroc_config.json" + +# Path to folder where marginalization data will be stored. +marg-data="" + +# Poll and print for queue sizes. +print-queue=0 + +# Whether to use a double or single precision pipeline. +use-double=0 diff --git a/data/monado/tumvi.toml b/data/monado/tumvi.toml new file mode 100644 index 0000000..060888d --- /dev/null +++ b/data/monado/tumvi.toml @@ -0,0 +1,19 @@ +# Options for the Basalt SLAM Tracker + +# Show GUI +show-gui=1 + +# Ground-truth camera calibration used for simulation. +cam-calib="/home/mateo/Documents/apps/bsltdeps/basalt/data/tumvi_512_ds_calib.json" + +# Path to config file. +config-path="/home/mateo/Documents/apps/bsltdeps/basalt/data/tumvi_512_config.json" + +# Path to folder where marginalization data will be stored. +marg-data="" + +# Poll and print for queue sizes. +print-queue=0 + +# Whether to use a double or single precision pipeline. +use-double=0 diff --git a/src/monado/.clang-format b/src/monado/.clang-format new file mode 100644 index 0000000..075c4b2 --- /dev/null +++ b/src/monado/.clang-format @@ -0,0 +1,7 @@ +--- +Language: Cpp +BasedOnStyle: Google +IndentWidth: 2 +IncludeBlocks: Preserve +ColumnLimit: 120 +... diff --git a/src/monado/.clang-tidy b/src/monado/.clang-tidy new file mode 100644 index 0000000..a08c66c --- /dev/null +++ b/src/monado/.clang-tidy @@ -0,0 +1,8 @@ +Checks: 'readability-*, -readability-magic-numbers, -readability-function-cognitive-complexity, -readability-else-after-return, -readability-redundant-access-specifiers, performance-*, modernize-*, -modernize-use-trailing-return-type, -modernize-avoid-c-arrays, -modernize-use-nodiscard, -modernize-use-auto, -modernize-pass-by-value, misc-assert-side-effect, -clang-analyzer-osx.*, -clang-analyzer-cplusplus.Move, -clang-analyzer-core.uninitialized.UndefReturn, -clang-analyzer-optin.portability.UnixAPI, -clang-analyzer-unix.Malloc' +HeaderFilterRegex: '.*/thirdparty/basalt-headers/include/basalt/*' + +CheckOptions: + - key: readability-identifier-naming + value: lower_case + - key: readability-braces-around-statements.ShortStatementLines + value: 2 diff --git a/src/monado/slam_tracker.cpp b/src/monado/slam_tracker.cpp new file mode 100644 index 0000000..8a1bf4a --- /dev/null +++ b/src/monado/slam_tracker.cpp @@ -0,0 +1,499 @@ +// Copyright 2022, Collabora, Ltd. + +#include "slam_tracker.hpp" +#include "slam_tracker_ui.hpp" + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include "sophus/se3.hpp" + +#include +#include +#include +#include "basalt/utils/vis_utils.h" + +namespace xrt::auxiliary::tracking::slam { + +const int IMPLEMENTATION_VERSION_MAJOR = HEADER_VERSION_MAJOR; +const int IMPLEMENTATION_VERSION_MINOR = HEADER_VERSION_MINOR; +const int IMPLEMENTATION_VERSION_PATCH = HEADER_VERSION_PATCH; + +using std::cout; +using std::make_shared; +using std::make_unique; +using std::pair; +using std::shared_ptr; +using std::static_pointer_cast; +using std::string; +using std::thread; +using std::to_string; +using std::unordered_set; +using std::vector; +using namespace basalt; + +string imu2str(const imu_sample &s) { + string str = "imu_sample "; + str += "t=" + to_string(s.timestamp) + " "; + str += "a=[" + to_string(s.ax) + ", " + to_string(s.ay) + ", " + to_string(s.az) + "] "; + str += "w=[" + to_string(s.wx) + ", " + to_string(s.wy) + ", " + to_string(s.wz) + "]"; + return str; +} + +string img2str(const img_sample &s) { + string str = "img_sample "; + str += s.is_left ? "left " : "right "; + str += "t=" + to_string(s.timestamp); + return str; +} + +string pose2str(const pose &p) { + string str = "pose "; + str += "p=[" + to_string(p.px) + ", " + to_string(p.py) + ", " + to_string(p.pz) + "] "; + str += "r=[" + to_string(p.rx) + ", " + to_string(p.ry) + ", " + to_string(p.rz) + ", " + to_string(p.rw) + "]"; + return str; +} + +struct slam_tracker::implementation { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + // Options parsed from unified config file + bool show_gui = true; + string cam_calib_path; + string config_path; + string marg_data_path; + bool print_queue = false; + bool use_double = false; + + // Basalt in its current state does not support monocular cameras, although it + // should be possible to adapt it to do so, see: + // https://gitlab.com/VladyslavUsenko/basalt/-/issues/2#note_201965760 + // https://gitlab.com/VladyslavUsenko/basalt/-/issues/25#note_362741510 + // https://github.com/DLR-RM/granite + static constexpr int NUM_CAMS = 2; + + // VIO members + Calibration calib; + VioConfig vio_config; + OpticalFlowBase::Ptr opt_flow_ptr; + VioEstimatorBase::Ptr vio; + bool expecting_left_frame = true; + + // Queues + std::atomic running = false; + tbb::concurrent_bounded_queue::Ptr> out_state_queue; + tbb::concurrent_bounded_queue::Ptr> monado_out_state_queue; + tbb::concurrent_bounded_queue *image_data_queue = nullptr; // Invariant: not null after ctor + tbb::concurrent_bounded_queue::Ptr> *imu_data_queue = nullptr; // Invariant: not null after ctor + + // Threads + thread state_consumer_thread; + thread queues_printer_thread; + + // External Queues + slam_tracker_ui ui{}; + MargDataSaver::Ptr marg_data_saver; + + // slam_tracker features + unordered_set supported_features{F_ADD_CAMERA_CALIBRATION, F_ADD_IMU_CALIBRATION, F_ENABLE_POSE_EXT_TIMING}; + + // Additional calibration data + vector added_cam_calibs{}; + vector added_imu_calibs{}; + + // Pose timing + bool pose_timing_enabled = false; + + public: + implementation(const string &unified_config) { + load_unified_config(unified_config); + + vio_config.load(config_path); + load_calibration_data(cam_calib_path); + } + + private: + void load_unified_config(const string &unified_config) { + CLI::App app{"Options for the Basalt SLAM Tracker"}; + + app.add_option("--show-gui", show_gui, "Show GUI"); + app.add_option("--cam-calib", cam_calib_path, "Ground-truth camera calibration used for simulation.")->required(); + app.add_option("--config-path", config_path, "Path to config file.")->required(); + app.add_option("--marg-data", marg_data_path, "Path to folder where marginalization data will be stored."); + app.add_option("--print-queue", print_queue, "Poll and print for queue sizes."); + app.add_option("--use-double", use_double, "Whether to use a double or single precision pipeline."); + + try { + // While --config-path sets the VIO configuration, --config sets the + // entire unified Basalt configuration, including --config-path + app.set_config("--config", unified_config, "Configuration file.", true); + app.allow_config_extras(false); // Makes parsing fail on unknown options + string unique_config = "--config=" + unified_config; + app.parse(unique_config); + } catch (const CLI::ParseError &e) { + app.exit(e); + ASSERT(false, "Config file error (%s)", unified_config.c_str()); + } + + cout << "Instantiating Basalt SLAM tracker\n"; + cout << "Using config file: " << app["--config"]->as() << "\n"; + cout << app.config_to_str(true, true) << "\n"; + } + + void load_calibration_data(const string &calib_path) { + std::ifstream os(calib_path, std::ios::binary); + if (os.is_open()) { + cereal::JSONInputArchive archive(os); + archive(calib); + cout << "Loaded camera with " << calib.intrinsics.size() << " cameras\n"; + } else { + std::cerr << "could not load camera calibration " << calib_path << "\n"; + std::abort(); + } + } + + pose get_pose_from_state(const PoseVelBiasState::Ptr &state) const { + pose p; + Sophus::SE3d T_w_i = state->T_w_i; + p.px = T_w_i.translation().x(); + p.py = T_w_i.translation().y(); + p.pz = T_w_i.translation().z(); + p.rx = T_w_i.unit_quaternion().x(); + p.ry = T_w_i.unit_quaternion().y(); + p.rz = T_w_i.unit_quaternion().z(); + p.rw = T_w_i.unit_quaternion().w(); + p.timestamp = state->t_ns; + p.next = nullptr; + + if (pose_timing_enabled) { + auto pose_timing = make_shared(); + pose_timing->timestamps = state->input_images->tss; + p.next = pose_timing; + } + + return p; + } + + void state_consumer() { + PoseVelBiasState::Ptr data; + + while (true) { + out_state_queue.pop(data); + if (data.get() == nullptr) { + monado_out_state_queue.push(nullptr); + break; + } + data->input_images->addTime("tracker_consumer_received"); + + if (show_gui) ui.log_vio_data(data); + + data->input_images->addTime("tracker_consumer_pushed"); + monado_out_state_queue.push(data); + } + + cout << "Finished state_consumer\n"; + } + + void queues_printer() { + while (running) { + cout << "[in] frames: " << image_data_queue->size() << "/" << image_data_queue->capacity() << " \t" + << "[in] imu: " << imu_data_queue->size() << "/" << imu_data_queue->capacity() << " \t" + << "[mid] keypoints: " << opt_flow_ptr->output_queue->size() << "/" << opt_flow_ptr->output_queue->capacity() + << " \t" + << "[out] pose: " << out_state_queue.size() << "/" << out_state_queue.capacity() << "\n"; + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + cout << "Finished queues_printer\n"; + } + + public: + void initialize() { + // Overwrite camera calibration data + for (const auto &c : added_cam_calibs) { + apply_cam_calibration(c); + } + + // Overwrite IMU calibration data + for (const auto &c : added_imu_calibs) { + apply_imu_calibration(c); + } + + // NOTE: This factory also starts the optical flow + opt_flow_ptr = OpticalFlowFactory::getOpticalFlow(vio_config, calib); + image_data_queue = &opt_flow_ptr->input_queue; + ASSERT_(image_data_queue != nullptr); + + vio = VioEstimatorFactory::getVioEstimator(vio_config, calib, constants::g, true, use_double); + imu_data_queue = &vio->imu_data_queue; + ASSERT_(imu_data_queue != nullptr); + + opt_flow_ptr->output_queue = &vio->vision_data_queue; + if (show_gui) { + ui.initialize(NUM_CAMS); + vio->out_vis_queue = &ui.out_vis_queue; + }; + vio->out_state_queue = &out_state_queue; + + if (!marg_data_path.empty()) { + marg_data_saver.reset(new MargDataSaver(marg_data_path)); + vio->out_marg_queue = &marg_data_saver->in_marg_queue; + } + } + + void start() { + running = true; + vio->initialize(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); + + if (show_gui) ui.start(vio->getT_w_i_init(), calib); + state_consumer_thread = thread(&slam_tracker::implementation::state_consumer, this); + if (print_queue) queues_printer_thread = thread(&slam_tracker::implementation::queues_printer, this); + } + + void stop() { + running = false; + image_data_queue->push(nullptr); + imu_data_queue->push(nullptr); + + if (print_queue) queues_printer_thread.join(); + state_consumer_thread.join(); + if (show_gui) ui.stop(); + + // TODO: There is a segfault when closing monado without starting the stream + // happens in a lambda from keypoint_vio.cpp and ends at line calib_bias.hpp:112 + } + + void finalize() { + // Only the OpticalFlow gets started by initialize, finish it with this + image_data_queue->push(nullptr); + } + + bool is_running() { return running; } + + void push_imu_sample(const imu_sample &s) { + // concurrent_bounded_queue expects Erasable and Allocator named + // requirements for the type, using a pointer because it already is. This is + // done in the others examples as well but it is far from optimal. + ImuData::Ptr data; + data.reset(new ImuData); + data->t_ns = s.timestamp; + data->accel = {s.ax, s.ay, s.az}; + data->gyro = {s.wx, s.wy, s.wz}; + imu_data_queue->push(data); + } + + private: + OpticalFlowInput::Ptr partial_frame; + + public: + void push_frame(const img_sample &s) { + ASSERT(expecting_left_frame == s.is_left, "Unexpected %s frame", s.is_left ? "left" : "right"); + expecting_left_frame = !expecting_left_frame; + + int i = -1; + if (s.is_left) { + partial_frame = make_shared(); + partial_frame->timing_enabled = pose_timing_enabled; + partial_frame->addTime("frame_ts", s.timestamp); + partial_frame->addTime("tracker_received"); + partial_frame->img_data.resize(NUM_CAMS); + partial_frame->t_ns = s.timestamp; + i = 0; + } else { + ASSERT(partial_frame->t_ns == s.timestamp, "Left and right frame timestamps differ: %ld != %ld", + partial_frame->t_ns, s.timestamp); + i = 1; + } + + int width = s.img.cols; + int height = s.img.rows; + // Forced to use uint16_t here, in place because of cameras with 12-bit grayscale support + auto &mimg = partial_frame->img_data[i].img; + mimg.reset(new ManagedImage(width, height)); + + // TODO: We could avoid this copy. Maybe by writing a custom + // allocator for ManagedImage that ties the OpenCV allocator + size_t full_size = width * height; + for (size_t j = 0; j < full_size; j++) { + mimg->ptr[j] = s.img.at(j) << 8; + } + + if (!s.is_left) { + partial_frame->addTime("tracker_pushed"); + image_data_queue->push(partial_frame); + if (show_gui) ui.update_last_image(partial_frame); + } + } + + bool try_dequeue_pose(pose &p) { + PoseVelBiasState::Ptr state; + bool dequeued = monado_out_state_queue.try_pop(state); + if (dequeued) { + state->input_images->addTime("monado_dequeued"); + p = get_pose_from_state(state); + } + return dequeued; + } + + bool supports_feature(int feature_id) { return supported_features.count(feature_id) == 1; } + + bool use_feature(int feature_id, const shared_ptr ¶ms, shared_ptr &result) { + result = nullptr; + if (feature_id == FID_ACC) { + shared_ptr casted_params = static_pointer_cast(params); + add_cam_calibration(*casted_params); + } else if (feature_id == FID_AIC) { + shared_ptr casted_params = static_pointer_cast(params); + add_imu_calibration(*casted_params); + } else if (feature_id == FID_EPET) { + result = enable_pose_ext_timing(); + } else { + return false; + } + return true; + } + + void add_cam_calibration(const cam_calibration &cam_calib) { added_cam_calibs.push_back(cam_calib); } + + void apply_cam_calibration(const cam_calibration &cam_calib) { + using Scalar = double; + int i = cam_calib.cam_index; + + const auto &tci = cam_calib.T_cam_imu; + Eigen::Matrix3d rci; + rci << tci(0, 0), tci(0, 1), tci(0, 2), tci(1, 0), tci(1, 1), tci(1, 2), tci(2, 0), tci(2, 1), tci(2, 2); + Eigen::Quaterniond q(rci); + Eigen::Vector3d p{tci(0, 3), tci(1, 3), tci(2, 3)}; + calib.T_i_c[i] = Calibration::SE3(q, p); + + GenericCamera model; + const vector &cmp = cam_calib.model_params; + if (cam_calib.model == cam_calibration::cam_model::pinhole) { + PinholeCamera::VecN mp; + mp << cam_calib.fx, cam_calib.fy, cam_calib.cx, cam_calib.cy; + PinholeCamera pinhole(mp); + model.variant = pinhole; + } else if (cam_calib.model == cam_calibration::cam_model::fisheye) { + KannalaBrandtCamera4::VecN mp; + mp << cam_calib.fx, cam_calib.fy, cam_calib.cx, cam_calib.cy, cmp[0], cmp[1], cmp[2], cmp[3]; + KannalaBrandtCamera4 kannala_brandt(mp); + model.variant = kannala_brandt; + } else { + ASSERT(false, "Unsupported camera model (%d)", static_cast(cam_calib.model)); + } + calib.intrinsics[i] = model; + + calib.resolution[i] = {cam_calib.width, cam_calib.height}; + + // NOTE: ignoring cam_calib.distortion_model and distortion_params as basalt can't use them + } + + void add_imu_calibration(const imu_calibration &imu_calib) { added_imu_calibs.push_back(imu_calib); } + + void apply_imu_calibration(const imu_calibration &imu_calib) { + using Scalar = double; + + int i = imu_calib.imu_index; + ASSERT(i == 0, "More than one IMU unsupported (%d)", i); + + static double frequency = -1; + if (frequency == -1) { + frequency = imu_calib.frequency; + calib.imu_update_rate = frequency; + } else { + ASSERT(frequency == calib.imu_update_rate, "Unsupported mix of IMU frequencies %lf != %lf", frequency, + calib.imu_update_rate); + } + + // Accelerometer calibration + + inertial_calibration accel = imu_calib.accel; + + Eigen::Matrix accel_bias_full; + const auto &abias = accel.offset; + const auto &atran = accel.transform; + + // TODO: Doing the same as rs_t265.cpp but that's incorrect. We should be doing an LQ decomposition of atran and + // using L. See https://gitlab.com/VladyslavUsenko/basalt-headers/-/issues/8 + accel_bias_full << abias(0), abias(1), abias(2), atran(0, 0) - 1, atran(1, 0), atran(2, 0), atran(1, 1) - 1, + atran(2, 1), atran(2, 2) - 1; + CalibAccelBias accel_bias; + accel_bias.getParam() = accel_bias_full; + calib.calib_accel_bias = accel_bias; + + calib.accel_noise_std = {accel.noise_std(0), accel.noise_std(1), accel.noise_std(2)}; + calib.accel_bias_std = {accel.bias_std(0), accel.bias_std(1), accel.bias_std(2)}; + + // Gyroscope calibration + + inertial_calibration gyro = imu_calib.gyro; + + Eigen::Matrix gyro_bias_full; + const auto &gbias = gyro.offset; + const auto >ran = gyro.transform; + gyro_bias_full << gbias(0), gbias(1), gbias(2), gtran(0, 0) - 1, gtran(1, 0), gtran(2, 0), gtran(0, 1), + gtran(1, 1) - 1, gtran(2, 1), gtran(0, 2), gtran(1, 2), gtran(2, 2) - 1; + CalibGyroBias gyro_bias; + gyro_bias.getParam() = gyro_bias_full; + calib.calib_gyro_bias = gyro_bias; + + calib.gyro_noise_std = {gyro.noise_std(0), gyro.noise_std(1), gyro.noise_std(2)}; + calib.gyro_bias_std = {gyro.bias_std(0), gyro.bias_std(1), gyro.bias_std(2)}; + } + + shared_ptr> enable_pose_ext_timing() { + pose_timing_enabled = true; + vector titles{"frame_ts", + "tracker_received", + "tracker_pushed", + "frames_received", // < Basalt computation starts + "opticalflow_produced", + "vio_start", + "imu_preintegrated", + "landmarks_updated", + "optimized", + "marginalized", + "pose_produced", // Basalt computation ends + "tracker_consumer_received", + "tracker_consumer_pushed", + "monado_dequeued"}; + return make_shared>(titles); + } +}; + +slam_tracker::slam_tracker(const string &config_file) { impl = make_unique(config_file); } + +slam_tracker::~slam_tracker() = default; + +void slam_tracker::initialize() { impl->initialize(); } + +void slam_tracker::start() { impl->start(); } + +void slam_tracker::stop() { impl->stop(); } + +void slam_tracker::finalize() { impl->finalize(); } + +bool slam_tracker::is_running() { return impl->is_running(); } + +void slam_tracker::push_imu_sample(const imu_sample &s) { impl->push_imu_sample(s); } + +void slam_tracker::push_frame(const img_sample &sample) { impl->push_frame(sample); } + +bool slam_tracker::try_dequeue_pose(pose &pose) { return impl->try_dequeue_pose(pose); } + +bool slam_tracker::supports_feature(int feature_id) { return impl->supports_feature(feature_id); } + +bool slam_tracker::use_feature(int feature_id, const shared_ptr ¶ms, shared_ptr &result) { + return impl->use_feature(feature_id, params, result); +} + +} // namespace xrt::auxiliary::tracking::slam diff --git a/src/monado/slam_tracker_ui.hpp b/src/monado/slam_tracker_ui.hpp new file mode 100644 index 0000000..3c22e08 --- /dev/null +++ b/src/monado/slam_tracker_ui.hpp @@ -0,0 +1,334 @@ +#pragma once + +#include +#include + +#include + +#include +#include +#include +#include +#include "sophus/se3.hpp" + +#include +#include +#include +#include "basalt/utils/vis_utils.h" + +#define ASSERT(cond, ...) \ + do { \ + if (!(cond)) { \ + printf("Assertion failed @%s:%d\n", __func__, __LINE__); \ + printf(__VA_ARGS__); \ + printf("\n"); \ + exit(EXIT_FAILURE); \ + } \ + } while (false); +#define ASSERT_(cond) ASSERT(cond, "%s", #cond); + +namespace xrt::auxiliary::tracking::slam { + +using std::cout; +using std::make_shared; +using std::shared_ptr; +using std::string; +using std::thread; +using std::to_string; +using std::vector; +using namespace basalt; + +class slam_tracker_ui { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + VioVisualizationData::Ptr curr_vis_data; + pangolin::DataLog vio_data_log; + thread vis_thread; + thread ui_runner_thread; + size_t num_cams = 0; + std::atomic running = false; + + public: + tbb::concurrent_bounded_queue out_vis_queue{}; + + void initialize(int ncams) { + vio_data_log.Clear(); + ASSERT_(ncams > 0); + num_cams = ncams; + } + + void start(const Sophus::SE3d &T_w_i_init, const basalt::Calibration &calib) { + running = true; + start_visualization_thread(); + start_ui(T_w_i_init, calib); + } + + void stop() { + running = false; + vis_thread.join(); + ui_runner_thread.join(); + } + + void start_visualization_thread() { + vis_thread = thread([&]() { + while (true) { + out_vis_queue.pop(curr_vis_data); + if (curr_vis_data.get() == nullptr) break; + } + cout << "Finished vis_thread\n"; + }); + } + + int64_t start_t_ns = -1; + std::vector vio_t_ns; + Eigen::aligned_vector vio_t_w_i; + + void log_vio_data(const PoseVelBiasState::Ptr &data) { + int64_t t_ns = data->t_ns; + if (start_t_ns < 0) start_t_ns = t_ns; + float since_start_ns = t_ns - start_t_ns; + + vio_t_ns.emplace_back(t_ns); + vio_t_w_i.emplace_back(data->T_w_i.translation()); + + Sophus::SE3d T_w_i = data->T_w_i; + Eigen::Vector3d vel_w_i = data->vel_w_i; + Eigen::Vector3d bg = data->bias_gyro; + Eigen::Vector3d ba = data->bias_accel; + + vector vals; + vals.push_back(since_start_ns * 1e-9); + for (int i = 0; i < 3; i++) vals.push_back(vel_w_i[i]); + for (int i = 0; i < 3; i++) vals.push_back(T_w_i.translation()[i]); + for (int i = 0; i < 3; i++) vals.push_back(bg[i]); + for (int i = 0; i < 3; i++) vals.push_back(ba[i]); + + vio_data_log.Log(vals); + } + + pangolin::Plotter *plotter; + pangolin::DataLog imu_data_log{}; + basalt::Calibration calib; + void start_ui(const Sophus::SE3d &T_w_i_init, const basalt::Calibration &c) { + ui_runner_thread = thread(&slam_tracker_ui::ui_runner, this, T_w_i_init, c); + } + + pangolin::Var follow{"ui.follow", true, false, true}; + pangolin::Var show_est_pos{"ui.show_est_pos", true, false, true}; + pangolin::Var show_est_vel{"ui.show_est_vel", false, false, true}; + pangolin::Var show_est_bg{"ui.show_est_bg", false, false, true}; + pangolin::Var show_est_ba{"ui.show_est_ba", false, false, true}; + + void ui_runner(const Sophus::SE3d &T_w_i_init, const basalt::Calibration &c) { + constexpr int UI_WIDTH = 200; + + calib = c; + string window_name = "Basalt SLAM Tracker for Monado"; + pangolin::CreateWindowAndBind(window_name, 1800, 1000); + + glEnable(GL_DEPTH_TEST); + + pangolin::View &img_view_display = pangolin::CreateDisplay() + .SetBounds(0.4, 1.0, pangolin::Attach::Pix(UI_WIDTH), 0.4) + .SetLayout(pangolin::LayoutEqual); + + pangolin::View &plot_display = pangolin::CreateDisplay().SetBounds(0.0, 0.4, pangolin::Attach::Pix(UI_WIDTH), 1.0); + + plotter = new pangolin::Plotter(&imu_data_log, 0.0, 100, -3.0, 3.0, 0.01, 0.01); + plot_display.AddDisplay(*plotter); + + pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH)); + + std::vector> img_view; + while (img_view.size() < calib.intrinsics.size()) { + std::shared_ptr iv(new pangolin::ImageView); + + size_t idx = img_view.size(); + img_view.push_back(iv); + + img_view_display.AddDisplay(*iv); + iv->extern_draw_function = [idx, this](auto &v) { this->draw_image_overlay(v, idx); }; + } + + Eigen::Vector3d cam_p(0.5, -2, -2); + cam_p = T_w_i_init.so3() * calib.T_i_c[0].so3() * cam_p; + cam_p[2] = 1; + + pangolin::OpenGlRenderState camera( + pangolin::ProjectionMatrix(640, 480, 400, 400, 320, 240, 0.001, 10000), + pangolin::ModelViewLookAt(cam_p[0], cam_p[1], cam_p[2], 0, 0, 0, pangolin::AxisZ)); + + pangolin::View &display3D = pangolin::CreateDisplay() + .SetAspect(-640 / 480.0) + .SetBounds(0.4, 1.0, 0.4, 1.0) + .SetHandler(new pangolin::Handler3D(camera)); + + while (running && !pangolin::ShouldQuit()) { + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + if (follow) { + // TODO: There is a small race condition here over + // curr_vis_data that is also present in the original basalt examples + if (curr_vis_data) { + auto T_w_i = curr_vis_data->states.back(); + T_w_i.so3() = Sophus::SO3d(); + + camera.Follow(T_w_i.matrix()); + } + } + + display3D.Activate(camera); + glClearColor(1.0, 1.0, 1.0, 1.0); + + draw_scene(); + + img_view_display.Activate(); + + { + pangolin::GlPixFormat fmt; + fmt.glformat = GL_LUMINANCE; + fmt.gltype = GL_UNSIGNED_SHORT; + fmt.scalable_internal_format = GL_LUMINANCE16; + + if (curr_vis_data && curr_vis_data->opt_flow_res && curr_vis_data->opt_flow_res->input_images) { + auto &img_data = curr_vis_data->opt_flow_res->input_images->img_data; + + for (size_t cam_id = 0; cam_id < num_cams; cam_id++) { + if (img_data[cam_id].img) { + img_view[cam_id]->SetImage(img_data[cam_id].img->ptr, img_data[cam_id].img->w, img_data[cam_id].img->h, + img_data[cam_id].img->pitch, fmt); + } + } + } + + draw_plots(); + } + + if (show_est_vel.GuiChanged() || show_est_pos.GuiChanged() || show_est_ba.GuiChanged() || + show_est_bg.GuiChanged()) { + draw_plots(); + } + + pangolin::FinishFrame(); + } + + pangolin::DestroyWindow(window_name); + cout << "Finished ui_runner\n"; + } + + pangolin::Var show_obs{"ui.show_obs", true, false, true}; + pangolin::Var show_ids{"ui.show_ids", false, false, true}; + + void draw_image_overlay(pangolin::View &v, size_t cam_id) { + UNUSED(v); + + if (show_obs) { + glLineWidth(1.0); + glColor3f(1.0, 0.0, 0.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + if (curr_vis_data && cam_id < curr_vis_data->projections.size()) { + const auto &points = curr_vis_data->projections[cam_id]; + + if (!points.empty()) { + double min_id = points[0][2]; + double max_id = points[0][2]; + + for (const auto &points2 : curr_vis_data->projections) { + for (const auto &p : points2) { + min_id = std::min(min_id, p[2]); + max_id = std::max(max_id, p[2]); + } + } + + for (const auto &c : points) { + const float radius = 6.5; + + float r; + float g; + float b; + getcolor(c[2] - min_id, max_id - min_id, b, g, r); + glColor3f(r, g, b); + + pangolin::glDrawCirclePerimeter(c[0], c[1], radius); + + if (show_ids) pangolin::GlFont::I().Text("%d", int(c[3])).Draw(c[0], c[1]); + } + } + + glColor3f(1.0, 0.0, 0.0); + pangolin::GlFont::I().Text("Tracked %d points", points.size()).Draw(5, 20); + } + } + } + + void draw_scene() { + glPointSize(3); + glColor3f(1.0, 0.0, 0.0); + glEnable(GL_BLEND); + glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); + + glColor3ubv(cam_color); + Eigen::aligned_vector sub_gt(vio_t_w_i.begin(), vio_t_w_i.end()); + pangolin::glDrawLineStrip(sub_gt); + + if (curr_vis_data) { + for (const auto &p : curr_vis_data->states) + for (const auto &t_i_c : calib.T_i_c) render_camera((p * t_i_c).matrix(), 2.0, state_color, 0.1); + + for (const auto &p : curr_vis_data->frames) + for (const auto &t_i_c : calib.T_i_c) render_camera((p * t_i_c).matrix(), 2.0, pose_color, 0.1); + + for (const auto &t_i_c : calib.T_i_c) + render_camera((curr_vis_data->states.back() * t_i_c).matrix(), 2.0, cam_color, 0.1); + + glColor3ubv(pose_color); + pangolin::glDrawPoints(curr_vis_data->points); + } + + pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0); + } + + OpticalFlowInput::Ptr last_img_data{}; + void update_last_image(OpticalFlowInput::Ptr &data) { last_img_data = data; } + void draw_plots() { + plotter->ClearSeries(); + plotter->ClearMarkers(); + + if (show_est_pos) { + plotter->AddSeries("$0", "$4", pangolin::DrawingModeLine, pangolin::Colour::Red(), "position x", &vio_data_log); + plotter->AddSeries("$0", "$5", pangolin::DrawingModeLine, pangolin::Colour::Green(), "position y", &vio_data_log); + plotter->AddSeries("$0", "$6", pangolin::DrawingModeLine, pangolin::Colour::Blue(), "position z", &vio_data_log); + } + + if (show_est_vel) { + plotter->AddSeries("$0", "$1", pangolin::DrawingModeLine, pangolin::Colour::Red(), "velocity x", &vio_data_log); + plotter->AddSeries("$0", "$2", pangolin::DrawingModeLine, pangolin::Colour::Green(), "velocity y", &vio_data_log); + plotter->AddSeries("$0", "$3", pangolin::DrawingModeLine, pangolin::Colour::Blue(), "velocity z", &vio_data_log); + } + + if (show_est_bg) { + plotter->AddSeries("$0", "$7", pangolin::DrawingModeLine, pangolin::Colour::Red(), "gyro bias x", &vio_data_log); + plotter->AddSeries("$0", "$8", pangolin::DrawingModeLine, pangolin::Colour::Green(), "gyro bias y", + &vio_data_log); + plotter->AddSeries("$0", "$9", pangolin::DrawingModeLine, pangolin::Colour::Blue(), "gyro bias z", &vio_data_log); + } + + if (show_est_ba) { + plotter->AddSeries("$0", "$10", pangolin::DrawingModeLine, pangolin::Colour::Red(), "accel bias x", + &vio_data_log); + plotter->AddSeries("$0", "$11", pangolin::DrawingModeLine, pangolin::Colour::Green(), "accel bias y", + &vio_data_log); + plotter->AddSeries("$0", "$12", pangolin::DrawingModeLine, pangolin::Colour::Blue(), "accel bias z", + &vio_data_log); + } + + if (last_img_data) { + double t = last_img_data->t_ns * 1e-9; + plotter->AddMarker(pangolin::Marker::Vertical, t, pangolin::Marker::Equal, pangolin::Colour::White()); + } + } +}; +} // namespace xrt::auxiliary::tracking::slam diff --git a/thirdparty/monado/slam_tracker.hpp b/thirdparty/monado/slam_tracker.hpp new file mode 100644 index 0000000..71e4eb8 --- /dev/null +++ b/thirdparty/monado/slam_tracker.hpp @@ -0,0 +1,296 @@ +// Copyright 2021, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief SLAM tracker class header for usage in Monado. + * @author Mateo de Mayo + * @ingroup aux_tracking + * + * This file contains the declaration of the @ref slam_tracker class. This + * header is intended to appear in both Monado and an external SLAM system. The + * implementation of `slam_tracker` is provided by the external system. + * Additional data types are declared for the communication between Monado and + * the system. + * + */ + +#pragma once + +#include + +#include +#include +#include +#include + +namespace xrt::auxiliary::tracking::slam { + +// For implementation: same as IMPLEMENTATION_VERSION_* +// For user: expected IMPLEMENTATION_VERSION_*. Should be checked in runtime. +constexpr int HEADER_VERSION_MAJOR = 2; //!< API Breakages +constexpr int HEADER_VERSION_MINOR = 0; //!< Backwards compatible API changes +constexpr int HEADER_VERSION_PATCH = 0; //!< Backw. comp. .h-implemented changes + +// Which header version the external system is implementing. +extern const int IMPLEMENTATION_VERSION_MAJOR; +extern const int IMPLEMENTATION_VERSION_MINOR; +extern const int IMPLEMENTATION_VERSION_PATCH; + +enum class pose_ext_type : int; + +/*! + * @brief Standard pose type to communicate Monado with the external SLAM system + */ +struct pose { + std::int64_t timestamp; //!< In same clock as input samples + float px, py, pz; //!< Position vector + float rx, ry, rz, rw = 1; //!< Orientation quaternion + std::shared_ptr next = nullptr; + + pose() = default; + pose(std::int64_t timestamp, // + float px, float py, float pz, // + float rx, float ry, float rz, float rw) + : timestamp(timestamp), // + px(px), py(py), pz(pz), // + rx(rx), ry(ry), rz(rz), rw(rw) {} + + std::shared_ptr + find_pose_extension(pose_ext_type required_type) const; +}; + +/*! + * @brief IMU Sample type to pass around between programs + */ +struct imu_sample { + std::int64_t timestamp; //!< In nanoseconds + double ax, ay, az; //!< Accel in meters per second squared (m / s^2) + double wx, wy, wz; //!< Gyro in radians per second (rad / s) + imu_sample() = default; + imu_sample(std::int64_t timestamp, double ax, double ay, double az, double wx, + double wy, double wz) + : timestamp(timestamp), ax(ax), ay(ay), az(az), wx(wx), wy(wy), wz(wz) {} +}; + +/*! + * @brief Image sample type to pass around between programs. It is expected that + * any SLAM system takes OpenCV matrices as input. + */ +struct img_sample { + std::int64_t timestamp; + cv::Mat img; + bool is_left; + img_sample() = default; + img_sample(std::int64_t timestamp, const cv::Mat &img, bool is_left) + : timestamp(timestamp), img(img), is_left(is_left) {} +}; + +/*! + * @brief slam_tracker serves as an interface between Monado and external SLAM + * systems. + * + * This class uses the pointer-to-implementation pattern, and its implementation + * should be provided by an external SLAM system. + */ +struct slam_tracker { + /*! + * @brief Construct a new slam tracker object + * + * @param config_file SLAM systems parameters tend to be numerous and very + * specific, so they usually use a configuration file as the main way to set + * them up. Therefore, this constructor receives a path to a + * implementation-specific configuration file. + */ + slam_tracker(const std::string &config_file); + ~slam_tracker(); + + slam_tracker(const slam_tracker &) = delete; + slam_tracker &operator=(const slam_tracker &) = delete; + + void initialize(); + void start(); + bool is_running(); + void stop(); + void finalize(); + + /*! + * @brief Push an IMU sample into the tracker. + * + * There must be a single producer thread pushing samples. + * Samples must have monotonically increasing timestamps. + * The implementation must be non-blocking. + * Thus, a separate consumer thread should process the samples. + */ + void push_imu_sample(const imu_sample &sample); + + /*! + * @brief Push an image sample into the tracker. + * + * Same conditions as @ref push_imu_sample apply. + * When using stereo frames, they must be pushed in a left-right order. + * The consecutive left-right pair must have the same timestamps. + */ + void push_frame(const img_sample &sample); + + /*! + * @brief Get the latest tracked pose from the SLAM system. + * + * There must be a single thread consuming this method. + * + * @param[out] out_pose Dequeued pose. + * @return true If a new pose was dequeued into @p out_pose. + * @return false If there was no pose to dequeue. + */ + bool try_dequeue_pose(pose &out_pose); + + //! Asks the SLAM system whether it supports a specific feature. + bool supports_feature(int feature_id); + + /*! + * @brief Use a special feature of the SLAM tracker. + * + * This method uses heap allocated objects for passing parameters and + * obtaining the results. Use `std::static_pointer_cast` to shared pointers to + * the expected types. + * + * @param feature_id Id of the special feature. + * @param params Pointer to the parameter object for this feature. + * @param result Pointer to the result produced by the feature call. + * @return false if the feature was not supported, true otherwise. + */ + bool use_feature(int feature_id, const std::shared_ptr ¶ms, + std::shared_ptr &result); + +private: + struct implementation; + std::unique_ptr impl; +}; + +/* + * Special features + * + * A special feature is comprised of an ID, a PARAMS type and a RESULT type. It + * can be defined using DEFINE_FEATURE. Once defined, the definition should not + * suffer future changes. + * + * One of the main concerns in the features interface is the ability to add new + * features without being required to update the SLAM systems that are not + * interested in implementing the feature. + * + */ + +#define DEFINE_FEATURE(NAME, SHORT_NAME, ID, PARAMS_TYPE, RESULT_TYPE) \ + using FPARAMS_##SHORT_NAME = PARAMS_TYPE; \ + using FRESULT_##SHORT_NAME = RESULT_TYPE; \ + constexpr int FID_##SHORT_NAME = ID; \ + constexpr int F_##NAME = ID; + +struct cam_calibration { + enum class cam_model { pinhole, fisheye }; + + int cam_index; //!< For multi-camera setups. For stereo 0 ~ left, 1 ~ right. + int width, height; // model_params; + cv::Matx T_cam_imu; //!< Transformation from camera to imu space +}; + +struct inertial_calibration { + // Calibration intrinsics to apply to each raw measurement. + + //! This transform will be applied to raw measurements. + cv::Matx transform; + + //! Offset to apply to raw measurements to; called bias in other contexts. + cv::Matx offset; + + // Parameters for the random processes that model this IMU. See section "2.1 + // Gyro Noise Model" of N. Trawny and S. I. Roumeliotis, "Indirect Kalman + // Filter for 3D Attitude Estimation". Analogous for accelerometers. + // http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf#page=15 + + //! IMU internal bias ~ wiener process with steps N(0, σ²); this field is σ; + //! [σ] = U / sqrt(sec³) with U = rad if gyroscope, U = m/s if accelerometer. + cv::Matx bias_std; + + //! IMU measurement noise ~ N(0, σ²); this field is σ. + //! [σ] = U / sqrt(sec) with U = rad if gyroscope, U = m/s if accelerometer. + cv::Matx noise_std; + + inertial_calibration() : transform(cv::Matx::eye()) {} +}; + +struct imu_calibration { + int imu_index; //!< For multi-imu setups. Usually just 0. + double frequency; //!< Samples per second + inertial_calibration accel; + inertial_calibration gyro; +}; + +/*! + * Feature ADD_CAMERA_CALIBRATION + * + * Use it after constructor but before `start()` to write or overwrite camera + * calibration data that might come from the system-specific config file. + */ +DEFINE_FEATURE(ADD_CAMERA_CALIBRATION, ACC, 1, cam_calibration, void) + +/*! + * Feature ADD_IMU_CALIBRATION + * + * Use it after constructor but before `start()` to write or overwrite IMU + * calibration data that might come from the system-specific config file. + */ +DEFINE_FEATURE(ADD_IMU_CALIBRATION, AIC, 2, imu_calibration, void) + +/*! + * Feature ENABLE_POSE_EXT_TIMING + * + * Use it after constructor but before `start()`. + * Returns a vector with names for the timestamps in `pose_ext_timing`. + */ +DEFINE_FEATURE(ENABLE_POSE_EXT_TIMING, EPET, 3, void, std::vector) + +/* + * Pose extensions + * + * A pose extension is a struct that gets linked in the `pose.next` field. You + * first ask if the implementation supports enabling such extension with a + * `supports_feature()` call with the appropriate `ENABLE_POSE_EXT_*`. Then, it + * can be enabled with the corresponding `use_feature()` call. + * + */ + +enum class pose_ext_type : int { + UNDEFINED = 0, + TIMING = 1, +}; + +struct pose_extension { + pose_ext_type type = pose_ext_type::UNDEFINED; + std::shared_ptr next = nullptr; + + pose_extension(pose_ext_type type) : type(type) {} +}; + +inline std::shared_ptr +pose::find_pose_extension(pose_ext_type required_type) const { + std::shared_ptr pe = next; + while (pe != nullptr && pe->type != required_type) { + pe = pe->next; + } + return pe; +} + +struct pose_ext_timing : pose_extension { + //! Internal pipeline timestamps of interest when generating the pose. In + //! steady clock ns. Must have the same number of elements in the same run. + std::vector timestamps{}; + + pose_ext_timing() : pose_extension{pose_ext_type::TIMING} {} +}; + +} // namespace xrt::auxiliary::tracking::slam