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

Jump to bottom

Issue 209 cpu timer #5

Open
wants to merge 8 commits into
base: master
Choose a base branch
from
+153 −82
Open

Issue 209 cpu timer #5

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
5701da5
MTP fix is done?
JeffreyZh4ng Sep 23, 2020
f687a3d
Add back prints
JeffreyZh4ng Sep 23, 2020
52b776d
Added CPU timer support
charmoniumQ Jan 24, 2021
cbb6e64
wip
charmoniumQ Jan 24, 2021
ef52974
Fix
charmoniumQ Feb 5, 2021
b2421de
wip
charmoniumQ Feb 7, 2021
41f631b
id -> name
charmoniumQ Feb 26, 2021
533178e
Experiments
charmoniumQ Mar 2, 2021
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
1 change: 1 addition & 0 deletions .gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,3 +5,4 @@ Build
.idea
doxgen_generated
*.so
common
5 changes: 3 additions & 2 deletions ov_core/src/track/Grider_FAST.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -96,8 +96,9 @@ namespace ov_core {
int ct_rows = std::floor(img.rows/size_y);
std::vector<std::vector<cv::KeyPoint>> collection(ct_cols*ct_rows);
parallel_for_(cv::Range(0, ct_cols*ct_rows), [&](const cv::Range& range) {
// PRINT_RECORD_FOR_THIS_BLOCK("slam2 par_for");
// We time this by timing OpenCV's parallel_for_
#ifdef ILLIXR_INTEGRATION
CPU_TIMER_TIME_BLOCK("perform_griding");
#endif /// ILLIXR_INTEGRATION

for (int r = range.start; r < range.end; r++) {
// Calculate what cell xy value we are in
Expand Down
56 changes: 42 additions & 14 deletions ov_core/src/track/TrackDescriptor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -188,20 +188,30 @@ void TrackDescriptor::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat

// Lets match temporally
#ifdef ILLIXR_INTEGRATION
std::thread t_ll = timed_thread("slam2 rob_match l", &TrackDescriptor::robust_match, this, boost::ref(pts_last[cam_id_left]), boost::ref(pts_left_new),
boost::ref(desc_last[cam_id_left]), boost::ref(desc_left_new), cam_id_left, cam_id_left, boost::ref(matches_ll));
std::thread t_rr = timed_thread("slam2 rob_match r", &TrackDescriptor::robust_match, this, boost::ref(pts_last[cam_id_right]), boost::ref(pts_right_new),
boost::ref(desc_last[cam_id_right]), boost::ref(desc_right_new), cam_id_right, cam_id_right, boost::ref(matches_rr));
parallel_for_(cv::Range(0, 2), [&](const cv::Range& range){
for (int i = range.start; i < range.end; i++) {
CPU_TIMER_TIME_BLOCK("robust_match");
robust_match(
pts_last[i == 0 ? cam_id_left : cam_id_right],
i == 0 ? pts_left_new : pts_right_new,
desc_last[i == 0 ? cam_id_left : cam_id_right],
i == 0 ? desc_left_new : desc_right_new,
i == 0 ? cam_id_left : cam_id_right,
i == 0 ? cam_id_left : cam_id_right,
i == 0 ? matches_ll : matches_rr
);
}
});
#else /// ILLIXR_INTEGRATION
boost::thread t_ll = boost::thread(&TrackDescriptor::robust_match, this, boost::ref(pts_last[cam_id_left]), boost::ref(pts_left_new),
boost::ref(desc_last[cam_id_left]), boost::ref(desc_left_new), cam_id_left, cam_id_left, boost::ref(matches_ll));
boost::thread t_rr = boost::thread(&TrackDescriptor::robust_match, this, boost::ref(pts_last[cam_id_right]), boost::ref(pts_right_new),
boost::ref(desc_last[cam_id_right]), boost::ref(desc_right_new), cam_id_right, cam_id_right, boost::ref(matches_rr));
#endif /// ILLIXR_INTEGRATION

// Wait till both threads finish
t_ll.join();
t_rr.join();
#endif /// ILLIXR_INTEGRATION

rT3 = boost::posix_time::microsec_clock::local_time();

Expand Down Expand Up @@ -352,38 +362,56 @@ void TrackDescriptor::perform_detection_stereo(const cv::Mat &img0, const cv::Ma
// Extract our features (use FAST with griding)
std::vector<cv::KeyPoint> pts0_ext, pts1_ext;
#ifdef ILLIXR_INTEGRATION
std::thread t_0 = timed_thread("slam2 grid l", &Grider_FAST::perform_griding, boost::cref(img0), boost::ref(pts0_ext),
num_features, grid_x, grid_y, threshold, true);
std::thread t_1 = timed_thread("slam2 grid r", &Grider_FAST::perform_griding, boost::cref(img1), boost::ref(pts1_ext),
num_features, grid_x, grid_y, threshold, true);
parallel_for_(cv::Range(0, 2), [&](const cv::Range& range){
for (int i = range.start; i < range.end; i++) {
CPU_TIMER_TIME_BLOCK("perform_griding");
Grider_FAST::perform_griding(
i == 0 ? img0 : img1,
i == 0 ? pts0_ext : pts1_ext,
num_features,
grid_x,
grid_y,
threshold,
true
);
}
});
#else /// ILLIXR_INTEGRATION
boost::thread t_0 = boost::thread(&Grider_FAST::perform_griding, boost::cref(img0), boost::ref(pts0_ext),
num_features, grid_x, grid_y, threshold, true);
boost::thread t_1 = boost::thread(&Grider_FAST::perform_griding, boost::cref(img1), boost::ref(pts1_ext),
num_features, grid_x, grid_y, threshold, true);
#endif /// ILLIXR_INTEGRATION

// Wait till both threads finish
t_0.join();
t_1.join();
#endif /// ILLIXR_INTEGRATION

// For all new points, extract their descriptors
cv::Mat desc0_ext, desc1_ext;

// Use C++11 lamdas so we can pass all theses variables by reference
#ifdef ILLIXR_INTEGRATION
std::thread t_desc0 = timed_thread("slam2 orb l", [&]{this->orb0->compute(img0, pts0_ext, desc0_ext);});
std::thread t_desc1 = timed_thread("slam2 orb r", [&]{this->orb1->compute(img1, pts1_ext, desc1_ext);});
parallel_for_(cv::Range(0, 2), [&](const cv::Range& range){
for (int i = range.start; i < range.end; i++) {
CPU_TIMER_TIME_BLOCK("orb_compute");
(i == 0 ? orb0 : orb1)->compute(
i == 0 ? img0 : img1,
i == 0 ? pts0_ext : pts1_ext,
i == 0 ? desc0_ext : desc1_ext
);
}
});
#else /// ILLIXR_INTEGRATION
// Use C++11 lamdas so we can pass all theses variables by reference
std::thread t_desc0 = std::thread([this,&img0,&pts0_ext,&desc0_ext]{this->orb0->compute(img0, pts0_ext, desc0_ext);});
std::thread t_desc1 = std::thread([this,&img1,&pts1_ext,&desc1_ext]{this->orb1->compute(img1, pts1_ext, desc1_ext);});
//std::thread t_desc0 = std::thread([this,&img0,&pts0_ext,&desc0_ext]{this->freak0->compute(img0, pts0_ext, desc0_ext);});
//std::thread t_desc1 = std::thread([this,&img1,&pts1_ext,&desc1_ext]{this->freak1->compute(img1, pts1_ext, desc1_ext);});
#endif /// ILLIXR_INTEGRATION

// Wait till both threads finish
t_desc0.join();
t_desc1.join();
#endif /// ILLIXR_INTEGRATION

// Do matching from the left to the right image
std::vector<cv::DMatch> matches;
Expand Down
87 changes: 63 additions & 24 deletions ov_core/src/track/TrackKLT.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -143,39 +143,57 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r
std::unique_lock<std::mutex> lck2(mtx_feeds.at(cam_id_right));

cv::Mat img_left, img_right;
std::vector<cv::Mat> imgpyr_left, imgpyr_right;

#ifdef ILLIXR_INTEGRATION
// Histogram equalize
std::thread t_lhe = timed_thread("slam2 hist l", cv::equalizeHist, cv::_InputArray(img_leftin ), cv::_OutputArray(img_left ));
std::thread t_rhe = timed_thread("slam2 hist r", cv::equalizeHist, cv::_InputArray(img_rightin), cv::_OutputArray(img_right));
parallel_for_(cv::Range(0, 2), [&](const cv::Range& range){
for (int i = range.start; i < range.end; i++) {
CPU_TIMER_TIME_BLOCK("hist_and_optical_flow")

// Histogram equalize
cv::equalizeHist(
cv::_InputArray (i == 0 ? img_leftin : img_rightin),
i == 0 ? img_left : img_right
);

// Extract image pyramids (boost seems to require us to put all the arguments even if there are defaults....)
cv::buildOpticalFlowPyramid(
i == 0 ? img_left : img_right,
cv::_OutputArray(i == 0 ? imgpyr_left : imgpyr_right),
win_size,
pyr_levels,
false,
cv::BORDER_REFLECT_101,
cv::BORDER_CONSTANT,
true
);
}
});
#else /// ILLIXR_INTEGRATION
// Histogram equalize
boost::thread t_lhe = boost::thread(cv::equalizeHist, boost::cref(img_leftin), boost::ref(img_left));
boost::thread t_rhe = boost::thread(cv::equalizeHist, boost::cref(img_rightin), boost::ref(img_right));
#endif /// ILLIXR_INTEGRATION

t_lhe.join();
t_rhe.join();

// Extract image pyramids (boost seems to require us to put all the arguments even if there are defaults....)
std::vector<cv::Mat> imgpyr_left, imgpyr_right;
#ifdef ILLIXR_INTEGRATION
std::thread t_lp = timed_thread("slam2 pyramid l", &cv::buildOpticalFlowPyramid, cv::_InputArray(img_left),
cv::_OutputArray(imgpyr_left), win_size, pyr_levels, false,
cv::BORDER_REFLECT_101, cv::BORDER_CONSTANT, true);
std::thread t_rp = timed_thread("slam2 pyramid r", &cv::buildOpticalFlowPyramid, cv::_InputArray(img_right),
cv::_OutputArray(imgpyr_right), win_size, pyr_levels,
false, cv::BORDER_REFLECT_101, cv::BORDER_CONSTANT, true);
#else /// ILLIXR_INTEGRATION
boost::thread t_lp = boost::thread(cv::buildOpticalFlowPyramid, boost::cref(img_left),
boost::ref(imgpyr_left), boost::ref(win_size), boost::ref(pyr_levels), false,
cv::BORDER_REFLECT_101, cv::BORDER_CONSTANT, true);
boost::thread t_rp = boost::thread(cv::buildOpticalFlowPyramid, boost::cref(img_right),
boost::ref(imgpyr_right), boost::ref(win_size), boost::ref(pyr_levels),
false, cv::BORDER_REFLECT_101, cv::BORDER_CONSTANT, true);
#endif /// ILLIXR_INTEGRATION

t_lp.join();
t_rp.join();

#endif /// ILLIXR_INTEGRATION
rT2 = boost::posix_time::microsec_clock::local_time();

{
#ifdef ILLIXR_INTEGRATION
CPU_TIMER_TIME_BLOCK("preform_detection");
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

perform

#endif /// ILLIXR_INTEGRATION
// If we didn't have any successful tracks last time, just extract this time
// This also handles, the tracking initalization on the first call to this extractor
if(pts_last[cam_id_left].empty() || pts_last[cam_id_right].empty()) {
Expand All @@ -195,6 +213,7 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r
pts_last[cam_id_left], pts_last[cam_id_right],
ids_last[cam_id_left], ids_last[cam_id_right]);
rT3 = boost::posix_time::microsec_clock::local_time();
}


//===================================================================================
Expand All @@ -207,20 +226,30 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r

// Lets track temporally
#ifdef ILLIXR_INTEGRATION
std::thread t_ll = timed_thread("slam2 matching l", &TrackKLT::perform_matching, this, boost::cref(img_pyramid_last[cam_id_left]), boost::cref(imgpyr_left),
boost::ref(pts_last[cam_id_left]), boost::ref(pts_left_new), cam_id_left, cam_id_left, boost::ref(mask_ll));
std::thread t_rr = timed_thread("slam2 matching r", &TrackKLT::perform_matching, this, boost::cref(img_pyramid_last[cam_id_right]), boost::cref(imgpyr_right),
boost::ref(pts_last[cam_id_right]), boost::ref(pts_right_new), cam_id_right, cam_id_right, boost::ref(mask_rr));
parallel_for_(cv::Range(0, 2), [&](const cv::Range& range){
for (int i = range.start; i < range.end; i++) {
Comment on lines +229 to +230
Copy link

@e3m3 e3m3 May 13, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Out of curiosity, does this spawn 2 threads, or 3 threads (where the 3rd thread fails the for guard immediately)?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

cv::Range is inclusive of the lower-bound and exclusive of the upper-bound, so the range contains 0 and 1.

CPU_TIMER_TIME_BLOCK("perform_matching");
perform_matching(
img_pyramid_last[i == 0 ? cam_id_left : cam_id_right],
i == 0 ? imgpyr_left : imgpyr_right,
pts_last[i == 0 ? cam_id_left : cam_id_right],
i == 0 ? pts_left_new : pts_right_new,
i == 0 ? cam_id_left : cam_id_right,
i == 0 ? cam_id_left : cam_id_right,
i == 0 ? mask_ll : mask_rr
);
}
});
#else /// ILLIXR_INTEGRATION
boost::thread t_ll = boost::thread(&TrackKLT::perform_matching, this, boost::cref(img_pyramid_last[cam_id_left]), boost::cref(imgpyr_left),
boost::ref(pts_last[cam_id_left]), boost::ref(pts_left_new), cam_id_left, cam_id_left, boost::ref(mask_ll));
boost::thread t_rr = boost::thread(&TrackKLT::perform_matching, this, boost::cref(img_pyramid_last[cam_id_right]), boost::cref(imgpyr_right),
boost::ref(pts_last[cam_id_right]), boost::ref(pts_right_new), cam_id_right, cam_id_right, boost::ref(mask_rr));
#endif /// ILLIXR_INTEGRATION

// Wait till both threads finish
t_ll.join();
t_rr.join();
#endif /// ILLIXR_INTEGRATION

rT4 = boost::posix_time::microsec_clock::local_time();

Expand All @@ -240,7 +269,15 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r
//===================================================================================
//===================================================================================

// Get our "good tracks"
std::vector<cv::KeyPoint> good_left, good_right;
std::vector<size_t> good_ids_left, good_ids_right;

// If any of our masks are empty, that means we didn't have enough to do ransac, so just return
{
#ifdef ILLIXR_INTEGRATION
CPU_TIMER_TIME_BLOCK("find_points");
#endif /// ILLIXR_INTEGRATION
if(mask_ll.empty() || mask_rr.empty()) {
img_last[cam_id_left] = img_left.clone();
img_last[cam_id_right] = img_right.clone();
Expand All @@ -254,10 +291,6 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r
return;
}

// Get our "good tracks"
std::vector<cv::KeyPoint> good_left, good_right;
std::vector<size_t> good_ids_left, good_ids_right;

// Loop through all left points
for(size_t i=0; i<pts_left_new.size(); i++) {
// Ensure we do not have any bad KLT tracks (i.e., points are negative)
Expand Down Expand Up @@ -305,10 +338,15 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r
//std::cout << "adding to right - " << ids_last[cam_id_right].at(i) << std::endl;
}
}
}

//===================================================================================
//===================================================================================

{
#ifdef ILLIXR_INTEGRATION
CPU_TIMER_TIME_BLOCK("database->update_feature");
#endif /// ILLIXR_INTEGRATION
// Update our feature database, with theses new observations
for(size_t i=0; i<good_left.size(); i++) {
cv::Point2f npt_l = undistort_point(good_left.at(i).pt, cam_id_left);
Expand All @@ -333,6 +371,7 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r
ids_last[cam_id_left] = good_ids_left;
ids_last[cam_id_right] = good_ids_right;
rT6 = boost::posix_time::microsec_clock::local_time();
}

#ifndef NDEBUG
// Timing information
Expand Down
14 changes: 11 additions & 3 deletions ov_msckf/src/core/VioManager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -186,14 +186,22 @@ void VioManager::feed_measurement_stereo(double timestamp, cv::Mat& img0, cv::Ma
trackFEATS->feed_stereo(timestamp, img0, img1, cam_id0, cam_id1);
} else {
#ifdef ILLIXR_INTEGRATION
std::thread t_l = timed_thread("slam2 feed l", &TrackBase::feed_monocular, trackFEATS, boost::ref(timestamp), boost::ref(img0), boost::ref(cam_id0));
std::thread t_r = timed_thread("slam2 feed r", &TrackBase::feed_monocular, trackFEATS, boost::ref(timestamp), boost::ref(img1), boost::ref(cam_id1));
parallel_for_(cv::Range(0, 2), [&](const cv::Range& range){
for (int i = range.start; i < range.end; i++) {
trackFEATS->feed_monocular(
timestamp,
i == 0 ? img0 : img1,
i == 0 ? cam_id0 : cam_id1
);
}
});
#else /// ILLIXR_INTEGRATION
boost::thread t_l = boost::thread(&TrackBase::feed_monocular, trackFEATS, boost::ref(timestamp), boost::ref(img0), boost::ref(cam_id0));
boost::thread t_r = boost::thread(&TrackBase::feed_monocular, trackFEATS, boost::ref(timestamp), boost::ref(img1), boost::ref(cam_id1));
#endif /// ILLIXR_INTEGRATION

t_l.join();
t_r.join();
#endif /// ILLIXR_INTEGRATION
}

// If aruoc is avalible, the also pass to it
Expand Down
Loading