update depth filter

Author: KenC1014

GEMstack/onboard/perception/fusion.py

@@ -19,7 +19,7 @@ def __init__(self):
         self.confidence = 0.7
         self.classes_to_detect = 0
         self.ground_threshold = 1.6
-        self.human_depth = 1.2
+        self.max_human_depth = 1.0
 
         # Load calibration data
         self.R = load_extrinsics(os.getcwd() + '/GEMstack/onboard/perception/calibration/extrinsics/R.npy')
@@ -93,7 +93,7 @@ def fusion_callback(self, rgb_image_msg: Image, lidar_pc2_msg: PointCloud2):
 
                 # Apply ground and max distance filter to the extracted 5D points
                 extracted_pts = filter_ground_points(extracted_pts, self.ground_threshold)
-                extracted_pts = filter_far_points(extracted_pts)
+                extracted_pts = filter_depth_points(extracted_pts)
 
                 # Extract 2D pedestrians points in camera frame
                 extracted_2d_pts = list(extracted_pts[:, :2].astype(int))

GEMstack/onboard/perception/fusion_utils.py

@@ -35,11 +35,14 @@ def filter_ground_points(lidar_points, ground_threshold = 0):
     return filtered_array
 
 
-def filter_far_points(lidar_points, human_depth=1.2):
-    """ Filter points beyond a max distance in a point cluster """
-    min_dist = np.min(lidar_points[:, 4])
-    max_depth = min_dist + human_depth
-    filtered_array = lidar_points[lidar_points[:, 4] < max_depth]
+def filter_depth_points(lidar_points, max_human_depth=1.0):
+    """ Filter points beyond a max possible human depth in a point cluster """
+    lidar_points_dist = lidar_points[:, 4]
+    min_dist = np.min(lidar_points_dist)
+    max_dist = np.max(lidar_points_dist)
+    max_possible_dist = min_dist + max_human_depth
+    actual_dist = min(max_dist, max_possible_dist)
+    filtered_array = lidar_points[lidar_points_dist < actual_dist]
     return filtered_array