Implement parallel PF weight updates using TBB

Author: jlblancoc

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:

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

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  "")

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()

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:

libs/nav/src/tpspace/CPTG_DiffDrive_CollisionGridBased.cpp

@@ -9,6 +9,8 @@
 
 #include "nav-precomp.h"  // Precomp header
 //
+#include <mrpt/core/WorkerThreadsPool.h>
+#include <mrpt/core/get_env.h>
 #include <mrpt/io/CFileGZInputStream.h>
 #include <mrpt/io/CFileGZOutputStream.h>
 #include <mrpt/kinematics/CVehicleVelCmd_DiffDriven.h>
@@ -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);

libs/obs/include/mrpt/obs/CObservation2DRangeScan.h

@@ -8,14 +8,18 @@
    +------------------------------------------------------------------------+ */
 #pragma once
 
+#include <mrpt/containers/NonCopiableData.h>
 #include <mrpt/core/aligned_std_vector.h>
+#include <mrpt/core/lock_helper.h>
 #include <mrpt/maps/CMetricMap.h>
 #include <mrpt/math/CPolygon.h>
 #include <mrpt/obs/CObservation.h>
 #include <mrpt/obs/T2DScanProperties.h>
 #include <mrpt/poses/CPose3D.h>
 #include <mrpt/serialization/CSerializable.h>
 
+#include <mutex>
+
 // 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<std::recursive_mutex> 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 <class POINTSMAP>
   inline const POINTSMAP* getAuxPointsMap() const
   {
+    auto lck = mrpt::lockHelper(m_cachedMapMtx.data);
     return static_cast<const POINTSMAP*>(m_cachedMap.get());
   }
 
@@ -201,6 +207,8 @@ class CObservation2DRangeScan : public CObservation
   template <class POINTSMAP>
   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<const POINTSMAP*>(m_cachedMap.get());
   }

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 "

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()

libs/slam/include/mrpt/slam/PF_implementations.h

@@ -11,6 +11,7 @@
 
 #include <mrpt/bayes/CParticleFilterCapable.h>
 #include <mrpt/bayes/CParticleFilterData.h>
+#include <mrpt/config.h>
 #include <mrpt/math/data_utils.h>     // averageLogLikelihood()
 #include <mrpt/math/distributions.h>  // chi2inv
 #include <mrpt/obs/CActionCollection.h>
@@ -21,6 +22,11 @@
 #include <mrpt/slam/TKLDParams.h>
 
 #include <cmath>
+#include <set>
+
+#if MRPT_HAS_TBB
+#include <tbb/tbb.h>
+#endif
 
 /** \file PF_implementations.h
  *  This file contains the implementations of the template members declared in
@@ -334,17 +340,31 @@ void PF_implementation<PARTICLE_TYPE, MYSELF, STORAGE>::PF_SLAM_implementation_p
     //	UPDATE STAGE
     // ----------------------------------------------------------------------
     // Compute all the likelihood values & update particles weight:
+#if MRPT_HAS_TBB
+    tbb::parallel_for(
+        tbb::blocked_range<size_t>(0, M),
+        [&](const tbb::blocked_range<size_t>& 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.

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