Use lambda as parallel_body in old OpenCV

Dockerfile

@@ -1,10 +1,15 @@
 FROM ros:noetic-robot
 
-RUN apt-get update && apt-get install -y libeigen3-dev git cmake ninja-build clang
+RUN apt-get update && apt-get install -y libeigen3-dev git cmake ninja-build clang build-essential python3-catkin-tools python3-osrf-pycommon python3-catkin-pkg
 
 RUN git clone https://github.com/opencv/opencv/
 RUN git clone https://github.com/opencv/opencv_contrib/
-RUN cmake -D OPENCV_EXTRA_MODULES_PATH=opencv_contrib/modules -S opencv -B opencv/build -G Ninja
+RUN cmake -D OPENCV_EXTRA_MODULES_PATH=../../opencv_contrib/modules -S opencv -B opencv/build -G Ninja
 RUN sudo ninja -C opencv/build install
 
-RUN sudo apt-get install -y python3-catkin-tools python3-osrf-pycommon python3-catkin-pkg
+RUN sh -c 'echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc'
+
+ENTRYPOINT /bin/bash
+
+# docker build . --tag open_vins
+# docker run --rm --interactive --tty --volume "${PWD}":/workspace/src/open_vins -w /workspace/ open_vins

ov_core/src/track/Grider_FAST.h

@@ -31,26 +31,10 @@
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 
+#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<void(const cv::Range &)> &body) {
-            _body = body;
-        }
-        void operator() (const cv::Range & range) const override {
-            _body(range);
-        }    
-    private:
-        std::function<void(const cv::Range &)> _body;
-    };
-
     /**
      * @brief Extracts FAST features in a grid pattern.
      *

ov_core/src/track/TrackAruco.cpp

@@ -19,7 +19,7 @@
  * along with this program.  If not, see <https://www.gnu.org/licenses/>.
  */
 #include "TrackAruco.h"
-
+#include "../utils/lambda_body.h"
 
 using namespace ov_core;
 
@@ -121,15 +121,15 @@ 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,
                 i == 0 ? img_leftin : img_rightin,
                 i == 0 ? cam_id_left : cam_id_right
             );
         }
-    });
+    }));
 }
 
 

ov_core/src/track/TrackDescriptor.cpp

@@ -19,7 +19,7 @@
  * along with this program.  If not, see <https://www.gnu.org/licenses/>.
  */
 #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<cv::DMatch> 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<cv::KeyPoint> 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<cv::DMatch> matches;

ov_core/src/track/TrackKLT.cpp

@@ -19,7 +19,7 @@
  * along with this program.  If not, see <https://www.gnu.org/licenses/>.
  */
 #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<cv::Mat> imgpyr_left, imgpyr_right;
     //cv::Ptr<cv::CLAHE> 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<cv::KeyPoint> 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();
 
 

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 <https://www.gnu.org/licenses/>.
+ */
+#ifndef LAMBDA_BODY_H
+#define LAMBDA_BODY_H
+
+#include <opencv2/opencv.hpp>
+#include <functional>
+
+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<void(const cv::Range &)> &body) {
+            _body = body;
+        }
+        void operator() (const cv::Range & range) const override {
+            _body(range);
+        }
+    private:
+        std::function<void(const cv::Range &)> _body;
+    };
+
+} /* namespace ov_core */
+
+#endif /* LAMBDA_BODY_H */

ov_msckf/src/core/VioManager.cpp

@@ -22,6 +22,7 @@
 
 #include <memory>
 #include "types/Landmark.h"
+#include "../../../ov_core/src/utils/lambda_body.h"
 
 
 
@@ -331,15 +332,15 @@ 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,
 						message.images.at(i),
 						message.sensor_ids.at(i)
                     );
                 }
-            });
+            }));
         }
     } else {
         printf(RED "invalid number of images passed %d, we only support mono or stereo tracking",num_images);