Untitled

mail@pastecode.io avatar
unknown
plain_text
2 months ago
886 B
5
Indexable
Never
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
Leave a Comment