Merge pull request TixiaoShan#168 from valgur/bugfix/cloud_transform

Fix a parallelism bug in transformPointCloud()
emi0116 · Dec 8, 2020 · 18ebdbe · 18ebdbe
2 parents 8145bad + b8d85c3
commit 18ebdbe
Showing 1 changed file with 5 additions and 7 deletions.
diff --git a/src/mapOptmization.cpp b/src/mapOptmization.cpp
@@ -278,8 +278,6 @@ class mapOptimization : public ParamServer
     {
         pcl::PointCloud<PointType>::Ptr cloudOut(new pcl::PointCloud<PointType>());
 
-        PointType *pointFrom;
-
         int cloudSize = cloudIn->size();
         cloudOut->resize(cloudSize);
 
@@ -288,11 +286,11 @@ class mapOptimization : public ParamServer
         #pragma omp parallel for num_threads(numberOfCores)
         for (int i = 0; i < cloudSize; ++i)
         {
-            pointFrom = &cloudIn->points[i];
-            cloudOut->points[i].x = transCur(0,0) * pointFrom->x + transCur(0,1) * pointFrom->y + transCur(0,2) * pointFrom->z + transCur(0,3);
-            cloudOut->points[i].y = transCur(1,0) * pointFrom->x + transCur(1,1) * pointFrom->y + transCur(1,2) * pointFrom->z + transCur(1,3);
-            cloudOut->points[i].z = transCur(2,0) * pointFrom->x + transCur(2,1) * pointFrom->y + transCur(2,2) * pointFrom->z + transCur(2,3);
-            cloudOut->points[i].intensity = pointFrom->intensity;
+            const auto &pointFrom = cloudIn->points[i];
+            cloudOut->points[i].x = transCur(0,0) * pointFrom.x + transCur(0,1) * pointFrom.y + transCur(0,2) * pointFrom.z + transCur(0,3);
+            cloudOut->points[i].y = transCur(1,0) * pointFrom.x + transCur(1,1) * pointFrom.y + transCur(1,2) * pointFrom.z + transCur(1,3);
+            cloudOut->points[i].z = transCur(2,0) * pointFrom.x + transCur(2,1) * pointFrom.y + transCur(2,2) * pointFrom.z + transCur(2,3);
+            cloudOut->points[i].intensity = pointFrom.intensity;
         }
         return cloudOut;
     }