Untitled
unknown
plain_text
a year ago
886 B
8
Indexable
def scan_to_points_v2(self, laser_scan): # Generate an array of angles based on the laser scan properties angles = np.linspace(laser_scan.angle_min, laser_scan.angle_max, num=len(laser_scan.ranges)) # Replace NaN and infinite values in ranges with zero valid_ranges = np.clip(laser_scan.ranges, 0, np.inf) valid_ranges[np.isnan(valid_ranges)] = 0 # Compute Cartesian coordinates x_coords = valid_ranges * np.cos(angles) y_coords = valid_ranges * np.sin(angles) # Combine x and y coordinates into a 2D array points = np.column_stack((x_coords, y_coords)) # Convert to 3D points in homogeneous coordinates (add a z-coordinate of 0 and a homogeneous coordinate of 1) points_3d_homogeneous = np.vstack((points.T, np.zeros(points.shape[0]), np.ones(points.shape[0]))) return points_3d_homogeneous
Editor is loading...
Leave a Comment