Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add frame_publisher::get_extraction_time_elapsed_ms #580

Merged
merged 1 commit into from
Apr 6, 2024
Merged

Add frame_publisher::get_extraction_time_elapsed_ms #580

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 9 additions & 3 deletions src/stella_vslam/publish/frame_publisher.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<cv::KeyPoint>& curr_keypts,
Expand Down Expand Up @@ -154,13 +158,15 @@ void frame_publisher::update(const std::vector<std::shared_ptr<data::landmark>>&
tracker_state_t tracking_state,
std::vector<cv::KeyPoint>& keypts,
const cv::Mat& img,
double elapsed_ms) {
double tracking_time_elapsed_ms,
double extraction_time_elapsed_ms) {
std::lock_guard<std::mutex> 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;
Expand Down
10 changes: 8 additions & 2 deletions src/stella_vslam/publish/frame_publisher.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,8 @@ class frame_publisher {
tracker_state_t tracking_state,
std::vector<cv::KeyPoint>& 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
Expand All @@ -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<cv::KeyPoint>& curr_keypts,
const std::vector<std::shared_ptr<data::landmark>>& curr_lms,
Expand Down Expand Up @@ -93,7 +96,10 @@ class frame_publisher {
std::vector<cv::KeyPoint> 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_;
Expand Down
25 changes: 19 additions & 6 deletions src/stella_vslam/system.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -441,7 +441,11 @@ std::shared_ptr<Mat44_t> 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<std::chrono::milliseconds>(end - start).count();
return feed_frame(frm, img, extraction_time_elapsed_ms);
}

std::shared_ptr<Mat44_t> system::feed_stereo_frame(const cv::Mat& left_img, const cv::Mat& right_img, const double timestamp, const cv::Mat& mask) {
Expand All @@ -452,7 +456,11 @@ std::shared_ptr<Mat44_t> 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<std::chrono::milliseconds>(end - start).count();
return feed_frame(frm, left_img, extraction_time_elapsed_ms);
}

std::shared_ptr<Mat44_t> system::feed_RGBD_frame(const cv::Mat& rgb_img, const cv::Mat& depthmap, const double timestamp, const cv::Mat& mask) {
Expand All @@ -463,23 +471,28 @@ std::shared_ptr<Mat44_t> 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<std::chrono::milliseconds>(end - start).count();
return feed_frame(frm, rgb_img, extraction_time_elapsed_ms);
}

std::shared_ptr<Mat44_t> system::feed_frame(const data::frame& frm, const cv::Mat& img) {
std::shared_ptr<Mat44_t> 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<std::chrono::milliseconds>(end - start).count();
double tracking_time_elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(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));
}
Expand Down
2 changes: 1 addition & 1 deletion src/stella_vslam/system.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -127,7 +127,7 @@ class system {
//-----------------------------------------
// data feeding methods

std::shared_ptr<Mat44_t> feed_frame(const data::frame& frm, const cv::Mat& img);
std::shared_ptr<Mat44_t> 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)
Expand Down
Loading