Adds in a class for performing parallel operations with particular th…

…read counts.

cpp/kiss_icp/core/Deskew.cpp

@@ -21,8 +21,7 @@
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 // SOFTWARE.
 #include "Deskew.hpp"
-
-#include <tbb/parallel_for.h>
+#include "Parallel.hpp"
 
 #include <Eigen/Core>
 #include <sophus/se3.hpp>
@@ -36,11 +35,15 @@ constexpr double mid_pose_timestamp{0.5};
 namespace kiss_icp {
 std::vector<Eigen::Vector3d> DeSkewScan(const std::vector<Eigen::Vector3d> &frame,
                                         const std::vector<double> &timestamps,
-                                        const Sophus::SE3d &delta) {
+                                        const Sophus::SE3d &delta,
+                                        int max_n_threads) {
+    // We trust this will not change as the config will not change over the duration of the run.
+    static parallel::NParallelFor n_parallel_for(max_n_threads);
+
     const auto delta_pose = delta.log();
     std::vector<Eigen::Vector3d> corrected_frame(frame.size());
-    // TODO(All): This tbb execution is ignoring the max_n_threads config value
-    tbb::parallel_for(size_t(0), frame.size(), [&](size_t i) {
+
+    n_parallel_for(size_t(0), frame.size(), [&](size_t i) {
         const auto motion = Sophus::SE3d::exp((timestamps[i] - mid_pose_timestamp) * delta_pose);
         corrected_frame[i] = motion * frame[i];
     });

cpp/kiss_icp/core/Deskew.hpp

@@ -31,6 +31,7 @@ namespace kiss_icp {
 /// Compensate the frame by interpolating the delta pose
 std::vector<Eigen::Vector3d> DeSkewScan(const std::vector<Eigen::Vector3d> &frame,
                                         const std::vector<double> &timestamps,
-                                        const Sophus::SE3d &delta);
+                                        const Sophus::SE3d &delta,
+                                        int max_n_threads);
 
 }  // namespace kiss_icp

cpp/kiss_icp/core/Parallel.cpp

@@ -0,0 +1,10 @@
+#include "Parallel.hpp"
+
+namespace kiss_icp::parallel {
+NParallel::NParallel(int num_threads) : number_of_threads_(num_threads), arena_(number_of_threads_) {}
+
+auto NParallel::get_max_threads() -> int {
+    return number_of_threads_;
+}
+
+}

cpp/kiss_icp/core/Parallel.hpp

@@ -0,0 +1,37 @@
+#include <tbb/parallel_for.h>
+#include <tbb/parallel_reduce.h>
+#include <tbb/task_arena.h>
+
+namespace kiss_icp::parallel {
+
+class NParallel{
+public:
+    NParallel(int n);
+    auto get_max_threads() -> int;
+
+    template <typename Index, typename Function>
+    void n_for(Index begin, Index end, const Function& func);
+
+    template <typename Index, typename Function>
+    void n_reduce(Index begin, Index end, const Function& func);
+
+private:
+    int number_of_threads_;
+    tbb::task_arena arena_;
+}
+
+template <typename Index, typename Function>
+void NParallel::n_for()(Index begin, Index end, const Function& func) {
+    arena_.execute([&]() {
+        tbb::parallel_for(begin, end, func);
+    });
+}
+
+template <typename Index, typename Function>
+Value NParallel::n_reduce()(const Range& range, const Value& identity, const RealBody& real_body, const Reduction& reduction); {
+    return arena_.execute([&]() -> Value {
+        return tbb::parallel_reduce(range, identity, real_body, reduction);
+    });
+}
+
+}  // namespace kiss_icp::parallel

cpp/kiss_icp/pipeline/KissICP.cpp

@@ -37,7 +37,7 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector<Eigen::Vec
                                                     const std::vector<double> &timestamps) {
     const auto &deskew_frame = [&]() -> std::vector<Eigen::Vector3d> {
         if (!config_.deskew || timestamps.empty()) return frame;
-        return DeSkewScan(frame, timestamps, last_delta_);
+        return DeSkewScan(frame, timestamps, last_delta_, config_.max_num_threads);
     }();
     return RegisterFrame(deskew_frame);
 }