Dreamview: downsample prediction points (#2953)

* Dreamview: downsample prediction points

* CR response

modules/dreamview/backend/simulation_world/simulation_world_service.cc

@@ -23,6 +23,7 @@
 
 #include "google/protobuf/util/json_util.h"
 #include "modules/canbus/proto/chassis.pb.h"
+#include "modules/common/configs/vehicle_config_helper.h"
 #include "modules/common/math/quaternion.h"
 #include "modules/common/proto/geometry.pb.h"
 #include "modules/common/proto/vehicle_signal.pb.h"
@@ -39,7 +40,6 @@
 #include "modules/planning/proto/planning.pb.h"
 #include "modules/planning/proto/planning_internal.pb.h"
 #include "modules/prediction/proto/prediction_obstacle.pb.h"
-#include "modules/common/configs/vehicle_config_helper.h"
 
 namespace apollo {
 namespace dreamview {
@@ -51,6 +51,7 @@ using apollo::common::VehicleConfigHelper;
 using apollo::common::adapter::AdapterManager;
 using apollo::common::monitor::MonitorMessage;
 using apollo::common::monitor::MonitorMessageItem;
+using apollo::common::PathPoint;
 using apollo::common::time::Clock;
 using apollo::common::time::ToSecond;
 using apollo::common::time::millis;
@@ -756,10 +757,8 @@ void SimulationWorldService::UpdatePlanningData(const PlanningData &data) {
   planning_data->clear_path();
   for (auto &path : data.path()) {
     // Downsample the path points for frontend display.
-    // Angle threshold is about 5.72 degree.
-    constexpr double angle_threshold = 0.1;
-    std::vector<int> sampled_indices =
-        DownsampleByAngle(path.path_point(), angle_threshold);
+    auto sampled_indices =
+        DownsampleByAngle(path.path_point(), kAngleThreshold);
 
     auto *downsampled_path = planning_data->add_path();
     downsampled_path->set_name(path.name());
@@ -809,14 +808,22 @@ void SimulationWorldService::UpdateSimulationWorld(
 }
 
 void SimulationWorldService::CreatePredictionTrajectory(
-    Object *world_object, const PredictionObstacle &obstacle) {
+    const PredictionObstacle &obstacle, Object *world_object) {
   for (const auto &traj : obstacle.trajectory()) {
     Prediction *prediction = world_object->add_prediction();
     prediction->set_probability(traj.probability());
+
+    std::vector<PathPoint> points;
     for (const auto &point : traj.trajectory_point()) {
+      points.push_back(point.path_point());
+    }
+    auto sampled_indices = DownsampleByAngle(points, kAngleThreshold);
+
+    for (auto index : sampled_indices) {
+      const auto &point = points[index];
       PolygonPoint *world_point = prediction->add_predicted_trajectory();
-      world_point->set_x(point.path_point().x() + map_service_->GetXOffset());
-      world_point->set_y(point.path_point().y() + map_service_->GetYOffset());
+      world_point->set_x(point.x() + map_service_->GetXOffset());
+      world_point->set_y(point.y() + map_service_->GetYOffset());
     }
   }
 }
@@ -832,7 +839,7 @@ void SimulationWorldService::UpdateSimulationWorld(
     auto &world_obj = CreateWorldObjectIfAbsent(obstacle.perception_obstacle());
 
     // Add prediction trajectory to the object.
-    CreatePredictionTrajectory(&world_obj, obstacle);
+    CreatePredictionTrajectory(obstacle, &world_obj);
 
     world_obj.set_timestamp_sec(
         std::max(obstacle.timestamp(), world_obj.timestamp_sec()));
@@ -853,10 +860,8 @@ void SimulationWorldService::UpdateSimulationWorld(
 
   for (const Path &path : paths) {
     // Downsample the path points for frontend display.
-    // Angle threshold is about 5.72 degree.
-    constexpr double angle_threshold = 0.1;
-    std::vector<int> sampled_indices =
-        DownsampleByAngle(path.path_points(), angle_threshold);
+    auto sampled_indices =
+        DownsampleByAngle(path.path_points(), kAngleThreshold);
 
     route_paths_.emplace_back();
     RoutePath *route_path = &route_paths_.back();

modules/dreamview/backend/simulation_world/simulation_world_service.h

@@ -59,6 +59,9 @@ class SimulationWorldService {
   // SimulationWorld.
   static constexpr int kMaxMonitorItems = 30;
 
+  // Angle threshold is about 5.72 degree.
+  static constexpr double kAngleThreshold = 0.1;
+
   /**
    * @brief Constructor of SimulationWorldService.
    * @param map_service the pointer of MapService.
@@ -169,8 +172,8 @@ class SimulationWorldService {
   void UpdateMainDecision(const apollo::planning::MainDecision &main_decision,
                           double update_timestamp_sec, Object *world_main_stop);
   void CreatePredictionTrajectory(
-      Object *world_object,
-      const apollo::prediction::PredictionObstacle &obstacle);
+      const apollo::prediction::PredictionObstacle &obstacle,
+      Object *world_object);
   void UpdatePlanningData(const apollo::planning_internal::PlanningData &data);
 
   void PopulateMapInfo(double radius);

modules/dreamview/backend/simulation_world/simulation_world_service_test.cc

@@ -438,14 +438,7 @@ TEST_F(SimulationWorldServiceTest, UpdatePrediction) {
       const Prediction& prediction = obj.prediction(j);
       EXPECT_NEAR((sim_world.object_size() - i - 1) * 0.1 + j,
                   prediction.probability(), kEpsilon);
-      EXPECT_EQ(prediction.predicted_trajectory_size(), 8);
-      for (int k = 0; k < prediction.predicted_trajectory_size(); ++k) {
-        const auto& pt = prediction.predicted_trajectory(k);
-        int val = j * 10 + k;
-        EXPECT_NEAR(val, pt.x(), kEpsilon);
-        EXPECT_NEAR(val, pt.y(), kEpsilon);
-        EXPECT_NEAR(0.0, pt.z(), kEpsilon);
-      }
+      EXPECT_EQ(prediction.predicted_trajectory_size(), 2);  // Downsampled
     }
     EXPECT_NEAR(123.456, obj.timestamp_sec(), kEpsilon);
   }

modules/dreamview/frontend/proto_bundle/map_proto_bundle.json

@@ -4,124 +4,6 @@
       "nested": {
         "hdmap": {
           "nested": {
-            "Projection": {
-              "fields": {
-                "proj": {
-                  "type": "string",
-                  "id": 1
-                }
-              }
-            },
-            "Header": {
-              "fields": {
-                "version": {
-                  "type": "bytes",
-                  "id": 1
-                },
-                "date": {
-                  "type": "bytes",
-                  "id": 2
-                },
-                "projection": {
-                  "type": "Projection",
-                  "id": 3
-                },
-                "district": {
-                  "type": "bytes",
-                  "id": 4
-                },
-                "generation": {
-                  "type": "bytes",
-                  "id": 5
-                },
-                "revMajor": {
-                  "type": "bytes",
-                  "id": 6
-                },
-                "revMinor": {
-                  "type": "bytes",
-                  "id": 7
-                },
-                "left": {
-                  "type": "double",
-                  "id": 8
-                },
-                "top": {
-                  "type": "double",
-                  "id": 9
-                },
-                "right": {
-                  "type": "double",
-                  "id": 10
-                },
-                "bottom": {
-                  "type": "double",
-                  "id": 11
-                },
-                "vendor": {
-                  "type": "bytes",
-                  "id": 12
-                }
-              }
-            },
-            "Map": {
-              "fields": {
-                "header": {
-                  "type": "Header",
-                  "id": 1
-                },
-                "crosswalk": {
-                  "rule": "repeated",
-                  "type": "Crosswalk",
-                  "id": 2
-                },
-                "junction": {
-                  "rule": "repeated",
-                  "type": "Junction",
-                  "id": 3
-                },
-                "lane": {
-                  "rule": "repeated",
-                  "type": "Lane",
-                  "id": 4
-                },
-                "stopSign": {
-                  "rule": "repeated",
-                  "type": "StopSign",
-                  "id": 5
-                },
-                "signal": {
-                  "rule": "repeated",
-                  "type": "Signal",
-                  "id": 6
-                },
-                "yield": {
-                  "rule": "repeated",
-                  "type": "YieldSign",
-                  "id": 7
-                },
-                "overlap": {
-                  "rule": "repeated",
-                  "type": "Overlap",
-                  "id": 8
-                },
-                "clearArea": {
-                  "rule": "repeated",
-                  "type": "ClearArea",
-                  "id": 9
-                },
-                "speedBump": {
-                  "rule": "repeated",
-                  "type": "SpeedBump",
-                  "id": 10
-                },
-                "road": {
-                  "rule": "repeated",
-                  "type": "Road",
-                  "id": 11
-                }
-              }
-            },
             "ClearArea": {
               "fields": {
                 "id": {
@@ -538,6 +420,124 @@
                 }
               }
             },
+            "Projection": {
+              "fields": {
+                "proj": {
+                  "type": "string",
+                  "id": 1
+                }
+              }
+            },
+            "Header": {
+              "fields": {
+                "version": {
+                  "type": "bytes",
+                  "id": 1
+                },
+                "date": {
+                  "type": "bytes",
+                  "id": 2
+                },
+                "projection": {
+                  "type": "Projection",
+                  "id": 3
+                },
+                "district": {
+                  "type": "bytes",
+                  "id": 4
+                },
+                "generation": {
+                  "type": "bytes",
+                  "id": 5
+                },
+                "revMajor": {
+                  "type": "bytes",
+                  "id": 6
+                },
+                "revMinor": {
+                  "type": "bytes",
+                  "id": 7
+                },
+                "left": {
+                  "type": "double",
+                  "id": 8
+                },
+                "top": {
+                  "type": "double",
+                  "id": 9
+                },
+                "right": {
+                  "type": "double",
+                  "id": 10
+                },
+                "bottom": {
+                  "type": "double",
+                  "id": 11
+                },
+                "vendor": {
+                  "type": "bytes",
+                  "id": 12
+                }
+              }
+            },
+            "Map": {
+              "fields": {
+                "header": {
+                  "type": "Header",
+                  "id": 1
+                },
+                "crosswalk": {
+                  "rule": "repeated",
+                  "type": "Crosswalk",
+                  "id": 2
+                },
+                "junction": {
+                  "rule": "repeated",
+                  "type": "Junction",
+                  "id": 3
+                },
+                "lane": {
+                  "rule": "repeated",
+                  "type": "Lane",
+                  "id": 4
+                },
+                "stopSign": {
+                  "rule": "repeated",
+                  "type": "StopSign",
+                  "id": 5
+                },
+                "signal": {
+                  "rule": "repeated",
+                  "type": "Signal",
+                  "id": 6
+                },
+                "yield": {
+                  "rule": "repeated",
+                  "type": "YieldSign",
+                  "id": 7
+                },
+                "overlap": {
+                  "rule": "repeated",
+                  "type": "Overlap",
+                  "id": 8
+                },
+                "clearArea": {
+                  "rule": "repeated",
+                  "type": "ClearArea",
+                  "id": 9
+                },
+                "speedBump": {
+                  "rule": "repeated",
+                  "type": "SpeedBump",
+                  "id": 10
+                },
+                "road": {
+                  "rule": "repeated",
+                  "type": "Road",
+                  "id": 11
+                }
+              }
+            },
             "BoundaryEdge": {
               "fields": {
                 "curve": {

modules/dreamview/frontend/src/renderer/prediction.js

@@ -6,7 +6,6 @@ import { DEFAULT_COLOR, ObstacleColorMapping } from "renderer/obstacles.js";
 import { drawCircle, drawSegmentsFromPoints } from "utils/draw";
 
 const _ = require('lodash');
-const downsamplingRatio = 10;
 const majorThickness = 3;
 
 export default class Prediction {
@@ -63,6 +62,7 @@ export default class Prediction {
                 scene.add(mesh);
 
                 // Downsampling points to draw circles
+                const downsamplingRatio = Math.ceil(predictedTraj.length / 3);
                 for (let j = 0; j < predictedTraj.length; j += downsamplingRatio) {
                     const circleMesh = this.getPredCircle();
                     circleMesh.position.set(predictedTraj[j].x, predictedTraj[j].y, 0.24);