From f8fd38b60e53ef958a3e1801ad28d949053abf22 Mon Sep 17 00:00:00 2001
From: FangzhenLi-hust <30279558+FangzhenLi-hust@users.noreply.github.com>
Date: Fri, 11 Aug 2017 14:14:33 -0700
Subject: [PATCH] filter nan points, and fix a bug of hdmap_input (#733)

* filter nan points, and fix a bug of hdmap_input

* set the log level to 3
---
 modules/perception/conf/perception.conf       |  5 ++++
 .../obstacle/onboard/hdmap_input.cc           |  3 ++
 .../obstacle/onboard/lidar_process.cc         | 30 +++++++------------
 3 files changed, 18 insertions(+), 20 deletions(-)

diff --git a/modules/perception/conf/perception.conf b/modules/perception/conf/perception.conf
index 26045ad70a..748605a4de 100644
--- a/modules/perception/conf/perception.conf
+++ b/modules/perception/conf/perception.conf
@@ -93,3 +93,8 @@
 # type: bool
 # default: false
 --enable_visualization=false
+
+# the log level 
+# type: int
+# default: 3
+--v=3
diff --git a/modules/perception/obstacle/onboard/hdmap_input.cc b/modules/perception/obstacle/onboard/hdmap_input.cc
index 3b29afc850..f6171c1e1e 100644
--- a/modules/perception/obstacle/onboard/hdmap_input.cc
+++ b/modules/perception/obstacle/onboard/hdmap_input.cc
@@ -168,6 +168,9 @@ void HDMapInput::DownSampleBoundary(const apollo::hdmap::LineSegment& line,
   double acos_theta = 0.0;
   size_t raw_cloud_size = raw_cloud->points.size();
   if (raw_cloud_size <= 3) {
+    for (size_t i = 0; i < raw_cloud_size; ++i) {
+        out_boundary_line->push_back(raw_cloud->points[i]);
+    }
     AINFO << "Points num < 3, so no need to downsample.";
     return;
   }
diff --git a/modules/perception/obstacle/onboard/lidar_process.cc b/modules/perception/obstacle/onboard/lidar_process.cc
index 9b63ccc317..bf7dd25ec1 100644
--- a/modules/perception/obstacle/onboard/lidar_process.cc
+++ b/modules/perception/obstacle/onboard/lidar_process.cc
@@ -185,15 +185,6 @@ bool LidarProcess::Process(double timestamp, PointCloudPtr point_cloud,
     }
   }
 
-  if (visualizer_ != nullptr) {
-      FrameContent content;
-      content.set_lidar_pose(*velodyne_trans);
-      content.set_lidar_cloud(point_cloud);
-      content.set_tracked_objects(objects_);
-      visualizer_->UpdateCameraSystem(&content);
-      visualizer_->Render(content);
-  }
-
   PERF_BLOCK_END("lidar_tracker");
   AINFO << "lidar process succ, there are " << objects_.size()
         << " tracked objects.";
@@ -241,13 +232,6 @@ bool LidarProcess::InitFrameDependence() {
     AINFO << "get and init hdmap_input succ.";
   }
 
-  if (FLAGS_enable_visualization) {
-    visualizer_.reset(new OpenglVisualizer());
-    if(!visualizer_->Init()) {
-      AERROR<<"init visialuzer failed";
-      return false;
-    }; 
-  }
   return true;
 }
 
@@ -324,12 +308,18 @@ void LidarProcess::TransPointCloudToPCL(const sensor_msgs::PointCloud2& in_msg,
   cloud->sensor_origin_ = in_cloud.sensor_origin_;
   cloud->sensor_orientation_ = in_cloud.sensor_orientation_;
   cloud->points.resize(in_cloud.points.size());
+  size_t points_num = 0;
   for (size_t idx = 0; idx < in_cloud.size(); ++idx) {
-    cloud->points[idx].x = in_cloud.points[idx].x;
-    cloud->points[idx].y = in_cloud.points[idx].y;
-    cloud->points[idx].z = in_cloud.points[idx].z;
-    cloud->points[idx].intensity = in_cloud.points[idx].intensity;
+    pcl_util::PointXYZIT& pt = in_cloud.points[idx];
+    if (!isnan(pt.x) && !isnan(pt.y) && !isnan(pt.z) && !isnan(pt.intensity)) {
+      cloud->points[points_num].x = pt.x;
+      cloud->points[points_num].y = pt.y;
+      cloud->points[points_num].z = pt.z;
+      cloud->points[points_num].intensity = pt.intensity;
+      points_num++;
+    }
   }
+  cloud->points.resize(points_num);
 }
 
 bool LidarProcess::GetVelodyneTrans(const double query_time, Matrix4d* trans) {
-- 
GitLab