diff --git a/.gitignore b/.gitignore index 1f1b57b24..08a5f8187 100644 --- a/.gitignore +++ b/.gitignore @@ -5,3 +5,4 @@ Build .idea doxgen_generated *.so +common diff --git a/ov_core/src/track/Grider_FAST.h b/ov_core/src/track/Grider_FAST.h index 9a043b16f..ee78cf6bf 100644 --- a/ov_core/src/track/Grider_FAST.h +++ b/ov_core/src/track/Grider_FAST.h @@ -96,8 +96,9 @@ namespace ov_core { int ct_rows = std::floor(img.rows/size_y); std::vector> 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 diff --git a/ov_core/src/track/TrackDescriptor.cpp b/ov_core/src/track/TrackDescriptor.cpp index 734c5a5d7..0df8bc4b2 100644 --- a/ov_core/src/track/TrackDescriptor.cpp +++ b/ov_core/src/track/TrackDescriptor.cpp @@ -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(); @@ -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 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 matches; diff --git a/ov_core/src/track/TrackKLT.cpp b/ov_core/src/track/TrackKLT.cpp index db20a15db..c328a57fc 100644 --- a/ov_core/src/track/TrackKLT.cpp +++ b/ov_core/src/track/TrackKLT.cpp @@ -143,39 +143,57 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r std::unique_lock lck2(mtx_feeds.at(cam_id_right)); cv::Mat img_left, img_right; + std::vector 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 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"); +#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()) { @@ -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(); + } //=================================================================================== @@ -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++) { + 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(); @@ -240,7 +269,15 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r //=================================================================================== //=================================================================================== + // Get our "good tracks" + std::vector good_left, good_right; + std::vector 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(); @@ -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 good_left, good_right; - std::vector good_ids_left, good_ids_right; - // Loop through all left points for(size_t i=0; iupdate_feature"); +#endif /// ILLIXR_INTEGRATION // Update our feature database, with theses new observations for(size_t i=0; ifeed_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 diff --git a/ov_msckf/src/slam2.cpp b/ov_msckf/src/slam2.cpp index a60e7bda3..277b55ec5 100644 --- a/ov_msckf/src/slam2.cpp +++ b/ov_msckf/src/slam2.cpp @@ -183,45 +183,27 @@ class slam2 : public plugin { , sb{pb->lookup_impl()} , _m_pose{sb->get_writer("slow_pose")} , _m_imu_integrator_input{sb->get_writer("imu_integrator_input")} - , _m_begin{std::chrono::system_clock::now()} , open_vins_estimator{manager_params} , imu_cam_buffer{nullptr} { - _m_pose.put(_m_pose.allocate( - std::chrono::time_point{}, - Eigen::Vector3f{0, 0, 0}, - Eigen::Quaternionf{1, 0, 0, 0} - )); - - // Disabling OpenCV threading is faster on x86 desktop but slower on - // jetson. Keeping this here for manual disabling. - // cv::setNumThreads(0); - -#ifdef CV_HAS_METRICS - cv::metrics::setAccount(new std::string{"-1"}); -#endif - - } - - - virtual void start() override { - plugin::start(); - sb->schedule(id, "imu_cam", [&](switchboard::ptr datum, std::size_t iteration_no) { + sb->schedule(get_name(), "imu_cam", [&](switchboard::ptr datum, std::size_t iteration_no) { this->feed_imu_cam(datum, iteration_no); }); } - void feed_imu_cam(switchboard::ptr datum, std::size_t iteration_no) { // Ensures that slam doesnt start before valid IMU readings come in - if (datum == NULL) { - assert(previous_timestamp == 0); - return; + double timestamp_in_seconds; + { + CPU_TIMER_TIME_BLOCK("IMU"); + if (datum == nullptr) { + std::cerr << "slam2:feed_imu_cam datum == nullptr\n"; + abort(); } // This ensures that every data point is coming in chronological order If youre failing this assert, // make sure that your data folder matches the name in offline_imu_cam/plugin.cc - double timestamp_in_seconds = (double(datum->dataset_time) / NANO_SEC); + timestamp_in_seconds = (double(datum->dataset_time) / NANO_SEC); assert(timestamp_in_seconds > previous_timestamp); previous_timestamp = timestamp_in_seconds; @@ -240,20 +222,27 @@ class slam2 : public plugin { imu_cam_buffer = datum; return; } - -#ifdef CV_HAS_METRICS - cv::metrics::setAccount(new std::string{std::to_string(iteration_no)}); - if (iteration_no % 20 == 0) { - cv::metrics::dump(); } -#else -#warning "No OpenCV metrics available. Please recompile OpenCV from git clone --branch 3.4.6-instrumented https://github.com/ILLIXR/opencv/. (see install_deps.sh)" -#endif - cv::Mat img0{imu_cam_buffer->img0.value()}; - cv::Mat img1{imu_cam_buffer->img1.value()}; + { + CPU_TIMER_TIME_BLOCK("cam"); + cv::Mat img0; + cv::Mat img1; + + { +#ifdef ILLIXR_INTEGRATION + CPU_TIMER_TIME_BLOCK("cv::Mat copy"); +#endif /// ILLIXR_INTEGRATION + img0 = cv::Mat{imu_cam_buffer->img0.value()}; + img1 = cv::Mat{imu_cam_buffer->img1.value()}; + } double buffer_timestamp_seconds = double(imu_cam_buffer->dataset_time) / NANO_SEC; - open_vins_estimator.feed_measurement_stereo(buffer_timestamp_seconds, img0, img1, 0, 1); + { +#ifdef ILLIXR_INTEGRATION + CPU_TIMER_TIME_BLOCK("feed_measurement_stereo"); +#endif /// ILLIXR_INTEGRATION + open_vins_estimator.feed_measurement_stereo(buffer_timestamp_seconds, img0, img1, 0, 1); + } // Get the pose returned from SLAM state = open_vins_estimator.get_state(); @@ -274,6 +263,10 @@ class slam2 : public plugin { assert(isfinite(swapped_pos[2])); if (open_vins_estimator.initialized()) { +#ifdef ILLIXR_INTEGRATION + CPU_TIMER_TIME_BLOCK("publish"); +#endif /// ILLIXR_INTEGRATION + if (isUninitialized) { isUninitialized = false; } @@ -312,6 +305,7 @@ class slam2 : public plugin { // const_cast(imu_cam_buffer)->img0.reset(); // const_cast(imu_cam_buffer)->img1.reset(); imu_cam_buffer = datum; + } } virtual ~slam2() override {} @@ -319,8 +313,8 @@ class slam2 : public plugin { private: const std::shared_ptr sb; switchboard::writer _m_pose; - switchboard::writer _m_imu_integrator_input; - time_type _m_begin; + switchboard::writer _m_imu_integrator_input; + State *state; VioManagerOptions manager_params = create_params();