diff --git a/CMakeLists.txt b/CMakeLists.txt index d819359592..44b88ddf66 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -308,6 +308,7 @@ include(cmakemodules/script_SIMD.cmake REQUIRED) # SSE2/SSE3/... optimiza include(cmakemodules/script_simpleini.cmake REQUIRED) # SimpleINI lib include(cmakemodules/script_suitesparse.cmake REQUIRED) # SuiteSparse libs include(cmakemodules/script_swissrange.cmake REQUIRED) # Support for SWISSRANGE 3D camera: +include(cmakemodules/script_tbb.cmake REQUIRED) # TBB include(cmakemodules/script_tinyxml2.cmake REQUIRED) # tinyxml2 lib include(cmakemodules/script_triclops.cmake REQUIRED) # Check for PointGreyResearch (PGR) Triclops library include(cmakemodules/script_videre_svs.cmake REQUIRED) # Support for Videre Design stereo camera: diff --git a/apps/ro-localization/CPosePDFParticlesExtended.h b/apps/ro-localization/CPosePDFParticlesExtended.h index 8f99645b37..13e58f7d0f 100644 --- a/apps/ro-localization/CPosePDFParticlesExtended.h +++ b/apps/ro-localization/CPosePDFParticlesExtended.h @@ -30,8 +30,6 @@ class TExtendedCPose2D mrpt::math::CVectorDouble state; }; -#define DUMMY_LINKAGE - /** Declares a class that represents a Probability Distribution * function (PDF) of a 2D pose (x,y,phi). * This class implements that PDF using a set of particles diff --git a/cmakemodules/script_show_final_summary.cmake b/cmakemodules/script_show_final_summary.cmake index 120854f2b7..18c193acb8 100644 --- a/cmakemodules/script_show_final_summary.cmake +++ b/cmakemodules/script_show_final_summary.cmake @@ -143,6 +143,7 @@ SHOW_CONFIG_LINE_SYSTEM("OpenGL GLES " CMAKE_MRPT_HAS_GL SHOW_CONFIG_LINE_SYSTEM("GLUT " CMAKE_MRPT_HAS_GLUT) SHOW_CONFIG_LINE_SYSTEM("PCAP (Wireshark logs for Velodyne) " CMAKE_MRPT_HAS_LIBPCAP) SHOW_CONFIG_LINE_SYSTEM("SuiteSparse " CMAKE_MRPT_HAS_SUITESPARSE "[Version: ${SuiteSparse_VERSION}]") +SHOW_CONFIG_LINE_SYSTEM("TBB " CMAKE_MRPT_HAS_TBB "[Version: ${TBB_VERSION}]") SHOW_CONFIG_LINE_SYSTEM("tinyxml2 " CMAKE_MRPT_HAS_TINYXML2) SHOW_CONFIG_LINE_SYSTEM("wxWidgets " CMAKE_MRPT_HAS_WXWIDGETS "[Version: ${wxWidgets_VERSION_STRING} ${CMAKE_WXWIDGETS_TOOLKIT_NAME}]") message(STATUS "") diff --git a/cmakemodules/script_tbb.cmake b/cmakemodules/script_tbb.cmake new file mode 100644 index 0000000000..a6ed26147d --- /dev/null +++ b/cmakemodules/script_tbb.cmake @@ -0,0 +1,16 @@ + +set(CMAKE_MRPT_HAS_TBB 0) +set(CMAKE_MRPT_HAS_TBB_SYSTEM 0) + +option(DISABLE_TBB "Force not using TBB" "OFF") +mark_as_advanced(DISABLE_TBB) +if(DISABLE_TBB) + return() +endif() + +find_package(TBB QUIET) + +if(TBB_FOUND) + set(CMAKE_MRPT_HAS_TBB 1) + set(CMAKE_MRPT_HAS_TBB_SYSTEM 1) +endif() diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md index be814b9b42..f4c940d1a5 100644 --- a/doc/source/doxygen-docs/changelog.md +++ b/doc/source/doxygen-docs/changelog.md @@ -2,6 +2,8 @@ # Version 2.14.0: UNRELEASED - Changes in libraries: + - \ref mrpt_slam_grp: + - Particle filtering algorithm pfStandardProposal now uses TBB (if present) for automatically running weight updates in parallel. - \ref mrpt_rtti_grp: - mrpt::rtti::CObject::GetRuntimeClassIdStatic() no longer depends on static variables, but on constexpr. This totally removes the possibility of initialization order fiasco while registering classes. - **IMPORTANT CHANGE**: To make the change above possible, these macros have changed: diff --git a/libs/nav/src/tpspace/CPTG_DiffDrive_CollisionGridBased.cpp b/libs/nav/src/tpspace/CPTG_DiffDrive_CollisionGridBased.cpp index 69823daba5..1834a80abd 100644 --- a/libs/nav/src/tpspace/CPTG_DiffDrive_CollisionGridBased.cpp +++ b/libs/nav/src/tpspace/CPTG_DiffDrive_CollisionGridBased.cpp @@ -9,6 +9,8 @@ #include "nav-precomp.h" // Precomp header // +#include +#include #include #include #include @@ -785,7 +787,7 @@ void CPTG_DiffDrive_CollisionGridBased::internal_initialize( } } // k - if (verbose) cout << format("Done! [%.03f sec]\n", tictac.Tac()); + if (verbose) cout << format("Done! [%.03f sec]", tictac.Tac()) << std::endl; // save it to the cache file for the next run: saveColGridsToFile(cacheFilename, m_robotShape); diff --git a/libs/obs/include/mrpt/obs/CObservation2DRangeScan.h b/libs/obs/include/mrpt/obs/CObservation2DRangeScan.h index 4e07f77cc9..88bf90ac02 100644 --- a/libs/obs/include/mrpt/obs/CObservation2DRangeScan.h +++ b/libs/obs/include/mrpt/obs/CObservation2DRangeScan.h @@ -8,7 +8,9 @@ +------------------------------------------------------------------------+ */ #pragma once +#include #include +#include #include #include #include @@ -16,6 +18,8 @@ #include #include +#include + // Add for declaration of mexplus::from template specialization DECLARE_MEXPLUS_FROM(mrpt::obs::CObservation2DRangeScan) @@ -166,6 +170,7 @@ class CObservation2DRangeScan : public CObservation * It's a generic smart pointer to avoid depending here in the library * mrpt-obs on classes on other libraries. */ + mutable mrpt::containers::NonCopiableData m_cachedMapMtx; mutable mrpt::maps::CMetricMap::Ptr m_cachedMap; /** Internal method, used from buildAuxPointsMap() */ void internal_buildAuxPointsMap(const void* options = nullptr) const; @@ -183,6 +188,7 @@ class CObservation2DRangeScan : public CObservation template inline const POINTSMAP* getAuxPointsMap() const { + auto lck = mrpt::lockHelper(m_cachedMapMtx.data); return static_cast(m_cachedMap.get()); } @@ -201,6 +207,8 @@ class CObservation2DRangeScan : public CObservation template inline const POINTSMAP* buildAuxPointsMap(const void* options = nullptr) const { + auto lck = mrpt::lockHelper(m_cachedMapMtx.data); + if (!m_cachedMap) internal_buildAuxPointsMap(options); return static_cast(m_cachedMap.get()); } diff --git a/libs/obs/src/CObservation2DRangeScan.cpp b/libs/obs/src/CObservation2DRangeScan.cpp index c67958a55b..0441ad13ba 100644 --- a/libs/obs/src/CObservation2DRangeScan.cpp +++ b/libs/obs/src/CObservation2DRangeScan.cpp @@ -178,6 +178,7 @@ void CObservation2DRangeScan::serializeFrom(mrpt::serialization::CArchive& in, u MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version); }; + auto lck = mrpt::lockHelper(m_cachedMapMtx.data); m_cachedMap.reset(); } @@ -423,6 +424,8 @@ void internal_set_build_points_map_from_scan2D(scan2pts_functor fn) ---------------------------------------------------------------*/ void CObservation2DRangeScan::internal_buildAuxPointsMap(const void* options) const { + auto lck = mrpt::lockHelper(m_cachedMapMtx.data); + if (!ptr_internal_build_points_map_from_scan2D) throw std::runtime_error( "[CObservation2DRangeScan::buildAuxPointsMap] ERROR: This function " diff --git a/libs/slam/CMakeLists.txt b/libs/slam/CMakeLists.txt index dde709aadd..efa99147ad 100644 --- a/libs/slam/CMakeLists.txt +++ b/libs/slam/CMakeLists.txt @@ -10,6 +10,12 @@ list(APPEND slam_EXTRA_SRCS list(APPEND slam_EXTRA_SRCS_NAME "slam" "slam" "slam-headers") +if(CMAKE_MRPT_HAS_TBB) + set(tbb_dep TBB) +else() + set(tbb_dep "") +endif() + #--------------------------------------------- # Macro declared in "DeclareMRPTLib.cmake": #--------------------------------------------- @@ -19,8 +25,13 @@ define_mrpt_lib( # Dependencies mrpt-vision mrpt-maps + # Other imported targets: + ${tbb_dep} # find_package() lib name ) if(BUILD_mrpt-slam) + if (CMAKE_MRPT_HAS_TBB) + target_link_libraries(slam PUBLIC TBB::tbb) + endif() endif() diff --git a/libs/slam/include/mrpt/slam/PF_implementations.h b/libs/slam/include/mrpt/slam/PF_implementations.h index e3bbdb1de4..12f1c11eb8 100644 --- a/libs/slam/include/mrpt/slam/PF_implementations.h +++ b/libs/slam/include/mrpt/slam/PF_implementations.h @@ -11,6 +11,7 @@ #include #include +#include #include // averageLogLikelihood() #include // chi2inv #include @@ -21,6 +22,11 @@ #include #include +#include + +#if MRPT_HAS_TBB +#include +#endif /** \file PF_implementations.h * This file contains the implementations of the template members declared in @@ -334,17 +340,31 @@ void PF_implementation::PF_SLAM_implementation_p // UPDATE STAGE // ---------------------------------------------------------------------- // Compute all the likelihood values & update particles weight: +#if MRPT_HAS_TBB + tbb::parallel_for( + tbb::blocked_range(0, M), + [&](const tbb::blocked_range& r) + { + for (size_t i = r.begin(); i != r.end(); ++i) + { +#else for (size_t i = 0; i < M; i++) { - bool pose_is_valid; - const mrpt::math::TPose3D partPose = - getLastPose(i, pose_is_valid); // Take the particle data: - auto partPose2 = mrpt::poses::CPose3D(partPose); - const double obs_log_lik = - PF_SLAM_computeObservationLikelihoodForParticle(PF_options, i, *sf, partPose2); - ASSERT_(!std::isnan(obs_log_lik) && std::isfinite(obs_log_lik)); - me->m_particles[i].log_w += obs_log_lik * PF_options.powFactor; +#endif + bool pose_is_valid; + const mrpt::math::TPose3D partPose = + getLastPose(i, pose_is_valid); // Take the particle data: + auto partPose2 = mrpt::poses::CPose3D(partPose); + const double obs_log_lik = + PF_SLAM_computeObservationLikelihoodForParticle(PF_options, i, *sf, partPose2); + ASSERT_(!std::isnan(obs_log_lik) && std::isfinite(obs_log_lik)); + me->m_particles[i].log_w += obs_log_lik * PF_options.powFactor; +#if MRPT_HAS_TBB + } // for each particle "i" + }); +#else } // for each particle "i" +#endif // Normalization of weights is done outside of this method // automatically. diff --git a/parse-files/config.h.in b/parse-files/config.h.in index fd336a9306..3eaacdb248 100644 --- a/parse-files/config.h.in +++ b/parse-files/config.h.in @@ -112,6 +112,8 @@ ${CMAKE_MRPT_BUILD_SHARED_LIB} #define MRPT_HAS_SIMPLEINI ${CMAKE_MRPT_HAS_SIMPLEINI} #define MRPT_HAS_SIMPLEINI_SYSTEM ${CMAKE_MRPT_HAS_SIMPLEINI_SYSTEM} +#define MRPT_HAS_TBB ${CMAKE_MRPT_HAS_TBB} + #cmakedefine MRPT_ROS_VERSION ${MRPT_ROS_VERSION} /** These two values are detected in Eigen when building MRPT, so we have