add ground and max dist filter

Author: KenC1014

GEMstack/onboard/perception/fusion.py

@@ -18,6 +18,8 @@ def __init__(self):
         self.visualization = True
         self.confidence = 0.7
         self.classes_to_detect = 0
+        self.ground_threshold = 1.6
+        self.max_dist_percent = 0.7
 
         # Load calibration data
         self.R = load_extrinsics(os.getcwd() + '/GEMstack/onboard/perception/calibration/extrinsics/R.npy')
@@ -84,18 +86,28 @@ def fusion_callback(self, rgb_image_msg: Image, lidar_pc2_msg: PointCloud2):
                                     (pts[:, 1] < bottom_bound)
                                     ]
 
-                extracted_2d_pts = list(np.array(extracted_pts)[:, :2].astype(int))
+                #if no points extracted for this bbox, skip
+                if len(extracted_pts) < 1:
+                    continue
+                
+                extracted_pts = filter_ground_points(extracted_pts, self.ground_threshold)
+                extracted_pts = filter_far_points(extracted_pts)
+                
+                extracted_2d_pts = list(extracted_pts[:, :2].astype(int))
                 flattened_pedestrians_2d_pts = flattened_pedestrians_2d_pts + extracted_2d_pts
-               
-                extracted_3d_pts = list(np.array(extracted_pts)[:, -3:])
+
+                extracted_3d_pts = list(extracted_pts[:, -3:])
                 pedestrians_3d_pts.append(extracted_3d_pts)
                 flattened_pedestrians_3d_pts = flattened_pedestrians_3d_pts + extracted_3d_pts
 
             # Used for visualization
             if(self.visualization):
                 cv_image = vis_2d_bbox(cv_image, xywh, box)
 
-        if len(pedestrians_3d_pts) > 0:
+        if len(flattened_pedestrians_3d_pts) > 0:
+            # print(f"x_dim pedestrians_3d_pts: {np.max(np.array(flattened_pedestrians_3d_pts)[:, 0]), np.min(np.array(flattened_pedestrians_3d_pts)[:, 0])}")
+            # print(f"y_dim pedestrians_3d_pts: {np.max(np.array(flattened_pedestrians_3d_pts)[:, 1]), np.min(np.array(flattened_pedestrians_3d_pts)[:, 1])}")
+            # print(f"z_dim pedestrians_3d_pts: {np.max(np.array(flattened_pedestrians_3d_pts)[:, 2]), np.min(np.array(flattened_pedestrians_3d_pts)[:, 2])}")
             # Draw projected 2D LiDAR points on the image.
             for pt in flattened_pedestrians_2d_pts:
                 cv2.circle(cv_image, pt, 2, (0, 0, 255), -1)

GEMstack/onboard/perception/fusion_utils.py

@@ -1,4 +1,5 @@
 from sensor_msgs.msg import PointCloud2, PointField
+from scipy.stats import zscore
 import numpy as np
 import sensor_msgs.point_cloud2 as pc2
 import open3d as o3d
@@ -23,12 +24,23 @@ def downsample_points(lidar_points):
     # Apply voxel grid downsampling
     voxel_size = 0.1  # Adjust for desired resolution
     downsampled_pcd = pcd.voxel_down_sample(voxel_size=voxel_size)
-
+    
     # Convert back to numpy array
     transformed_points = np.asarray(downsampled_pcd.points)
     return transformed_points
 
 
+def filter_ground_points(lidar_points, ground_threshold = 0):
+    filtered_array = lidar_points[lidar_points[:, 3] < ground_threshold]
+    return filtered_array
+
+
+def filter_far_points(lidar_points, max_dist_percent=0.85):
+    max_dist = np.max(lidar_points[:, 4])
+    filtered_array = lidar_points[lidar_points[:, 4] < max_dist_percent * max_dist]
+    return filtered_array
+
+
 # Credits: The following lines of codes (from 33 to 92) are adapted from the Calibration Team B
 def load_extrinsics(extrinsics_file):
     """

GEMstack/onboard/perception/transform.py

@@ -9,7 +9,7 @@ def publish_tf():
 
     while not rospy.is_shutdown():
         br.sendTransform(
-            (0, 1.5, 7),  # (x, y, z) translation
+            (0, 1.6, 7),  # (x, y, z) translation
             tf.transformations.quaternion_from_euler(0.5* np.pi, 0, 0),  # (roll, pitch, yaw)
             rospy.Time.now(),
             "os_sensor",  # Child frame (sensor)