add od3 dialect

Author: KenC1014

GEMstack/onboard/perception/fusion.py

@@ -50,6 +50,19 @@ def fusion_callback(self, image: Image, lidar_pc2_msg: PointCloud2):
 
         # Project the transformed points into the image plane.
         projected_pts = project_points(lidar_in_camera, self.K)
+
+        # Convert numpy array to Open3D point cloud
+        transformed_points = projected_pts
+        pcd = o3d.geometry.PointCloud()
+        pcd.points = o3d.utility.Vector3dVector(transformed_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)
+        print(f"after :{transformed_points.shape}")
 
         # Process bboxes
         self.last_person_boxes = []

GEMstack/onboard/perception/fusion_utils.py

@@ -8,10 +8,7 @@
 
 def convert_pointcloud2_to_xyz(lidar_pc2_msg: PointCloud2):
     """ Convert 1D PointCloud2 data to x, y, z coords """
-    lidar_points = []
-    for point in pc2.read_points(lidar_pc2_msg, field_names=("x", "y", "z", "intensity"), skip_nans=True):
-        lidar_points.append([point[0], point[1], point[2]])
-    return np.array(lidar_points)
+    return np.array(list(pc2.read_points(lidar_pc2_msg, skip_nans=True)), dtype=np.float32)[:, :3]
 
 
 # Credits: The following lines of codes (17 to 159 excluding lines 80 to 115) are adapted from the Calibration Team B