Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
29 changes: 14 additions & 15 deletions .gitignore
Original file line number Diff line number Diff line change
@@ -1,18 +1,17 @@
# 铏氭嫙鐜
venv/
# 瑙嗛鏂囦欢
*.mp4
*.avi
*.mov
# 缁撴灉鐩綍
lane_detection_results/
# Python缂撳瓨
# Python缓存(核心,解决缓存文件问题)
__pycache__/
*.pyc
# 鏃ュ織鏂囦欢
lane_detection.log
# 鏋勫缓鐩綍
src/openpilot_model/build/
src/openpilot_model/log/
# VS Code閰嶇疆
*.pyo
*.pyd
.pytest_cache/
__pycache__/

# 系统/编辑器缓存
.DS_Store
Thumbs.db
.vscode/
.idea/

# 其他缓存(按需补充)
*.log
*.tmp
1 change: 1 addition & 0 deletions src/2d-carla-tracking/.gitignore
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
__pycache__/
165 changes: 165 additions & 0 deletions src/2d-carla-tracking/2d-detection-frcnn.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,165 @@
import carla

import os
import queue
import random

import cv2
import torch
import numpy as np

from what.cli.model import *
from what.utils.file import get_file

from what.models.detection.frcnn.faster_rcnn import FasterRCNN
from what.models.detection.datasets.voc import VOC_CLASS_NAMES
from what.models.detection.utils.box_utils import draw_bounding_boxes

from utils.box_utils import draw_bounding_boxes
from utils.projection import *
from utils.world import *

# Check what_model_list for all available models
index = 8

# Download the model first if not exists
WHAT_MODEL_FILE = what_model_list[index][WHAT_MODEL_FILE_INDEX]
WHAT_MODEL_URL = what_model_list[index][WHAT_MODEL_URL_INDEX]
WHAT_MODEL_HASH = what_model_list[index][WHAT_MODEL_HASH_INDEX]

if not os.path.isfile(os.path.join(WHAT_MODEL_PATH, WHAT_MODEL_FILE)):
get_file(WHAT_MODEL_FILE,
WHAT_MODEL_PATH,
WHAT_MODEL_URL,
WHAT_MODEL_HASH)

# Faster RCNN
device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu")

model = FasterRCNN(device=device)
model.load(os.path.join(WHAT_MODEL_PATH, WHAT_MODEL_FILE), map_location=device)

def camera_callback(image, rgb_image_queue):
rgb_image_queue.put(np.reshape(np.copy(image.raw_data),
(image.height, image.width, 4)))

# Part 1
client = carla.Client('localhost', 2000)
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)

# Get the world spectator
spectator = world.get_spectator()

# Get the map spawn points
spawn_points = world.get_map().get_spawn_points()

# Spawn the ego vehicle
bp_lib = world.get_blueprint_library()
vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020')
vehicle = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points))

# Spawn the camera
camera_bp = bp_lib.find('sensor.camera.rgb')
# [Windows Only] Fixes https://github.com/carla-simulator/carla/issues/6085
camera_bp.set_attribute('image_size_x', '640')
camera_bp.set_attribute('image_size_y', '640')

camera_init_trans = carla.Transform(carla.Location(x=1, z=2))
camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=vehicle)

# Create a queue to store and retrieve the sensor data
image_queue = queue.Queue()
camera.listen(lambda image: camera_callback(image, image_queue))

# Clear existing NPCs
clear_npc(world)
clear_static_vehicle(world)

# Part 2

# Remember the edge pairs
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]]

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

# Get the attributes from the camera
image_w = camera_bp.get_attribute("image_size_x").as_int()
image_h = camera_bp.get_attribute("image_size_y").as_int()
fov = camera_bp.get_attribute("fov").as_float()

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

for i in range(20):
vehicle_bp = bp_lib.filter('vehicle')

# Exclude bicycle
car_bp = [bp for bp in vehicle_bp if int(
bp.get_attribute('number_of_wheels')) == 4]
npc = world.try_spawn_actor(random.choice(
car_bp), random.choice(spawn_points))

if npc:
npc.set_autopilot(True)

vehicle.set_autopilot(True)

# Main loop
while True:
try:
world.tick()

# Move the spectator to the top of the vehicle
transform = carla.Transform(vehicle.get_transform().transform(
carla.Location(x=-4, z=50)), carla.Rotation(yaw=-180, pitch=-90))
spectator.set_transform(transform)

# Display RGB camera image
origin_image = image_queue.get()

# Image preprocessing
image = cv2.cvtColor(origin_image, cv2.COLOR_BGR2RGB)

# Image preprocessing
height, width, _ = image.shape

# NHWC -> NCHW
input = np.array(image).transpose((2, 0, 1))
input = torch.from_numpy(input)[None]

inputs, boxes, labels, scores = model.predict(input)

# Only draw 6: bus, 7: car (remove background)
boxes = np.array(boxes)[0]
boxes = np.array([box for box, label in zip(
boxes, labels[0]) if label in [6, 7]])
scores = np.array([score for score, label in zip(
scores[0], labels[0]) if label in [6, 7]])
labels = np.array([6 for label in labels[0] if label in [6, 7]])

output = draw_bounding_boxes(origin_image,
boxes,
labels,
VOC_CLASS_NAMES[1:],
scores)

cv2.imshow('2D Faster RCNN', output)

# Quit if user presses 'q'
if cv2.waitKey(1) & 0xFF == ord('q'):
break

except KeyboardInterrupt as e:
break

clear(world, camera)
cv2.destroyAllWindows()
173 changes: 173 additions & 0 deletions src/2d-carla-tracking/2d-detection-gt.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,173 @@
import carla

import queue
import random

import cv2
import numpy as np

from what.models.detection.datasets.coco import COCO_CLASS_NAMES

from utils.box_utils import draw_bounding_boxes
from utils.projection import *
from utils.world import *

def camera_callback(image, rgb_image_queue):
rgb_image_queue.put(np.reshape(np.copy(image.raw_data),
(image.height, image.width, 4)))

# Part 1
client = carla.Client('localhost', 2000)
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)

# Get the world spectator
spectator = world.get_spectator()

# Get the map spawn points
spawn_points = world.get_map().get_spawn_points()

# Spawn the ego vehicle
bp_lib = world.get_blueprint_library()
vehicle_bp = bp_lib.find('vehicle.lincoln.mkz_2020')
vehicle = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points))

# Spawn the camera
camera_bp = bp_lib.find('sensor.camera.rgb')
# [Windows Only] Fixes https://github.com/carla-simulator/carla/issues/6085
camera_bp.set_attribute('image_size_x', '640')
camera_bp.set_attribute('image_size_y', '640')

camera_init_trans = carla.Transform(carla.Location(x=1, z=2))
camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=vehicle)

# Create a queue to store and retrieve the sensor data
image_queue = queue.Queue()
camera.listen(lambda image: camera_callback(image, image_queue))

# Clear existing NPCs
clear_npc(world)
clear_static_vehicle(world)

# Part 2

# Remember the edge pairs
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]]

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

# Get the attributes from the camera
image_w = camera_bp.get_attribute("image_size_x").as_int()
image_h = camera_bp.get_attribute("image_size_y").as_int()
fov = camera_bp.get_attribute("fov").as_float()

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

# Spawn NPCs
for i in range(50):
vehicle_bp = bp_lib.filter('vehicle')

# Exclude bicycle
car_bp = [bp for bp in vehicle_bp if int(
bp.get_attribute('number_of_wheels')) == 4]
npc = world.try_spawn_actor(random.choice(
car_bp), random.choice(spawn_points))

if npc:
npc.set_autopilot(True)

vehicle.set_autopilot(True)

# Main Loop
while True:
try:
world.tick()

# Move the spectator to the top of the vehicle
transform = carla.Transform(vehicle.get_transform().transform(
carla.Location(x=-4, z=50)), carla.Rotation(yaw=-180, pitch=-90))
spectator.set_transform(transform)

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

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

boxes = []
ids = []
for npc in world.get_actors().filter('*vehicle*'):

# Filter out the ego vehicle
if npc.id != vehicle.id:

bb = npc.bounding_box
dist = npc.get_transform().location.distance(vehicle.get_transform().location)

# Filter for the vehicles within 50m
if dist < 50:
# 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 = vehicle.get_transform().get_forward_vector()
ray = npc.get_transform().location - vehicle.get_transform().location

if forward_vec.dot(ray) > 0:

verts = [v for v in bb.get_world_vertices(
npc.get_transform())]

points_2d = []

for vert in verts:
ray0 = vert - camera.get_transform().location
cam_forward_vec = camera.get_transform().get_forward_vector()

if (cam_forward_vec.dot(ray0) > 0):
p = get_image_point(vert, K, world_2_camera)
else:
p = get_image_point(vert, K_b, world_2_camera)

points_2d.append(p)

x_min, x_max, y_min, y_max = get_2d_box_from_3d_edges(
points_2d, edges, image_h, image_w)

# Exclude very small bounding boxes
if (y_max - y_min) * (x_max - x_min) > 100 and (x_max - x_min) > 20:
if point_in_canvas((x_min, y_min), image_h, image_w) and point_in_canvas((x_max, y_max), image_h, image_w):
ids.append(npc.id)
boxes.append(
np.array([x_min, y_min, x_max, y_max]))

# Only draw 2: car, 5: bus, 7: truck
boxes = np.array(boxes)
labels = np.array([2] * len(boxes))
probs = np.array([1.0] * len(boxes))

if len(boxes) > 0:
# Draw bounding boxes onto the image
output = draw_bounding_boxes(
image, boxes, labels, COCO_CLASS_NAMES, ids)

cv2.imshow('2D Ground Truth', image)

if cv2.waitKey(1) == ord('q'):
break

except KeyboardInterrupt as e:
break

clear(world, camera)
cv2.destroyAllWindows()
Loading
Loading