diff --git a/src/carla_tracking/main.py b/src/carla_tracking/main.py index e838be2983..315266afca 100644 --- a/src/carla_tracking/main.py +++ b/src/carla_tracking/main.py @@ -10,263 +10,742 @@ import math import time import sys +import json +import logging from pathlib import Path +from scipy.optimize import linear_sum_assignment + + +# -------------------------- 日志配置 -------------------------- +def setup_logging(): + """设置日志系统""" + logger = logging.getLogger("carla_tracking") + logger.setLevel(logging.INFO) + + # 创建控制台处理器 + console_handler = logging.StreamHandler() + console_handler.setLevel(logging.INFO) + + # 创建文件处理器 + file_handler = logging.FileHandler("carla_tracking.log", mode='w') + file_handler.setLevel(logging.DEBUG) + + # 设置格式 + formatter = logging.Formatter( + '%(asctime)s - %(name)s - %(levelname)s - %(message)s' + ) + console_handler.setFormatter(formatter) + file_handler.setFormatter(formatter) + + # 添加处理器 + logger.addHandler(console_handler) + logger.addHandler(file_handler) + + return logger + + +logger = setup_logging() + + +# -------------------------- 配置管理 -------------------------- +class ConfigManager: + """配置管理器""" + + def __init__(self, config_file=None): + self.config = self._load_default_config() + + if config_file and os.path.exists(config_file): + self.load_config(config_file) + + def _load_default_config(self): + """加载默认配置""" + return { + 'carla': { + 'host': 'localhost', + 'port': 2000, + 'timeout': 10.0, + 'sync_mode': True, + 'fixed_delta_seconds': 0.05 + }, + 'detection': { + 'conf_thres': 0.15, + 'iou_thres': 0.4, + 'model_type': 'yolov5m', + 'device': 'cuda' if torch.cuda.is_available() else 'cpu' + }, + 'tracking': { + 'max_age': 30, + 'min_hits': 3, + 'iou_threshold': 0.3, + 'use_acceleration': True, + 'use_adaptive_threshold': True, + 'depth_aware': True, + 'min_confidence': 0.1 + }, + 'vehicle': { + 'max_speed': 50.0, + 'target_speed': 30.0, + 'safety_distance': 15.0, + 'enable_physics': True + }, + 'npc': { + 'count': 20, + 'min_distance': 20.0 + }, + 'camera': { + 'width': 800, + 'height': 600, + 'fov': 90 + }, + 'performance': { + 'show_panel': True, + 'log_interval': 100, + 'max_fps': 30 + } + } + + def load_config(self, config_file): + """从文件加载配置""" + try: + with open(config_file, 'r') as f: + user_config = json.load(f) + self._update_config(self.config, user_config) + logger.info(f"加载配置文件: {config_file}") + except Exception as e: + logger.warning(f"加载配置文件失败: {e}") + + def _update_config(self, base, update): + """递归更新配置""" + for key, value in update.items(): + if key in base and isinstance(base[key], dict) and isinstance(value, dict): + self._update_config(base[key], value) + else: + base[key] = value + + def save_config(self, config_file): + """保存配置到文件""" + try: + with open(config_file, 'w') as f: + json.dump(self.config, f, indent=4) + logger.info(f"配置文件已保存: {config_file}") + except Exception as e: + logger.error(f"保存配置文件失败: {e}") + + def get(self, key, default=None): + """获取配置值""" + keys = key.split('.') + value = self.config + for k in keys: + if isinstance(value, dict) and k in value: + value = value[k] + else: + return default + return value + + +# -------------------------- 优化的卡尔曼滤波器 -------------------------- +class OptimizedKalmanFilter: + """优化的卡尔曼滤波器""" + + def __init__(self, dt=1.0, use_acceleration=True): + self.dt = dt + self.use_acceleration = use_acceleration + + if use_acceleration: + # 8维状态: [x, y, w, h, vx, vy, vw, vh] + self.state_dim = 8 + self.meas_dim = 4 + + # 状态转移矩阵 + self.F = np.eye(self.state_dim, dtype=np.float32) + self.F[0, 4] = dt + self.F[1, 5] = dt + self.F[2, 6] = dt + self.F[3, 7] = dt + + # 测量矩阵 + self.H = np.zeros((self.meas_dim, self.state_dim), dtype=np.float32) + self.H[0, 0] = 1 + self.H[1, 1] = 1 + self.H[2, 2] = 1 + self.H[3, 3] = 1 + + # 初始化状态 + self.x = np.zeros((self.state_dim, 1), dtype=np.float32) + + # 初始化协方差矩阵 + self.P = np.eye(self.state_dim, dtype=np.float32) * 1000 + + # 过程噪声协方差 + self.Q = np.eye(self.state_dim, dtype=np.float32) * 0.1 + self.Q[4:, 4:] = np.eye(4) * 0.01 + + # 测量噪声协方差 + self.R = np.eye(self.meas_dim, dtype=np.float32) * 5 + else: + # 4维状态: [x, y, w, h] + self.state_dim = 4 + self.meas_dim = 4 + + self.F = np.eye(self.state_dim, dtype=np.float32) + self.H = np.eye(self.state_dim, dtype=np.float32) + self.x = np.zeros((self.state_dim, 1), dtype=np.float32) + self.P = np.eye(self.state_dim, dtype=np.float32) * 1000 + self.Q = np.eye(self.state_dim, dtype=np.float32) * 0.5 + self.R = np.eye(self.state_dim, dtype=np.float32) * 10 -# -------------------------- 优化版SORT跟踪器 -------------------------- -class KalmanFilter: - def __init__(self): - self.dt = 1.0 - self.x = np.zeros((6, 1)) # 增加速度和加速度状态 - self.F = np.array([[1, 0, self.dt, 0, 0.5 * self.dt ** 2, 0], - [0, 1, 0, self.dt, 0, 0.5 * self.dt ** 2], - [0, 0, 1, 0, self.dt, 0], - [0, 0, 0, 1, 0, self.dt], - [0, 0, 0, 0, 1, 0], - [0, 0, 0, 0, 0, 1]], dtype=np.float32) - self.H = np.array([[1, 0, 0, 0, 0, 0], - [0, 1, 0, 0, 0, 0]], dtype=np.float32) - self.P = np.eye(6, dtype=np.float32) * 1000 - self.Q = np.eye(6, dtype=np.float32) * 0.1 - self.R = np.eye(2, dtype=np.float32) * 5 + # 预计算转置矩阵 + self.H_T = self.H.T + self.F_T = self.F.T def predict(self): + """预测下一时刻状态""" self.x = self.F @ self.x - self.P = self.F @ self.P @ self.F.T + self.Q - return self.x[:2] + self.P = self.F @ self.P @ self.F_T + self.Q + + if self.use_acceleration: + return self.x[:4].flatten() + else: + return self.x.flatten() + + def update(self, measurement): + """使用测量值更新状态""" + measurement = np.array(measurement, dtype=np.float32).reshape(-1, 1) - def update(self, z): - y = z - self.H @ self.x - S = self.H @ self.P @ self.H.T + self.R - K = self.P @ self.H.T @ np.linalg.inv(S) + # 计算残差 + y = measurement - self.H @ self.x + + # 计算残差协方差 + S = self.H @ self.P @ self.H_T + self.R + + # 计算卡尔曼增益 + K = self.P @ self.H_T @ np.linalg.inv(S) + + # 更新状态 self.x = self.x + K @ y - self.P = self.P - K @ self.H @ self.P - return self.x[:2] + + # 更新协方差 + I = np.eye(self.state_dim, dtype=np.float32) + self.P = (I - K @ self.H) @ self.P + + return self.x[:4].flatten() -class Track: - __slots__ = ('id', 'kf', 'x1', 'y1', 'x2', 'y2', 'center', 'width', 'height', - 'hits', 'age', 'history', 'distance', 'distance_history', - 'velocity', 'last_distance', 'world_location', 'world_velocity') +# -------------------------- 优化的跟踪目标 -------------------------- +class OptimizedTrack: + """优化的跟踪目标""" - def __init__(self, box, track_id): + __slots__ = ('id', 'kf', 'bbox', 'center', 'width', 'height', 'aspect_ratio', + 'hits', 'age', 'total_visible_count', 'consecutive_invisible_count', + 'history', 'distance_history', 'velocity_history', + 'current_distance', 'velocity', 'confidence', 'class_id', + 'is_confirmed', 'last_seen') + + def __init__(self, bbox, track_id, class_id=2, use_acceleration=True): self.id = track_id - self.kf = KalmanFilter() - self.x1, self.y1, self.x2, self.y2 = box - self.center = np.array([[(self.x1 + self.x2) / 2], [(self.y1 + self.y2) / 2]], dtype=np.float32) - self.kf.x[:2] = self.center - self.width = self.x2 - self.x1 - self.height = self.y2 - self.y1 + self.class_id = class_id + + # 初始化卡尔曼滤波器 + self.kf = OptimizedKalmanFilter(use_acceleration=use_acceleration) + + # 初始边界框 + self.bbox = np.array(bbox, dtype=np.float32) + + # 初始中心点 + self.center = np.array([(bbox[0] + bbox[2]) / 2, (bbox[1] + bbox[3]) / 2], + dtype=np.float32) + + # 尺寸信息 + self.width = bbox[2] - bbox[0] + self.height = bbox[3] - bbox[1] + self.aspect_ratio = self.width / max(self.height, 1e-6) + + # 初始化滤波器状态 + if use_acceleration: + self.kf.x[:4] = np.array([self.center[0], self.center[1], + self.width, self.height], + dtype=np.float32).reshape(-1, 1) + else: + self.kf.x = np.array([self.center[0], self.center[1], + self.width, self.height], + dtype=np.float32).reshape(-1, 1) + + # 统计信息 self.hits = 1 self.age = 0 - self.history = deque(maxlen=8) - self.history.append(self.center) - self.distance = None - self.distance_history = deque(maxlen=5) + self.total_visible_count = 1 + self.consecutive_invisible_count = 0 + + # 历史记录 + self.history = deque(maxlen=30) + self.history.append(self.center.copy()) + + self.distance_history = deque(maxlen=10) + self.velocity_history = deque(maxlen=10) + + # 当前状态 + self.current_distance = None self.velocity = 0.0 - self.last_distance = None - self.world_location = None # 在CARLA世界中的位置 - self.world_velocity = None # 在CARLA世界中的速度 + self.confidence = 1.0 + self.is_confirmed = False + self.last_seen = 0 + + self.mark_seen() + + def mark_seen(self): + """标记为目标被看到""" + self.consecutive_invisible_count = 0 + self.last_seen = self.age + + def mark_missed(self): + """标记为目标丢失""" + self.consecutive_invisible_count += 1 def predict(self): + """预测下一时刻状态""" self.age += 1 - center = self.kf.predict() - - if self.history: - self.history.append(center) - - # 平滑处理 - n = len(self.history) - if n > 1: - weights = np.linspace(0.1, 1.0, n) - weights /= weights.sum() - history_array = np.array([h.flatten() for h in self.history]) - smoothed_center = np.average(history_array, axis=0, weights=weights) - smoothed_center = smoothed_center.reshape(2, 1) - else: - smoothed_center = center + + # 使用卡尔曼滤波器预测 + predicted_state = self.kf.predict() + + # 更新边界框 + if self.kf.use_acceleration: + cx, cy, w, h = predicted_state else: - smoothed_center = center + cx, cy, w, h = predicted_state + + # 确保尺寸合理 + w = max(w, 1.0) + h = max(h, 1.0) + + # 更新边界框 + self.bbox = np.array([ + cx - w / 2, cy - h / 2, + cx + w / 2, cy + h / 2 + ], dtype=np.float32) + + # 更新中心点 + self.center = np.array([cx, cy], dtype=np.float32) + self.width = w + self.height = h + + # 添加到历史记录 + self.history.append(self.center.copy()) + + return self.bbox + + def update(self, bbox, confidence=1.0, distance=None): + """使用检测结果更新跟踪目标""" + # 转换边界框 + w = bbox[2] - bbox[0] + h = bbox[3] - bbox[1] + cx = (bbox[0] + bbox[2]) / 2 + cy = (bbox[1] + bbox[3]) / 2 + + measurement = np.array([cx, cy, w, h], dtype=np.float32) + + # 更新卡尔曼滤波器 + updated_state = self.kf.update(measurement) # 更新边界框 - self.x1 = smoothed_center[0, 0] - self.width / 2 - self.y1 = smoothed_center[1, 0] - self.height / 2 - self.x2 = self.x1 + self.width - self.y2 = self.y1 + self.height - - return [self.x1, self.y1, self.x2, self.y2] - - def update(self, box, distance=None): - self.x1, self.y1, self.x2, self.y2 = box - self.center = np.array([[(self.x1 + self.x2) / 2], [(self.y1 + self.y2) / 2]], dtype=np.float32) - self.kf.update(self.center) - self.width = self.x2 - self.x1 - self.height = self.y2 - self.y1 + if self.kf.use_acceleration: + cx, cy, w, h = updated_state + else: + cx, cy, w, h = updated_state + + # 确保尺寸合理 + w = max(w, 1.0) + h = max(h, 1.0) + + self.bbox = np.array([ + cx - w / 2, cy - h / 2, + cx + w / 2, cy + h / 2 + ], dtype=np.float32) + + # 更新属性 + self.center = np.array([cx, cy], dtype=np.float32) + self.width = w + self.height = h + self.aspect_ratio = w / max(h, 1e-6) + + # 更新统计信息 self.hits += 1 - self.age = 0 - self.history.append(self.center) + self.total_visible_count += 1 + self.consecutive_invisible_count = 0 + self.last_seen = self.age + # 更新距离信息 if distance is not None: - self.last_distance = self.distance self.distance_history.append(distance) if self.distance_history: - self.distance = float(np.median(self.distance_history)) + self.current_distance = float(np.median(list(self.distance_history))) - def get_box(self): - return [self.x1, self.y1, self.x2, self.y2] - - def get_distance(self): - return self.distance if self.distance else 0.0 + # 更新置信度 + alpha = 0.3 + self.confidence = alpha * confidence + (1 - alpha) * self.confidence + + # 确认目标 + if self.hits >= 3 and not self.is_confirmed: + self.is_confirmed = True + + # 添加到历史记录 + self.history.append(self.center.copy()) + + def get_velocity(self): + """计算目标速度""" + if len(self.history) < 2: + return 0.0 + + # 使用最近5个位置计算速度 + recent_history = list(self.history)[-5:] + if len(recent_history) < 2: + return 0.0 + + distances = [] + for i in range(1, len(recent_history)): + dx = recent_history[i][0] - recent_history[i - 1][0] + dy = recent_history[i][1] - recent_history[i - 1][1] + distance = np.sqrt(dx * dx + dy * dy) + distances.append(distance) + + if distances: + avg_distance = np.mean(distances) + # 假设每秒30帧 + self.velocity = avg_distance * 30 + self.velocity_history.append(self.velocity) + + # 使用中值滤波平滑速度 + if len(self.velocity_history) >= 3: + self.velocity = float(np.median(list(self.velocity_history))) + + return self.velocity + + def get_similarity_score(self, bbox): + """计算与给定边界框的相似度分数""" + # 计算中心点距离 + other_cx = (bbox[0] + bbox[2]) / 2 + other_cy = (bbox[1] + bbox[3]) / 2 + other_w = bbox[2] - bbox[0] + other_h = bbox[3] - bbox[1] + + # 位置相似度 + dx = other_cx - self.center[0] + dy = other_cy - self.center[1] + distance = np.sqrt(dx * dx + dy * dy) + + # 自适应距离阈值 + max_distance = max(self.width, self.height) * 3 + + if distance > max_distance: + return 0.0 + + position_similarity = np.exp(-distance / (max_distance * 0.5)) + + # 尺寸相似度 + size_ratio1 = other_w / max(self.width, 1e-6) + size_ratio2 = other_h / max(self.height, 1e-6) + + if size_ratio1 > 2 or size_ratio1 < 0.5 or size_ratio2 > 2 or size_ratio2 < 0.5: + return 0.0 + + size_similarity = np.exp(-abs(size_ratio1 - 1) - abs(size_ratio2 - 1)) + + # 长宽比相似度 + other_aspect = other_w / max(other_h, 1e-6) + aspect_similarity = np.exp(-abs(other_aspect - self.aspect_ratio)) + + # 综合相似度 + similarity = 0.5 * position_similarity + 0.3 * size_similarity + 0.2 * aspect_similarity + + return similarity + + @property + def is_reliable(self): + """判断目标是否可靠""" + return self.is_confirmed and self.consecutive_invisible_count < 5 + + @property + def should_delete(self): + """判断是否应该删除目标""" + if not self.is_confirmed and self.consecutive_invisible_count >= 3: + return True + if self.is_confirmed and self.consecutive_invisible_count >= 10: + return True + return False + + +# -------------------------- 优化的SORT跟踪器 -------------------------- +class OptimizedSORT: + """优化的SORT跟踪器""" + + def __init__(self, config=None): + # 默认配置 + self.config = { + 'max_age': 30, + 'min_hits': 3, + 'iou_threshold': 0.3, + 'use_acceleration': True, + 'use_adaptive_threshold': True, + 'min_confidence': 0.1 + } + # 更新配置 + if config: + self.config.update(config) -class Sort: - def __init__(self, max_age=8, min_hits=3, iou_threshold=0.4): - self.max_age = max_age - self.min_hits = min_hits - self.iou_threshold = iou_threshold + # 跟踪目标列表 self.tracks = [] self.next_id = 1 - self.depths = [] - self.depth_thresholds = {'near': 15.0, 'medium': 30.0, 'far': 50.0} - from scipy.optimize import linear_sum_assignment - self.linear_sum_assignment = linear_sum_assignment - - def set_depths(self, depths): - if depths: - depths_array = np.array(depths, dtype=np.float32) - valid_mask = (depths_array > 0.1) & (depths_array < 200) - self.depths = np.where(valid_mask, depths_array, 50.0) - else: - self.depths = [] - def update(self, detections): - if len(detections) == 0: - for track in self.tracks: - track.predict() - track.age += 1 - self.tracks = [t for t in self.tracks if t.age <= self.max_age] - if self.tracks and self.min_hits > 0: - return np.array([[t.x1, t.y1, t.x2, t.y2, t.id] - for t in self.tracks if t.hits >= self.min_hits]) - return np.array([]) - - # 先预测所有track - for track in self.tracks: - track.predict() - - if len(self.tracks) == 0: - tracks_to_return = [] - for i, det in enumerate(detections): - track = Track(det[:4], self.next_id) - if i < len(self.depths): - track.distance = self.depths[i] - track.distance_history.append(self.depths[i]) - self.tracks.append(track) - tracks_to_return.append([track.x1, track.y1, track.x2, track.y2, track.id]) - self.next_id += 1 - return np.array(tracks_to_return) if tracks_to_return else np.array([]) - - track_boxes = np.array([t.get_box() for t in self.tracks], dtype=np.float32) - iou_matrix = self._iou_batch(track_boxes, detections[:, :4].astype(np.float32)) - - if iou_matrix.size > 0: - matches, unmatched_tracks, unmatched_dets = self._hungarian_algorithm(iou_matrix) - - for track_idx, det_idx in matches: - if iou_matrix[track_idx, det_idx] >= self._get_dynamic_iou_threshold(det_idx): - det_distance = self.depths[det_idx] if det_idx < len(self.depths) else None - self.tracks[track_idx].update(detections[det_idx][:4], det_distance) + # 缓存 + self._iou_cache = {} + self._prediction_cache = {} - for track_idx in unmatched_tracks: - self.tracks[track_idx].age += 1 - - for det_idx in unmatched_dets: - box = detections[det_idx][:4] - if self._is_valid_detection(box, det_idx): - track = Track(box, self.next_id) - if det_idx < len(self.depths): - track.distance = self.depths[det_idx] - track.distance_history.append(self.depths[det_idx]) - self.tracks.append(track) - self.next_id += 1 - - self.tracks = [t for t in self.tracks if self._should_keep_track(t)] - - valid_tracks = [t for t in self.tracks if t.hits >= self.min_hits] - if valid_tracks: - return np.array([[t.x1, t.y1, t.x2, t.y2, t.id] for t in valid_tracks]) - return np.array([]) - - def _get_dynamic_iou_threshold(self, det_idx): - if det_idx < len(self.depths): - dist = self.depths[det_idx] - if dist < self.depth_thresholds['near']: - return 0.5 - elif dist > self.depth_thresholds['far']: - return 0.3 - return self.iou_threshold - - def _is_valid_detection(self, box, det_idx): - width = box[2] - box[0] - height = box[3] - box[1] - aspect_ratio = width / max(height, 1e-6) - - if det_idx < len(self.depths): - dist = self.depths[det_idx] - if dist < self.depth_thresholds['near']: - min_size = 10 - elif dist < self.depth_thresholds['medium']: - min_size = 5 - elif dist < self.depth_thresholds['far']: - min_size = 3 - else: - min_size = 2 - else: - min_size = 5 + logger.info(f"初始化优化SORT跟踪器") + + def update(self, detections, depths=None): + """更新跟踪器状态""" + # 预处理检测结果 + detections = self._preprocess_detections(detections) + + # 预测所有现有跟踪目标 + self._predict_tracks() + + # 匹配检测和跟踪目标 + matched_pairs, unmatched_detections, unmatched_tracks = self._match_detections(detections) + + # 更新匹配的跟踪目标 + self._update_matched_tracks(matched_pairs, detections, depths) + + # 为未匹配的检测创建新跟踪目标 + self._create_new_tracks(unmatched_detections, detections, depths) + + # 标记未匹配的跟踪目标为丢失 + self._mark_missed_tracks(unmatched_tracks) + + # 清理无效的跟踪目标 + self._cleanup_tracks() + + # 获取输出结果 + output_tracks = self._get_output_tracks() + + return output_tracks - return (0.2 < aspect_ratio < 5.0 and - width > min_size and - height > min_size) + def _preprocess_detections(self, detections): + """预处理检测结果""" + if len(detections) == 0: + return [] - def _should_keep_track(self, track): - if track.distance and track.distance > self.depth_thresholds['far']: - return track.age <= self.max_age + 2 - return track.age <= self.max_age + # 转换为numpy数组 + detections_np = np.array(detections, dtype=np.float32) - def _iou_batch(self, b1, b2): - if b1.shape[0] == 0 or b2.shape[0] == 0: - return np.zeros((b1.shape[0], b2.shape[0]), dtype=np.float32) + # 过滤低置信度检测 + if detections_np.shape[1] > 4: + conf_mask = detections_np[:, 4] >= self.config['min_confidence'] + detections_np = detections_np[conf_mask] - b1_x1, b1_y1, b1_x2, b1_y2 = b1[:, 0], b1[:, 1], b1[:, 2], b1[:, 3] - b2_x1, b2_y1, b2_x2, b2_y2 = b2[:, 0], b2[:, 1], b2[:, 2], b2[:, 3] + # 过滤无效边界框 + valid_mask = (detections_np[:, 2] > detections_np[:, 0]) & \ + (detections_np[:, 3] > detections_np[:, 1]) - inter_x1 = np.maximum(b1_x1[:, None], b2_x1[None, :]) - inter_y1 = np.maximum(b1_y1[:, None], b2_y1[None, :]) - inter_x2 = np.minimum(b1_x2[:, None], b2_x2[None, :]) - inter_y2 = np.minimum(b1_y2[:, None], b2_y2[None, :]) + return detections_np[valid_mask] - inter_area = np.maximum(0, inter_x2 - inter_x1) * np.maximum(0, inter_y2 - inter_y1) - b1_area = (b1_x2 - b1_x1) * (b1_y2 - b1_y1) - b2_area = (b2_x2 - b2_x1) * (b2_y2 - b2_y1) + def _predict_tracks(self): + """预测所有跟踪目标的下一个状态""" + for track in self.tracks: + predicted_bbox = track.predict() + self._prediction_cache[track.id] = predicted_bbox - return inter_area / (b1_area[:, None] + b2_area[None, :] - inter_area + 1e-6) + def _match_detections(self, detections): + """匹配检测和跟踪目标""" + if len(self.tracks) == 0 or len(detections) == 0: + return [], list(range(len(detections))), list(range(len(self.tracks))) + + # 构建成本矩阵 + cost_matrix = self._build_cost_matrix(detections) - def _hungarian_algorithm(self, cost_matrix): if cost_matrix.size == 0: - return [], list(range(cost_matrix.shape[0])), list(range(cost_matrix.shape[1])) + return [], list(range(len(detections))), list(range(len(self.tracks))) + + # 使用匈牙利算法进行匹配 + try: + row_indices, col_indices = linear_sum_assignment(cost_matrix) + except Exception as e: + logger.error(f"匈牙利算法错误: {e}") + return [], list(range(len(detections))), list(range(len(self.tracks))) - row_ind, col_ind = self.linear_sum_assignment(-cost_matrix) - matches = list(zip(row_ind, col_ind)) + # 提取匹配对 + matched_pairs = [] + for row, col in zip(row_indices, col_indices): + if cost_matrix[row, col] < self._get_adaptive_threshold(): + matched_pairs.append((row, col)) - all_rows = set(range(cost_matrix.shape[0])) - all_cols = set(range(cost_matrix.shape[1])) - matched_rows = set(row_ind) - matched_cols = set(col_ind) + # 找出未匹配的检测和跟踪目标 + all_rows = set(range(len(self.tracks))) + all_cols = set(range(len(detections))) + matched_rows = set([p[0] for p in matched_pairs]) + matched_cols = set([p[1] for p in matched_pairs]) unmatched_tracks = list(all_rows - matched_rows) - unmatched_dets = list(all_cols - matched_cols) + unmatched_detections = list(all_cols - matched_cols) + + return matched_pairs, unmatched_detections, unmatched_tracks + + def _build_cost_matrix(self, detections): + """构建成本矩阵""" + n_tracks = len(self.tracks) + n_detections = len(detections) + + # 初始化成本矩阵 + cost_matrix = np.ones((n_tracks, n_detections), dtype=np.float32) * 1000 + + # 为每个跟踪目标-检测对计算成本 + for i, track in enumerate(self.tracks): + predicted_bbox = self._prediction_cache.get(track.id, track.bbox) + + for j, detection in enumerate(detections): + # 提取检测边界框 + det_bbox = detection[:4] + + # 计算IOU + iou = self._calculate_iou(predicted_bbox, det_bbox) + + # 计算相似度 + similarity = track.get_similarity_score(det_bbox) + + # 综合成本 + cost = 1.0 - similarity * iou + + cost_matrix[i, j] = cost + + return cost_matrix + + def _calculate_iou(self, bbox1, bbox2): + """计算两个边界框的IOU""" + # 使用缓存 + key = (tuple(bbox1), tuple(bbox2)) + if key in self._iou_cache: + return self._iou_cache[key] + + # 计算交集 + x1 = max(bbox1[0], bbox2[0]) + y1 = max(bbox1[1], bbox2[1]) + x2 = min(bbox1[2], bbox2[2]) + y2 = min(bbox1[3], bbox2[3]) + + # 检查是否有交集 + if x2 <= x1 or y2 <= y1: + self._iou_cache[key] = 0.0 + return 0.0 - return matches, unmatched_tracks, unmatched_dets + # 计算面积 + intersection = (x2 - x1) * (y2 - y1) + area1 = (bbox1[2] - bbox1[0]) * (bbox1[3] - bbox1[1]) + area2 = (bbox2[2] - bbox2[0]) * (bbox2[3] - bbox2[1]) + + # 计算IOU + iou = intersection / (area1 + area2 - intersection + 1e-6) + + # 缓存结果 + self._iou_cache[key] = iou + + return iou + + def _get_adaptive_threshold(self): + """获取自适应匹配阈值""" + return 1.0 - self.config['iou_threshold'] + + def _update_matched_tracks(self, matched_pairs, detections, depths): + """更新匹配的跟踪目标""" + for track_idx, det_idx in matched_pairs: + track = self.tracks[track_idx] + detection = detections[det_idx] + + # 提取检测信息 + bbox = detection[:4] + confidence = detection[4] if len(detection) > 4 else 1.0 + + # 获取深度信息 + depth = depths[det_idx] if depths and det_idx < len(depths) else None + + # 更新跟踪目标 + track.update(bbox, confidence, depth) + + def _create_new_tracks(self, unmatched_detections, detections, depths): + """为未匹配的检测创建新跟踪目标""" + for det_idx in unmatched_detections: + detection = detections[det_idx] + + # 提取检测信息 + bbox = detection[:4] + confidence = detection[4] if len(detection) > 4 else 1.0 + class_id = int(detection[5]) if len(detection) > 5 else 2 + + # 获取深度信息 + depth = depths[det_idx] if depths and det_idx < len(depths) else None + + # 创建新跟踪目标 + new_track = OptimizedTrack( + bbox=bbox, + track_id=self.next_id, + class_id=class_id, + use_acceleration=self.config['use_acceleration'] + ) + + # 设置初始距离 + if depth is not None: + new_track.distance_history.append(depth) + new_track.current_distance = depth + + # 添加到跟踪列表 + self.tracks.append(new_track) + self.next_id += 1 + + def _mark_missed_tracks(self, unmatched_tracks): + """标记未匹配的跟踪目标为丢失""" + for track_idx in unmatched_tracks: + self.tracks[track_idx].mark_missed() + + def _cleanup_tracks(self): + """清理无效的跟踪目标""" + self.tracks = [track for track in self.tracks if not track.should_delete] + + def _get_output_tracks(self): + """获取输出跟踪结果""" + output = [] + + for track in self.tracks: + if track.is_reliable: + output.append([ + track.bbox[0], # x1 + track.bbox[1], # y1 + track.bbox[2], # x2 + track.bbox[3], # y2 + track.id, # track_id + track.class_id, # class_id + track.confidence # confidence + ]) + + return output + + def get_tracks_info(self): + """获取跟踪目标详细信息""" + info = [] + for track in self.tracks: + if track.is_reliable: + info.append({ + 'id': track.id, + 'bbox': track.bbox.tolist(), + 'center': track.center.tolist(), + 'velocity': track.get_velocity(), + 'distance': track.current_distance, + 'confidence': track.confidence, + 'age': track.age, + 'hits': track.hits, + 'history': [h.tolist() for h in track.history] + }) + return info # -------------------------- YOLOv5检测模型 -------------------------- @@ -274,6 +753,7 @@ def _hungarian_algorithm(self, cost_matrix): def load_detection_model(model_type): + """加载检测模型""" device = 'cuda' if torch.cuda.is_available() else 'cpu' model_paths = { @@ -298,7 +778,7 @@ def load_detection_model(model_type): if os.path.exists(path): model_type = key model_path = path - print(f"使用备用模型:{model_type}") + logger.info(f"使用备用模型:{model_type}") break if not model_path or not os.path.exists(model_path): @@ -310,55 +790,83 @@ def load_detection_model(model_type): if device == 'cuda': model.half() + # 预热模型 with torch.no_grad(): dummy_input = torch.randn(1, 3, 640, 640, device=device) if device == 'cuda': dummy_input = dummy_input.half() _ = model(dummy_input) - print(f"模型加载成功:{model_type} (设备:{device})") + logger.info(f"模型加载成功:{model_type} (设备:{device})") return model, model.names -# -------------------------- 运动控制器 -------------------------- +# -------------------------- 车辆控制器 -------------------------- class VehicleController: """车辆运动控制器""" - def __init__(self, vehicle): + def __init__(self, vehicle, config): self.vehicle = vehicle - self.max_speed = 50.0 # 最大速度 km/h - self.target_speed = 30.0 # 目标速度 km/h - self.obstacle_distance = 100.0 # 安全距离 - self.last_update = time.time() + self.config = config + + self.max_speed = config.get('vehicle.max_speed', 50.0) + self.target_speed = config.get('vehicle.target_speed', 30.0) + self.safety_distance = config.get('vehicle.safety_distance', 15.0) + self.control_state = { 'throttle': 0.0, 'steer': 0.0, 'brake': 0.0, - 'reverse': False + 'reverse': False, + 'speed': 0.0 } + # PID控制器参数 + self.Kp = 0.01 + self.Ki = 0.001 + self.Kd = 0.005 + self.last_error = 0.0 + self.integral = 0.0 + def update_control(self, detected_obstacles): """根据检测到的障碍物更新控制""" control = carla.VehicleControl() # 获取当前速度 - velocity = self.vehicle.get_velocity() - speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2) * 3.6 # m/s to km/h + try: + velocity = self.vehicle.get_velocity() + speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2) * 3.6 + self.control_state['speed'] = speed + except: + speed = 0.0 + + # PID控制速度 + error = self.target_speed - speed + self.integral += error + derivative = error - self.last_error + + throttle_base = self.Kp * error + self.Ki * self.integral + self.Kd * derivative + throttle = np.clip(throttle_base, 0.0, 0.8) - # 基本PID控制器 - speed_error = self.target_speed - speed - throttle = np.clip(speed_error * 0.01, 0.0, 0.8) + self.last_error = error # 检查障碍物 brake = 0.0 if detected_obstacles: - closest_distance = min([d for d in detected_obstacles if d < 50]) - if closest_distance < 15: # 紧急制动 - throttle = 0.0 - brake = 1.0 - elif closest_distance < 30: # 减速 - throttle = np.clip(throttle * 0.3, 0.0, 0.3) - brake = 0.3 + # 过滤有效距离 + valid_distances = [d for d in detected_obstacles if d is not None and d < 50] + if valid_distances: + closest_distance = min(valid_distances) + + if closest_distance < self.safety_distance * 0.5: # 紧急制动 + throttle = 0.0 + brake = 1.0 + elif closest_distance < self.safety_distance: # 减速 + throttle = np.clip(throttle * 0.3, 0.0, 0.3) + brake = 0.3 + elif closest_distance < self.safety_distance * 1.5: # 轻微减速 + throttle = np.clip(throttle * 0.7, 0.0, 0.5) + brake = 0.1 # 随机转向模拟真实驾驶 if random.random() < 0.05: # 5%概率微调方向 @@ -382,38 +890,41 @@ def update_control(self, detected_obstacles): return control def set_target_speed(self, speed): + """设置目标速度""" self.target_speed = np.clip(speed, 0.0, self.max_speed) def emergency_stop(self): + """紧急停止""" control = carla.VehicleControl() control.throttle = 0.0 control.brake = 1.0 return control -# -------------------------- NPC车辆运动控制 -------------------------- +# -------------------------- NPC管理器 -------------------------- class NPCManager: """NPC车辆管理器""" - def __init__(self, client): + def __init__(self, client, config): self.client = client + self.config = config self.traffic_manager = None self.npc_vehicles = [] try: self.traffic_manager = self.client.get_trafficmanager() self.traffic_manager.set_synchronous_mode(True) - print("交通管理器初始化成功") + logger.info("交通管理器初始化成功") except Exception as e: - print(f"交通管理器初始化失败: {e}") + logger.warning(f"交通管理器初始化失败: {e}") def spawn_npcs(self, world, count=30, ego_vehicle=None): - """生成NPC车辆并设置自动驾驶""" + """生成NPC车辆""" bp_lib = world.get_blueprint_library() spawn_points = world.get_map().get_spawn_points() if not spawn_points: - print("没有可用的生成点") + logger.warning("没有可用的生成点") return [] # 清理现有NPC @@ -421,6 +932,7 @@ def spawn_npcs(self, world, count=30, ego_vehicle=None): random.shuffle(spawn_points) spawned_count = 0 + min_distance = self.config.get('npc.min_distance', 20.0) for spawn_point in spawn_points: if spawned_count >= count: @@ -434,7 +946,7 @@ def spawn_npcs(self, world, count=30, ego_vehicle=None): (spawn_point.location.x - ego_loc.x) ** 2 + (spawn_point.location.y - ego_loc.y) ** 2 ) - if dist < 20.0: # 离主车辆太近 + if dist < min_distance: continue except: pass @@ -467,28 +979,23 @@ def spawn_npcs(self, world, count=30, ego_vehicle=None): self.traffic_manager.vehicle_percentage_speed_difference( npc, random.uniform(-30.0, 30.0) ) - # 不同版本的CARLA API可能没有set_lane_change_permission - try: - self.traffic_manager.set_lane_change_permission(npc, False) - except AttributeError: - # 旧版本API,忽略这个设置 - pass except Exception as e: - print(f"设置NPC自动驾驶失败: {e}") + logger.warning(f"设置NPC自动驾驶失败: {e}") self.npc_vehicles.append(npc) spawned_count += 1 - print(f"生成NPC {spawned_count}/{count}") + if spawned_count % 10 == 0: + logger.info(f"已生成NPC {spawned_count}/{count}") - print(f"总共生成 {len(self.npc_vehicles)} 辆NPC车辆") + logger.info(f"总共生成 {len(self.npc_vehicles)} 辆NPC车辆") return self.npc_vehicles def update_npc_behavior(self): """更新NPC行为""" for npc in self.npc_vehicles: try: - # 随机更新NPC的速度差异,增加变化性 - if random.random() < 0.01 and self.traffic_manager: # 1%概率 + # 随机更新NPC的速度差异 + if random.random() < 0.01 and self.traffic_manager: self.traffic_manager.vehicle_percentage_speed_difference( npc, random.uniform(-40.0, 40.0) ) @@ -506,116 +1013,59 @@ def destroy_all_npcs(self): self.npc_vehicles.clear() -# -------------------------- 性能监控面板函数 -------------------------- -def draw_performance_panel(image, timings, fps, frame_count): - """ - 在图像上绘制性能监控面板 - """ - h, w = image.shape[:2] - - def get_avg_time(key, default=0.0): - if key in timings and timings[key]: - recent = timings[key][-10:] - return np.mean(recent) if recent else default - return default - - # 计算各阶段耗时(毫秒) - carla_time = get_avg_time('carla_tick') * 1000 - image_time = get_avg_time('image_get') * 1000 - depth_time = get_avg_time('depth_get') * 1000 - detection_time = get_avg_time('detection') * 1000 - tracking_time = get_avg_time('tracking') * 1000 - drawing_time = get_avg_time('drawing') * 1000 - display_time = get_avg_time('display') * 1000 - total_time = get_avg_time('total') * 1000 - - # 面板位置和尺寸 - panel_x = 10 - panel_y = 10 - panel_width = 280 - panel_height = 200 - - # 创建半透明面板背景 - panel_bg = np.zeros((panel_height, panel_width, 3), dtype=np.uint8) - panel_bg[:] = (20, 20, 20) - - # 叠加面板到图像上 - x1, y1 = panel_x, panel_y - x2, y2 = panel_x + panel_width, panel_y + panel_height - - if x2 <= w and y2 <= h: - alpha = 0.7 - image[y1:y2, x1:x2] = cv2.addWeighted( - image[y1:y2, x1:x2], 1 - alpha, panel_bg, alpha, 0 - ) - - # 绘制标题 - title = "性能监控面板" - cv2.putText(image, title, (panel_x + 10, panel_y + 20), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 1) - - cv2.line(image, (panel_x, panel_y + 25), - (panel_x + panel_width, panel_y + 25), (100, 100, 100), 1) - - # 绘制性能指标 - y_offset = 45 - line_height = 20 - - cv2.putText(image, f"FPS: {fps:.1f}", (panel_x + 10, panel_y + y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1) - cv2.putText(image, f"Frame: {frame_count}", (panel_x + 120, panel_y + y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 200, 200), 1) - - y_offset += line_height +# -------------------------- 工具函数 -------------------------- +def preprocess_depth_image(depth_image): + """预处理深度图像""" + if depth_image is None: + return None - color = (0, 255, 0) if carla_time < 5.0 else (0, 165, 255) if carla_time < 10.0 else (0, 0, 255) - cv2.putText(image, f"CARLA: {carla_time:.1f}ms", (panel_x + 10, panel_y + y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) + if depth_image.dtype == np.float16: + depth_image = depth_image.astype(np.float32) - color = (0, 255, 0) if image_time < 1.0 else (0, 165, 255) if image_time < 3.0 else (0, 0, 255) - cv2.putText(image, f"Image: {image_time:.1f}ms", (panel_x + 120, panel_y + y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) + depth_image = np.clip(depth_image, 0.1, 200.0) - y_offset += line_height + if depth_image.shape[0] > 3 and depth_image.shape[1] > 3: + depth_image = cv2.GaussianBlur(depth_image, (3, 3), 0.5) - if depth_time > 0: - color = (0, 255, 0) if depth_time < 2.0 else (0, 165, 255) if depth_time < 5.0 else (0, 0, 255) - cv2.putText(image, f"Depth: {depth_time:.1f}ms", (panel_x + 10, panel_y + y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) + max_val = np.max(depth_image) + if max_val > 0: + depth_image = np.power(depth_image / max_val, 0.7) * max_val - y_offset += line_height + return depth_image - color = (0, 255, 0) if detection_time < 10.0 else (0, 165, 255) if detection_time < 20.0 else (0, 0, 255) - cv2.putText(image, f"Detection: {detection_time:.1f}ms", (panel_x + 10, panel_y + y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) - y_offset += line_height +def get_target_distance(depth_image, box, use_median=True): + """获取目标距离""" + if depth_image is None: + return 50.0 - color = (0, 255, 0) if tracking_time < 5.0 else (0, 165, 255) if tracking_time < 10.0 else (0, 0, 255) - cv2.putText(image, f"Tracking: {tracking_time:.1f}ms", (panel_x + 10, panel_y + y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) + if depth_image.dtype == np.float16: + depth_image = depth_image.astype(np.float32) - color = (0, 255, 0) if drawing_time < 2.0 else (0, 165, 255) if drawing_time < 5.0 else (0, 0, 255) - cv2.putText(image, f"Drawing: {drawing_time:.1f}ms", (panel_x + 120, panel_y + y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) + x1, y1, x2, y2 = map(int, box) - y_offset += line_height + h, w = depth_image.shape + x1, y1 = max(0, x1), max(0, y1) + x2, y2 = min(w - 1, x2), min(h - 1, y2) - color = (0, 255, 0) if display_time < 2.0 else (0, 165, 255) if display_time < 5.0 else (0, 0, 255) - cv2.putText(image, f"Display: {display_time:.1f}ms", (panel_x + 10, panel_y + y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) + if x1 >= x2 or y1 >= y2: + return 50.0 - y_offset += line_height + depth_roi = depth_image[y1:y2, x1:x2] + valid_mask = depth_roi > 0.1 + valid_depths = depth_roi[valid_mask] - color = (0, 255, 0) if total_time < 50.0 else (0, 165, 255) if total_time < 100.0 else (0, 0, 255) - cv2.putText(image, f"Total: {total_time:.1f}ms", (panel_x + 10, panel_y + y_offset), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) + if valid_depths.size == 0: + return 50.0 - return image + if use_median: + return float(np.median(valid_depths)) + else: + return float(np.mean(valid_depths)) -# -------------------------- 核心工具函数 -------------------------- def draw_bounding_boxes(image, boxes, labels, class_names, **kwargs): + """绘制边界框""" track_ids = kwargs.get('track_ids') probs = kwargs.get('probs') distances = kwargs.get('distances') @@ -624,8 +1074,6 @@ def draw_bounding_boxes(image, boxes, labels, class_names, **kwargs): result = image.copy() h, w = image.shape[:2] - color_cache = {} - for i, box in enumerate(boxes): x1, y1, x2, y2 = map(int, box) @@ -637,137 +1085,209 @@ def draw_bounding_boxes(image, boxes, labels, class_names, **kwargs): if x1 >= x2 or y1 >= y2: continue - # 颜色计算 - color = (0, 255, 0) - if distances and i < len(distances) and distances[i]: + # 根据距离设置颜色 + if distances and i < len(distances) and distances[i] is not None: dist = distances[i] - if dist not in color_cache: - if dist < 15: - r, g = 255, int(255 * (dist / 15)) - color_cache[dist] = (0, g, r) - elif dist < 30: - r, g = int(255 * (1 - (dist - 15) / 15)), 255 - color_cache[dist] = (0, g, r) - elif dist < 50: - b, g = int(255 * ((dist - 30) / 20)), 255 - color_cache[dist] = (b, g, 0) - else: - color_cache[dist] = (0, 255, 0) - color = color_cache[dist] + if dist < 15: + color = (0, 0, 255) # 红色,近距离 + elif dist < 30: + color = (0, 165, 255) # 橙色 + else: + color = (0, 255, 0) # 绿色,远距离 + else: + color = (0, 255, 0) + # 绘制边界框 cv2.rectangle(result, (x1, y1), (x2, y2), color, 2) # 构建文本信息 - text_parts = [ - class_names.get(labels[i], f"cls{labels[i]}") if i < len(labels) else "", - f"{probs[i]:.2f}" if probs and i < len(probs) else "", - f"ID:{track_ids[i]}" if track_ids and i < len(track_ids) else "", - f"D:{distances[i]:.1f}m" if distances and i < len(distances) and distances[i] else "", - f"S:{velocities[i]:.1f}m/s" if velocities and i < len(velocities) and velocities[i] else "" - ] + text_parts = [] + + if i < len(labels): + text_parts.append(class_names.get(labels[i], f"cls{labels[i]}")) + + if probs and i < len(probs): + text_parts.append(f"{probs[i]:.2f}") + + if track_ids and i < len(track_ids): + text_parts.append(f"ID:{track_ids[i]}") + + if distances and i < len(distances) and distances[i] is not None: + text_parts.append(f"D:{distances[i]:.1f}m") + + if velocities and i < len(velocities) and velocities[i] is not None: + text_parts.append(f"V:{velocities[i]:.1f}") label_text = " ".join(filter(None, text_parts)) - text_size = cv2.getTextSize(label_text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 2)[0] - text_bg_y1 = max(0, y1 - text_size[1] - 5) - text_bg_y2 = max(0, y1) + if label_text: + text_size = cv2.getTextSize(label_text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 2)[0] + text_bg_y1 = max(0, y1 - text_size[1] - 5) + text_bg_y2 = max(0, y1) - cv2.rectangle(result, (x1, text_bg_y1), - (x1 + text_size[0], text_bg_y2), color, -1) - cv2.putText(result, label_text, (x1, y1 - 3), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) + cv2.rectangle(result, (x1, text_bg_y1), + (x1 + text_size[0] + 5, text_bg_y2), color, -1) + cv2.putText(result, label_text, (x1 + 2, y1 - 3), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2) return result -def preprocess_depth_image(depth_image): - if depth_image is None: - return None +def draw_trajectories(image, tracks_info): + """绘制目标轨迹""" + if not tracks_info: + return image - if depth_image.dtype == np.float16: - depth_image = depth_image.astype(np.float32) + result = image.copy() - depth_image = np.clip(depth_image, 0.1, 200.0) + for track in tracks_info: + if 'history' not in track or len(track['history']) < 2: + continue - if depth_image.shape[0] > 3 and depth_image.shape[1] > 3: - depth_image = cv2.GaussianBlur(depth_image, (3, 3), 0.5) + # 为不同目标使用不同颜色 + color_id = track['id'] % 10 + colors = [ + (255, 0, 0), (0, 255, 0), (0, 0, 255), + (255, 255, 0), (255, 0, 255), (0, 255, 255), + (128, 0, 0), (0, 128, 0), (0, 0, 128), + (128, 128, 0) + ] + color = colors[color_id] - max_val = np.max(depth_image) - if max_val > 0: - depth_image = np.power(depth_image / max_val, 0.7) * max_val + # 绘制轨迹线 + points = [] + for point in track['history'][-20:]: # 只绘制最近20个点 + if point: + x, y = int(point[0]), int(point[1]) + points.append((x, y)) - return depth_image + if len(points) >= 2: + for i in range(1, len(points)): + cv2.line(result, points[i - 1], points[i], color, 1) + # 绘制轨迹点 + for point in points: + cv2.circle(result, point, 2, color, -1) -def get_target_distance(depth_image, box, use_median=True): - if depth_image is None: - return 50.0 + return result - if depth_image.dtype == np.float16: - depth_image = depth_image.astype(np.float32) - x1, y1, x2, y2 = map(int, box) +def draw_performance_panel(image, timings, fps, frame_count, config): + """绘制性能监控面板""" + if not config.get('performance.show_panel', True): + return image - h, w = depth_image.shape - x1, y1 = max(0, x1), max(0, y1) - x2, y2 = min(w - 1, x2), min(h - 1, y2) + h, w = image.shape[:2] - if x1 >= x2 or y1 >= y2: - return 50.0 + def get_avg_time(key, default=0.0): + if key in timings and timings[key]: + # timings[key] 是 deque,需要转换为列表再切片 + timing_list = list(timings[key]) + recent = timing_list[-10:] if len(timing_list) >= 10 else timing_list + return np.mean(recent) if recent else default + return default - depth_roi = depth_image[y1:y2, x1:x2] - valid_mask = depth_roi > 0.1 - valid_depths = depth_roi[valid_mask] + # 计算各阶段耗时(毫秒) + carla_time = get_avg_time('carla_tick') * 1000 + image_time = get_avg_time('image_get') * 1000 + depth_time = get_avg_time('depth_get') * 1000 + detection_time = get_avg_time('detection') * 1000 + tracking_time = get_avg_time('tracking') * 1000 + drawing_time = get_avg_time('drawing') * 1000 + total_time = get_avg_time('total') * 1000 - if valid_depths.size == 0: - return 50.0 + # 面板位置和尺寸 + panel_x = 10 + panel_y = 10 + panel_width = 300 + panel_height = 220 - if use_median: - return float(np.median(valid_depths)) - else: - h_roi, w_roi = depth_roi.shape - if h_roi == 0 or w_roi == 0: - return float(np.mean(valid_depths)) - - cy, cx = h_roi // 2, w_roi // 2 - y_coords, x_coords = np.ogrid[:h_roi, :w_roi] - dist_from_center = np.sqrt((x_coords - cx) ** 2 + (y_coords - cy) ** 2) - max_dist = np.sqrt(cx ** 2 + cy ** 2) + 1e-6 - weights = 1 - (dist_from_center / max_dist) - weights = weights * valid_mask - - if np.sum(weights) > 0: - return float(np.sum(depth_roi * weights) / np.sum(weights)) - else: - return float(np.mean(valid_depths)) + # 创建半透明面板背景 + if panel_x + panel_width <= w and panel_y + panel_height <= h: + overlay = image.copy() + cv2.rectangle(overlay, (panel_x, panel_y), + (panel_x + panel_width, panel_y + panel_height), + (20, 20, 20), -1) + image = cv2.addWeighted(image, 0.3, overlay, 0.7, 0) + + # 绘制标题 + cv2.putText(image, "性能监控面板", (panel_x + 10, panel_y + 20), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 1) + + cv2.line(image, (panel_x, panel_y + 25), + (panel_x + panel_width, panel_y + 25), (100, 100, 100), 1) + + # 绘制性能指标 + y_offset = 45 + line_height = 20 + + # FPS和帧数 + fps_color = (0, 255, 0) if fps > 20 else (0, 165, 255) if fps > 10 else (0, 0, 255) + cv2.putText(image, f"FPS: {fps:.1f}", (panel_x + 10, panel_y + y_offset), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, fps_color, 1) + cv2.putText(image, f"Frame: {frame_count}", (panel_x + 120, panel_y + y_offset), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 200, 200), 1) + + y_offset += line_height + + # 各阶段时间 + metrics = [ + ('CARLA', carla_time, 5.0, 10.0), + ('Image', image_time, 1.0, 3.0), + ('Depth', depth_time, 2.0, 5.0), + ('Detection', detection_time, 10.0, 20.0), + ('Tracking', tracking_time, 5.0, 10.0), + ('Drawing', drawing_time, 2.0, 5.0), + ('Total', total_time, 50.0, 100.0) + ] + + for i in range(0, len(metrics), 2): + name1, time1, good1, warn1 = metrics[i] + color1 = (0, 255, 0) if time1 < good1 else (0, 165, 255) if time1 < warn1 else (0, 0, 255) + cv2.putText(image, f"{name1}: {time1:.1f}ms", (panel_x + 10, panel_y + y_offset), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, color1, 1) + + if i + 1 < len(metrics): + name2, time2, good2, warn2 = metrics[i + 1] + color2 = (0, 255, 0) if time2 < good2 else (0, 165, 255) if time2 < warn2 else (0, 0, 255) + cv2.putText(image, f"{name2}: {time2:.1f}ms", (panel_x + 150, panel_y + y_offset), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, color2, 1) + + y_offset += line_height + + return image # -------------------------- CARLA相关函数 -------------------------- -def setup_carla_client(host='localhost', port=2000): +def setup_carla_client(config): """连接CARLA服务器""" + host = config.get('carla.host', 'localhost') + port = config.get('carla.port', 2000) + timeout = config.get('carla.timeout', 10.0) + try: client = carla.Client(host, port) - client.set_timeout(10.0) + client.set_timeout(timeout) world = client.get_world() # 设置同步模式 settings = world.get_settings() - settings.synchronous_mode = True - settings.fixed_delta_seconds = 0.05 + settings.synchronous_mode = config.get('carla.sync_mode', True) + settings.fixed_delta_seconds = config.get('carla.fixed_delta_seconds', 0.05) world.apply_settings(settings) - print(f"连接到CARLA服务器 {host}:{port}") - print(f"地图: {world.get_map().name}") - print(f"同步模式: {settings.synchronous_mode}") - print(f"时间步长: {settings.fixed_delta_seconds}") + logger.info(f"连接到CARLA服务器 {host}:{port}") + logger.info(f"地图: {world.get_map().name}") + logger.info(f"同步模式: {settings.synchronous_mode}") + logger.info(f"时间步长: {settings.fixed_delta_seconds}") return world, client except Exception as e: - print(f"连接CARLA失败: {e}") + logger.error(f"连接CARLA失败: {e}") raise -def spawn_ego_vehicle(world, enable_physics=True): +def spawn_ego_vehicle(world, config): """生成主车辆""" bp_lib = world.get_blueprint_library() @@ -779,6 +1299,7 @@ def spawn_ego_vehicle(world, enable_physics=True): pass if not vehicle_bp: + # 尝试其他车辆 small_vehicles = [ 'vehicle.audi.a2', 'vehicle.audi.tt', @@ -799,12 +1320,12 @@ def spawn_ego_vehicle(world, enable_physics=True): try: vehicle_bp = random.choice(list(bp_lib.filter('vehicle.*'))) except: - print("错误:没有找到可用的车辆蓝图") + logger.error("错误:没有找到可用的车辆蓝图") return None spawn_points = world.get_map().get_spawn_points() if not spawn_points: - print("错误:没有可用的生成点") + logger.error("错误:没有可用的生成点") return None # 选择远离其他车辆的生成点 @@ -830,15 +1351,15 @@ def spawn_ego_vehicle(world, enable_physics=True): try: vehicle = world.spawn_actor(vehicle_bp, spawn_point) if vehicle: - print(f"主车辆生成成功: {vehicle_bp.id}") - print(f"位置: X={spawn_point.location.x:.1f}, Y={spawn_point.location.y:.1f}") + logger.info(f"主车辆生成成功: {vehicle_bp.id}") # 启用物理模拟 + enable_physics = config.get('vehicle.enable_physics', True) vehicle.set_simulate_physics(enable_physics) return vehicle except Exception as e: - print(f"生成车辆失败: {e}") + logger.warning(f"生成车辆失败: {e}") continue # 如果找不到合适的位置,强制生成 @@ -846,22 +1367,23 @@ def spawn_ego_vehicle(world, enable_physics=True): try: vehicle = world.spawn_actor(vehicle_bp, spawn_points[0]) if vehicle: + enable_physics = config.get('vehicle.enable_physics', True) vehicle.set_simulate_physics(enable_physics) - print(f"主车辆强制生成: {vehicle_bp.id}") + logger.info(f"主车辆强制生成: {vehicle_bp.id}") return vehicle except Exception as e: - print(f"强制生成车辆失败: {e}") + logger.error(f"强制生成车辆失败: {e}") return None -# 优化内存占用的回调函数 +# 回调函数 def camera_callback(image, rgb_image_queue): """RGB图像回调函数""" try: array = np.frombuffer(image.raw_data, dtype=np.uint8) array = array.reshape((image.height, image.width, 4)) - rgb = array[..., :3] # 只存储RGB通道 + rgb = array[..., :3] if rgb_image_queue.full(): try: @@ -870,7 +1392,7 @@ def camera_callback(image, rgb_image_queue): pass rgb_image_queue.put(rgb) except Exception as e: - print(f"相机回调错误: {e}") + logger.error(f"相机回调错误: {e}") def depth_camera_callback(image, depth_queue): @@ -895,57 +1417,63 @@ def depth_camera_callback(image, depth_queue): pass depth_queue.put(depth_in_meters) except Exception as e: - print(f"深度相机回调错误: {e}") + logger.error(f"深度相机回调错误: {e}") # -------------------------- 主函数 -------------------------- def main(): + # 初始化 + parser = argparse.ArgumentParser(description='CARLA目标检测与跟踪 - 优化版') + parser.add_argument('--config', type=str, default=None, help='配置文件路径') + parser.add_argument('--model', type=str, default='yolov5m', help='模型类型') + parser.add_argument('--host', type=str, default='localhost', help='CARLA服务器地址') + parser.add_argument('--port', type=int, default=2000, help='CARLA服务器端口') + parser.add_argument('--conf-thres', type=float, default=0.15, help='置信度阈值') + parser.add_argument('--iou-thres', type=float, default=0.4, help='IOU阈值') + parser.add_argument('--use-depth', action='store_true', default=True, help='使用深度信息') + parser.add_argument('--show-depth', action='store_true', help='显示深度图像') + parser.add_argument('--npc-count', type=int, default=20, help='NPC数量') + parser.add_argument('--target-speed', type=float, default=30.0, help='目标速度 (km/h)') + parser.add_argument('--manual-control', action='store_true', help='手动控制模式') + + args = parser.parse_args() + + # 加载配置 + config_manager = ConfigManager(args.config) + + # 更新命令行参数到配置 + if args.model: + config_manager.config['detection']['model_type'] = args.model + if args.host: + config_manager.config['carla']['host'] = args.host + if args.port: + config_manager.config['carla']['port'] = args.port + if args.conf_thres: + config_manager.config['detection']['conf_thres'] = args.conf_thres + if args.iou_thres: + config_manager.config['detection']['iou_thres'] = args.iou_thres + if args.target_speed: + config_manager.config['vehicle']['target_speed'] = args.target_speed + if args.npc_count: + config_manager.config['npc']['count'] = args.npc_count + + config = config_manager + # 初始化变量 world = vehicle = camera = depth_camera = None image_queue = depth_queue = None client = controller = npc_manager = None frame_count = 0 - parser = argparse.ArgumentParser(description='CARLA目标检测与跟踪 - 动态车辆版本') - parser.add_argument('--model', type=str, default='yolov5m', - choices=['yolov5s', 'yolov5su', 'yolov5m', 'yolov5mu', 'yolov5x']) - parser.add_argument('--host', type=str, default='localhost') - parser.add_argument('--port', type=int, default=2000) - parser.add_argument('--conf-thres', type=float, default=0.15) - parser.add_argument('--iou-thres', type=float, default=0.4) - parser.add_argument('--use-depth', action='store_true', default=True) - parser.add_argument('--show-depth', action='store_true') - parser.add_argument('--npc-count', type=int, default=20) - parser.add_argument('--enable-physics', action='store_true', default=True, - help='启用物理模拟(使车辆可以碰撞)') - parser.add_argument('--target-speed', type=float, default=30.0, - help='目标速度 (km/h)') - parser.add_argument('--manual-control', action='store_true', - help='手动控制模式(使用键盘WASD控制)') - args = parser.parse_args() - try: - # 1. 初始化设备 - device = 'cuda' if torch.cuda.is_available() else 'cpu' - print(f"使用设备: {device}") - - # 2. 连接CARLA - print("连接CARLA服务器...") - world, client = setup_carla_client(args.host, args.port) - - # 获取交通管理器 - traffic_manager = None - try: - traffic_manager = client.get_trafficmanager() - traffic_manager.set_synchronous_mode(True) - print("交通管理器初始化成功") - except Exception as e: - print(f"交通管理器初始化失败,NPC可能无法自动驾驶: {e}") + # 1. 连接CARLA + logger.info("连接CARLA服务器...") + world, client = setup_carla_client(config) spectator = world.get_spectator() - # 3. 清理环境 - print("清理环境...") + # 2. 清理环境 + logger.info("清理环境...") try: for actor in world.get_actors().filter('vehicle.*'): try: @@ -955,44 +1483,28 @@ def main(): pass except: pass + time.sleep(1) - # 4. 生成主车辆 - print("生成主车辆...") - vehicle = spawn_ego_vehicle(world, enable_physics=args.enable_physics) + # 3. 生成主车辆 + logger.info("生成主车辆...") + vehicle = spawn_ego_vehicle(world, config) if not vehicle: - print("主车辆生成失败,程序退出!") + logger.error("主车辆生成失败,程序退出!") return # 初始化车辆控制器 - controller = VehicleController(vehicle) - controller.set_target_speed(args.target_speed) - - if not args.manual_control and traffic_manager: - try: - # 设置自动驾驶(用于基础运动) - vehicle.set_autopilot(True, traffic_manager.get_port()) - traffic_manager.distance_to_leading_vehicle(vehicle, 2.5) - traffic_manager.vehicle_percentage_speed_difference(vehicle, 0.0) - print("主车辆自动驾驶已启用") - except Exception as e: - print(f"主车辆自动驾驶设置失败: {e}") + controller = VehicleController(vehicle, config) - try: - ego_location = vehicle.get_location() - print(f"主车辆位置: X={ego_location.x:.1f}, Y={ego_location.y:.1f}, Z={ego_location.z:.1f}") - except: - print("无法获取主车辆位置") - - # 5. 生成传感器 - print("生成相机传感器...") + # 4. 生成传感器 + logger.info("生成相机传感器...") bp_lib = world.get_blueprint_library() # RGB相机 camera_bp = bp_lib.find('sensor.camera.rgb') - camera_bp.set_attribute('image_size_x', '800') - camera_bp.set_attribute('image_size_y', '600') - camera_bp.set_attribute('fov', '90') + camera_bp.set_attribute('image_size_x', str(config.get('camera.width', 800))) + camera_bp.set_attribute('image_size_y', str(config.get('camera.height', 600))) + camera_bp.set_attribute('fov', str(config.get('camera.fov', 90))) camera_bp.set_attribute('sensor_tick', '0.05') camera_transform = carla.Transform(carla.Location(x=1.5, z=1.8), @@ -1001,9 +1513,9 @@ def main(): camera = world.spawn_actor(camera_bp, camera_transform, attach_to=vehicle) image_queue = queue.Queue(maxsize=3) camera.listen(lambda image: camera_callback(image, image_queue)) - print("RGB相机传感器生成成功!") + logger.info("RGB相机传感器生成成功!") except Exception as e: - print(f"RGB相机生成失败: {e}") + logger.error(f"RGB相机生成失败: {e}") return # 深度相机 @@ -1012,64 +1524,70 @@ def main(): if args.use_depth: try: depth_bp = bp_lib.find('sensor.camera.depth') - depth_bp.set_attribute('image_size_x', '800') - depth_bp.set_attribute('image_size_y', '600') - depth_bp.set_attribute('fov', '90') + depth_bp.set_attribute('image_size_x', str(config.get('camera.width', 800))) + depth_bp.set_attribute('image_size_y', str(config.get('camera.height', 600))) + depth_bp.set_attribute('fov', str(config.get('camera.fov', 90))) depth_bp.set_attribute('sensor_tick', '0.05') depth_camera = world.spawn_actor(depth_bp, camera_transform, attach_to=vehicle) depth_queue = queue.Queue(maxsize=3) depth_camera.listen(lambda image: depth_camera_callback(image, depth_queue)) - print("深度相机传感器生成成功!") + logger.info("深度相机传感器生成成功!") except Exception as e: - print(f"深度相机生成失败: {e}") + logger.warning(f"深度相机生成失败: {e}") - # 6. 生成NPC车辆 - print(f"生成 {args.npc_count} 辆NPC车辆...") - npc_manager = NPCManager(client) - npc_vehicles = npc_manager.spawn_npcs(world, count=args.npc_count, ego_vehicle=vehicle) + # 5. 生成NPC车辆 + npc_count = config.get('npc.count', 20) + logger.info(f"生成 {npc_count} 辆NPC车辆...") + npc_manager = NPCManager(client, config) + npc_vehicles = npc_manager.spawn_npcs(world, count=npc_count, ego_vehicle=vehicle) - if len(npc_vehicles) < args.npc_count // 2: - print(f"警告:只成功生成了 {len(npc_vehicles)} 辆NPC车辆") + if len(npc_vehicles) < npc_count // 2: + logger.warning(f"只成功生成了 {len(npc_vehicles)} 辆NPC车辆") # 等待NPC车辆稳定 - print("等待NPC车辆初始化...") + logger.info("等待NPC车辆初始化...") for _ in range(10): world.tick() - # 7. 加载检测模型和跟踪器 - print("加载检测模型和跟踪器...") + # 6. 加载检测模型和跟踪器 + logger.info("加载检测模型和跟踪器...") try: - model, class_names = load_detection_model(args.model) + model_type = config.get('detection.model_type', 'yolov5m') + model, class_names = load_detection_model(model_type) except Exception as e: - print(f"模型加载失败: {e}") + logger.error(f"模型加载失败: {e}") return - tracker = Sort() + # 初始化优化跟踪器 + tracking_config = config.get('tracking', {}) + tracker = OptimizedSORT(tracking_config) - # 8. 主循环 - print("\n开始目标检测与跟踪") - print("=" * 50) + # 7. 主循环 + logger.info("\n开始目标检测与跟踪") + logger.info("=" * 50) if args.manual_control: - print("手动控制模式:使用WASD控制车辆") - print("W: 加速, S: 刹车/倒车, A: 左转, D: 右转") - print("Q: 退出程序, R: 重新生成NPC") + logger.info("手动控制模式:使用WASD控制车辆") else: - print("自动控制模式:车辆将自动行驶") - print("按 'q' 键退出程序,按 'r' 重新生成NPC") + logger.info("自动控制模式:车辆将自动行驶") - detection_stats = {'total_detections': 0, 'total_frames': 0, 'max_vehicles_per_frame': 0} + # 统计信息 + detection_stats = { + 'total_detections': 0, + 'total_frames': 0, + 'max_vehicles_per_frame': 0 + } - # 性能监控变量 + # 性能监控 - 修复:确保所有timings都是deque timings = { - 'carla_tick': [], - 'image_get': [], - 'depth_get': [], - 'detection': [], - 'tracking': [], - 'drawing': [], - 'display': [], - 'total': [] + 'carla_tick': deque(maxlen=100), + 'image_get': deque(maxlen=100), + 'depth_get': deque(maxlen=100), + 'detection': deque(maxlen=100), + 'tracking': deque(maxlen=100), + 'drawing': deque(maxlen=100), + 'display': deque(maxlen=100), + 'total': deque(maxlen=100) } # 手动控制变量 @@ -1080,6 +1598,7 @@ def main(): 'reverse': False } + # 主循环 while True: try: frame_start = time.time() @@ -1095,7 +1614,7 @@ def main(): if npc_manager: npc_manager.update_npc_behavior() - # 移动视角跟随主车辆 + # 移动视角 try: ego_transform = vehicle.get_transform() spectator_transform = carla.Transform( @@ -1108,22 +1627,21 @@ def main(): # 手动控制 if args.manual_control: - # 读取键盘输入 key = cv2.waitKey(1) & 0xFF # 控制逻辑 - if key == ord('w'): # 加速 + if key == ord('w'): manual_controls['throttle'] = min(manual_controls['throttle'] + 0.1, 1.0) manual_controls['brake'] = 0.0 - elif key == ord('s'): # 刹车/倒车 + elif key == ord('s'): manual_controls['brake'] = min(manual_controls['brake'] + 0.1, 1.0) manual_controls['throttle'] = 0.0 - elif key == ord('a'): # 左转 + elif key == ord('a'): manual_controls['steer'] = max(manual_controls['steer'] - 0.1, -1.0) - elif key == ord('d'): # 右转 + elif key == ord('d'): manual_controls['steer'] = min(manual_controls['steer'] + 0.1, 1.0) else: - # 逐渐回正转向 + # 逐渐回正 if manual_controls['steer'] > 0: manual_controls['steer'] = max(manual_controls['steer'] - 0.05, 0) elif manual_controls['steer'] < 0: @@ -1133,13 +1651,14 @@ def main(): manual_controls['throttle'] = max(manual_controls['throttle'] - 0.05, 0) manual_controls['brake'] = max(manual_controls['brake'] - 0.05, 0) - # 应用手动控制 + # 应用控制 control = carla.VehicleControl() control.throttle = manual_controls['throttle'] control.brake = manual_controls['brake'] control.steer = manual_controls['steer'] control.hand_brake = False control.reverse = manual_controls['reverse'] + try: vehicle.apply_control(control) except: @@ -1179,7 +1698,11 @@ def main(): boxes, labels, probs, depths = [], [], [], [] try: - results = model(image, conf=args.conf_thres, iou=args.iou_thres, + conf_thres = config.get('detection.conf_thres', 0.15) + iou_thres = config.get('detection.iou_thres', 0.4) + device = config.get('detection.device', 'cuda' if torch.cuda.is_available() else 'cpu') + + results = model(image, conf=conf_thres, iou=iou_thres, device=device, imgsz=640, verbose=False) for r in results: @@ -1189,7 +1712,7 @@ def main(): cls = int(box.cls[0].cpu().numpy()) # 只保留车辆相关类别 - if cls in [2, 3, 5, 7]: # 车辆类别 + if cls in [2, 3, 5, 7]: box_width = x2 - x1 box_height = y2 - y1 @@ -1214,14 +1737,14 @@ def main(): # 计算目标距离 if depth_image is not None: - dist = get_target_distance(depth_image, [x1, y1, x2, y2], use_median=True) + dist = get_target_distance(depth_image, [x1, y1, x2, y2]) depths.append(dist) except Exception as e: - print(f"检测模型推理出错: {e}") + logger.error(f"检测模型推理出错: {e}") timings['detection'].append(time.time() - detection_start) - # 更新检测统计 + # 更新统计 detection_stats['total_detections'] += len(boxes) detection_stats['max_vehicles_per_frame'] = max( detection_stats['max_vehicles_per_frame'], len(boxes) @@ -1230,31 +1753,38 @@ def main(): # 目标跟踪 tracking_start = time.time() if boxes: - boxes_np = np.array(boxes, dtype=np.float32) - probs_np = np.array(probs, dtype=np.float32).reshape(-1, 1) - dets = np.hstack([boxes_np, probs_np]) if probs_np.size > 0 else boxes_np - - if depths: - tracker.set_depths(depths) + # 准备检测数据 + detections = [] + for i in range(len(boxes)): + det = boxes[i] + [probs[i]] + [labels[i]] + detections.append(det) - track_results = tracker.update(dets) + # 更新跟踪器 + track_results = tracker.update(detections, depths) - if track_results.size > 0: + if track_results: track_boxes = [] track_ids = [] + track_classes = [] + track_confidences = [] track_distances = [] track_velocities = [] + # 获取跟踪目标信息 + tracks_info = tracker.get_tracks_info() + for track in track_results: - x1, y1, x2, y2, track_id = track + x1, y1, x2, y2, track_id, class_id, confidence = track track_boxes.append([x1, y1, x2, y2]) track_ids.append(int(track_id)) - - # 获取距离和速度 - track_obj = next((t for t in tracker.tracks if t.id == track_id), None) - if track_obj: - track_distances.append(track_obj.get_distance()) - track_velocities.append(track_obj.velocity) + track_classes.append(int(class_id)) + track_confidences.append(float(confidence)) + + # 查找对应的跟踪目标信息 + track_info = next((t for t in tracks_info if t['id'] == track_id), None) + if track_info: + track_distances.append(track_info.get('distance')) + track_velocities.append(track_info.get('velocity')) else: track_distances.append(None) track_velocities.append(None) @@ -1264,32 +1794,23 @@ def main(): if track_boxes: image = draw_bounding_boxes( image, track_boxes, - labels=[2] * len(track_boxes), + labels=track_classes, class_names=class_names, track_ids=track_ids, - probs=[0.9] * len(track_boxes), + probs=track_confidences, distances=track_distances, velocities=track_velocities ) + # 绘制轨迹 + image = draw_trajectories(image, tracks_info) + cv2.putText(image, f'Vehicles: {len(track_boxes)}', (width - 200, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2) - # 自动控制模式下的障碍物检测 - if not args.manual_control and track_distances: - # 找到最近的车 - valid_distances = [d for d in track_distances if d is not None] - if valid_distances: - closest_distance = min(valid_distances) - if closest_distance < 20: # 20米内有障碍物 - # 可以在这里添加减速逻辑 - pass - timings['drawing'].append(time.time() - drawing_start) - else: - timings['tracking'].append(0.0) - timings['drawing'].append(0.0) + # 这里确保timing只添加一次 timings['tracking'].append(time.time() - tracking_start) # 计算FPS @@ -1298,21 +1819,20 @@ def main(): timings['total'].append(total_time) # 绘制性能监控面板 - image = draw_performance_panel(image, timings, fps, frame_count) + image = draw_performance_panel(image, timings, fps, frame_count, config) # 显示其他信息 - info = [ + info_lines = [ f"FPS: {fps:.1f}", f"Frame: {frame_count}", f"Tracks: {len(tracker.tracks)}", f"Detections: {len(boxes)}", - f"Model: {args.model}", + f"Model: {config.get('detection.model_type', 'yolov5m')}", "Press 'q' to quit, 'r' to reset NPCs" ] - # 调整信息位置 - y_pos = 220 - for line in info: + y_pos = 250 + for line in info_lines: cv2.putText(image, line, (10, y_pos), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 255), 2) y_pos += 25 @@ -1321,7 +1841,7 @@ def main(): if args.manual_control: try: velocity = vehicle.get_velocity() - speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2) * 3.6 # m/s to km/h + speed = math.sqrt(velocity.x ** 2 + velocity.y ** 2) * 3.6 control_info = [ f"Speed: {speed:.1f} km/h", f"Throttle: {manual_controls['throttle']:.1f}", @@ -1340,44 +1860,50 @@ def main(): cv2.imshow(window_name, cv2.cvtColor(image, cv2.COLOR_RGB2BGR)) timings['display'].append(time.time() - display_start) - # 每100帧打印一次性能统计 - if frame_count % 100 == 0 and frame_count > 0: - print( - f"\n[帧数 {frame_count}] FPS: {fps:.1f} | 检测到车辆: {len(boxes)} | 跟踪目标: {len(tracker.tracks)}") + # 定期打印统计信息 + log_interval = config.get('performance.log_interval', 100) + if frame_count % log_interval == 0 and frame_count > 0: + avg_fps = 1.0 / np.mean(list(timings['total'])) if timings['total'] else 0 + logger.info( + f"[帧数 {frame_count}] FPS: {avg_fps:.1f} | " + f"检测: {len(boxes)} | 跟踪: {len(tracker.tracks)}" + ) # 退出检查 key = cv2.waitKey(1) & 0xFF if key == ord('q'): - print("用户触发退出程序...") + logger.info("用户触发退出程序...") break elif key == ord('r'): # 重新生成NPC - print("重新生成NPC车辆...") + logger.info("重新生成NPC车辆...") if npc_manager: npc_manager.destroy_all_npcs() - npc_vehicles = npc_manager.spawn_npcs(world, count=args.npc_count, ego_vehicle=vehicle) - print(f"重新生成 {len(npc_vehicles)} 辆NPC车辆完成") + npc_vehicles = npc_manager.spawn_npcs(world, count=npc_count, ego_vehicle=vehicle) + logger.info(f"重新生成 {len(npc_vehicles)} 辆NPC车辆完成") # FPS控制 + max_fps = config.get('performance.max_fps', 30) elapsed = time.time() - frame_start - if elapsed < 0.05: - time.sleep(max(0, 0.05 - elapsed)) + if elapsed < 1.0 / max_fps: + time.sleep(max(0, 1.0 / max_fps - elapsed)) + except KeyboardInterrupt: + logger.info("\n用户中断程序...") + break except Exception as e: - print(f"主循环出错: {e}") + logger.error(f"主循环出错: {e}") import traceback traceback.print_exc() break - except KeyboardInterrupt: - print("\n用户中断程序...") except Exception as e: + logger.error(f"程序运行出错: {e}") import traceback - print(f"程序运行出错:{str(e)}") traceback.print_exc() finally: # 清理资源 - print("\n正在清理资源...") + logger.info("\n正在清理资源...") # 停止传感器 if camera: @@ -1420,24 +1946,30 @@ def main(): if torch.cuda.is_available(): torch.cuda.empty_cache() + # 保存配置 + config_manager.save_config("carla_tracking_config.json") + # 打印最终统计 - print("\n" + "=" * 50) - print("程序运行统计:") - print(f"总帧数: {frame_count}") + logger.info("\n" + "=" * 50) + logger.info("程序运行统计:") + logger.info(f"总帧数: {frame_count}") if timings.get('total'): try: avg_fps = 1.0 / np.mean(list(timings['total'])) if timings['total'] else 0 - print(f"平均FPS: {avg_fps:.1f}") + logger.info(f"平均FPS: {avg_fps:.1f}") except: pass if detection_stats: - print(f"总检测次数: {detection_stats['total_detections']}") - print(f"平均每帧检测: {detection_stats['total_detections'] / max(1, detection_stats['total_frames']):.1f}") - print(f"最大单帧车辆数: {detection_stats['max_vehicles_per_frame']}") - print("=" * 50) - print("资源清理完成,程序正常退出!") + logger.info(f"总检测次数: {detection_stats['total_detections']}") + if detection_stats['total_frames'] > 0: + avg_det = detection_stats['total_detections'] / detection_stats['total_frames'] + logger.info(f"平均每帧检测: {avg_det:.1f}") + logger.info(f"最大单帧车辆数: {detection_stats['max_vehicles_per_frame']}") + + logger.info("=" * 50) + logger.info("资源清理完成,程序正常退出!") if __name__ == "__main__":