761 lines
26 KiB
Python
761 lines
26 KiB
Python
"""对比原始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()
|