From 71b1c5ec502f3395340fdcd5e5a7ff7180d2cf63 Mon Sep 17 00:00:00 2001 From: ymd-stella Date: Sat, 6 Apr 2024 10:18:22 +0900 Subject: [PATCH] Add frame_publisher::get_extraction_time_elapsed_ms --- src/stella_vslam/publish/frame_publisher.cc | 12 +++++++--- src/stella_vslam/publish/frame_publisher.h | 10 +++++++-- src/stella_vslam/system.cc | 25 ++++++++++++++++----- src/stella_vslam/system.h | 2 +- 4 files changed, 37 insertions(+), 12 deletions(-) diff --git a/src/stella_vslam/publish/frame_publisher.cc b/src/stella_vslam/publish/frame_publisher.cc index a66fd346..c8f94f79 100644 --- a/src/stella_vslam/publish/frame_publisher.cc +++ b/src/stella_vslam/publish/frame_publisher.cc @@ -110,7 +110,11 @@ cv::Mat frame_publisher::get_image() { } double frame_publisher::get_tracking_time_elapsed_ms() { - return elapsed_ms_; + return tracking_time_elapsed_ms_; +} + +double frame_publisher::get_extraction_time_elapsed_ms() { + return extraction_time_elapsed_ms_; } unsigned int frame_publisher::draw_tracked_points(cv::Mat& img, const std::vector& curr_keypts, @@ -154,13 +158,15 @@ void frame_publisher::update(const std::vector>& tracker_state_t tracking_state, std::vector& keypts, const cv::Mat& img, - double elapsed_ms) { + double tracking_time_elapsed_ms, + double extraction_time_elapsed_ms) { std::lock_guard lock(mtx_); img.copyTo(img_); curr_keypts_ = keypts; - elapsed_ms_ = elapsed_ms; + tracking_time_elapsed_ms_ = tracking_time_elapsed_ms; + extraction_time_elapsed_ms_ = extraction_time_elapsed_ms; mapping_is_enabled_ = mapping_is_enabled; tracking_state_ = tracking_state; curr_lms_ = curr_lms; diff --git a/src/stella_vslam/publish/frame_publisher.h b/src/stella_vslam/publish/frame_publisher.h index 5c9ad27e..d948c871 100644 --- a/src/stella_vslam/publish/frame_publisher.h +++ b/src/stella_vslam/publish/frame_publisher.h @@ -43,7 +43,8 @@ class frame_publisher { tracker_state_t tracking_state, std::vector& keypts, const cv::Mat& img, - double elapsed_ms); + double tracking_time_elapsed_ms, + double extraction_time_elapsed_ms); /** * Get the current image with tracking information @@ -63,6 +64,8 @@ class frame_publisher { double get_tracking_time_elapsed_ms(); + double get_extraction_time_elapsed_ms(); + protected: unsigned int draw_tracked_points(cv::Mat& img, const std::vector& curr_keypts, const std::vector>& curr_lms, @@ -93,7 +96,10 @@ class frame_publisher { std::vector curr_keypts_; //! elapsed time for tracking - double elapsed_ms_ = 0.0; + double tracking_time_elapsed_ms_ = 0.0; + + //! elapsed time for feature extraction + double extraction_time_elapsed_ms_ = 0.0; //! mapping module status bool mapping_is_enabled_; diff --git a/src/stella_vslam/system.cc b/src/stella_vslam/system.cc index 7eadf0eb..cb68f8d4 100644 --- a/src/stella_vslam/system.cc +++ b/src/stella_vslam/system.cc @@ -441,7 +441,11 @@ std::shared_ptr system::feed_monocular_frame(const cv::Mat& img, const spdlog::warn("preprocess: empty image"); return nullptr; } - return feed_frame(create_monocular_frame(img, timestamp, mask), img); + const auto start = std::chrono::system_clock::now(); + auto frm = create_monocular_frame(img, timestamp, mask); + const auto end = std::chrono::system_clock::now(); + double extraction_time_elapsed_ms = std::chrono::duration_cast(end - start).count(); + return feed_frame(frm, img, extraction_time_elapsed_ms); } std::shared_ptr system::feed_stereo_frame(const cv::Mat& left_img, const cv::Mat& right_img, const double timestamp, const cv::Mat& mask) { @@ -452,7 +456,11 @@ std::shared_ptr system::feed_stereo_frame(const cv::Mat& left_img, cons spdlog::warn("preprocess: empty image"); return nullptr; } - return feed_frame(create_stereo_frame(left_img, right_img, timestamp, mask), left_img); + const auto start = std::chrono::system_clock::now(); + auto frm = create_stereo_frame(left_img, right_img, timestamp, mask); + const auto end = std::chrono::system_clock::now(); + double extraction_time_elapsed_ms = std::chrono::duration_cast(end - start).count(); + return feed_frame(frm, left_img, extraction_time_elapsed_ms); } std::shared_ptr system::feed_RGBD_frame(const cv::Mat& rgb_img, const cv::Mat& depthmap, const double timestamp, const cv::Mat& mask) { @@ -463,23 +471,28 @@ std::shared_ptr system::feed_RGBD_frame(const cv::Mat& rgb_img, const c spdlog::warn("preprocess: empty image"); return nullptr; } - return feed_frame(create_RGBD_frame(rgb_img, depthmap, timestamp, mask), rgb_img); + const auto start = std::chrono::system_clock::now(); + auto frm = create_RGBD_frame(rgb_img, depthmap, timestamp, mask); + const auto end = std::chrono::system_clock::now(); + double extraction_time_elapsed_ms = std::chrono::duration_cast(end - start).count(); + return feed_frame(frm, rgb_img, extraction_time_elapsed_ms); } -std::shared_ptr system::feed_frame(const data::frame& frm, const cv::Mat& img) { +std::shared_ptr system::feed_frame(const data::frame& frm, const cv::Mat& img, const double extraction_time_elapsed_ms) { const auto start = std::chrono::system_clock::now(); const auto cam_pose_wc = tracker_->feed_frame(frm); const auto end = std::chrono::system_clock::now(); - double elapsed_ms = std::chrono::duration_cast(end - start).count(); + double tracking_time_elapsed_ms = std::chrono::duration_cast(end - start).count(); frame_publisher_->update(tracker_->curr_frm_.get_landmarks(), !mapper_->is_paused(), tracker_->tracking_state_, keypts_, img, - elapsed_ms); + tracking_time_elapsed_ms, + extraction_time_elapsed_ms); if (tracker_->tracking_state_ == tracker_state_t::Tracking && cam_pose_wc) { map_publisher_->set_current_cam_pose(util::converter::inverse_pose(*cam_pose_wc)); } diff --git a/src/stella_vslam/system.h b/src/stella_vslam/system.h index 6bdd062b..feeed497 100644 --- a/src/stella_vslam/system.h +++ b/src/stella_vslam/system.h @@ -127,7 +127,7 @@ class system { //----------------------------------------- // data feeding methods - std::shared_ptr feed_frame(const data::frame& frm, const cv::Mat& img); + std::shared_ptr feed_frame(const data::frame& frm, const cv::Mat& img, double extraction_time_elapsed_ms); //! Feed a monocular frame to SLAM system //! (NOTE: distorted images are acceptable if calibrated)