打通新版Stanley轨迹跟踪闭环并补充实验测试与数据分析脚本
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -0,0 +1,760 @@
|
||||
"""对比原始Detour差分与C#在线车辆状态估计结果。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from collections import Counter
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib
|
||||
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import pandas as pd
|
||||
|
||||
|
||||
REQUIRED_COLUMNS = {
|
||||
"ElapsedSeconds",
|
||||
"DetourX",
|
||||
"DetourY",
|
||||
"DetourTheta",
|
||||
}
|
||||
|
||||
|
||||
def wrap_radians(angle: float | np.ndarray) -> float | np.ndarray:
|
||||
"""将弧度归一化到[-π, π)区间。"""
|
||||
return (angle + np.pi) % (2.0 * np.pi) - np.pi
|
||||
|
||||
|
||||
def angle_difference(target: float, current: float) -> float:
|
||||
"""计算从当前角到目标角的最短有符号弧度差。"""
|
||||
return float(wrap_radians(target - current))
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Pose:
|
||||
"""保存世界坐标系中的二维位姿，单位为m和rad。"""
|
||||
|
||||
x: float
|
||||
y: float
|
||||
yaw: float
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class State:
|
||||
"""保存脚本复现得到的世界位姿和世界速度。"""
|
||||
|
||||
timestamp: float
|
||||
pose: Pose
|
||||
vx: float
|
||||
vy: float
|
||||
omega: float
|
||||
velocity_valid: bool
|
||||
|
||||
|
||||
class LowPassFilter:
|
||||
"""复现FirstOrderLowPassFilter的一阶低通计算。"""
|
||||
|
||||
def __init__(self, time_constant_seconds: float) -> None:
|
||||
if not np.isfinite(time_constant_seconds) or time_constant_seconds <= 0:
|
||||
raise ValueError("滤波时间常数必须是正有限值。")
|
||||
self.time_constant_seconds = float(time_constant_seconds)
|
||||
self.initialized = False
|
||||
self.value = 0.0
|
||||
|
||||
def update(self, value: float, delta_time_seconds: float) -> float:
|
||||
"""按照真实采样间隔更新滤波输出。"""
|
||||
if not self.initialized:
|
||||
self.value = float(value)
|
||||
self.initialized = True
|
||||
return self.value
|
||||
alpha = delta_time_seconds / (
|
||||
self.time_constant_seconds + delta_time_seconds
|
||||
)
|
||||
self.value += alpha * (float(value) - self.value)
|
||||
return self.value
|
||||
|
||||
def reset(self) -> None:
|
||||
"""清除滤波历史。"""
|
||||
self.initialized = False
|
||||
self.value = 0.0
|
||||
|
||||
|
||||
class VelocityEstimator:
|
||||
"""复现VelocityEstimator2D的世界速度差分与低通处理。"""
|
||||
|
||||
def __init__(self, linear_tau: float, angular_tau: float) -> None:
|
||||
self.vx_filter = LowPassFilter(linear_tau)
|
||||
self.vy_filter = LowPassFilter(linear_tau)
|
||||
self.omega_filter = LowPassFilter(angular_tau)
|
||||
self.previous_pose: Pose | None = None
|
||||
self.previous_timestamp = 0.0
|
||||
|
||||
def reset(self, pose: Pose | None = None, timestamp: float = 0.0) -> State | None:
|
||||
"""清除历史，并可使用当前位姿建立新的零速差分基准。"""
|
||||
self.vx_filter.reset()
|
||||
self.vy_filter.reset()
|
||||
self.omega_filter.reset()
|
||||
self.previous_pose = pose
|
||||
self.previous_timestamp = float(timestamp)
|
||||
if pose is None:
|
||||
return None
|
||||
return State(timestamp, pose, 0.0, 0.0, 0.0, False)
|
||||
|
||||
def update(self, pose: Pose, timestamp: float) -> State:
|
||||
"""使用一个新的有效位姿更新速度估计。"""
|
||||
if self.previous_pose is None:
|
||||
state = self.reset(pose, timestamp)
|
||||
assert state is not None
|
||||
return state
|
||||
delta_time = timestamp - self.previous_timestamp
|
||||
if delta_time <= 0.0:
|
||||
raise ValueError("新样本时间戳必须严格递增。")
|
||||
raw_vx = (pose.x - self.previous_pose.x) / delta_time
|
||||
raw_vy = (pose.y - self.previous_pose.y) / delta_time
|
||||
raw_omega = angle_difference(
|
||||
pose.yaw,
|
||||
self.previous_pose.yaw,
|
||||
) / delta_time
|
||||
state = State(
|
||||
timestamp,
|
||||
pose,
|
||||
self.vx_filter.update(raw_vx, delta_time),
|
||||
self.vy_filter.update(raw_vy, delta_time),
|
||||
self.omega_filter.update(raw_omega, delta_time),
|
||||
True,
|
||||
)
|
||||
self.previous_pose = pose
|
||||
self.previous_timestamp = timestamp
|
||||
return state
|
||||
|
||||
def rebase_preserving_velocity(
|
||||
self,
|
||||
pose: Pose,
|
||||
timestamp: float,
|
||||
) -> State:
|
||||
"""更新差分基准但保留三个低通滤波器的当前输出。"""
|
||||
self.previous_pose = pose
|
||||
self.previous_timestamp = timestamp
|
||||
velocity_valid = (
|
||||
self.vx_filter.initialized
|
||||
and self.vy_filter.initialized
|
||||
and self.omega_filter.initialized
|
||||
)
|
||||
return State(
|
||||
timestamp,
|
||||
pose,
|
||||
self.vx_filter.value if velocity_valid else 0.0,
|
||||
self.vy_filter.value if velocity_valid else 0.0,
|
||||
self.omega_filter.value if velocity_valid else 0.0,
|
||||
velocity_valid,
|
||||
)
|
||||
|
||||
|
||||
class DetourProviderSimulator:
|
||||
"""按当前简化版DetourVehicleStateProvider处理离线CSV样本。"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
linear_tau: float = 0.15,
|
||||
angular_tau: float = 0.20,
|
||||
maximum_linear_speed: float = 1.20,
|
||||
maximum_angular_speed: float = np.pi / 4.0,
|
||||
position_jump_margin: float = 0.03,
|
||||
heading_jump_margin: float = np.deg2rad(5.0),
|
||||
stationary_seconds: float = 0.35,
|
||||
) -> None:
|
||||
self.estimator = VelocityEstimator(linear_tau, angular_tau)
|
||||
self.maximum_linear_speed = maximum_linear_speed
|
||||
self.maximum_angular_speed = maximum_angular_speed
|
||||
self.position_jump_margin = position_jump_margin
|
||||
self.heading_jump_margin = heading_jump_margin
|
||||
self.stationary_seconds = stationary_seconds
|
||||
|
||||
self.accepted_pose: Pose | None = None
|
||||
self.accepted_timestamp = 0.0
|
||||
self.latest_state: State | None = None
|
||||
self.stationary_hold = False
|
||||
|
||||
@staticmethod
|
||||
def poses_equal(first: Pose, second: Pose) -> bool:
|
||||
"""判断两次读取是否为Detour保持输出的同一数值帧。"""
|
||||
return (
|
||||
abs(first.x - second.x) <= 1e-9
|
||||
and abs(first.y - second.y) <= 1e-9
|
||||
and abs(angle_difference(first.yaw, second.yaw)) <= 1e-8
|
||||
)
|
||||
|
||||
def motion_plausible(self, start: Pose, end: Pose, delta_time: float) -> bool:
|
||||
"""按照车辆绝对运动能力判断两帧是否连续。"""
|
||||
if not np.isfinite(delta_time) or delta_time <= 0.0:
|
||||
return False
|
||||
displacement = np.hypot(end.x - start.x, end.y - start.y)
|
||||
heading_change = abs(angle_difference(end.yaw, start.yaw))
|
||||
return (
|
||||
displacement
|
||||
<= self.maximum_linear_speed * delta_time
|
||||
+ self.position_jump_margin
|
||||
and heading_change
|
||||
<= self.maximum_angular_speed * delta_time
|
||||
+ self.heading_jump_margin
|
||||
)
|
||||
|
||||
def accept_after_reset(self, pose: Pose, timestamp: float) -> State:
|
||||
"""接受首帧或确认后的重定位并清除速度历史。"""
|
||||
state = self.estimator.reset(pose, timestamp)
|
||||
assert state is not None
|
||||
self.latest_state = state
|
||||
self.accepted_pose = pose
|
||||
self.accepted_timestamp = timestamp
|
||||
self.stationary_hold = False
|
||||
return state
|
||||
|
||||
def accept_continuous(self, pose: Pose, timestamp: float) -> State:
|
||||
"""接受连续正常位姿并更新速度估计。"""
|
||||
state = self.estimator.update(pose, timestamp)
|
||||
self.latest_state = state
|
||||
self.accepted_pose = pose
|
||||
self.accepted_timestamp = timestamp
|
||||
self.stationary_hold = False
|
||||
return state
|
||||
|
||||
def handle_repeated(self, timestamp: float) -> tuple[State, str]:
|
||||
"""保留重复帧，并在长期不变后将估计速度归零。"""
|
||||
assert self.accepted_pose is not None
|
||||
assert self.latest_state is not None
|
||||
unchanged = timestamp - self.accepted_timestamp
|
||||
if not self.stationary_hold and unchanged >= self.stationary_seconds:
|
||||
self.latest_state = State(
|
||||
timestamp,
|
||||
self.accepted_pose,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
True,
|
||||
)
|
||||
self.stationary_hold = True
|
||||
return self.latest_state, "stationary_zero"
|
||||
return self.latest_state, "duplicate"
|
||||
|
||||
def process(
|
||||
self,
|
||||
pose: Pose,
|
||||
timestamp: float,
|
||||
velocity_innovation_abnormal: bool = False,
|
||||
) -> tuple[State | None, str]:
|
||||
"""处理一帧CSV中的Detour读取结果。"""
|
||||
if self.accepted_pose is None:
|
||||
return self.accept_after_reset(pose, timestamp), "initialized"
|
||||
if self.poses_equal(pose, self.accepted_pose):
|
||||
return self.handle_repeated(timestamp)
|
||||
if self.stationary_hold:
|
||||
return self.accept_after_reset(pose, timestamp), "restart_after_stationary"
|
||||
|
||||
elapsed = timestamp - self.accepted_timestamp
|
||||
if not self.motion_plausible(self.accepted_pose, pose, elapsed):
|
||||
assert self.latest_state is not None
|
||||
return self.latest_state, "physical_anomaly"
|
||||
if velocity_innovation_abnormal:
|
||||
self.latest_state = self.estimator.rebase_preserving_velocity(
|
||||
pose,
|
||||
timestamp,
|
||||
)
|
||||
self.accepted_pose = pose
|
||||
self.accepted_timestamp = timestamp
|
||||
self.stationary_hold = False
|
||||
return self.latest_state, "velocity_rebase"
|
||||
return self.accept_continuous(pose, timestamp), "accepted"
|
||||
|
||||
|
||||
def segmented_unwrap_degrees(values_radians: np.ndarray) -> np.ndarray:
|
||||
"""分别展开由NaN分隔的有效航向区间。"""
|
||||
result = np.full(values_radians.shape, np.nan, dtype=float)
|
||||
finite = np.isfinite(values_radians)
|
||||
indices = np.flatnonzero(finite)
|
||||
if not indices.size:
|
||||
return result
|
||||
starts = np.r_[0, np.flatnonzero(np.diff(indices) > 1) + 1]
|
||||
ends = np.r_[starts[1:], indices.size]
|
||||
for start, end in zip(starts, ends):
|
||||
segment_indices = indices[start:end]
|
||||
result[segment_indices] = np.rad2deg(
|
||||
np.unwrap(values_radians[segment_indices])
|
||||
)
|
||||
return result
|
||||
|
||||
|
||||
def calculate_naive_derivatives(
|
||||
time: np.ndarray,
|
||||
x: np.ndarray,
|
||||
y: np.ndarray,
|
||||
yaw: np.ndarray,
|
||||
) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""直接逐记录帧差分，保留重复帧造成的零值和更新尖峰。"""
|
||||
speed = np.full(time.shape, np.nan, dtype=float)
|
||||
omega = np.full(time.shape, np.nan, dtype=float)
|
||||
delta_time = np.diff(time)
|
||||
valid = np.isfinite(delta_time) & (delta_time > 0.0)
|
||||
delta_x = np.diff(x)
|
||||
delta_y = np.diff(y)
|
||||
delta_yaw = wrap_radians(np.diff(yaw))
|
||||
speed_values = np.full(delta_time.shape, np.nan, dtype=float)
|
||||
omega_values = np.full(delta_time.shape, np.nan, dtype=float)
|
||||
speed_values[valid] = (
|
||||
np.hypot(delta_x[valid], delta_y[valid])
|
||||
/ delta_time[valid]
|
||||
)
|
||||
omega_values[valid] = delta_yaw[valid] / delta_time[valid]
|
||||
speed[1:] = speed_values
|
||||
omega[1:] = omega_values
|
||||
return speed, omega
|
||||
|
||||
|
||||
def configure_matplotlib() -> None:
|
||||
"""配置常见中文字体和负号显示。"""
|
||||
plt.rcParams["font.sans-serif"] = [
|
||||
"Microsoft YaHei",
|
||||
"SimHei",
|
||||
"Arial Unicode MS",
|
||||
"DejaVu Sans",
|
||||
]
|
||||
plt.rcParams["axes.unicode_minus"] = False
|
||||
|
||||
|
||||
def load_csv(csv_path: Path) -> pd.DataFrame:
|
||||
"""读取并校验状态估计对比所需的CSV字段。"""
|
||||
frame = pd.read_csv(csv_path)
|
||||
missing = REQUIRED_COLUMNS.difference(frame.columns)
|
||||
if missing:
|
||||
raise ValueError(
|
||||
f"{csv_path.name}缺少字段：{', '.join(sorted(missing))}"
|
||||
)
|
||||
for column in REQUIRED_COLUMNS:
|
||||
frame[column] = pd.to_numeric(frame[column], errors="coerce")
|
||||
frame = (
|
||||
frame.dropna(subset=list(REQUIRED_COLUMNS))
|
||||
.sort_values("ElapsedSeconds")
|
||||
.drop_duplicates("ElapsedSeconds", keep="last")
|
||||
.reset_index(drop=True)
|
||||
)
|
||||
if len(frame) < 3:
|
||||
raise ValueError(f"{csv_path.name}有效数据不足3行。")
|
||||
frame["ElapsedSeconds"] -= frame["ElapsedSeconds"].iloc[0]
|
||||
return frame
|
||||
|
||||
|
||||
def detect_visual_anomalies(
|
||||
time: np.ndarray,
|
||||
x: np.ndarray,
|
||||
y: np.ndarray,
|
||||
yaw: np.ndarray,
|
||||
linear_tau: float,
|
||||
angular_tau: float,
|
||||
position_residual_meters: float,
|
||||
heading_residual_radians: float,
|
||||
stationary_seconds: float,
|
||||
) -> np.ndarray:
|
||||
"""用恒速预测残差标注可疑跳变，不修改任何状态估计数据。"""
|
||||
anomalies = np.zeros(time.shape, dtype=bool)
|
||||
estimator = VelocityEstimator(linear_tau, angular_tau)
|
||||
previous_pose: Pose | None = None
|
||||
previous_update_time = 0.0
|
||||
latest_state: State | None = None
|
||||
stationary = False
|
||||
|
||||
for index, timestamp in enumerate(time):
|
||||
pose = Pose(
|
||||
float(x[index]),
|
||||
float(y[index]),
|
||||
float(wrap_radians(yaw[index])),
|
||||
)
|
||||
|
||||
if previous_pose is None:
|
||||
latest_state = estimator.reset(pose, float(timestamp))
|
||||
previous_pose = pose
|
||||
previous_update_time = float(timestamp)
|
||||
continue
|
||||
|
||||
if DetourProviderSimulator.poses_equal(pose, previous_pose):
|
||||
if (
|
||||
not stationary
|
||||
and timestamp - previous_update_time >= stationary_seconds
|
||||
):
|
||||
stationary = True
|
||||
continue
|
||||
|
||||
# 静止后的第一个新定位只重新建立差分基准，避免把起步误标为跳变。
|
||||
if stationary:
|
||||
latest_state = estimator.reset(pose, float(timestamp))
|
||||
previous_pose = pose
|
||||
previous_update_time = float(timestamp)
|
||||
stationary = False
|
||||
continue
|
||||
|
||||
delta_time = float(timestamp) - previous_update_time
|
||||
if (
|
||||
latest_state is not None
|
||||
and latest_state.velocity_valid
|
||||
and delta_time > 0.0
|
||||
):
|
||||
predicted_x = previous_pose.x + latest_state.vx * delta_time
|
||||
predicted_y = previous_pose.y + latest_state.vy * delta_time
|
||||
predicted_yaw = float(
|
||||
wrap_radians(
|
||||
previous_pose.yaw + latest_state.omega * delta_time
|
||||
)
|
||||
)
|
||||
position_residual = np.hypot(
|
||||
pose.x - predicted_x,
|
||||
pose.y - predicted_y,
|
||||
)
|
||||
heading_residual = abs(
|
||||
angle_difference(pose.yaw, predicted_yaw)
|
||||
)
|
||||
|
||||
if (
|
||||
position_residual > position_residual_meters
|
||||
or heading_residual > heading_residual_radians
|
||||
):
|
||||
anomalies[index] = True
|
||||
# 标注后从当前观测重新开始，避免一个跳变引发连续误标。
|
||||
latest_state = estimator.rebase_preserving_velocity(
|
||||
pose,
|
||||
float(timestamp),
|
||||
)
|
||||
previous_pose = pose
|
||||
previous_update_time = float(timestamp)
|
||||
continue
|
||||
|
||||
latest_state = estimator.update(pose, float(timestamp))
|
||||
previous_pose = pose
|
||||
previous_update_time = float(timestamp)
|
||||
|
||||
return anomalies
|
||||
|
||||
|
||||
def simulate(
|
||||
frame: pd.DataFrame,
|
||||
args: argparse.Namespace,
|
||||
) -> tuple[pd.DataFrame, Counter]:
|
||||
"""使用当前C#参数处理整份Detour记录。"""
|
||||
time = frame["ElapsedSeconds"].to_numpy(float)
|
||||
raw_x = frame["DetourX"].to_numpy(float) / 1000.0
|
||||
raw_y = frame["DetourY"].to_numpy(float) / 1000.0
|
||||
raw_yaw = np.deg2rad(frame["DetourTheta"].to_numpy(float))
|
||||
raw_speed, raw_omega = calculate_naive_derivatives(
|
||||
time,
|
||||
raw_x,
|
||||
raw_y,
|
||||
raw_yaw,
|
||||
)
|
||||
visual_anomalies = detect_visual_anomalies(
|
||||
time,
|
||||
raw_x,
|
||||
raw_y,
|
||||
raw_yaw,
|
||||
args.linear_tau,
|
||||
args.angular_tau,
|
||||
args.annotation_position_residual_mm / 1000.0,
|
||||
np.deg2rad(args.annotation_heading_residual_deg),
|
||||
args.stationary_seconds,
|
||||
)
|
||||
|
||||
simulator = DetourProviderSimulator(
|
||||
linear_tau=args.linear_tau,
|
||||
angular_tau=args.angular_tau,
|
||||
maximum_linear_speed=args.maximum_linear_speed,
|
||||
maximum_angular_speed=np.deg2rad(args.maximum_angular_speed_deg),
|
||||
position_jump_margin=args.position_jump_margin_mm / 1000.0,
|
||||
heading_jump_margin=np.deg2rad(args.heading_jump_margin_deg),
|
||||
stationary_seconds=args.stationary_seconds,
|
||||
)
|
||||
|
||||
processed_x = np.full(time.shape, np.nan)
|
||||
processed_y = np.full(time.shape, np.nan)
|
||||
processed_yaw = np.full(time.shape, np.nan)
|
||||
processed_speed = np.full(time.shape, np.nan)
|
||||
processed_omega = np.full(time.shape, np.nan)
|
||||
events: list[str] = []
|
||||
|
||||
for index, timestamp in enumerate(time):
|
||||
pose = Pose(
|
||||
raw_x[index],
|
||||
raw_y[index],
|
||||
float(wrap_radians(raw_yaw[index])),
|
||||
)
|
||||
state, event = simulator.process(
|
||||
pose,
|
||||
float(timestamp),
|
||||
bool(visual_anomalies[index]),
|
||||
)
|
||||
events.append(event)
|
||||
if state is None:
|
||||
continue
|
||||
processed_x[index] = state.pose.x
|
||||
processed_y[index] = state.pose.y
|
||||
processed_yaw[index] = state.pose.yaw
|
||||
if state.velocity_valid:
|
||||
processed_speed[index] = np.hypot(state.vx, state.vy)
|
||||
processed_omega[index] = state.omega
|
||||
|
||||
result = pd.DataFrame(
|
||||
{
|
||||
"TimeSeconds": time,
|
||||
"RawX": raw_x,
|
||||
"RawY": raw_y,
|
||||
"RawYawRadians": raw_yaw,
|
||||
"RawSpeed": raw_speed,
|
||||
"RawOmegaRadiansPerSecond": raw_omega,
|
||||
"ProcessedX": processed_x,
|
||||
"ProcessedY": processed_y,
|
||||
"ProcessedYawRadians": processed_yaw,
|
||||
"ProcessedSpeed": processed_speed,
|
||||
"ProcessedOmegaRadiansPerSecond": processed_omega,
|
||||
"VisualAnomaly": visual_anomalies,
|
||||
"Event": events,
|
||||
}
|
||||
)
|
||||
counts = Counter(events)
|
||||
counts["visual_anomaly"] = int(visual_anomalies.sum())
|
||||
return result, counts
|
||||
|
||||
|
||||
def plot_comparison(
|
||||
csv_path: Path,
|
||||
frame: pd.DataFrame,
|
||||
result: pd.DataFrame,
|
||||
event_counts: Counter,
|
||||
output_directory: str | None,
|
||||
show: bool,
|
||||
) -> Path:
|
||||
"""生成位置、航向、线速度和角速度处理前后对比图。"""
|
||||
time = result["TimeSeconds"].to_numpy(float)
|
||||
anomalous = result["VisualAnomaly"].to_numpy(bool)
|
||||
raw_yaw_degrees = segmented_unwrap_degrees(
|
||||
result["RawYawRadians"].to_numpy(float)
|
||||
)
|
||||
processed_yaw_degrees = segmented_unwrap_degrees(
|
||||
result["ProcessedYawRadians"].to_numpy(float)
|
||||
)
|
||||
|
||||
fig, axes = plt.subplots(
|
||||
5,
|
||||
1,
|
||||
figsize=(13.0, 16.0),
|
||||
sharex=True,
|
||||
)
|
||||
|
||||
series = [
|
||||
("RawX", "ProcessedX", "世界坐标X / m"),
|
||||
("RawY", "ProcessedY", "世界坐标Y / m"),
|
||||
]
|
||||
for axis, (raw_name, processed_name, ylabel) in zip(axes[:2], series):
|
||||
axis.plot(time, result[raw_name], color="0.65", linewidth=1.0, label="原始Detour")
|
||||
axis.plot(time, result[processed_name], color="tab:blue", linewidth=1.5, label="在线处理后")
|
||||
axis.scatter(
|
||||
time[anomalous],
|
||||
result.loc[anomalous, raw_name],
|
||||
color="tab:red",
|
||||
marker="x",
|
||||
s=26,
|
||||
label="异常位置",
|
||||
zorder=3,
|
||||
)
|
||||
axis.set_ylabel(ylabel)
|
||||
axis.grid(True, alpha=0.3)
|
||||
axis.legend(loc="best")
|
||||
|
||||
axes[2].plot(time, raw_yaw_degrees, color="0.65", linewidth=1.0, label="原始Detour")
|
||||
axes[2].plot(time, processed_yaw_degrees, color="tab:blue", linewidth=1.5, label="在线处理后")
|
||||
axes[2].scatter(
|
||||
time[anomalous],
|
||||
raw_yaw_degrees[anomalous],
|
||||
color="tab:red",
|
||||
marker="x",
|
||||
s=26,
|
||||
label="异常位置",
|
||||
zorder=3,
|
||||
)
|
||||
axes[2].set_ylabel("展开航向角 / deg")
|
||||
axes[2].grid(True, alpha=0.3)
|
||||
axes[2].legend(loc="best")
|
||||
|
||||
axes[3].plot(time, result["RawSpeed"], color="0.65", linewidth=1.0, label="逐记录帧直接差分")
|
||||
axes[3].plot(time, result["ProcessedSpeed"], color="tab:green", linewidth=1.5, label="去重、跳变保护和低通后")
|
||||
if "CommandSpeed" in frame.columns:
|
||||
command_speed = pd.to_numeric(
|
||||
frame["CommandSpeed"], errors="coerce"
|
||||
).to_numpy(float)
|
||||
axes[3].plot(time, command_speed, linestyle="--", linewidth=1.0, color="tab:orange", label="记录的命令线速度")
|
||||
axes[3].set_ylabel("合线速度 / (m/s)")
|
||||
axes[3].grid(True, alpha=0.3)
|
||||
axes[3].legend(loc="best")
|
||||
|
||||
axes[4].plot(
|
||||
time,
|
||||
np.rad2deg(result["RawOmegaRadiansPerSecond"]),
|
||||
color="0.65",
|
||||
linewidth=1.0,
|
||||
label="逐记录帧最短角差",
|
||||
)
|
||||
axes[4].plot(
|
||||
time,
|
||||
np.rad2deg(result["ProcessedOmegaRadiansPerSecond"]),
|
||||
color="tab:purple",
|
||||
linewidth=1.5,
|
||||
label="去重、跳变保护和低通后",
|
||||
)
|
||||
if "CommandAngularSpeedRadPerSecond" in frame.columns:
|
||||
command_omega = np.rad2deg(
|
||||
pd.to_numeric(
|
||||
frame["CommandAngularSpeedRadPerSecond"],
|
||||
errors="coerce",
|
||||
).to_numpy(float)
|
||||
)
|
||||
axes[4].plot(time, command_omega, linestyle="--", linewidth=1.0, color="tab:orange", label="记录的命令角速度")
|
||||
elif "CommandAngularSpeed" in frame.columns:
|
||||
# 旧版CSV只有CommandAngularSpeed列，该列历史单位是deg/s；
|
||||
# 新版CSV另增RadPerSecond列，不能把旧列再次按rad/s换算。
|
||||
command_omega = pd.to_numeric(
|
||||
frame["CommandAngularSpeed"],
|
||||
errors="coerce",
|
||||
).to_numpy(float)
|
||||
axes[4].plot(time, command_omega, linestyle="--", linewidth=1.0, color="tab:orange", label="记录的命令角速度")
|
||||
axes[4].set_ylabel("角速度 / (deg/s)")
|
||||
axes[4].set_xlabel("时间 / s")
|
||||
axes[4].grid(True, alpha=0.3)
|
||||
axes[4].legend(loc="best")
|
||||
|
||||
controller = (
|
||||
str(frame["ControllerName"].iloc[0])
|
||||
if "ControllerName" in frame.columns
|
||||
else "UnknownController"
|
||||
)
|
||||
trajectory = (
|
||||
str(frame["TrajectoryName"].iloc[0])
|
||||
if "TrajectoryName" in frame.columns
|
||||
else csv_path.stem
|
||||
)
|
||||
anomaly_count = int(anomalous.sum())
|
||||
fig.suptitle(
|
||||
"Detour状态估计处理前后对比\n"
|
||||
f"{controller} - {trajectory}，"
|
||||
f"标注异常位置{anomaly_count}帧",
|
||||
fontsize=14,
|
||||
)
|
||||
fig.tight_layout(rect=(0.0, 0.0, 1.0, 0.965))
|
||||
|
||||
if output_directory:
|
||||
destination_directory = Path(output_directory)
|
||||
else:
|
||||
destination_directory = csv_path.parent / "state_estimation_plots"
|
||||
destination_directory.mkdir(parents=True, exist_ok=True)
|
||||
destination = destination_directory / (
|
||||
csv_path.stem + "_state_estimation_comparison.png"
|
||||
)
|
||||
fig.savefig(destination, dpi=220, bbox_inches="tight")
|
||||
if show:
|
||||
plt.show()
|
||||
plt.close(fig)
|
||||
return destination
|
||||
|
||||
|
||||
def discover_csv_files(arguments: list[str]) -> list[Path]:
|
||||
"""解析文件或目录；目录会被递归展开为全部轨迹CSV。"""
|
||||
input_paths = (
|
||||
[Path(argument).resolve() for argument in arguments]
|
||||
if arguments
|
||||
else [Path(__file__).resolve().parent]
|
||||
)
|
||||
csv_files: set[Path] = set()
|
||||
|
||||
for input_path in input_paths:
|
||||
if input_path.is_file():
|
||||
if input_path.suffix.lower() == ".csv":
|
||||
csv_files.add(input_path)
|
||||
continue
|
||||
|
||||
if input_path.is_dir():
|
||||
csv_files.update(
|
||||
path.resolve()
|
||||
for path in input_path.rglob("*.csv")
|
||||
if not any(
|
||||
part.startswith("state_estimation_plots")
|
||||
for part in path.parts
|
||||
)
|
||||
)
|
||||
continue
|
||||
|
||||
raise FileNotFoundError(
|
||||
f"输入文件或目录不存在：{input_path}"
|
||||
)
|
||||
|
||||
return sorted(csv_files)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
"""解析命令行并批量生成Detour状态估计对比图。"""
|
||||
configure_matplotlib()
|
||||
parser = argparse.ArgumentParser(
|
||||
description="比较原始Detour与当前C#在线状态估计算法。"
|
||||
)
|
||||
parser.add_argument(
|
||||
"files",
|
||||
nargs="*",
|
||||
help="一个或多个轨迹实验CSV或包含CSV的目录",
|
||||
)
|
||||
parser.add_argument("--output-dir")
|
||||
parser.add_argument("--show", action="store_true")
|
||||
parser.add_argument("--linear-tau", type=float, default=0.15)
|
||||
parser.add_argument("--angular-tau", type=float, default=0.20)
|
||||
parser.add_argument("--maximum-linear-speed", type=float, default=1.20)
|
||||
parser.add_argument("--maximum-angular-speed-deg", type=float, default=45.0)
|
||||
parser.add_argument("--position-jump-margin-mm", type=float, default=30.0)
|
||||
parser.add_argument("--heading-jump-margin-deg", type=float, default=5.0)
|
||||
parser.add_argument(
|
||||
"--annotation-position-residual-mm",
|
||||
type=float,
|
||||
default=40.0,
|
||||
help="只用于图中红色异常位置标注的预测位置残差阈值",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--annotation-heading-residual-deg",
|
||||
type=float,
|
||||
default=5.0,
|
||||
help="只用于图中红色异常位置标注的预测航向残差阈值",
|
||||
)
|
||||
parser.add_argument("--stationary-seconds", type=float, default=0.35)
|
||||
args = parser.parse_args()
|
||||
|
||||
processed_count = 0
|
||||
for csv_path in discover_csv_files(args.files):
|
||||
try:
|
||||
frame = load_csv(csv_path)
|
||||
result, counts = simulate(frame, args)
|
||||
destination = plot_comparison(
|
||||
csv_path,
|
||||
frame,
|
||||
result,
|
||||
counts,
|
||||
args.output_dir,
|
||||
args.show,
|
||||
)
|
||||
print(
|
||||
f"{csv_path.name}: "
|
||||
f"重复帧={counts['duplicate']}，"
|
||||
f"异常位置={counts['visual_anomaly']}"
|
||||
)
|
||||
print(f"已生成：{destination}")
|
||||
processed_count += 1
|
||||
except Exception as exception:
|
||||
print(f"跳过{csv_path.name}：{exception}")
|
||||
|
||||
if processed_count == 0:
|
||||
raise SystemExit("没有找到包含有效Detour字段的轨迹实验CSV。")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,100 @@
|
||||
"""绘制控制器下发角速度命令曲线。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
from plot_trajectory_comparison import (
|
||||
configure_matplotlib,
|
||||
discover_csv_files,
|
||||
load_and_resample,
|
||||
output_path,
|
||||
shade_localization_jump_windows,
|
||||
)
|
||||
|
||||
|
||||
def plot_angular_command(
|
||||
csv_path: Path,
|
||||
frequency_hz: float,
|
||||
filter_window_seconds: float,
|
||||
output_directory: str | None,
|
||||
show: bool,
|
||||
) -> Path:
|
||||
"""生成单份CSV的命令角速度曲线。"""
|
||||
frame, metadata = load_and_resample(
|
||||
csv_path,
|
||||
frequency_hz,
|
||||
filter_window_seconds,
|
||||
)
|
||||
time = frame["TimeSeconds"].to_numpy(dtype=float)
|
||||
angular_command = frame[
|
||||
"CommandAngularSpeedRadPerSec"
|
||||
].to_numpy(dtype=float)
|
||||
maximum = float(np.max(angular_command))
|
||||
minimum = float(np.min(angular_command))
|
||||
|
||||
fig, ax = plt.subplots(figsize=(10.0, 5.5))
|
||||
ax.plot(
|
||||
time,
|
||||
angular_command,
|
||||
color="tab:red",
|
||||
linewidth=1.6,
|
||||
label="CommandAngularSpeed",
|
||||
)
|
||||
ax.axhline(0.0, color="black", linewidth=0.8)
|
||||
shade_localization_jump_windows(ax, metadata)
|
||||
ax.set_xlabel("时间 / s")
|
||||
ax.set_ylabel("命令角速度 / (rad/s)")
|
||||
ax.set_title(
|
||||
f"角速度指令曲线\n"
|
||||
f"{metadata['controller_name']} - "
|
||||
f"{metadata['trajectory_name']}，"
|
||||
f"范围=[{minimum:.3f}, {maximum:.3f}]rad/s"
|
||||
)
|
||||
ax.grid(True, alpha=0.3)
|
||||
ax.legend()
|
||||
fig.tight_layout()
|
||||
|
||||
destination = output_path(
|
||||
csv_path,
|
||||
output_directory,
|
||||
"angular_command",
|
||||
)
|
||||
fig.savefig(destination, dpi=300, bbox_inches="tight")
|
||||
if show:
|
||||
plt.show()
|
||||
plt.close(fig)
|
||||
return destination
|
||||
|
||||
|
||||
def main() -> None:
|
||||
configure_matplotlib()
|
||||
parser = argparse.ArgumentParser(
|
||||
description="绘制控制器下发角速度命令曲线。"
|
||||
)
|
||||
parser.add_argument("files", nargs="*", help="一个或多个CSV文件")
|
||||
parser.add_argument("--frequency", type=float, default=20.0)
|
||||
parser.add_argument("--window", type=float, default=0.55)
|
||||
parser.add_argument("--output-dir")
|
||||
parser.add_argument("--show", action="store_true")
|
||||
args = parser.parse_args()
|
||||
|
||||
for csv_path in discover_csv_files(args.files):
|
||||
destination = plot_angular_command(
|
||||
csv_path,
|
||||
args.frequency,
|
||||
args.window,
|
||||
args.output_dir,
|
||||
args.show,
|
||||
)
|
||||
print(f"已生成：{destination}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,177 @@
|
||||
"""绘制控制器参考速度与Detour差分实际速度对比图。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
from plot_trajectory_comparison import (
|
||||
configure_matplotlib,
|
||||
discover_csv_files,
|
||||
load_and_resample,
|
||||
output_path,
|
||||
segmented_savgol,
|
||||
shade_localization_jump_windows,
|
||||
)
|
||||
|
||||
|
||||
def calculate_actual_speed_mps(
|
||||
frame,
|
||||
filter_window_seconds: float,
|
||||
) -> np.ndarray:
|
||||
"""使用Savitzky-Golay求位置导数并计算Detour实际合速度。"""
|
||||
time = frame["TimeSeconds"].to_numpy(dtype=float)
|
||||
dt = float(np.median(np.diff(time)))
|
||||
# 直接对固定频率重采样后的位置做SG求导，避免“先平滑再求导”
|
||||
# 造成两次滤波和过度削弱速度峰值。
|
||||
x_mm = frame["DetourXRawMm"].to_numpy(dtype=float)
|
||||
y_mm = frame["DetourYRawMm"].to_numpy(dtype=float)
|
||||
vx_mm_per_second = segmented_savgol(
|
||||
x_mm,
|
||||
dt,
|
||||
filter_window_seconds,
|
||||
derivative=1,
|
||||
)
|
||||
vy_mm_per_second = segmented_savgol(
|
||||
y_mm,
|
||||
dt,
|
||||
filter_window_seconds,
|
||||
derivative=1,
|
||||
)
|
||||
|
||||
speed = np.hypot(
|
||||
vx_mm_per_second,
|
||||
vy_mm_per_second,
|
||||
) / 1000.0
|
||||
speed[
|
||||
frame["InvalidNearLocalizationJump"].to_numpy(dtype=bool)
|
||||
] = np.nan
|
||||
return speed
|
||||
|
||||
|
||||
def plot_speed(
|
||||
csv_path: Path,
|
||||
frequency_hz: float,
|
||||
filter_window_seconds: float,
|
||||
output_directory: str | None,
|
||||
show: bool,
|
||||
) -> Path:
|
||||
"""生成单份CSV的参考/实际速度响应图。"""
|
||||
frame, metadata = load_and_resample(
|
||||
csv_path,
|
||||
frequency_hz,
|
||||
filter_window_seconds,
|
||||
)
|
||||
time = frame["TimeSeconds"].to_numpy(dtype=float)
|
||||
command_speed = frame["CommandSpeedMps"].to_numpy(dtype=float)
|
||||
actual_speed = calculate_actual_speed_mps(
|
||||
frame,
|
||||
filter_window_seconds,
|
||||
)
|
||||
is_in_place_rotation = (
|
||||
str(metadata["trajectory_name"])
|
||||
.lower()
|
||||
.startswith("rotate")
|
||||
)
|
||||
# 原地自转CSV中的ReferenceSpeed历史上保存的是角速度上限deg/s，
|
||||
# 不能作为线速度m/s使用；其参考线速度应为0。
|
||||
configured_speed = (
|
||||
0.0
|
||||
if is_in_place_rotation
|
||||
else float(metadata["reference_speed_mps"])
|
||||
)
|
||||
|
||||
moving = (
|
||||
(command_speed > max(0.02, configured_speed * 0.1)) &
|
||||
np.isfinite(actual_speed)
|
||||
)
|
||||
if np.any(moving):
|
||||
speed_rmse = float(
|
||||
np.sqrt(
|
||||
np.mean(
|
||||
(actual_speed[moving] - command_speed[moving]) ** 2
|
||||
)
|
||||
)
|
||||
)
|
||||
else:
|
||||
speed_rmse = float("nan")
|
||||
|
||||
fig, ax = plt.subplots(figsize=(10.0, 5.8))
|
||||
ax.plot(
|
||||
time,
|
||||
command_speed,
|
||||
linewidth=1.8,
|
||||
label="控制器参考/下发线速度",
|
||||
)
|
||||
ax.plot(
|
||||
time,
|
||||
actual_speed,
|
||||
linewidth=1.5,
|
||||
label="Detour差分实际线速度（SG求导）",
|
||||
)
|
||||
ax.axhline(
|
||||
configured_speed,
|
||||
linestyle=":",
|
||||
linewidth=1.3,
|
||||
color="tab:green",
|
||||
label=(
|
||||
"原地自转参考线速度 0 m/s"
|
||||
if is_in_place_rotation
|
||||
else f"配置巡航速度 {configured_speed:.3f} m/s"
|
||||
),
|
||||
)
|
||||
shade_localization_jump_windows(ax, metadata)
|
||||
ax.set_xlabel("时间 / s")
|
||||
ax.set_ylabel("线速度 / (m/s)")
|
||||
ax.set_title(
|
||||
f"参考速度与实际速度对比\n"
|
||||
f"{metadata['controller_name']} - "
|
||||
f"{metadata['trajectory_name']}，"
|
||||
f"运动段RMSE={speed_rmse:.4f} m/s"
|
||||
)
|
||||
ax.grid(True, alpha=0.3)
|
||||
ax.legend()
|
||||
fig.tight_layout()
|
||||
|
||||
destination = output_path(
|
||||
csv_path,
|
||||
output_directory,
|
||||
"speed_response",
|
||||
)
|
||||
fig.savefig(destination, dpi=300, bbox_inches="tight")
|
||||
if show:
|
||||
plt.show()
|
||||
plt.close(fig)
|
||||
return destination
|
||||
|
||||
|
||||
def main() -> None:
|
||||
configure_matplotlib()
|
||||
parser = argparse.ArgumentParser(
|
||||
description="绘制参考速度与Detour差分实际速度对比图。"
|
||||
)
|
||||
parser.add_argument("files", nargs="*", help="一个或多个CSV文件")
|
||||
parser.add_argument("--frequency", type=float, default=20.0)
|
||||
parser.add_argument("--window", type=float, default=0.55)
|
||||
parser.add_argument("--output-dir")
|
||||
parser.add_argument("--show", action="store_true")
|
||||
args = parser.parse_args()
|
||||
|
||||
for csv_path in discover_csv_files(args.files):
|
||||
destination = plot_speed(
|
||||
csv_path,
|
||||
args.frequency,
|
||||
args.window,
|
||||
args.output_dir,
|
||||
args.show,
|
||||
)
|
||||
print(f"已生成：{destination}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,172 @@
|
||||
"""绘制横向误差和航向误差随时间变化图。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
from plot_trajectory_comparison import (
|
||||
build_reference,
|
||||
configure_matplotlib,
|
||||
discover_csv_files,
|
||||
load_and_resample,
|
||||
output_path,
|
||||
shade_localization_jump_windows,
|
||||
)
|
||||
|
||||
|
||||
def plot_errors(
|
||||
csv_path: Path,
|
||||
frequency_hz: float,
|
||||
filter_window_seconds: float,
|
||||
output_directory: str | None,
|
||||
show: bool,
|
||||
) -> Path:
|
||||
"""生成单份CSV的横向/航向误差图。"""
|
||||
frame, metadata = load_and_resample(
|
||||
csv_path,
|
||||
frequency_hz,
|
||||
filter_window_seconds,
|
||||
)
|
||||
reference = build_reference(frame, metadata)
|
||||
time = frame["TimeSeconds"].to_numpy()
|
||||
lateral = np.asarray(reference["lateral_error_mm"])
|
||||
heading = np.asarray(reference["heading_error_degrees"])
|
||||
invalid = frame[
|
||||
"InvalidNearLocalizationJump"
|
||||
].to_numpy(dtype=bool)
|
||||
lateral_for_statistics = lateral.copy()
|
||||
heading_for_statistics = heading.copy()
|
||||
lateral_for_statistics[invalid] = np.nan
|
||||
heading_for_statistics[invalid] = np.nan
|
||||
|
||||
lateral_rmse = float(
|
||||
np.sqrt(np.nanmean(lateral_for_statistics**2))
|
||||
)
|
||||
heading_rmse = float(
|
||||
np.sqrt(np.nanmean(heading_for_statistics**2))
|
||||
)
|
||||
lateral_max = float(
|
||||
np.nanmax(np.abs(lateral_for_statistics))
|
||||
)
|
||||
heading_max = float(
|
||||
np.nanmax(np.abs(heading_for_statistics))
|
||||
)
|
||||
is_in_place_rotation = (
|
||||
reference["kind"] == "in_place_rotation"
|
||||
)
|
||||
|
||||
fig, axes = plt.subplots(
|
||||
2,
|
||||
1,
|
||||
figsize=(10.0, 7.0),
|
||||
sharex=True,
|
||||
)
|
||||
axes[0].plot(time, lateral, linewidth=1.5)
|
||||
axes[0].axhline(0.0, color="black", linewidth=0.8)
|
||||
if is_in_place_rotation:
|
||||
axes[0].set_ylabel("旋转中心位置漂移 / mm")
|
||||
axes[0].set_title(
|
||||
f"原地自转位置漂移：RMS={lateral_rmse:.2f} mm，"
|
||||
f"最大值={lateral_max:.2f} mm"
|
||||
)
|
||||
else:
|
||||
axes[0].set_ylabel("横向误差 / mm")
|
||||
axes[0].set_title(
|
||||
f"横向误差：RMSE={lateral_rmse:.2f} mm，"
|
||||
f"最大绝对值={lateral_max:.2f} mm"
|
||||
)
|
||||
shade_localization_jump_windows(axes[0], metadata)
|
||||
axes[0].grid(True, alpha=0.3)
|
||||
|
||||
axes[1].plot(
|
||||
time,
|
||||
heading,
|
||||
color="tab:orange",
|
||||
linewidth=1.5,
|
||||
)
|
||||
axes[1].axhline(0.0, color="black", linewidth=0.8)
|
||||
axes[1].set_xlabel("时间 / s")
|
||||
axes[1].set_ylabel(
|
||||
"目标角度剩余误差 / °"
|
||||
if is_in_place_rotation
|
||||
else "航向误差 / °"
|
||||
)
|
||||
axes[1].set_title(
|
||||
(
|
||||
f"目标角度剩余误差：RMSE={heading_rmse:.2f}°，"
|
||||
f"最大绝对值={heading_max:.2f}°"
|
||||
)
|
||||
if is_in_place_rotation
|
||||
else (
|
||||
f"航向误差：RMSE={heading_rmse:.2f}°，"
|
||||
f"最大绝对值={heading_max:.2f}°"
|
||||
)
|
||||
)
|
||||
shade_localization_jump_windows(axes[1], metadata)
|
||||
axes[1].grid(True, alpha=0.3)
|
||||
if metadata["localization_jump_events"]:
|
||||
axes[1].legend(loc="best")
|
||||
|
||||
fig.suptitle(
|
||||
f"横向/航向误差随时间变化\n"
|
||||
f"{metadata['controller_name']} - "
|
||||
f"{metadata['trajectory_name']}"
|
||||
)
|
||||
fig.tight_layout()
|
||||
|
||||
destination = output_path(
|
||||
csv_path,
|
||||
output_directory,
|
||||
"tracking_errors",
|
||||
)
|
||||
fig.savefig(destination, dpi=300, bbox_inches="tight")
|
||||
if show:
|
||||
plt.show()
|
||||
plt.close(fig)
|
||||
|
||||
if is_in_place_rotation:
|
||||
print(
|
||||
f"{csv_path.name}: position drift RMS="
|
||||
f"{lateral_rmse:.3f} mm, "
|
||||
f"target-angle error RMS={heading_rmse:.3f} deg"
|
||||
)
|
||||
else:
|
||||
print(
|
||||
f"{csv_path.name}: lateral RMSE="
|
||||
f"{lateral_rmse:.3f} mm, "
|
||||
f"heading RMSE={heading_rmse:.3f} deg"
|
||||
)
|
||||
return destination
|
||||
|
||||
|
||||
def main() -> None:
|
||||
configure_matplotlib()
|
||||
parser = argparse.ArgumentParser(
|
||||
description="绘制横向误差和航向误差随时间变化图。"
|
||||
)
|
||||
parser.add_argument("files", nargs="*", help="一个或多个CSV文件")
|
||||
parser.add_argument("--frequency", type=float, default=20.0)
|
||||
parser.add_argument("--window", type=float, default=0.55)
|
||||
parser.add_argument("--output-dir")
|
||||
parser.add_argument("--show", action="store_true")
|
||||
args = parser.parse_args()
|
||||
|
||||
for csv_path in discover_csv_files(args.files):
|
||||
destination = plot_errors(
|
||||
csv_path,
|
||||
args.frequency,
|
||||
args.window,
|
||||
args.output_dir,
|
||||
args.show,
|
||||
)
|
||||
print(f"已生成：{destination}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,920 @@
|
||||
"""绘制理想轨迹与Detour实际轨迹对比图。
|
||||
|
||||
不传CSV路径时，默认处理本脚本目录下的全部CSV文件。
|
||||
本文件也提供其余三个绘图脚本共用的数据预处理函数。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import re
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import matplotlib
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import pandas as pd
|
||||
from scipy.signal import savgol_filter
|
||||
|
||||
|
||||
SCRIPT_DIR = Path(__file__).resolve().parent
|
||||
REQUIRED_COLUMNS = {
|
||||
"ElapsedSeconds",
|
||||
"TrajectoryName",
|
||||
"DetourX",
|
||||
"DetourY",
|
||||
"DetourTheta",
|
||||
"CommandSpeed",
|
||||
"CommandAngularSpeed",
|
||||
"ReferenceStartX",
|
||||
"ReferenceStartY",
|
||||
"ReferenceEndX",
|
||||
"ReferenceEndY",
|
||||
"ReferenceSpeed",
|
||||
}
|
||||
|
||||
|
||||
def configure_matplotlib() -> None:
|
||||
"""配置中文字体和图片输出风格。"""
|
||||
matplotlib.rcParams["font.sans-serif"] = [
|
||||
"Microsoft YaHei",
|
||||
"SimHei",
|
||||
"Arial Unicode MS",
|
||||
"DejaVu Sans",
|
||||
]
|
||||
matplotlib.rcParams["axes.unicode_minus"] = False
|
||||
matplotlib.rcParams["figure.dpi"] = 120
|
||||
|
||||
|
||||
def _odd_window_length(
|
||||
sample_count: int,
|
||||
sample_interval: float,
|
||||
window_seconds: float,
|
||||
polynomial_order: int = 2,
|
||||
) -> int | None:
|
||||
"""计算不超过数据长度的Savitzky-Golay奇数窗口。"""
|
||||
requested = max(
|
||||
polynomial_order + 2,
|
||||
int(round(window_seconds / sample_interval)),
|
||||
)
|
||||
if requested % 2 == 0:
|
||||
requested += 1
|
||||
|
||||
maximum = sample_count if sample_count % 2 == 1 else sample_count - 1
|
||||
window = min(requested, maximum)
|
||||
minimum = polynomial_order + 2
|
||||
if minimum % 2 == 0:
|
||||
minimum += 1
|
||||
|
||||
return window if window >= minimum else None
|
||||
|
||||
|
||||
def wrap_degrees(angle_degrees: np.ndarray) -> np.ndarray:
|
||||
"""将角度差归一化到[-180°, 180°)。"""
|
||||
return (angle_degrees + 180.0) % 360.0 - 180.0
|
||||
|
||||
|
||||
def build_complete_s_curve(
|
||||
start: np.ndarray,
|
||||
end: np.ndarray,
|
||||
offset_mm: float,
|
||||
samples_per_segment: int = 120,
|
||||
) -> tuple[np.ndarray, np.ndarray]:
|
||||
"""重建测试使用的三段三次贝塞尔完整S曲线及各点切线航向。"""
|
||||
line = end - start
|
||||
length = float(np.linalg.norm(line))
|
||||
if length <= 1e-6:
|
||||
raise ValueError("S型曲线的起点和终点不能重合。")
|
||||
|
||||
forward = line / length
|
||||
left = np.array([-forward[1], forward[0]])
|
||||
controls = [
|
||||
np.array([
|
||||
[0.0, 0.0],
|
||||
[length / 12.0, 0.0],
|
||||
[length / 6.0, offset_mm],
|
||||
[length * 0.25, offset_mm],
|
||||
]),
|
||||
np.array([
|
||||
[length * 0.25, offset_mm],
|
||||
[length / 3.0, offset_mm],
|
||||
[length * 2.0 / 3.0, -offset_mm],
|
||||
[length * 0.75, -offset_mm],
|
||||
]),
|
||||
np.array([
|
||||
[length * 0.75, -offset_mm],
|
||||
[length * 5.0 / 6.0, -offset_mm],
|
||||
[length * 11.0 / 12.0, 0.0],
|
||||
[length, 0.0],
|
||||
]),
|
||||
]
|
||||
|
||||
local_parts: list[np.ndarray] = []
|
||||
derivative_parts: list[np.ndarray] = []
|
||||
for index, points in enumerate(controls):
|
||||
t = np.linspace(0.0, 1.0, samples_per_segment + 1)
|
||||
if index > 0:
|
||||
t = t[1:]
|
||||
one_minus_t = 1.0 - t
|
||||
local = (
|
||||
one_minus_t[:, None] ** 3 * points[0]
|
||||
+ 3.0
|
||||
* one_minus_t[:, None] ** 2
|
||||
* t[:, None]
|
||||
* points[1]
|
||||
+ 3.0
|
||||
* one_minus_t[:, None]
|
||||
* t[:, None] ** 2
|
||||
* points[2]
|
||||
+ t[:, None] ** 3 * points[3]
|
||||
)
|
||||
derivative = (
|
||||
3.0
|
||||
* one_minus_t[:, None] ** 2
|
||||
* (points[1] - points[0])
|
||||
+ 6.0
|
||||
* one_minus_t[:, None]
|
||||
* t[:, None]
|
||||
* (points[2] - points[1])
|
||||
+ 3.0
|
||||
* t[:, None] ** 2
|
||||
* (points[3] - points[2])
|
||||
)
|
||||
local_parts.append(local)
|
||||
derivative_parts.append(derivative)
|
||||
|
||||
local_points = np.vstack(local_parts)
|
||||
local_derivatives = np.vstack(derivative_parts)
|
||||
world_points = (
|
||||
start
|
||||
+ local_points[:, 0, None] * forward
|
||||
+ local_points[:, 1, None] * left
|
||||
)
|
||||
world_derivatives = (
|
||||
local_derivatives[:, 0, None] * forward
|
||||
+ local_derivatives[:, 1, None] * left
|
||||
)
|
||||
headings = np.rad2deg(
|
||||
np.arctan2(world_derivatives[:, 1], world_derivatives[:, 0])
|
||||
)
|
||||
return world_points, headings
|
||||
|
||||
|
||||
def segmented_savgol(
|
||||
values: np.ndarray,
|
||||
sample_interval: float,
|
||||
window_seconds: float,
|
||||
derivative: int = 0,
|
||||
polynomial_order: int = 2,
|
||||
) -> np.ndarray:
|
||||
"""对含NaN断点的数据逐段执行SG滤波或求导。"""
|
||||
values = np.asarray(values, dtype=float)
|
||||
result = np.full_like(values, np.nan)
|
||||
finite_indices = np.flatnonzero(np.isfinite(values))
|
||||
if finite_indices.size == 0:
|
||||
return result
|
||||
|
||||
breaks = np.flatnonzero(np.diff(finite_indices) > 1)
|
||||
starts = np.r_[0, breaks + 1]
|
||||
ends = np.r_[breaks + 1, finite_indices.size]
|
||||
|
||||
for start_index, end_index in zip(starts, ends):
|
||||
indices = finite_indices[start_index:end_index]
|
||||
segment = values[indices]
|
||||
window = _odd_window_length(
|
||||
len(segment),
|
||||
sample_interval,
|
||||
window_seconds,
|
||||
polynomial_order,
|
||||
)
|
||||
if window is not None:
|
||||
result[indices] = savgol_filter(
|
||||
segment,
|
||||
window,
|
||||
polynomial_order,
|
||||
deriv=derivative,
|
||||
delta=sample_interval,
|
||||
mode="interp",
|
||||
)
|
||||
elif derivative == 0:
|
||||
result[indices] = segment
|
||||
elif len(segment) >= 2:
|
||||
result[indices] = np.gradient(segment, sample_interval)
|
||||
|
||||
return result
|
||||
|
||||
|
||||
def shade_localization_jump_windows(
|
||||
axis,
|
||||
metadata: dict[str, Any],
|
||||
) -> None:
|
||||
"""在时间曲线中标记不应参与车辆动力学评价的定位跳变窗口。"""
|
||||
for index, (start, end) in enumerate(
|
||||
metadata["jump_exclusion_windows"]
|
||||
):
|
||||
axis.axvspan(
|
||||
start,
|
||||
end,
|
||||
color="tab:red",
|
||||
alpha=0.12,
|
||||
label="Detour定位跳变排除窗口" if index == 0 else None,
|
||||
)
|
||||
|
||||
|
||||
def load_and_resample(
|
||||
csv_path: Path,
|
||||
frequency_hz: float = 20.0,
|
||||
filter_window_seconds: float = 0.55,
|
||||
) -> tuple[pd.DataFrame, dict[str, Any]]:
|
||||
"""压缩Detour保持帧，检测定位跳变，再分段重采样和平滑。"""
|
||||
if not np.isfinite(frequency_hz) or frequency_hz <= 0.0:
|
||||
raise ValueError("重采样频率必须是正有限值。")
|
||||
|
||||
raw = pd.read_csv(csv_path)
|
||||
missing = REQUIRED_COLUMNS.difference(raw.columns)
|
||||
if missing:
|
||||
raise ValueError(
|
||||
f"{csv_path.name}缺少列：{', '.join(sorted(missing))}"
|
||||
)
|
||||
|
||||
numeric_columns = [
|
||||
"ElapsedSeconds",
|
||||
"DetourX",
|
||||
"DetourY",
|
||||
"DetourTheta",
|
||||
"CommandSpeed",
|
||||
"CommandAngularSpeed",
|
||||
"ReferenceStartX",
|
||||
"ReferenceStartY",
|
||||
"ReferenceEndX",
|
||||
"ReferenceEndY",
|
||||
"ReferenceSpeed",
|
||||
]
|
||||
optional_numeric_columns = [
|
||||
"CommandAngularSpeedRadPerSecond",
|
||||
"ReferenceAngularSpeedRadPerSecond",
|
||||
"ReferenceMotionFrameYawDegrees",
|
||||
]
|
||||
numeric_columns.extend(
|
||||
column
|
||||
for column in optional_numeric_columns
|
||||
if column in raw.columns
|
||||
)
|
||||
for column in numeric_columns:
|
||||
raw[column] = pd.to_numeric(raw[column], errors="coerce")
|
||||
|
||||
raw = (
|
||||
raw.dropna(subset=[
|
||||
"ElapsedSeconds",
|
||||
"DetourX",
|
||||
"DetourY",
|
||||
"DetourTheta",
|
||||
])
|
||||
.sort_values("ElapsedSeconds")
|
||||
.drop_duplicates("ElapsedSeconds", keep="last")
|
||||
.reset_index(drop=True)
|
||||
)
|
||||
if len(raw) < 5:
|
||||
raise ValueError(f"{csv_path.name}有效数据不足5行。")
|
||||
|
||||
time_raw = raw["ElapsedSeconds"].to_numpy(dtype=float)
|
||||
time_raw = time_raw - time_raw[0]
|
||||
raw["ElapsedSeconds"] = time_raw
|
||||
duration = float(time_raw[-1])
|
||||
sample_interval = 1.0 / frequency_hz
|
||||
time_uniform = np.arange(
|
||||
0.0,
|
||||
duration + sample_interval * 0.5,
|
||||
sample_interval,
|
||||
)
|
||||
|
||||
def interpolate_command(column: str) -> np.ndarray:
|
||||
values = raw[column].to_numpy(dtype=float)
|
||||
return np.interp(time_uniform, time_raw, values)
|
||||
|
||||
# 记录器频率高于Detour更新频率，会得到A,A,B,B形式的保持帧。
|
||||
# 速度估计前先保留真正发生位姿更新的样本。
|
||||
x_all = raw["DetourX"].to_numpy(dtype=float)
|
||||
y_all = raw["DetourY"].to_numpy(dtype=float)
|
||||
theta_all = raw["DetourTheta"].to_numpy(dtype=float)
|
||||
position_change = np.hypot(np.diff(x_all), np.diff(y_all))
|
||||
heading_change = np.abs(wrap_degrees(np.diff(theta_all)))
|
||||
update_mask = np.r_[
|
||||
True,
|
||||
(position_change > 1e-6) | (heading_change > 1e-6),
|
||||
]
|
||||
updates = raw.loc[update_mask].copy().reset_index(drop=True)
|
||||
if len(updates) < 3:
|
||||
raise ValueError(f"{csv_path.name}有效Detour更新点不足3个。")
|
||||
|
||||
update_time = updates["ElapsedSeconds"].to_numpy(dtype=float)
|
||||
update_x = updates["DetourX"].to_numpy(dtype=float)
|
||||
update_y = updates["DetourY"].to_numpy(dtype=float)
|
||||
update_theta = updates["DetourTheta"].to_numpy(dtype=float)
|
||||
update_command_speed = np.abs(
|
||||
updates["CommandSpeed"].to_numpy(dtype=float)
|
||||
)
|
||||
if "CommandAngularSpeedRadPerSecond" in updates.columns:
|
||||
update_command_angular_rad = np.abs(
|
||||
updates[
|
||||
"CommandAngularSpeedRadPerSecond"
|
||||
].to_numpy(dtype=float)
|
||||
)
|
||||
else:
|
||||
# 旧CSV中的CommandAngularSpeed单位为deg/s。
|
||||
update_command_angular_rad = np.deg2rad(
|
||||
np.abs(
|
||||
updates[
|
||||
"CommandAngularSpeed"
|
||||
].to_numpy(dtype=float)
|
||||
)
|
||||
)
|
||||
|
||||
# 自适应跳变阈值：正常移动允许达到参考位移的3倍并保留15mm余量；
|
||||
# 低速阶段仍至少允许30mm，防止把普通定位噪声误判为跳变。
|
||||
update_dt = np.diff(update_time)
|
||||
update_distance = np.hypot(np.diff(update_x), np.diff(update_y))
|
||||
expected_distance = (
|
||||
0.5 *
|
||||
(update_command_speed[1:] + update_command_speed[:-1]) *
|
||||
update_dt *
|
||||
1000.0
|
||||
)
|
||||
distance_threshold = np.maximum(
|
||||
30.0,
|
||||
expected_distance * 3.0 + 15.0,
|
||||
)
|
||||
update_heading_delta = np.abs(
|
||||
wrap_degrees(np.diff(update_theta))
|
||||
)
|
||||
expected_heading_delta = (
|
||||
0.5 *
|
||||
(
|
||||
update_command_angular_rad[1:] +
|
||||
update_command_angular_rad[:-1]
|
||||
) *
|
||||
update_dt *
|
||||
180.0 / np.pi
|
||||
)
|
||||
heading_threshold = np.maximum(
|
||||
5.0,
|
||||
expected_heading_delta * 3.0 + 2.0,
|
||||
)
|
||||
jump_before_current = (
|
||||
(update_distance > distance_threshold) |
|
||||
(update_heading_delta > heading_threshold)
|
||||
)
|
||||
jump_at_update = np.r_[False, jump_before_current]
|
||||
segment_ids = np.cumsum(jump_at_update.astype(int))
|
||||
|
||||
jump_events: list[dict[str, float]] = []
|
||||
for current_index in np.flatnonzero(jump_at_update):
|
||||
previous_index = current_index - 1
|
||||
jump_events.append({
|
||||
"time_seconds": float(update_time[current_index]),
|
||||
"distance_mm": float(update_distance[previous_index]),
|
||||
"heading_change_degrees":
|
||||
float(update_heading_delta[previous_index]),
|
||||
"before_x_mm": float(update_x[previous_index]),
|
||||
"before_y_mm": float(update_y[previous_index]),
|
||||
"after_x_mm": float(update_x[current_index]),
|
||||
"after_y_mm": float(update_y[current_index]),
|
||||
})
|
||||
|
||||
# 不跨越定位跳变插值。跳变前后之间保留NaN，使轨迹图自然断线，
|
||||
# 也防止SG滤波把坐标修正涂抹成车辆高速运动。
|
||||
x_resampled = np.full_like(time_uniform, np.nan)
|
||||
y_resampled = np.full_like(time_uniform, np.nan)
|
||||
theta_resampled = np.full_like(time_uniform, np.nan)
|
||||
update_theta_unwrapped = np.rad2deg(
|
||||
np.unwrap(np.deg2rad(update_theta))
|
||||
)
|
||||
maximum_segment_id = int(segment_ids[-1])
|
||||
for segment_id in range(maximum_segment_id + 1):
|
||||
segment_mask = segment_ids == segment_id
|
||||
segment_time = update_time[segment_mask]
|
||||
if segment_time.size == 0:
|
||||
continue
|
||||
|
||||
interval_start = (
|
||||
0.0 if segment_id == 0 else float(segment_time[0])
|
||||
)
|
||||
interval_end = (
|
||||
duration
|
||||
if segment_id == maximum_segment_id
|
||||
else float(segment_time[-1])
|
||||
)
|
||||
uniform_mask = (
|
||||
(time_uniform >= interval_start) &
|
||||
(time_uniform <= interval_end)
|
||||
)
|
||||
x_resampled[uniform_mask] = np.interp(
|
||||
time_uniform[uniform_mask],
|
||||
segment_time,
|
||||
update_x[segment_mask],
|
||||
)
|
||||
y_resampled[uniform_mask] = np.interp(
|
||||
time_uniform[uniform_mask],
|
||||
segment_time,
|
||||
update_y[segment_mask],
|
||||
)
|
||||
theta_resampled[uniform_mask] = np.interp(
|
||||
time_uniform[uniform_mask],
|
||||
segment_time,
|
||||
update_theta_unwrapped[segment_mask],
|
||||
)
|
||||
|
||||
x_filtered = segmented_savgol(
|
||||
x_resampled,
|
||||
sample_interval,
|
||||
filter_window_seconds,
|
||||
)
|
||||
y_filtered = segmented_savgol(
|
||||
y_resampled,
|
||||
sample_interval,
|
||||
filter_window_seconds,
|
||||
)
|
||||
theta_filtered = segmented_savgol(
|
||||
theta_resampled,
|
||||
sample_interval,
|
||||
filter_window_seconds,
|
||||
)
|
||||
|
||||
exclusion_half_width = max(
|
||||
0.30,
|
||||
filter_window_seconds * 0.5,
|
||||
)
|
||||
jump_exclusion_windows = [
|
||||
(
|
||||
max(0.0, event["time_seconds"] - exclusion_half_width),
|
||||
min(duration, event["time_seconds"] + exclusion_half_width),
|
||||
)
|
||||
for event in jump_events
|
||||
]
|
||||
invalid_near_jump = np.zeros(len(time_uniform), dtype=bool)
|
||||
for start, end in jump_exclusion_windows:
|
||||
invalid_near_jump |= (
|
||||
(time_uniform >= start) & (time_uniform <= end)
|
||||
)
|
||||
|
||||
if "CommandAngularSpeedRadPerSecond" in raw.columns:
|
||||
angular_command_rad = interpolate_command(
|
||||
"CommandAngularSpeedRadPerSecond"
|
||||
)
|
||||
else:
|
||||
angular_command_rad = np.deg2rad(
|
||||
interpolate_command("CommandAngularSpeed")
|
||||
)
|
||||
|
||||
frame = pd.DataFrame({
|
||||
"TimeSeconds": time_uniform,
|
||||
"DetourXRawMm": x_resampled,
|
||||
"DetourYRawMm": y_resampled,
|
||||
"DetourXFilteredMm": x_filtered,
|
||||
"DetourYFilteredMm": y_filtered,
|
||||
"DetourThetaUnwrappedDeg": theta_filtered,
|
||||
"DetourThetaDeg": wrap_degrees(theta_filtered),
|
||||
"CommandSpeedMps": interpolate_command("CommandSpeed"),
|
||||
"CommandAngularSpeedRadPerSec":
|
||||
angular_command_rad,
|
||||
"InvalidNearLocalizationJump": invalid_near_jump,
|
||||
})
|
||||
|
||||
first = raw.iloc[0]
|
||||
metadata: dict[str, Any] = {
|
||||
"csv_path": csv_path,
|
||||
"trajectory_name": str(first["TrajectoryName"]),
|
||||
"controller_name": str(first.get("ControllerName", "")),
|
||||
"trial_number": str(first.get("TrialNumber", "")),
|
||||
# 蟹行轨迹的运动前向相对车体X轴逆时针偏置90°。
|
||||
# DetourTheta始终是车体航向，计算航向误差时必须扣除该偏置。
|
||||
"motion_frame_yaw_degrees": float(
|
||||
first["ReferenceMotionFrameYawDegrees"]
|
||||
if (
|
||||
"ReferenceMotionFrameYawDegrees" in raw.columns
|
||||
and pd.notna(
|
||||
first["ReferenceMotionFrameYawDegrees"]
|
||||
)
|
||||
)
|
||||
else (
|
||||
90.0
|
||||
if "crab" in (
|
||||
str(first["TrajectoryName"]) +
|
||||
str(first.get("ControllerName", ""))
|
||||
).lower()
|
||||
else 0.0
|
||||
)
|
||||
),
|
||||
"reference_start_mm": np.array(
|
||||
[first["ReferenceStartX"], first["ReferenceStartY"]],
|
||||
dtype=float,
|
||||
),
|
||||
"reference_end_mm": np.array(
|
||||
[first["ReferenceEndX"], first["ReferenceEndY"]],
|
||||
dtype=float,
|
||||
),
|
||||
"reference_speed_mps": float(first["ReferenceSpeed"]),
|
||||
"reference_angular_speed_rad_per_second": float(
|
||||
first.get(
|
||||
"ReferenceAngularSpeedRadPerSecond",
|
||||
0.0,
|
||||
)
|
||||
),
|
||||
# 圆弧构造时使用了测试开始处Detour航向，因此这里取首帧航向。
|
||||
"start_heading_degrees": float(first["DetourTheta"]),
|
||||
"sample_interval_seconds": sample_interval,
|
||||
"filter_window_seconds": filter_window_seconds,
|
||||
"raw_sample_count": len(raw),
|
||||
"detour_update_count": len(updates),
|
||||
"held_sample_count": int(len(raw) - len(updates)),
|
||||
"localization_jump_events": jump_events,
|
||||
"jump_exclusion_windows": jump_exclusion_windows,
|
||||
}
|
||||
return frame, metadata
|
||||
|
||||
|
||||
def build_reference(
|
||||
frame: pd.DataFrame,
|
||||
metadata: dict[str, Any],
|
||||
) -> dict[str, np.ndarray | float | str]:
|
||||
"""根据CSV元数据建立直线、圆弧、完整S曲线或原地自转参考及误差。"""
|
||||
trajectory_name = str(metadata["trajectory_name"])
|
||||
start = np.asarray(metadata["reference_start_mm"], dtype=float)
|
||||
end = np.asarray(metadata["reference_end_mm"], dtype=float)
|
||||
motion_frame_yaw_degrees = float(
|
||||
metadata.get("motion_frame_yaw_degrees", 0.0)
|
||||
)
|
||||
actual = frame[
|
||||
["DetourXFilteredMm", "DetourYFilteredMm"]
|
||||
].to_numpy(dtype=float)
|
||||
actual_heading = frame["DetourThetaUnwrappedDeg"].to_numpy(dtype=float)
|
||||
|
||||
radius_match = re.search(
|
||||
r"LeftArc(?P<sweep>[0-9.]+)_R(?P<radius>[0-9.]+)mm",
|
||||
trajectory_name,
|
||||
flags=re.IGNORECASE,
|
||||
)
|
||||
if radius_match:
|
||||
radius = float(radius_match.group("radius"))
|
||||
sweep_degrees = float(radius_match.group("sweep"))
|
||||
start_body_heading = float(metadata["start_heading_degrees"])
|
||||
start_motion_heading = (
|
||||
start_body_heading + motion_frame_yaw_degrees
|
||||
)
|
||||
heading_radians = np.deg2rad(start_motion_heading)
|
||||
center = start + radius * np.array(
|
||||
[-np.sin(heading_radians), np.cos(heading_radians)]
|
||||
)
|
||||
start_radial_degrees = start_motion_heading - 90.0
|
||||
|
||||
radial = actual - center
|
||||
distance_to_center = np.linalg.norm(radial, axis=1)
|
||||
radial_angle_degrees = np.rad2deg(
|
||||
np.arctan2(radial[:, 1], radial[:, 0])
|
||||
)
|
||||
radial_angle_radians = np.deg2rad(radial_angle_degrees)
|
||||
reference_points = center + radius * np.column_stack([
|
||||
np.cos(radial_angle_radians),
|
||||
np.sin(radial_angle_radians),
|
||||
])
|
||||
# 对逆时针圆弧，正横向误差表示车辆位于轨迹左侧（圆内侧）。
|
||||
lateral_error = radius - distance_to_center
|
||||
reference_motion_heading = radial_angle_degrees + 90.0
|
||||
reference_heading = (
|
||||
reference_motion_heading - motion_frame_yaw_degrees
|
||||
)
|
||||
heading_error = wrap_degrees(
|
||||
actual_heading - reference_heading
|
||||
)
|
||||
|
||||
plot_angles = np.deg2rad(
|
||||
np.linspace(
|
||||
start_radial_degrees,
|
||||
start_radial_degrees + sweep_degrees,
|
||||
361,
|
||||
)
|
||||
)
|
||||
ideal_plot = center + radius * np.column_stack([
|
||||
np.cos(plot_angles),
|
||||
np.sin(plot_angles),
|
||||
])
|
||||
return {
|
||||
"kind": "left_arc",
|
||||
"ideal_plot_mm": ideal_plot,
|
||||
"reference_points_mm": reference_points,
|
||||
"reference_heading_degrees": reference_heading,
|
||||
"reference_motion_heading_degrees":
|
||||
reference_motion_heading,
|
||||
"lateral_error_mm": lateral_error,
|
||||
"heading_error_degrees": heading_error,
|
||||
"center_mm": center,
|
||||
"radius_mm": radius,
|
||||
}
|
||||
|
||||
s_curve_match = re.search(
|
||||
r"SCurve(?P<length>[0-9.]+)m_A(?P<offset>[0-9.]+)mm",
|
||||
trajectory_name,
|
||||
flags=re.IGNORECASE,
|
||||
)
|
||||
if s_curve_match:
|
||||
offset_mm = float(s_curve_match.group("offset"))
|
||||
ideal_plot, ideal_heading = build_complete_s_curve(
|
||||
start,
|
||||
end,
|
||||
offset_mm,
|
||||
)
|
||||
delta = actual[:, np.newaxis, :] - ideal_plot[np.newaxis, :, :]
|
||||
nearest_indices = np.argmin(
|
||||
np.sum(delta * delta, axis=2),
|
||||
axis=1,
|
||||
)
|
||||
reference_points = ideal_plot[nearest_indices]
|
||||
reference_motion_heading = ideal_heading[nearest_indices]
|
||||
reference_heading = (
|
||||
reference_motion_heading - motion_frame_yaw_degrees
|
||||
)
|
||||
heading_radians = np.deg2rad(reference_motion_heading)
|
||||
left_normals = np.column_stack([
|
||||
-np.sin(heading_radians),
|
||||
np.cos(heading_radians),
|
||||
])
|
||||
lateral_error = np.sum(
|
||||
(actual - reference_points) * left_normals,
|
||||
axis=1,
|
||||
)
|
||||
heading_error = wrap_degrees(
|
||||
actual_heading - reference_heading
|
||||
)
|
||||
return {
|
||||
"kind": "s_curve",
|
||||
"ideal_plot_mm": ideal_plot,
|
||||
"reference_points_mm": reference_points,
|
||||
"reference_heading_degrees": reference_heading,
|
||||
"reference_motion_heading_degrees":
|
||||
reference_motion_heading,
|
||||
"lateral_error_mm": lateral_error,
|
||||
"heading_error_degrees": heading_error,
|
||||
"offset_mm": offset_mm,
|
||||
}
|
||||
|
||||
line = end - start
|
||||
length = float(np.linalg.norm(line))
|
||||
if length <= 1e-6:
|
||||
rotation_match = re.search(
|
||||
r"Rotate(?P<angle>[+-]?[0-9.]+)",
|
||||
trajectory_name,
|
||||
flags=re.IGNORECASE,
|
||||
)
|
||||
if rotation_match:
|
||||
relative_angle_degrees = float(
|
||||
rotation_match.group("angle")
|
||||
)
|
||||
target_heading_degrees = (
|
||||
float(metadata["start_heading_degrees"]) +
|
||||
relative_angle_degrees
|
||||
)
|
||||
reference_points = np.repeat(
|
||||
start[np.newaxis, :],
|
||||
len(frame),
|
||||
axis=0,
|
||||
)
|
||||
position_drift = np.linalg.norm(
|
||||
actual - start,
|
||||
axis=1,
|
||||
)
|
||||
reference_heading = np.full(
|
||||
len(frame),
|
||||
target_heading_degrees,
|
||||
)
|
||||
heading_error = wrap_degrees(
|
||||
actual_heading - reference_heading
|
||||
)
|
||||
ideal_plot = np.repeat(
|
||||
start[np.newaxis, :],
|
||||
2,
|
||||
axis=0,
|
||||
)
|
||||
return {
|
||||
"kind": "in_place_rotation",
|
||||
"ideal_plot_mm": ideal_plot,
|
||||
"reference_points_mm": reference_points,
|
||||
"reference_heading_degrees": reference_heading,
|
||||
# 对原地自转，该字段表示偏离初始旋转中心的距离。
|
||||
"lateral_error_mm": position_drift,
|
||||
"heading_error_degrees": heading_error,
|
||||
"rotation_center_mm": start,
|
||||
"relative_angle_degrees": relative_angle_degrees,
|
||||
"target_heading_degrees": target_heading_degrees,
|
||||
}
|
||||
|
||||
raise ValueError(
|
||||
f"{trajectory_name}无法识别为圆弧，且参考直线长度为0。"
|
||||
)
|
||||
|
||||
tangent = line / length
|
||||
left_normal = np.array([-tangent[1], tangent[0]])
|
||||
displacement = actual - start
|
||||
progress = np.clip(displacement @ tangent, 0.0, length)
|
||||
reference_points = start + np.outer(progress, tangent)
|
||||
lateral_error = (actual - reference_points) @ left_normal
|
||||
reference_motion_heading_scalar = np.rad2deg(
|
||||
np.arctan2(tangent[1], tangent[0])
|
||||
)
|
||||
reference_heading_scalar = (
|
||||
reference_motion_heading_scalar -
|
||||
motion_frame_yaw_degrees
|
||||
)
|
||||
reference_heading = np.full(
|
||||
len(frame),
|
||||
reference_heading_scalar,
|
||||
)
|
||||
heading_error = wrap_degrees(
|
||||
actual_heading - reference_heading
|
||||
)
|
||||
ideal_plot = np.linspace(start, end, 361)
|
||||
return {
|
||||
"kind": "line",
|
||||
"ideal_plot_mm": ideal_plot,
|
||||
"reference_points_mm": reference_points,
|
||||
"reference_heading_degrees": reference_heading,
|
||||
"reference_motion_heading_degrees": np.full(
|
||||
len(frame),
|
||||
reference_motion_heading_scalar,
|
||||
),
|
||||
"lateral_error_mm": lateral_error,
|
||||
"heading_error_degrees": heading_error,
|
||||
}
|
||||
|
||||
|
||||
def discover_csv_files(arguments: list[str]) -> list[Path]:
|
||||
"""解析命令行CSV；未指定时使用脚本目录下全部CSV。"""
|
||||
if arguments:
|
||||
files = [Path(item).expanduser().resolve() for item in arguments]
|
||||
else:
|
||||
files = sorted(SCRIPT_DIR.glob("*.csv"))
|
||||
if not files:
|
||||
raise FileNotFoundError("没有找到可处理的CSV文件。")
|
||||
return files
|
||||
|
||||
|
||||
def output_path(
|
||||
csv_path: Path,
|
||||
output_directory: str | None,
|
||||
suffix: str,
|
||||
) -> Path:
|
||||
"""构造图片输出路径并创建目录。"""
|
||||
directory = (
|
||||
Path(output_directory).expanduser().resolve()
|
||||
if output_directory
|
||||
else csv_path.parent / "plots"
|
||||
)
|
||||
directory.mkdir(parents=True, exist_ok=True)
|
||||
return directory / f"{csv_path.stem}_{suffix}.png"
|
||||
|
||||
|
||||
def plot_trajectory(
|
||||
csv_path: Path,
|
||||
frequency_hz: float,
|
||||
filter_window_seconds: float,
|
||||
output_directory: str | None,
|
||||
show: bool,
|
||||
) -> Path:
|
||||
"""生成单份CSV的理想/实际轨迹对比图。"""
|
||||
frame, metadata = load_and_resample(
|
||||
csv_path,
|
||||
frequency_hz,
|
||||
filter_window_seconds,
|
||||
)
|
||||
reference = build_reference(frame, metadata)
|
||||
|
||||
actual_x_m = frame["DetourXFilteredMm"].to_numpy() / 1000.0
|
||||
actual_y_m = frame["DetourYFilteredMm"].to_numpy() / 1000.0
|
||||
ideal_m = np.asarray(reference["ideal_plot_mm"]) / 1000.0
|
||||
|
||||
fig, ax = plt.subplots(figsize=(8.0, 7.0))
|
||||
ax.plot(
|
||||
ideal_m[:, 0],
|
||||
ideal_m[:, 1],
|
||||
"--",
|
||||
linewidth=2.2,
|
||||
label="理想轨迹",
|
||||
)
|
||||
ax.plot(
|
||||
actual_x_m,
|
||||
actual_y_m,
|
||||
linewidth=1.8,
|
||||
label="Detour实际轨迹（滤波后）",
|
||||
)
|
||||
if reference["kind"] == "in_place_rotation":
|
||||
ax.scatter(
|
||||
[ideal_m[0, 0]],
|
||||
[ideal_m[0, 1]],
|
||||
marker="*",
|
||||
s=100,
|
||||
label="理想旋转中心",
|
||||
zorder=5,
|
||||
)
|
||||
else:
|
||||
ax.scatter(
|
||||
[ideal_m[0, 0]],
|
||||
[ideal_m[0, 1]],
|
||||
marker="o",
|
||||
s=55,
|
||||
label="起点",
|
||||
zorder=5,
|
||||
)
|
||||
ax.scatter(
|
||||
[ideal_m[-1, 0]],
|
||||
[ideal_m[-1, 1]],
|
||||
marker="x",
|
||||
s=65,
|
||||
label="终点",
|
||||
zorder=5,
|
||||
)
|
||||
for event_index, event in enumerate(
|
||||
metadata["localization_jump_events"]
|
||||
):
|
||||
before = np.array([
|
||||
event["before_x_mm"],
|
||||
event["before_y_mm"],
|
||||
]) / 1000.0
|
||||
after = np.array([
|
||||
event["after_x_mm"],
|
||||
event["after_y_mm"],
|
||||
]) / 1000.0
|
||||
ax.scatter(
|
||||
[before[0], after[0]],
|
||||
[before[1], after[1]],
|
||||
marker="x",
|
||||
color="tab:red",
|
||||
s=55,
|
||||
zorder=6,
|
||||
label="Detour定位跳变前/后"
|
||||
if event_index == 0 else None,
|
||||
)
|
||||
ax.annotate(
|
||||
f"定位跳变 {event['distance_mm']:.1f} mm\n"
|
||||
f"t={event['time_seconds']:.2f} s",
|
||||
xy=(after[0], after[1]),
|
||||
xytext=(8, 8),
|
||||
textcoords="offset points",
|
||||
color="tab:red",
|
||||
fontsize=9,
|
||||
)
|
||||
ax.set_aspect("equal", adjustable="box")
|
||||
ax.set_xlabel("世界坐标 X / m")
|
||||
ax.set_ylabel("世界坐标 Y / m")
|
||||
ax.set_title(
|
||||
f"理想轨迹与实际轨迹对比\n"
|
||||
f"{metadata['controller_name']} - "
|
||||
f"{metadata['trajectory_name']}"
|
||||
)
|
||||
ax.grid(True, alpha=0.3)
|
||||
ax.legend()
|
||||
fig.tight_layout()
|
||||
|
||||
destination = output_path(
|
||||
csv_path,
|
||||
output_directory,
|
||||
"trajectory_comparison",
|
||||
)
|
||||
fig.savefig(destination, dpi=300, bbox_inches="tight")
|
||||
if show:
|
||||
plt.show()
|
||||
plt.close(fig)
|
||||
print(
|
||||
f"{csv_path.name}: 原始采样{metadata['raw_sample_count']}帧，"
|
||||
f"有效Detour更新{metadata['detour_update_count']}帧，"
|
||||
f"保持重复{metadata['held_sample_count']}帧，"
|
||||
f"定位跳变{len(metadata['localization_jump_events'])}次"
|
||||
)
|
||||
return destination
|
||||
|
||||
|
||||
def main() -> None:
|
||||
configure_matplotlib()
|
||||
parser = argparse.ArgumentParser(
|
||||
description="绘制理想轨迹与Detour实际轨迹对比图。"
|
||||
)
|
||||
parser.add_argument("files", nargs="*", help="一个或多个CSV文件")
|
||||
parser.add_argument("--frequency", type=float, default=20.0)
|
||||
parser.add_argument("--window", type=float, default=0.55)
|
||||
parser.add_argument("--output-dir")
|
||||
parser.add_argument("--show", action="store_true")
|
||||
args = parser.parse_args()
|
||||
|
||||
for csv_path in discover_csv_files(args.files):
|
||||
destination = plot_trajectory(
|
||||
csv_path,
|
||||
args.frequency,
|
||||
args.window,
|
||||
args.output_dir,
|
||||
args.show,
|
||||
)
|
||||
print(f"已生成：{destination}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,4 @@
|
||||
numpy>=1.26
|
||||
pandas>=2.2
|
||||
matplotlib>=3.8
|
||||
scipy>=1.12
|
||||
@@ -0,0 +1,191 @@
|
||||
"""一次运行四个轨迹实验绘图脚本。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import os
|
||||
import subprocess
|
||||
import sys
|
||||
from concurrent.futures import ThreadPoolExecutor, as_completed
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
SCRIPT_NAMES = (
|
||||
"plot_trajectory_comparison.py",
|
||||
"plot_tracking_errors.py",
|
||||
"plot_speed_response.py",
|
||||
"plot_angular_command.py",
|
||||
)
|
||||
|
||||
|
||||
def build_command(
|
||||
script_path: Path,
|
||||
files: list[str],
|
||||
frequency_hz: float,
|
||||
filter_window_seconds: float,
|
||||
output_directory: str | None,
|
||||
show: bool,
|
||||
) -> list[str]:
|
||||
"""为一个绘图脚本构造与统一入口一致的命令行参数。"""
|
||||
command = [
|
||||
sys.executable,
|
||||
str(script_path),
|
||||
*files,
|
||||
"--frequency",
|
||||
str(frequency_hz),
|
||||
"--window",
|
||||
str(filter_window_seconds),
|
||||
]
|
||||
|
||||
if output_directory:
|
||||
command.extend(["--output-dir", output_directory])
|
||||
|
||||
if show:
|
||||
command.append("--show")
|
||||
|
||||
return command
|
||||
|
||||
|
||||
def run_script(
|
||||
script_path: Path,
|
||||
files: list[str],
|
||||
frequency_hz: float,
|
||||
filter_window_seconds: float,
|
||||
output_directory: str | None,
|
||||
show: bool,
|
||||
) -> tuple[str, int, str, str]:
|
||||
"""运行一个绘图脚本并返回名称、退出码及标准输出和错误。"""
|
||||
result = subprocess.run(
|
||||
build_command(
|
||||
script_path,
|
||||
files,
|
||||
frequency_hz,
|
||||
filter_window_seconds,
|
||||
output_directory,
|
||||
show,
|
||||
),
|
||||
cwd=script_path.parent,
|
||||
capture_output=True,
|
||||
text=True,
|
||||
encoding="utf-8",
|
||||
errors="replace",
|
||||
env={
|
||||
**os.environ,
|
||||
"PYTHONIOENCODING": "utf-8",
|
||||
},
|
||||
check=False,
|
||||
)
|
||||
|
||||
return (
|
||||
script_path.name,
|
||||
result.returncode,
|
||||
result.stdout.strip(),
|
||||
result.stderr.strip(),
|
||||
)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
"""并行执行四类实验图的生成任务。"""
|
||||
parser = argparse.ArgumentParser(
|
||||
description="一次生成轨迹、误差、速度响应和角速度指令四类图。"
|
||||
)
|
||||
parser.add_argument(
|
||||
"files",
|
||||
nargs="*",
|
||||
help="一个或多个CSV文件；省略时处理data_process目录下全部CSV。",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--frequency",
|
||||
type=float,
|
||||
default=20.0,
|
||||
help="固定重采样频率，默认20 Hz。",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--window",
|
||||
type=float,
|
||||
default=0.55,
|
||||
help="Savitzky-Golay滤波窗口，默认0.55 s。",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--output-dir",
|
||||
help="图片输出目录；省略时由各绘图脚本使用默认目录。",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--show",
|
||||
action="store_true",
|
||||
help="生成后请求显示图片。",
|
||||
)
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.frequency <= 0.0:
|
||||
parser.error("--frequency必须大于0。")
|
||||
|
||||
if args.window <= 0.0:
|
||||
parser.error("--window必须大于0。")
|
||||
|
||||
script_directory = Path(__file__).resolve().parent
|
||||
script_paths = [
|
||||
script_directory / name
|
||||
for name in SCRIPT_NAMES
|
||||
]
|
||||
missing_scripts = [
|
||||
str(path)
|
||||
for path in script_paths
|
||||
if not path.is_file()
|
||||
]
|
||||
if missing_scripts:
|
||||
parser.error(
|
||||
"缺少绘图脚本：" + "，".join(missing_scripts)
|
||||
)
|
||||
|
||||
print("开始并行生成四类实验图……")
|
||||
failures: list[str] = []
|
||||
|
||||
with ThreadPoolExecutor(
|
||||
max_workers=len(script_paths)
|
||||
) as executor:
|
||||
futures = [
|
||||
executor.submit(
|
||||
run_script,
|
||||
script_path,
|
||||
args.files,
|
||||
args.frequency,
|
||||
args.window,
|
||||
args.output_dir,
|
||||
args.show,
|
||||
)
|
||||
for script_path in script_paths
|
||||
]
|
||||
|
||||
for future in as_completed(futures):
|
||||
script_name, return_code, stdout, stderr = (
|
||||
future.result()
|
||||
)
|
||||
print(f"\n[{script_name}]")
|
||||
if stdout:
|
||||
print(stdout)
|
||||
if stderr:
|
||||
print(stderr, file=sys.stderr)
|
||||
|
||||
if return_code == 0:
|
||||
print("执行成功。")
|
||||
else:
|
||||
failures.append(script_name)
|
||||
print(
|
||||
f"执行失败，退出码={return_code}。",
|
||||
file=sys.stderr,
|
||||
)
|
||||
|
||||
if failures:
|
||||
print(
|
||||
"\n以下脚本执行失败：" +
|
||||
"，".join(failures),
|
||||
file=sys.stderr,
|
||||
)
|
||||
raise SystemExit(1)
|
||||
|
||||
print("\n四类实验图均已生成。")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user