Issue 209 cpu timer

.gitignore

@@ -5,3 +5,4 @@ Build
 .idea
 doxgen_generated
 *.so
+common

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<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

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<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;

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<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");
+#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<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();
@@ -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)
@@ -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);
@@ -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

ov_msckf/src/core/VioManager.cpp

@@ -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