From 486e5bf62e4f8112c1dd6f805e89e80c6d31b04a Mon Sep 17 00:00:00 2001 From: SirMDA Date: Thu, 12 Dec 2024 17:16:24 +0100 Subject: [PATCH 1/2] fixed lidar_distance node speed by removing for loop --- code/perception/src/lidar_distance.py | 129 ++++++++++++++++---------- 1 file changed, 79 insertions(+), 50 deletions(-) mode change 100644 => 100755 code/perception/src/lidar_distance.py diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py old mode 100644 new mode 100755 index 939224c5..bea1d550 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -3,7 +3,7 @@ import rospy import ros_numpy import numpy as np -import lidar_filter_utility +from lidar_filter_utility import bounding_box, remove_field_name from sensor_msgs.msg import PointCloud2, Image as ImageMsg from sklearn.cluster import DBSCAN from cv_bridge import CvBridge @@ -189,14 +189,12 @@ def calculate_image(self, coordinates, focus): return None # Apply bounding box filter - reconstruct_bit_mask = lidar_filter_utility.bounding_box(coordinates, **params) + reconstruct_bit_mask = bounding_box(coordinates, **params) reconstruct_coordinates = coordinates[reconstruct_bit_mask] # Remove the "intensity" field and convert to a NumPy array reconstruct_coordinates_xyz = np.array( - lidar_filter_utility.remove_field_name( - reconstruct_coordinates, "intensity" - ).tolist() + remove_field_name(reconstruct_coordinates, "intensity").tolist() ) # Reconstruct the image based on the focus @@ -256,51 +254,82 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus): img = np.zeros(shape=(720, 1280), dtype=np.float32) dist_array = np.zeros(shape=(720, 1280, 3), dtype=np.float32) - # Process each point in the point cloud - for c in coordinates_xyz: - if focus == "Center": # Compute image for the center view - point = np.array([c[1], c[2], c[0], 1]) - pixel = np.matmul(m, point) # Project 3D point to 2D image coordinates - x, y = int(pixel[0] / pixel[2]), int( - pixel[1] / pixel[2] - ) # Normalize coordinates - if ( - 0 <= x <= 1280 and 0 <= y <= 720 - ): # Check if coordinates are within image bounds - img[719 - y][1279 - x] = c[0] # Set depth value - dist_array[719 - y][1279 - x] = np.array( - [c[0], c[1], c[2]], dtype=np.float32 - ) - - if focus == "Back": # Compute image for the rear view - point = np.array([c[1], c[2], c[0], 1]) - pixel = np.matmul(m, point) - x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) - if 0 <= x <= 1280 and 0 <= y < 720: - img[y][1279 - x] = -c[0] - dist_array[y][1279 - x] = np.array( - [-c[0], c[1], c[2]], dtype=np.float32 - ) - - if focus == "Left": # Compute image for the left view - point = np.array([c[0], c[2], c[1], 1]) - pixel = np.matmul(m, point) - x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) - if 0 <= x <= 1280 and 0 <= y <= 720: - img[719 - y][1279 - x] = c[1] - dist_array[y][1279 - x] = np.array( - [c[0], c[1], c[2]], dtype=np.float32 - ) - - if focus == "Right": # Compute image for the right view - point = np.array([c[0], c[2], c[1], 1]) - pixel = np.matmul(m, point) - x, y = int(pixel[0] / pixel[2]), int(pixel[1] / pixel[2]) - if 0 <= x < 1280 and 0 <= y < 720: - img[y][1279 - x] = -c[1] - dist_array[y][1279 - x] = np.array( - [c[0], c[1], c[2]], dtype=np.float32 - ) + # Prepare points based on focus + if focus == "Center": + points = np.column_stack( + ( + coordinates_xyz[:, 1], + coordinates_xyz[:, 2], + coordinates_xyz[:, 0], + np.ones(coordinates_xyz.shape[0]), + ) + ) + elif focus == "Back": + points = np.column_stack( + ( + coordinates_xyz[:, 1], + coordinates_xyz[:, 2], + coordinates_xyz[:, 0], + np.ones(coordinates_xyz.shape[0]), + ) + ) + elif focus == "Left": + points = np.column_stack( + ( + coordinates_xyz[:, 0], + coordinates_xyz[:, 2], + coordinates_xyz[:, 1], + np.ones(coordinates_xyz.shape[0]), + ) + ) + elif focus == "Right": + points = np.column_stack( + ( + coordinates_xyz[:, 0], + coordinates_xyz[:, 2], + coordinates_xyz[:, 1], + np.ones(coordinates_xyz.shape[0]), + ) + ) + else: + rospy.logwarn(f"Unknown focus: {focus}. Skipping image calculation.") + return None + + # Project 3D points to 2D image coordinates + pixels = np.dot(m, points.T).T + x = (pixels[:, 0] / pixels[:, 2]).astype(int) + y = (pixels[:, 1] / pixels[:, 2]).astype(int) + + # Filter valid coordinates + valid_indices = (x >= 0) & (x < 1280) & (y >= 0) & (y < 720) + x = x[valid_indices] + y = y[valid_indices] + valid_coordinates = coordinates_xyz[valid_indices] + + if focus == "Center": + img[719 - y, 1279 - x] = valid_coordinates[:, 0] + dist_array[719 - y, 1279 - x] = valid_coordinates + elif focus == "Back": + img[y, 1279 - x] = -valid_coordinates[:, 0] + dist_array[y, 1279 - x] = np.column_stack( + ( + -valid_coordinates[:, 0], + valid_coordinates[:, 1], + valid_coordinates[:, 2], + ) + ) + elif focus == "Left": + img[719 - y, 1279 - x] = valid_coordinates[:, 1] + dist_array[719 - y, 1279 - x] = valid_coordinates + elif focus == "Right": + img[y, 1279 - x] = -valid_coordinates[:, 1] + dist_array[y, 1279 - x] = np.column_stack( + ( + valid_coordinates[:, 0], + -valid_coordinates[:, 1], + valid_coordinates[:, 2], + ) + ) return dist_array From eefcd29f6690efbfa765c140a77261c161386f05 Mon Sep 17 00:00:00 2001 From: SirMDA Date: Thu, 12 Dec 2024 17:33:37 +0100 Subject: [PATCH 2/2] optimized focus if clause --- code/perception/src/lidar_distance.py | 22 ++-------------------- 1 file changed, 2 insertions(+), 20 deletions(-) diff --git a/code/perception/src/lidar_distance.py b/code/perception/src/lidar_distance.py index bea1d550..5c937be0 100755 --- a/code/perception/src/lidar_distance.py +++ b/code/perception/src/lidar_distance.py @@ -255,16 +255,7 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus): dist_array = np.zeros(shape=(720, 1280, 3), dtype=np.float32) # Prepare points based on focus - if focus == "Center": - points = np.column_stack( - ( - coordinates_xyz[:, 1], - coordinates_xyz[:, 2], - coordinates_xyz[:, 0], - np.ones(coordinates_xyz.shape[0]), - ) - ) - elif focus == "Back": + if focus in ["Center", "Back"]: points = np.column_stack( ( coordinates_xyz[:, 1], @@ -273,16 +264,7 @@ def reconstruct_img_from_lidar(self, coordinates_xyz, focus): np.ones(coordinates_xyz.shape[0]), ) ) - elif focus == "Left": - points = np.column_stack( - ( - coordinates_xyz[:, 0], - coordinates_xyz[:, 2], - coordinates_xyz[:, 1], - np.ones(coordinates_xyz.shape[0]), - ) - ) - elif focus == "Right": + elif focus in ["Left", "Right"]: points = np.column_stack( ( coordinates_xyz[:, 0],