perf(map_based_prediction): performance improvement of interpolateRef…

…erencePath with cumulative sum (#810)

perf(map_based_prediction): performance improvement of interpolateReferencePath with cumulative sum (autowarefoundation#2883)

* perf(map_based_prediction): performance improvement of interpolateReferencePath with cumulative sum



* style(pre-commit): autofix

---------

Signed-off-by: veqcc <[email protected]>
Co-authored-by: Ryuta Kambe <[email protected]>
Co-authored-by: veqcc <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

perception/map_based_prediction/src/path_generator.cpp

@@ -287,15 +287,18 @@ PosePath PathGenerator::interpolateReferencePath(
     return interpolated_path;
   }
 
-  std::vector<double> base_path_x;
-  std::vector<double> base_path_y;
-  std::vector<double> base_path_z;
-  std::vector<double> base_path_s;
+  std::vector<double> base_path_x(base_path.size());
+  std::vector<double> base_path_y(base_path.size());
+  std::vector<double> base_path_z(base_path.size());
+  std::vector<double> base_path_s(base_path.size(), 0.0);
   for (size_t i = 0; i < base_path.size(); ++i) {
-    base_path_x.push_back(base_path.at(i).position.x);
-    base_path_y.push_back(base_path.at(i).position.y);
-    base_path_z.push_back(base_path.at(i).position.z);
-    base_path_s.push_back(motion_utils::calcSignedArcLength(base_path, 0, i));
+    base_path_x.at(i) = base_path.at(i).position.x;
+    base_path_y.at(i) = base_path.at(i).position.y;
+    base_path_z.at(i) = base_path.at(i).position.z;
+    if (i > 0) {
+      base_path_s.at(i) = base_path_s.at(i - 1) + tier4_autoware_utils::calcDistance2d(
+                                                    base_path.at(i - 1), base_path.at(i));
+    }
   }
 
   std::vector<double> resampled_s(frenet_predicted_path.size());