第4课: 3D目标检测

import numpy as np from dataclasses import dataclass from typing import List, Tuple, Optional import time class PointPillarsModel: """3D目标检测核心算法实现""" def __init__(self, state_dim=4, meas_dim=4): self.state_dim = state_dim self.meas_dim = meas_dim self.A = np.eye(state_dim) # 系统矩阵 self.H = np.eye(meas_dim, state_dim) # 观测矩阵 self.Q = np.eye(state_dim) * 0.01 # 过程噪声 self.R = np.eye(meas_dim) * 0.1 # 观测噪声 self.P = np.eye(state_dim) * 1.0 # 协方差 self.x = np.zeros(state_dim) # 状态向量 def predict(self, dt=0.1): """预测步骤: x_pred = A*x, P_pred = A*P*A^T + Q""" F = np.eye(self.state_dim) F[0, 2] = dt; F[1, 3] = dt # 位置=位置+速度*dt self.x = F @ self.x self.P = F @ self.P @ F.T + self.Q return self.x.copy() def update(self, z): """更新步骤: 卡尔曼增益计算和状态修正""" z = np.array(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) # 增益 self.x = self.x + K @ y # 状态更新 I_KH = np.eye(self.state_dim) - K @ self.H self.P = I_KH @ self.P @ I_KH.T + K @ self.R @ K.T # 协方差更新 return self.x.copy() def process(self, measurements): """处理完整测量序列""" estimates = [] for z in measurements: self.predict() x_est = self.update(z) estimates.append(x_est) return np.array(estimates) class Anchor设计Processor: """Anchor设计处理器""" def __init__(self, threshold=1.5, min_size=5): self.threshold = threshold self.min_size = min_size def process(self, data): """处理输入数据, 返回处理结果""" arr = np.array(data, dtype=np.float64) n = len(arr) # 距离矩阵 dist = np.zeros((n, n)) for i in range(n): for j in range(i+1, n): dist[i,j] = np.linalg.norm(arr[i] - arr[j]) dist[j,i] = dist[i,j] # 基于距离阈值的聚类 labels = np.full(n, -1, dtype=int) current = 0 for i in range(n): if labels[i] >= 0: continue labels[i] = current queue = [i] while queue: ci = queue.pop(0) for j in range(n): if labels[j] < 0 and dist[ci,j] < self.threshold: labels[j] = current queue.append(j) current += 1 # 过滤小簇 valid = [] for c in range(current): if (labels == c).sum() >= self.min_size: valid.append(c) return {'labels': labels, 'n_clusters': len(valid), 'sizes': [(labels==c).sum() for c in valid]} # ====== 仿真测试 ====== np.random.seed(42) print("=" * 50) print(" 3D目标检测 仿真测试") print("=" * 50) # 生成仿真数据 n_steps = 100 true_state = np.zeros((n_steps, 4)) measurements = [] for i in range(n_steps): t = i * 0.1 true_state[i] = [10*np.sin(0.2*t), 5*np.cos(0.3*t), 2*np.cos(0.1*t), 1*np.sin(0.15*t)] noise = np.random.randn(4) * 0.5 measurements.append(true_state[i] + noise) # 运行PointPillars模型 model = PointPillarsModel() t0 = time.time() estimates = model.process(measurements) elapsed = time.time() - t0 # 计算误差 rmse = np.sqrt(np.mean((estimates - true_state)**2, axis=0)) print(f"\n处理 {n_steps} 步, 耗时 {elapsed:.3f}s") print(f"各维度RMSE: {rmse.round(4)}") print(f"平均RMSE: {rmse.mean():.4f}") # 运行Anchor设计处理器 processor = Anchor设计Processor() test_data = np.random.randn(50, 3) * 2 test_data[:15] += np.array([5, 5, 5]) test_data[15:30] += np.array([-5, -3, 4]) result = processor.process(test_data) print(f"\n处理器: {result['n_clusters']}个有效簇, 大小: {result['sizes']}") print("\n3D目标检测仿真验证通过!")