use pcl voxel grid to downsample point cloud

modules/dreamview/backend/point_cloud/point_cloud_updater.cc

@@ -20,6 +20,7 @@
 #include "modules/common/log.h"
 #include "modules/common/time/time.h"
 #include "modules/dreamview/backend/common/dreamview_gflags.h"
+#include "pcl/filters/voxel_grid.h"
 #include "pcl/point_cloud.h"
 #include "pcl/point_types.h"
 #include "pcl_conversions/pcl_conversions.h"
@@ -34,8 +35,6 @@ using apollo::localization::LocalizationEstimate;
 using sensor_msgs::PointCloud2;
 using Json = nlohmann::json;
 
-constexpr int PointCloudUpdater::kDownsampleRate;
-
 PointCloudUpdater::PointCloudUpdater(WebSocketHandler *websocket)
     : websocket_(websocket) {
   RegisterMessageHandlers();
@@ -102,18 +101,25 @@ void PointCloudUpdater::UpdatePointCloud(const PointCloud2 &point_cloud) {
 
   last_point_cloud_time_ = point_cloud.header.stamp.toSec();
   // transform from ros to pcl
-  pcl::PointCloud<pcl::PointXYZ> pcl_data;
-  pcl::fromROSMsg(point_cloud, pcl_data);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_ptr(
+      new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::fromROSMsg(point_cloud, *pcl_ptr);
+
+  pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
+  voxel_grid.setInputCloud(pcl_ptr);
+  voxel_grid.setLeafSize(1.0f, 1.0f, 0.2f);
+  voxel_grid.filter(*pcl_ptr);
+  AINFO << "filtered point cloud data size: " << pcl_ptr->size();
 
   point_cloud_.Clear();
-  for (size_t idx = 0; idx < pcl_data.size(); idx += kDownsampleRate) {
-    pcl::PointXYZ &pt = pcl_data.points[idx];
+  for (size_t idx = 0; idx < pcl_ptr->size(); ++idx) {
+    pcl::PointXYZ &pt = pcl_ptr->points[idx];
     if (!std::isnan(pt.x) && !std::isnan(pt.y) && !std::isnan(pt.z)) {
       point_cloud_.add_num(pt.x);
       point_cloud_.add_num(pt.y);
       // TODO(unacao): velodyne height should be updated by hmi store
       // upon vehicle change.
-      point_cloud_.add_num(pt.z + 1.91);
+      point_cloud_.add_num(pt.z + 1.91f);
     }
   }
   {

modules/dreamview/backend/point_cloud/point_cloud_updater.h

@@ -60,8 +60,6 @@ class PointCloudUpdater {
   void Start();
 
  private:
-  static constexpr int kDownsampleRate = 12;
-
   void RegisterMessageHandlers();
 
   void UpdatePointCloud(const sensor_msgs::PointCloud2 &point_cloud);

modules/dreamview/frontend/proto_bundle/map_proto_bundle.json

@@ -720,6 +720,31 @@
                 }
               }
             },
+            "SpeedControl": {
+              "fields": {
+                "name": {
+                  "type": "string",
+                  "id": 1
+                },
+                "polygon": {
+                  "type": "apollo.hdmap.Polygon",
+                  "id": 2
+                },
+                "speedLimit": {
+                  "type": "double",
+                  "id": 3
+                }
+              }
+            },
+            "SpeedControls": {
+              "fields": {
+                "speedControl": {
+                  "rule": "repeated",
+                  "type": "SpeedControl",
+                  "id": 1
+                }
+              }
+            },
             "StopSign": {
               "fields": {
                 "id": {

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

 import * as THREE from "three";
 
-const MAX_POINTS = 8400;
+const MAX_POINTS = 10000;
 const HEIGHT_COLOR_MAPPING = {
     0.5: 0xFF0000,
     1.0: 0xFF7F00,
@@ -35,7 +35,7 @@ export default class PointCloud {
         geometry.colors = colors;
 
         const material = new THREE.PointsMaterial({
-            size: 0.2,
+            size: 0.25,
             transparent: true,
             opacity: 0.7,
             vertexColors: THREE.VertexColors