Added multi-threaded scan matching using OpenMP

gridfastslam/CMakeLists.txt

@@ -1,9 +1,17 @@
+find_package(OpenMP REQUIRED)
+if(OPENMP_FOUND)
+  set(OpenMP_LIBS gomp)
+  set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+endif()
+
 add_library(gridfastslam
     gfsreader.cpp
     gridslamprocessor.cpp
     gridslamprocessor_tree.cpp
     motionmodel.cpp
 )
-target_link_libraries(gridfastslam scanmatcher sensor_range)
+
+target_link_libraries(gridfastslam scanmatcher sensor_range ${OpenMP_LIBS})
 
 install(TARGETS gridfastslam DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})

include/gmapping/gridfastslam/gridslamprocessor.h

@@ -15,6 +15,8 @@
 #include <gmapping/scanmatcher/scanmatcher.h>
 #include "motionmodel.h"
 
+#include <omp.h>
+
 
 namespace GMapping {
 
@@ -305,11 +307,10 @@ namespace GMapping {
     // stream in which to write the messages
     std::ostream& m_infoStream;
 
-
     // the functions below performs side effect on the internal structure,
     //should be called only inside the processScan method
   private:
-
+    std::vector<double> durations;
     /**scanmatches all the particles*/
     inline void scanMatch(const double *plainReading);
     /**normalizes the particle weights*/

include/gmapping/gridfastslam/gridslamprocessor.hxx

@@ -8,15 +8,20 @@
 If the scan matching fails, the particle gets a default likelihood.*/
 inline void GridSlamProcessor::scanMatch(const double* plainReading){
   // sample a new pose from each scan in the reference
-
+  double avg_duration = 0;
   double sumScore=0;
-  for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){
+  double start = omp_get_wtime();
+
+#pragma omp parallel for reduction(+:sumScore)
+  for (int i = 0; i < m_particles.size(); i++){
     OrientedPoint corrected;
     double score, l, s;
-    score=m_matcher.optimize(corrected, it->map, it->pose, plainReading);
+
+    score=m_matcher.optimize(corrected, m_particles[i].map, m_particles[i].pose, plainReading);
     //    it->pose=corrected;
     if (score>m_minimumScore){
-      it->pose=corrected;
+      m_particles[i].pose=corrected;
+      // m_infoStream << "Pose was corrected!" << std::endl;
     } else {
 	if (m_infoStream){
 	  m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <<std::endl;
@@ -25,18 +30,29 @@ inline void GridSlamProcessor::scanMatch(const double* plainReading){
 	}
     }
 
-    m_matcher.likelihoodAndScore(s, l, it->map, it->pose, plainReading);
+    m_matcher.likelihoodAndScore(s, l, m_particles[i].map, m_particles[i].pose, plainReading);
     sumScore+=score;
-    it->weight+=l;
-    it->weightSum+=l;
+    m_particles[i].weight+=l;
+    m_particles[i].weightSum+=l;
 
     //set up the selective copy of the active area
     //by detaching the areas that will be updated
     m_matcher.invalidateActiveArea();
-    m_matcher.computeActiveArea(it->map, it->pose, plainReading);
+    m_matcher.computeActiveArea(m_particles[i].map, m_particles[i].pose, plainReading);
   }
-  if (m_infoStream)
+
+  double duration = omp_get_wtime() - start;
+  durations.push_back(duration);
+  for(int i = 0; i < durations.size(); i++) {
+    avg_duration += durations[i];
+  }
+  if(durations.size() > 0) avg_duration /= durations.size();
+
+
+  if (m_infoStream) {
     m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl;	
+    m_infoStream << "Average Scan Matching Time=" << avg_duration << "s" << std::endl;
+  }
 }
 
 inline void GridSlamProcessor::normalize(){