Avoid Represent Vehicles non visibles by the camera

CARLA version: 0.9.14 Platform/OS: Ubuntu Problem you have experienced: Avoid to represent non visible objects by the camera to generate a dataset What you expected to happen: Only draw the bbox for visible elements Steps to reproduce: The code is the following:
import carla
import random
import time
import queue
import numpy as np
import cv2

from pascal_voc_writer import Writer

def build_projection_matrix(w, h, fov):
    focal = w / (2.0 * np.tan(fov * np.pi / 360.0))
    K = np.identity(3)
    K[0, 0] = K[1, 1] = focal
    K[0, 2] = w / 2.0
    K[1, 2] = h / 2.0
    return K
def cast_ray(initial_location, final_location,world):
    # Cast a ray from initial_location to final_location and detect intersecting geometries
    result = world.cast_ray(initial_location, final_location)
    return result

def get_image_point(loc, K, w2c):
    # Calculate 2D projection of 3D coordinate

    # Format the input coordinate (loc is a carla.Position object)
    point = np.array([loc.x, loc.y, loc.z, 1])
    # transform to camera coordinates
    point_camera = np.dot(w2c, point)

    # Convert from UE4's coordinate system to a "standard" system (x, y, z) -> (y, -z, x)
    point_camera = [point_camera[1], -point_camera[2], point_camera[0]]

    # Project 3D->2D using the camera matrix
    point_img = np.dot(K, point_camera)
    # Normalize
    point_img[0] /= point_img[2]
    point_img[1] /= point_img[2]

    return point_img[0:2]
def is_obstructing(camera, vehicle, world):
    # Cast a ray from the camera to the vehicle and check for obstructions
    camera_location = camera.get_transform().location
    vehicle_location = vehicle.get_transform().location
    result = cast_ray(camera_location, vehicle_location,world)

    # Obtener todos los objetos del mundo
    all_objects = world.get_actors()

    # Filtrar los objetos para obtener solo los edificios
    building_objects = [obj for obj in all_objects if obj.attributes.get('role_name', '').startswith('building')]

    contador = 0
    for point in result:
        try:
            if point.label.startswith('vehicle'):
                pass
        except:
            contador += 1

    if contador==0:
        return True
    else:
        return False

    return False

def main():
    # Connect to the CARLA server
    client = carla.Client('localhost', 2000)
    #client.set_timeout(5.0)

    try:
        # Load the map and set the initial configuration
        world = client.get_world()
        # Set up the simulator in synchronous mode
        settings = world.get_settings()
        settings.synchronous_mode = True # Enables synchronous mode
        settings.fixed_delta_seconds = 0.05
        world.apply_settings(settings)

        # Adjust the desired view distance (in meters)
        settings.view_distance = 2000

        # Apply the new settings to the world
        #world.apply_settings(settings)
        blueprint_library = world.get_blueprint_library()

        # Remove all previously spawned vehicles and pedestrians
        vehicle_actors = world.get_actors().filter('vehicle.*')
        for vehicle in vehicle_actors:
            pass

        pedestrian_actors = world.get_actors().filter('walker.pedestrian.*')
        for pedestrian in pedestrian_actors:
            pass

        # Create the camera
        camera_transform = carla.Transform(carla.Location(x=-50.41874313354492, y=33.17692184448242, z=20.430110931396484),
                                           carla.Rotation(pitch=-9.42114543914795, yaw=-153.6269989013672, roll=7.139936769817723e-06))
        camera_blueprint = blueprint_library.find('sensor.camera.rgb')
        camera_blueprint.set_attribute('image_size_x', '1280')
        camera_blueprint.set_attribute('image_size_y', '720')
        camera = world.spawn_actor(camera_blueprint, camera_transform)

        world_2_camera = np.array(camera.get_transform().get_inverse_matrix())

        image_w = camera_blueprint.get_attribute("image_size_x").as_int()
        image_h = camera_blueprint.get_attribute("image_size_y").as_int()
        fov = camera_blueprint.get_attribute("fov").as_float()

        bp_lib = world.get_blueprint_library()
        spawn_points = world.get_map().get_spawn_points()

        for i in range(50):
            # Filtrar los blueprints de vehículos, excluyendo los de dos ruedas
            filtered_bp = [bp for bp in bp_lib.filter('vehicle') if not bp.has_attribute('number_of_wheels') or bp.get_attribute('number_of_wheels').as_int() > 2]

            if len(filtered_bp) > 0:
                vehicle_bp = random.choice(filtered_bp)
                npc = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points))

                if npc:
                    npc.set_autopilot(True)

        # Calculate the camera projection matrix to project from 3D -> 2D
        K = build_projection_matrix(image_w, image_h, fov)

        image_queue = queue.Queue()
        camera.listen(image_queue.put)

        # Variables for image numbering
        image_counter = 0

        # Capture function for images
        def capture_data(image):
            nonlocal image_counter
            # Save the image to disk
            image_counter += 1

        # Configure the capture function
        # camera.listen(lambda data: capture_data(data))

        camera_location = camera_transform.location
        print(camera_location)

        edges = [[0,1], [1,3], [3,2], [2,0], [0,4], [4,5], [5,1], [5,7], [7,6], [6,4], [6,2], [7,3]]

        # Main simulation loop
        while True:

            # Retrieve and reshape the image
            world.tick()
            image = image_queue.get()

            img = np.reshape(np.copy(image.raw_data), (image.height, image.width, 4))

            # Get the camera matrix 
            world_2_camera = np.array(camera.get_transform().get_inverse_matrix())

            for npc in world.get_actors().filter('*vehicle*'):
                # Filter out the ego vehicle
                if npc.id != None:
                    bb = npc.bounding_box
                    dist = npc.get_transform().location.distance(camera.get_transform().location)
                    # Filter for the vehicles within 50m
                    if dist > 0 and not is_obstructing(camera, npc, world):
                    # Calculate the dot product between the forward vector
                    # of the vehicle and the vector between the vehicle
                    # and the other vehicle. We threshold this dot product
                    # to limit to drawing bounding boxes IN FRONT OF THE CAMERA
                        forward_vec = camera.get_transform().get_forward_vector()
                        ray = npc.get_transform().location - camera.get_transform().location
                        if forward_vec.dot(ray) > 1:
                            p1 = get_image_point(bb.location, K, world_2_camera)
                            verts = [v for v in bb.get_world_vertices(npc.get_transform())]
                            x_max = -10000
                            x_min = 10000
                            y_max = -10000
                            y_min = 10000
                            for vert in verts:
                                p = get_image_point(vert, K, world_2_camera)
                                # Find the rightmost vertex
                                if p[0] > x_max:
                                    x_max = p[0]
                                # Find the leftmost vertex
                                if p[0] < x_min:
                                    x_min = p[0]
                                # Find the highest vertex
                                if p[1] > y_max:
                                    y_max = p[1]
                                # Find the lowest  vertex
                                if p[1] < y_min:
                                    y_min = p[1]
                            cv2.line(img, (int(x_min),int(y_min)), (int(x_max),int(y_min)), (0,0,255, 255), 1)
                            cv2.line(img, (int(x_min),int(y_max)), (int(x_max),int(y_max)), (0,0,255, 255), 1)
                            cv2.line(img, (int(x_min),int(y_min)), (int(x_min),int(y_max)), (0,0,255, 255), 1)
                            cv2.line(img, (int(x_max),int(y_min)), (int(x_max),int(y_max)), (0,0,255, 255), 1)

            cv2.imshow('ImageWindowName',img)
            if cv2.waitKey(1) == ord('q'):
                break
        cv2.destroyAllWindows()

    finally:
        camera.destroy()

if __name__ == '__main__':
    main()
carla-simulator / carla

Avoid Represent Vehicles non visibles by the camera #6505
