diff --git a/cpp/kiss_icp/core/Deskew.cpp b/cpp/kiss_icp/core/Deskew.cpp index fe20b77e..324d45c6 100644 --- a/cpp/kiss_icp/core/Deskew.cpp +++ b/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 +#include "Parallel.hpp" #include #include @@ -36,11 +35,15 @@ constexpr double mid_pose_timestamp{0.5}; namespace kiss_icp { std::vector DeSkewScan(const std::vector &frame, const std::vector ×tamps, - 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 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]; }); diff --git a/cpp/kiss_icp/core/Deskew.hpp b/cpp/kiss_icp/core/Deskew.hpp index e229063a..6cc8ca8a 100644 --- a/cpp/kiss_icp/core/Deskew.hpp +++ b/cpp/kiss_icp/core/Deskew.hpp @@ -31,6 +31,7 @@ namespace kiss_icp { /// Compensate the frame by interpolating the delta pose std::vector DeSkewScan(const std::vector &frame, const std::vector ×tamps, - const Sophus::SE3d &delta); + const Sophus::SE3d &delta, + int max_n_threads); } // namespace kiss_icp diff --git a/cpp/kiss_icp/core/Parallel.cpp b/cpp/kiss_icp/core/Parallel.cpp new file mode 100644 index 00000000..86c9ed2f --- /dev/null +++ b/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_; +} + +} diff --git a/cpp/kiss_icp/core/Parallel.hpp b/cpp/kiss_icp/core/Parallel.hpp new file mode 100644 index 00000000..596f5d42 --- /dev/null +++ b/cpp/kiss_icp/core/Parallel.hpp @@ -0,0 +1,37 @@ +#include +#include +#include + +namespace kiss_icp::parallel { + +class NParallel{ +public: + NParallel(int n); + auto get_max_threads() -> int; + + template + void n_for(Index begin, Index end, const Function& func); + + template + void n_reduce(Index begin, Index end, const Function& func); + +private: + int number_of_threads_; + tbb::task_arena arena_; +} + +template +void NParallel::n_for()(Index begin, Index end, const Function& func) { + arena_.execute([&]() { + tbb::parallel_for(begin, end, func); + }); +} + +template +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 diff --git a/cpp/kiss_icp/pipeline/KissICP.cpp b/cpp/kiss_icp/pipeline/KissICP.cpp index 6c8f168a..811bceff 100644 --- a/cpp/kiss_icp/pipeline/KissICP.cpp +++ b/cpp/kiss_icp/pipeline/KissICP.cpp @@ -37,7 +37,7 @@ KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector ×tamps) { const auto &deskew_frame = [&]() -> std::vector { 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); }