From 41b04f62925700ef67852cb25537764a4e3d017b Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Wed, 13 Jul 2022 20:16:00 +0000 Subject: [PATCH] Update slam_tracker.hpp to 4.0.0 (feature info pose extension) --- CMakeLists.txt | 4 +- README.md | 2 + include/basalt/optical_flow/optical_flow.h | 19 ++--- src/monado/slam_tracker.cpp | 94 +++++++++++++++------- src/vi_estimator/sqrt_keypoint_vio.cpp | 29 ++++++- src/vio.cpp | 6 +- thirdparty/monado/slam_tracker.hpp | 93 +++++++++++++++++++-- 7 files changed, 189 insertions(+), 58 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c192193..71cc8f4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -383,8 +383,8 @@ target_sources(basalt ) target_link_libraries(basalt - 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) + PUBLIC ${STD_CXX_FS} basalt::opencv basalt::basalt-headers TBB::tbb pangolin basalt::cli11 basalt::monado + PRIVATE basalt::magic_enum rosbag apriltag opengv nlohmann::json fmt::fmt) 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/README.md b/README.md index abc185a..6356680 100644 --- a/README.md +++ b/README.md @@ -119,7 +119,9 @@ Run an OpenXR app like `hello_xr` with the following environment variables set export EUROC_PATH=/path/to/euroc/V1_01_easy/ # Set euroc dataset path. You can get a dataset from http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/vicon_room1/V1_01_easy/V1_01_easy.zip export EUROC_LOG=debug export EUROC_HMD=false # if false, a fake controller will be tracked, else a fake HMD +export EUROC_PLAY_FROM_START=true export SLAM_LOG=debug +export SLAM_SUBMIT_FROM_START=true export SLAM_CONFIG=$bsltdeps/basalt/data/monado/euroc.toml # Point to Basalt config file for Euroc export OXR_DEBUG_GUI=1 # We will need the debug ui to start streaming the dataset ``` diff --git a/include/basalt/optical_flow/optical_flow.h b/include/basalt/optical_flow/optical_flow.h index 6a8b125..bb47c7d 100644 --- a/include/basalt/optical_flow/optical_flow.h +++ b/include/basalt/optical_flow/optical_flow.h @@ -45,12 +45,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include namespace basalt { using KeypointId = size_t; +using xrt::auxiliary::tracking::slam::timestats; struct OpticalFlowInput { using Ptr = std::shared_ptr; @@ -58,20 +60,9 @@ struct OpticalFlowInput { int64_t t_ns; std::vector img_data; - // Keep track of internal timestamps for this input - bool timing_enabled = false; - std::vector tss{}; - void addTime(const char* /* name */, int64_t custom_ts = INT64_MIN) { - if (!timing_enabled) { - return; - } - - if (custom_ts != INT64_MIN) { - tss.push_back(custom_ts); - } else { - auto ts = std::chrono::steady_clock::now().time_since_epoch().count(); - tss.push_back(ts); - } + timestats stats; //!< Keeps track of internal metrics for this t_ns + void addTime(const char* name, int64_t custom_ts = INT64_MIN) { + stats.addTime(name, custom_ts); } }; diff --git a/src/monado/slam_tracker.cpp b/src/monado/slam_tracker.cpp index b918732..a055896 100644 --- a/src/monado/slam_tracker.cpp +++ b/src/monado/slam_tracker.cpp @@ -63,6 +63,23 @@ string pose2str(const pose &p) { return str; } +static const vector timing_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", +}; + struct slam_tracker::implementation { EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -110,14 +127,20 @@ struct slam_tracker::implementation { 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}; + unordered_set supported_features{ + F_ADD_CAMERA_CALIBRATION, + F_ADD_IMU_CALIBRATION, + F_ENABLE_POSE_EXT_TIMING, + F_ENABLE_POSE_EXT_FEATURES, + }; // Additional calibration data vector added_cam_calibs{}; vector added_imu_calibs{}; - // Pose timing + // Enabled pose extensions bool pose_timing_enabled = false; + bool pose_features_enabled = false; public: implementation(const string &unified_config) { @@ -186,10 +209,20 @@ struct slam_tracker::implementation { 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; + auto *next = &p.next; + + if (state->input_images->stats.timing_enabled) { + pose_ext_timing_data petd = state->input_images->stats; + auto pose_timing = make_shared(petd); + *next = pose_timing; + next = &pose_timing->next; + } + + if (state->input_images->stats.features_enabled) { + pose_ext_features_data pefd = state->input_images->stats; + auto pose_features = make_shared(pefd); + *next = pose_features; + next = &pose_features->next; } return p; @@ -241,7 +274,7 @@ struct slam_tracker::implementation { apply_cam_calibration(c); } - bool calib_from_monado = added_cam_calibs.size() == 2; + bool calib_from_monado = added_cam_calibs.size() == NUM_CAMS; bool view_offset_unknown = calib.view_offset(0) == 0 && calib.view_offset(1) == 0; if (calib_from_monado || view_offset_unknown) { compute_view_offset(); @@ -331,11 +364,24 @@ struct slam_tracker::implementation { 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; + + // Initialize stats + partial_frame->stats.ts = s.timestamp; + if (pose_timing_enabled) { + partial_frame->stats.timing_enabled = true; + partial_frame->stats.timing.reserve(timing_titles.size()); + partial_frame->stats.timing_titles = &timing_titles; + partial_frame->addTime("frame_ts", s.timestamp); + partial_frame->addTime("tracker_received"); + } + if (pose_features_enabled) { + partial_frame->stats.features_enabled = true; + partial_frame->stats.features_per_cam.resize(NUM_CAMS); + } + i = 0; } else { ASSERT(partial_frame->t_ns == s.timestamp, "Left and right frame timestamps differ: %ld != %ld", @@ -384,7 +430,11 @@ struct slam_tracker::implementation { shared_ptr casted_params = static_pointer_cast(params); add_imu_calibration(*casted_params); } else if (feature_id == FID_EPET) { - result = enable_pose_ext_timing(); + shared_ptr casted_params = static_pointer_cast(params); + result = enable_pose_ext_timing(*casted_params); + } else if (feature_id == FID_EPEF) { + shared_ptr casted_params = static_pointer_cast(params); + enable_pose_ext_features(*casted_params); } else { return false; } @@ -510,24 +560,12 @@ struct slam_tracker::implementation { calib_data_ready.imu = true; } - 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); + shared_ptr> enable_pose_ext_timing(bool enable) { + pose_timing_enabled = enable; + return make_shared>(timing_titles); } + + void enable_pose_ext_features(bool enable) { pose_features_enabled = enable; } }; slam_tracker::slam_tracker(const string &config_file) { impl = make_unique(config_file); } diff --git a/src/vi_estimator/sqrt_keypoint_vio.cpp b/src/vi_estimator/sqrt_keypoint_vio.cpp index fd71628..0dec1be 100644 --- a/src/vi_estimator/sqrt_keypoint_vio.cpp +++ b/src/vi_estimator/sqrt_keypoint_vio.cpp @@ -53,6 +53,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include namespace basalt { @@ -160,7 +161,7 @@ void SqrtKeypointVioEstimator::initialize(const Eigen::Vector3d& bg_, typename ImuData::Ptr data = popFromImuDataQueue(); - bool run = data != nullptr; // End VIO otherwise + bool run = data != nullptr; // End VIO otherwise if (run) { data->accel = calib.calib_accel_bias.getCalibrated(data->accel); data->gyro = calib.calib_gyro_bias.getCalibrated(data->gyro); @@ -486,6 +487,14 @@ bool SqrtKeypointVioEstimator::measure( optimize_and_marg(opt_flow_meas->input_images, num_points_connected, lost_landmaks); + const size_t num_cams = opt_flow_meas->observations.size(); + const bool features_ext = opt_flow_meas->input_images->stats.features_enabled; + std::vector> projections{}; + if (features_ext || out_vis_queue) { + projections.resize(num_cams); + computeProjections(projections, last_state_t_ns); + } + if (out_state_queue) { PoseVelBiasStateWithLin p = frame_states.at(last_state_t_ns); @@ -494,6 +503,21 @@ bool SqrtKeypointVioEstimator::measure( data->input_images = opt_flow_meas->input_images; data->input_images->addTime("pose_produced"); + + if (features_ext) { + for (size_t i = 0; i < num_cams; i++) { + for (const Eigen::Vector4d& v : projections[i]) { + using Feature = + xrt::auxiliary::tracking::slam::pose_ext_features_data::feature; + Feature lm{}; + lm.id = v.w(); + lm.u = v.x(); + lm.v = v.y(); + lm.depth = v.z(); + data->input_images->stats.addFeature(i, lm); + } + } + } out_state_queue->push(data); } @@ -513,8 +537,7 @@ bool SqrtKeypointVioEstimator::measure( get_current_points(data->points, data->point_ids); - data->projections.resize(opt_flow_meas->observations.size()); - computeProjections(data->projections, last_state_t_ns); + data->projections = projections; data->opt_flow_res = prev_opt_flow_res[last_state_t_ns]; diff --git a/src/vio.cpp b/src/vio.cpp index 855eaca..8cdd386 100644 --- a/src/vio.cpp +++ b/src/vio.cpp @@ -143,7 +143,7 @@ size_t last_frame_processed = 0; tbb::concurrent_unordered_map> timestamp_to_id; std::mutex m; -std::condition_variable cv; +std::condition_variable cvar; bool step_by_step = false; size_t max_frames = 0; @@ -169,7 +169,7 @@ void feed_images() { if (step_by_step) { std::unique_lock lk(m); - cv.wait(lk); + cvar.wait(lk); } basalt::OpticalFlowInput::Ptr data(new basalt::OpticalFlowInput); @@ -829,7 +829,7 @@ bool next_step() { if (show_frame < int(vio_dataset->get_image_timestamps().size()) - 1) { show_frame = show_frame + 1; show_frame.Meta().gui_changed = true; - cv.notify_one(); + cvar.notify_one(); return true; } else { return false; diff --git a/thirdparty/monado/slam_tracker.hpp b/thirdparty/monado/slam_tracker.hpp index 0274c65..4058ee1 100644 --- a/thirdparty/monado/slam_tracker.hpp +++ b/thirdparty/monado/slam_tracker.hpp @@ -19,15 +19,17 @@ #include #include +#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 = 3; //!< API Breakages +constexpr int HEADER_VERSION_MAJOR = 4; //!< API Breakages constexpr int HEADER_VERSION_MINOR = 0; //!< Backwards compatible API changes constexpr int HEADER_VERSION_PATCH = 0; //!< Backw. comp. .h-implemented changes @@ -199,7 +201,7 @@ struct cam_calibration { double fx, fy; // distortion; //!< Parameters for the distortion_model + std::vector distortion{}; //!< Parameters for the distortion_model cv::Matx t_imu_cam; //!< Transformation from IMU to camera }; @@ -254,10 +256,17 @@ DEFINE_FEATURE(ADD_IMU_CALIBRATION, AIC, 2, imu_calibration, void) /*! * Feature ENABLE_POSE_EXT_TIMING * - * Use it after constructor but before `start()`. + * Enable/disable adding internal timestamps to the estimated poses. * Returns a vector with names for the timestamps in `pose_ext_timing`. */ -DEFINE_FEATURE(ENABLE_POSE_EXT_TIMING, EPET, 3, void, std::vector) +DEFINE_FEATURE(ENABLE_POSE_EXT_TIMING, EPET, 3, bool, std::vector) + +/*! + * Feature ENABLE_POSE_EXT_FEATURES + * + * Enable/disable adding feature information to the estimated poses. + */ +DEFINE_FEATURE(ENABLE_POSE_EXT_FEATURES, EPEF, 4, bool, void) /* * Pose extensions @@ -272,6 +281,7 @@ DEFINE_FEATURE(ENABLE_POSE_EXT_TIMING, EPET, 3, void, std::vector) enum class pose_ext_type : int { UNDEFINED = 0, TIMING = 1, + FEATURES = 2, }; struct pose_extension { @@ -290,12 +300,79 @@ pose::find_pose_extension(pose_ext_type required_type) const { 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{}; +// Timing pose extension +struct pose_ext_timing_data { + //! Internal pipeline stage timestamps of interest when generating the pose. + //! In steady clock ns. Must have the same number of elements in the same run. + std::vector timing{}; + //! Names of each timing stage. Should point to static memory. + const std::vector *timing_titles = nullptr; +}; + +struct pose_ext_timing : pose_extension, pose_ext_timing_data { pose_ext_timing() : pose_extension{pose_ext_type::TIMING} {} + pose_ext_timing(const pose_ext_timing_data &petd) + : pose_extension{pose_ext_type::TIMING}, pose_ext_timing_data{petd} {} +}; + +// Features pose extension +struct pose_ext_features_data { + struct feature { + std::int64_t id; + float u; + float v; + float depth; + }; + + std::vector> features_per_cam{}; +}; + +struct pose_ext_features : pose_extension, pose_ext_features_data { + pose_ext_features() : pose_extension{pose_ext_type::FEATURES} {} + pose_ext_features(const pose_ext_features_data &pefd) + : pose_extension{pose_ext_type::FEATURES}, pose_ext_features_data{pefd} {} +}; + +/*! + * Utility object to keep track of different stats for a particular timestamp. + * Stats usually correspond with a particular pose extension. + */ +struct timestats : pose_ext_timing_data, pose_ext_features_data { + using ptr = std::shared_ptr; + + std::int64_t ts = -1; + bool timing_enabled = false; + bool features_enabled = false; + + void addTime(const char *name, int64_t ts = INT64_MIN) { + if (!timing_enabled) { + return; + } + if (timing_titles) { + std::string expected = timing_titles->at(timing.size()); + if (expected != name) { + std::cout << "Invalid timing stage\n"; + std::cout << "expected: " << expected; + std::cout << ", got: " << name << std::endl; + exit(EXIT_FAILURE); + } + } + if (ts == INT64_MIN) { + ts = std::chrono::steady_clock::now().time_since_epoch().count(); + } + timing.push_back(ts); + } + + void addFeature(size_t cam, const feature &f) { + if (!features_enabled) { + return; + } + if (cam >= features_per_cam.size()) { + features_per_cam.resize(cam + 1); + } + features_per_cam.at(cam).push_back(f); + } }; } // namespace xrt::auxiliary::tracking::slam