From e22f1c67cebe6bd591d487b192c804417963141c Mon Sep 17 00:00:00 2001 From: Samuel Grayson Date: Wed, 19 May 2021 16:14:40 -0500 Subject: [PATCH] Use lambda as parallel_body in old OpenCV --- ov_core/src/track/Grider_FAST.h | 18 +--------- ov_core/src/track/TrackAruco.cpp | 6 ++-- ov_core/src/track/TrackDescriptor.cpp | 10 +++--- ov_core/src/track/TrackKLT.cpp | 10 +++--- ov_core/src/utils/lambda_body.h | 48 +++++++++++++++++++++++++++ ov_msckf/src/core/VioManager.cpp | 5 +-- 6 files changed, 65 insertions(+), 32 deletions(-) create mode 100644 ov_core/src/utils/lambda_body.h diff --git a/ov_core/src/track/Grider_FAST.h b/ov_core/src/track/Grider_FAST.h index 3777df626..c8438400f 100644 --- a/ov_core/src/track/Grider_FAST.h +++ b/ov_core/src/track/Grider_FAST.h @@ -31,26 +31,10 @@ #include #include +#include "../utils/lambda_body.h" namespace ov_core { - /** - * This is a utility class required to build with older version of opencv - * On newer versions this doesn't seem to be needed, but here we just use it to ensure we can work for more opencv version. - * https://answers.opencv.org/question/65800/how-to-use-lambda-as-a-parameter-to-parallel_for_/?answer=130691#post-id-130691 - */ - class LambdaBody : public cv::ParallelLoopBody { - public: - explicit LambdaBody(const std::function &body) { - _body = body; - } - void operator() (const cv::Range & range) const override { - _body(range); - } - private: - std::function _body; - }; - /** * @brief Extracts FAST features in a grid pattern. * diff --git a/ov_core/src/track/TrackAruco.cpp b/ov_core/src/track/TrackAruco.cpp index b818da2fb..c61c8e172 100644 --- a/ov_core/src/track/TrackAruco.cpp +++ b/ov_core/src/track/TrackAruco.cpp @@ -19,7 +19,7 @@ * along with this program. If not, see . */ #include "TrackAruco.h" - +#include "../utils/lambda_body.h" using namespace ov_core; @@ -121,7 +121,7 @@ void TrackAruco::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img // There is not such thing as stereo tracking for aruco // Thus here we should just call the monocular version two times - parallel_for_(cv::Range(0, 2), [&](const cv::Range& range){ + parallel_for_(cv::Range(0, 2), LambdaBody([&](const cv::Range& range){ for (int i = range.start; i < range.end; i++) { feed_monocular( timestamp, @@ -129,7 +129,7 @@ void TrackAruco::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img i == 0 ? cam_id_left : cam_id_right ); } - }); + })); } diff --git a/ov_core/src/track/TrackDescriptor.cpp b/ov_core/src/track/TrackDescriptor.cpp index 062459c61..b3bdf65bb 100644 --- a/ov_core/src/track/TrackDescriptor.cpp +++ b/ov_core/src/track/TrackDescriptor.cpp @@ -19,7 +19,7 @@ * along with this program. If not, see . */ #include "TrackDescriptor.h" - +#include "../utils/lambda_body.h" using namespace ov_core; @@ -182,7 +182,7 @@ void TrackDescriptor::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat // Our matches temporally std::vector matches_ll, matches_rr; - parallel_for_(cv::Range(0, 2), [&](const cv::Range& range){ + parallel_for_(cv::Range(0, 2), LambdaBody([&](const cv::Range& range){ for (int i = range.start; i < range.end; i++) { robust_match( pts_last[i == 0 ? cam_id_left : cam_id_right], @@ -194,7 +194,7 @@ void TrackDescriptor::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat i == 0 ? matches_ll : matches_rr ); } - }); + })); rT3 = boost::posix_time::microsec_clock::local_time(); @@ -344,7 +344,7 @@ void TrackDescriptor::perform_detection_stereo(const cv::Mat &img0, const cv::Ma // Extract our features (use FAST with griding), and thier descriptors std::vector pts0_ext, pts1_ext; cv::Mat desc0_ext, desc1_ext; - parallel_for_(cv::Range(0, 2), [&](const cv::Range& range){ + parallel_for_(cv::Range(0, 2), LambdaBody([&](const cv::Range& range){ for (int i = range.start; i < range.end; i++) { Grider_FAST::perform_griding( i == 0 ? img0 : img1, @@ -361,7 +361,7 @@ void TrackDescriptor::perform_detection_stereo(const cv::Mat &img0, const cv::Ma i == 0 ? desc0_ext : desc1_ext ); } - }); + })); // 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 be6361298..2d96e7d55 100644 --- a/ov_core/src/track/TrackKLT.cpp +++ b/ov_core/src/track/TrackKLT.cpp @@ -19,7 +19,7 @@ * along with this program. If not, see . */ #include "TrackKLT.h" - +#include "../utils/lambda_body.h" using namespace ov_core; @@ -146,7 +146,7 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r cv::Mat img_left, img_right; std::vector imgpyr_left, imgpyr_right; //cv::Ptr clahe = cv::createCLAHE(eq_clip_limit, eq_win_size); - parallel_for_(cv::Range(0, 2), [&](const cv::Range& range){ + parallel_for_(cv::Range(0, 2), LambdaBody([&](const cv::Range& range){ for (int i = range.start; i < range.end; i++) { // Histogram equalize @@ -169,7 +169,7 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r true ); } - }); + })); rT2 = boost::posix_time::microsec_clock::local_time(); // If we didn't have any successful tracks last time, just extract this time @@ -202,7 +202,7 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r std::vector pts_right_new = pts_last[cam_id_right]; // Lets track temporally - parallel_for_(cv::Range(0, 2), [&](const cv::Range& range){ + parallel_for_(cv::Range(0, 2), LambdaBody([&](const cv::Range& range){ for (int i = range.start; i < range.end; i++) { perform_matching( img_pyramid_last[i == 0 ? cam_id_left : cam_id_right], @@ -214,7 +214,7 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r i == 0 ? mask_ll : mask_rr ); } - }); + })); rT4 = boost::posix_time::microsec_clock::local_time(); diff --git a/ov_core/src/utils/lambda_body.h b/ov_core/src/utils/lambda_body.h new file mode 100644 index 000000000..45480a45a --- /dev/null +++ b/ov_core/src/utils/lambda_body.h @@ -0,0 +1,48 @@ +/* + * OpenVINS: An Open Platform for Visual-Inertial Research + * Copyright (C) 2019 Patrick Geneva + * Copyright (C) 2019 Kevin Eckenhoff + * Copyright (C) 2019 Guoquan Huang + * Copyright (C) 2019 OpenVINS Contributors + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#ifndef LAMBDA_BODY_H +#define LAMBDA_BODY_H + +#include +#include + +namespace ov_core { + + /** + * This is a utility class required to build with older version of opencv + * On newer versions this doesn't seem to be needed, but here we just use it to ensure we can work for more opencv version. + * https://answers.opencv.org/question/65800/how-to-use-lambda-as-a-parameter-to-parallel_for_/?answer=130691#post-id-130691 + */ + class LambdaBody : public cv::ParallelLoopBody { + public: + explicit LambdaBody(const std::function &body) { + _body = body; + } + void operator() (const cv::Range & range) const override { + _body(range); + } + private: + std::function _body; + }; + +} /* namespace ov_core */ + +#endif /* LAMBDA_BODY_H */ diff --git a/ov_msckf/src/core/VioManager.cpp b/ov_msckf/src/core/VioManager.cpp index 1d477c8cb..6ef3056f6 100644 --- a/ov_msckf/src/core/VioManager.cpp +++ b/ov_msckf/src/core/VioManager.cpp @@ -22,6 +22,7 @@ #include #include "types/Landmark.h" +#include "../../../ov_core/src/utils/lambda_body.h" @@ -331,7 +332,7 @@ void VioManager::track_image_and_update(const ov_core::CameraData &message_const message.images.at(0), message.images.at(1), message.sensor_ids.at(0), message.sensor_ids.at(1)); } else { - parallel_for_(cv::Range(0, 2), [&](const cv::Range& range){ + parallel_for_(cv::Range(0, 2), LambdaBody[&]((const cv::Range& range){ for (int i = range.start; i < range.end; i++) { trackFEATS->feed_monocular( message.timestamp, @@ -339,7 +340,7 @@ void VioManager::track_image_and_update(const ov_core::CameraData &message_const message.sensor_ids.at(i) ); } - }); + })); } } else { printf(RED "invalid number of images passed %d, we only support mono or stereo tracking",num_images);