Planning: used std::transform for coordinate transformation to increase...

Planning: used std::transform for coordinate transformation to increase algorithm efficiency and save space.

modules/planning/common/trajectory/discretized_trajectory.cc

@@ -132,21 +132,5 @@ double DiscretizedTrajectory::GetSpatialLength() const {
          trajectory_points_.front().path_point().s();
 }
 
-std::uint32_t DiscretizedTrajectory::NumOfPoints() const {
-  return trajectory_points_.size();
-}
-
-const std::vector<TrajectoryPoint>& DiscretizedTrajectory::trajectory_points()
-    const {
-  return trajectory_points_;
-}
-
-void DiscretizedTrajectory::SetTrajectoryPoints(
-    const std::vector<common::TrajectoryPoint>& trajectory_points) {
-  trajectory_points_ = trajectory_points;
-}
-
-void DiscretizedTrajectory::Clear() { trajectory_points_.clear(); }
-
 }  // namespace planning
 }  // namespace apollo

modules/planning/common/trajectory/discretized_trajectory.h

@@ -78,6 +78,7 @@ class DiscretizedTrajectory : public Trajectory {
   uint32_t NumOfPoints() const;
 
   const std::vector<common::TrajectoryPoint>& trajectory_points() const;
+  std::vector<common::TrajectoryPoint>& trajectory_points();
 
   virtual void Clear();
 
@@ -85,6 +86,27 @@ class DiscretizedTrajectory : public Trajectory {
   std::vector<common::TrajectoryPoint> trajectory_points_;
 };
 
+inline std::uint32_t DiscretizedTrajectory::NumOfPoints() const {
+  return trajectory_points_.size();
+}
+
+inline const std::vector<common::TrajectoryPoint>&
+DiscretizedTrajectory::trajectory_points() const {
+  return trajectory_points_;
+}
+
+inline std::vector<common::TrajectoryPoint>&
+DiscretizedTrajectory::trajectory_points() {
+  return trajectory_points_;
+}
+
+inline void DiscretizedTrajectory::SetTrajectoryPoints(
+    const std::vector<common::TrajectoryPoint>& trajectory_points) {
+  trajectory_points_ = trajectory_points;
+}
+
+inline void DiscretizedTrajectory::Clear() { trajectory_points_.clear(); }
+
 }  // namespace planning
 }  // namespace apollo
 

modules/planning/common/trajectory/trajectory_stitcher.cc

@@ -63,31 +63,39 @@ void TrajectoryStitcher::TransformLastPublishedTrajectory(
     return;
   }
   const double time_diff = current_time - prev_trajectory->header_time();
-  auto matched_point = prev_trajectory->Evaluate(time_diff);
+  const auto& matched_point = prev_trajectory->Evaluate(time_diff);
   if (!matched_point.has_path_point()) {
     return;
   }
-  const double cos_theta = std::cos(-matched_point.path_point().theta());
-  const double sin_theta = std::sin(-matched_point.path_point().theta());
-  std::vector<TrajectoryPoint> transformed_points;
-  for (const auto& old_point : prev_trajectory->trajectory_points()) {
-    TrajectoryPoint point = old_point;
-    Eigen::Vector3d before_rotate(
-        old_point.path_point().x() - matched_point.path_point().x(),
-        old_point.path_point().y() - matched_point.path_point().y(),
-        old_point.path_point().z() - matched_point.path_point().z());
-    const double after_rotate_x =
-        before_rotate.x() * cos_theta - before_rotate.y() * sin_theta;
-    const double after_rotate_y =
-        before_rotate.x() * sin_theta + before_rotate.y() * cos_theta;
-    point.mutable_path_point()->set_x(after_rotate_x);
-    point.mutable_path_point()->set_y(after_rotate_y);
-    point.mutable_path_point()->set_z(before_rotate.z());
-    point.mutable_path_point()->set_theta(common::math::WrapAngle(
-        old_point.path_point().theta() - matched_point.path_point().theta()));
-    transformed_points.emplace_back(point);
-  }
-  prev_trajectory->SetTrajectoryPoints(transformed_points);
+
+  const double cos_theta = std::cos(matched_point.path_point().theta());
+  const double sin_theta = std::sin(matched_point.path_point().theta());
+  const double x_shift = matched_point.path_point().x();
+  const double y_shift = matched_point.path_point().y();
+  const double z_shift = matched_point.path_point().z();
+
+  auto& points = prev_trajectory->trajectory_points();
+  // x_new = (x_old - x_shift) * cos(theta) + (y_old - y_shift) * sin(theta)
+  // y_new = (y_old - y_shift) * cos(theta) - (x_old - x_shift) * sin(theta)
+  std::transform(
+      std::begin(points), std::end(points), std::begin(points),
+      [&](const TrajectoryPoint& old_point) {
+        const double x_new =
+            (old_point.path_point().x() - x_shift) * cos_theta +
+            (old_point.path_point().y() - y_shift) * sin_theta;
+        const double y_new =
+            (old_point.path_point().y() - y_shift) * cos_theta -
+            (old_point.path_point().x() - x_shift) * sin_theta;
+        const double z_new = old_point.path_point().z() - z_shift;
+        TrajectoryPoint new_point = old_point;
+        new_point.mutable_path_point()->set_x(x_new);
+        new_point.mutable_path_point()->set_y(y_new);
+        new_point.mutable_path_point()->set_z(z_new);
+        new_point.mutable_path_point()->set_theta(
+            common::math::WrapAngle(old_point.path_point().theta() -
+                                    matched_point.path_point().theta()));
+        return new_point;
+      });
 }
 
 // only used in navigation mode