坐标系转换快速定位问题
1.存储相关数据这里我是为了把车位局部和全局互相转换把相关矢量数据保存
// 车位日志写入（线程安全，懒加载表头）
namespace {
std::mutex g_slot_log_mu;
bool g_slot_log_header_written = false;
uint64_t g_slot_log_write_count = 0;
std::ofstream g_slot_log_ofs; // persistent streamstd::string ExpandUserPath(const std::string& p) {if (p.size() >= 2 && p[0] == '~' && p[1] == '/') {const char* home = ::getenv("HOME");if (home) return std::string(home) + p.substr(1);}return p;
}void EnsureSlotLogOpen() {if (g_slot_log_ofs.is_open()) return;const std::string path = ExpandUserPath("~/code/ci_ws_j6p/slot_log.csv");g_slot_log_ofs.open(path, std::ios::app);if (!g_slot_log_ofs.is_open()) {VEXUS_WARN_EVERY_SEC(NODE, 2) << "slot_log open failed: " << path;return;}
}// 提取车辆坐标系下的 Z 轴 yaw（忽略 pitch/roll 干扰），使用旋转矩阵前两列
// 早先实现使用 eulerAngles(2,1,0)[0] 可能在存在非零 pitch/roll 时产生耦合误差
double ExtractYaw(const Sophus::SE3& pose) {const Eigen::Matrix3d& R = pose.so3().matrix();// yaw = atan2(r10, r00)return std::atan2(R(1, 0), R(0, 0));
}void LogSlotCorner(uint64_t ts_ns,int slot_id,double dr_x,double dr_y,double dr_yaw,double global_x,double global_y,double local_x,double local_y) {std::lock_guard<std::mutex> lk(g_slot_log_mu);EnsureSlotLogOpen();if (!g_slot_log_ofs.is_open()) return;if (!g_slot_log_header_written) {g_slot_log_ofs << "timestamp_ns,slot_id,dr_x,dr_y,dr_yaw,global_x,""global_y,local_x,local_y\n";g_slot_log_header_written = true;}g_slot_log_ofs << ts_ns << ',' << slot_id << ',' << std::setprecision(15)<< dr_x << ',' << dr_y << ',' << dr_yaw << ',' << global_x<< ',' << global_y << ',' << local_x << ',' << local_y<< '\n';++g_slot_log_write_count;if (g_slot_log_write_count % 200 == 0) {g_slot_log_ofs.flush();VEXUS_INFO(NODE) << "slot_log flushed count=" << g_slot_log_write_count;}
}
} 2.数据分析单纯从数字上很难分辨出问题因此需要脚本处理数据
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
plot_slot_log.py读取 slot_log.csv，绘制两张图：
1. Global 图：DR 轨迹 + yaw 方向箭头 + 全局车位角点散点（按 slot_id 上色）
2. Local 聚合图：将所有局部车位角点散点绘制到车辆坐标系原点（参考每帧 DR 位姿变换后结果）新增 verify 模式：计算每条记录局部点通过 DR 位姿重投影到全局后的误差，与日志中的 global_x, global_y 比较。输出统计（count, mean, max, rmse, 95th）并可选写入误差 CSV。使用示例：python3 plot_slot_log.py --file ~/code/ci_ws_j6p/slot_log.csv --downsample 5 --show-yawpython3 plot_slot_log.py --file ~/code/ci_ws_j6p/slot_log.csv --verify --error-csv slot_error.csvpython3 plot_slot_log.py --file /tmp/slot_log.csv --slot-id 12 --slot-id 15 --save figure.png参数：--file 日志文件路径--downsample DR 轨迹点下采样步长（默认 1 不下采样）--slot-id 只显示指定 slot_id（可重复指定）--show-yaw 在 Global 图显示 DR 朝向箭头--save 保存图片到路径，不传则直接显示--alpha 点透明度（默认 0.8）--figsize 图尺寸，默认 14x6（格式: 宽,高）--max-points 截取前 N 条记录--yaw-interval 箭头绘制间隔--yaw-scale 箭头长度缩放--verify 开启误差验证（不影响绘图）--error-csv 写入误差明细到指定 CSV 文件CSV 格式 (header)：timestamp_ns,slot_id,dr_x,dr_y,dr_yaw,global_x,global_y,local_x,local_y注意：脚本假设一帧内多个角点共享同一个 DR 位姿；DR 轨迹使用每个时间戳第一次出现的记录。
"""
import csv
import math
import argparse
from collections import defaultdict
import numpy as np
import matplotlib.pyplot as pltdef safe_float(x):if x is None:raise ValueError('None value')s = str(x).strip()if s == '' or s.lower() == 'nan':raise ValueError('empty or nan')return float(s)def load_log(path, slot_filter, verbose=False):global_points = [] # (ts, slot_id, dr_x, dr_y, dr_yaw, gx, gy)local_points = [] # (ts, slot_id, lx, ly)dr_traj_map = {} # ts -> (dr_x, dr_y, dr_yaw)# 为 verify 也保留 global (gx, gy) 与 local (lx, ly)raw_rows = [] # (ts, slot_id, dr_x, dr_y, dr_yaw, gx, gy, lx, ly)total_rows = 0skipped_rows = 0with open(path, 'r') as f:reader = csv.DictReader(f)for row in reader:total_rows += 1try:ts = int(row.get('timestamp_ns', '0'))slot_id = int(row.get('slot_id', '-1'))if slot_filter and slot_id not in slot_filter:continuedr_x = safe_float(row.get('dr_x'))dr_y = safe_float(row.get('dr_y'))dr_yaw = safe_float(row.get('dr_yaw'))gx = safe_float(row.get('global_x'))gy = safe_float(row.get('global_y'))lx = safe_float(row.get('local_x'))ly = safe_float(row.get('local_y'))except (Exception) as e:skipped_rows += 1if verbose and skipped_rows < 20:print(f'[skip row {total_rows}] reason={e} raw={row}')continueglobal_points.append((ts, slot_id, dr_x, dr_y, dr_yaw, gx, gy))local_points.append((ts, slot_id, lx, ly))raw_rows.append((ts, slot_id, dr_x, dr_y, dr_yaw, gx, gy, lx, ly))if ts not in dr_traj_map:dr_traj_map[ts] = (dr_x, dr_y, dr_yaw)dr_traj = [(ts, *dr_traj_map[ts]) for ts in sorted(dr_traj_map.keys())]if verbose:print(f'Loaded rows={total_rows}, valid={total_rows-skipped_rows}, skipped={skipped_rows}')return global_points, local_points, dr_traj, raw_rowsdef compute_verify_errors(raw_rows):"""返回误差统计和误差数组。raw_rows: 列表 (ts, slot_id, dr_x, dr_y, dr_yaw, gx, gy, lx, ly)重投影公式：gx_est = dr_x + cos(yaw)*lx + sin(yaw)*lygy_est = dr_y - sin(yaw)*lx + cos(yaw)*ly误差: dx = gx_est - gx, dy = gy_est - gy, err = sqrt(dx^2 + dy^2)"""errs = []for (ts, slot_id, dr_x, dr_y, dr_yaw, gx, gy, lx, ly) in raw_rows:c = math.cos(dr_yaw); s = math.sin(dr_yaw)gx_est = dr_x + c*lx - s*ly # 修正符号gy_est = dr_y + s*lx + c*ly # 修正符号dx = gx_est - gxdy = gy_est - gyerr = math.hypot(dx, dy)errs.append((ts, slot_id, dx, dy, err, gx, gy, gx_est, gy_est))if not errs:return {}, []err_vals = [e[4] for e in errs]# 统计arr = np.array(err_vals)stats = {'count': int(arr.size),'mean': float(arr.mean()),'max': float(arr.max()),'rmse': float(math.sqrt(np.mean(arr**2))),'p95': float(np.percentile(arr, 95)),'median': float(np.median(arr))}return stats, errsdef plot_global(ax, global_points, dr_traj, downsample, show_yaw, alpha, yaw_interval=10, yaw_scale=0.6, verify_stats=None):if not global_points:ax.set_title('Global: no slot data')returnslot_color = {}cmap = plt.get_cmap('tab20')for ts, slot_id, dr_x, dr_y, dr_yaw, gx, gy in global_points:if slot_id not in slot_color:slot_color[slot_id] = cmap(len(slot_color) % 20)ax.scatter(gx, gy, s=10, c=[slot_color[slot_id]], alpha=alpha)if dr_traj:xs = [p[1] for p in dr_traj[::downsample]]; ys = [p[2] for p in dr_traj[::downsample]]ax.plot(xs, ys, 'k-', linewidth=1.0, label='DR trajectory')if show_yaw:sel = dr_traj[::max(1, yaw_interval)]qx = [p[1] for p in sel]; qy = [p[2] for p in sel]qu = [yaw_scale*math.cos(p[3]) for p in sel]; qv = [yaw_scale*math.sin(p[3]) for p in sel]ax.quiver(qx, qy, qu, qv, angles='xy', scale_units='xy', scale=1, color='red', width=0.004, alpha=0.75, label='DR yaw')ax.set_aspect('equal', adjustable='datalim')ax.set_xlabel('X (global)'); ax.set_ylabel('Y (global)')base_title = 'Global Slot Corners + DR Trajectory'if verify_stats:base_title += f"\nVerify count={verify_stats['count']} mean={verify_stats['mean']:.3f} rmse={verify_stats['rmse']:.3f} max={verify_stats['max']:.3f} p95={verify_stats['p95']:.3f}"ax.set_title(base_title)ax.grid(True); ax.legend(loc='best')def plot_local(ax, local_points, dr_traj, downsample, alpha, show_yaw=False, yaw_interval=10, yaw_scale=0.6):if not local_points or not dr_traj:ax.set_title('Local reprojection: no data')returnts_list = [t[0] for t in dr_traj]pose_map = {t[0]: t[1:] for t in dr_traj}import bisectdef find_pose(ts):if ts in pose_map:return pose_map[ts]idx = bisect.bisect_left(ts_list, ts)if idx == 0: return pose_map[ts_list[0]]if idx >= len(ts_list): return pose_map[ts_list[-1]]prev_ts = ts_list[idx-1]; next_ts = ts_list[idx]return pose_map[prev_ts] if (ts - prev_ts) < (next_ts - ts) else pose_map[next_ts]reproj = []for (ts, slot_id, lx, ly) in local_points[::downsample]:dr_x, dr_y, dr_yaw = find_pose(ts)c = math.cos(dr_yaw); s = math.sin(dr_yaw)gx = dr_x + c*lx - s*ly # 修正符号gy = dr_y + s*lx + c*ly # 修正符号reproj.append((gx, gy))xs = [p[0] for p in reproj]; ys = [p[1] for p in reproj]ax.scatter(xs, ys, s=10, c='purple', alpha=alpha, label='Reprojected slot corners')if dr_traj:dr_xs = [p[1] for p in dr_traj[::downsample]]; dr_ys = [p[2] for p in dr_traj[::downsample]]ax.plot(dr_xs, dr_ys, 'k--', linewidth=1.0, label='DR trajectory (subset)')ax.scatter(dr_xs, dr_ys, s=12, c='black', alpha=0.6, label='DR poses')if show_yaw:sel = dr_traj[::max(1, yaw_interval)]qx = [p[1] for p in sel]; qy = [p[2] for p in sel]qu = [yaw_scale*math.cos(p[3]) for p in sel]; qv = [yaw_scale*math.sin(p[3]) for p in sel]ax.quiver(qx, qy, qu, qv, angles='xy', scale_units='xy', scale=1, color='red', width=0.003, alpha=0.7, label='DR yaw')ax.set_aspect('equal', adjustable='datalim')ax.set_xlabel('X (global)'); ax.set_ylabel('Y (global)')ax.set_title('Reprojected Local Slot Corners + DR Poses')ax.grid(True); ax.legend(loc='best')def main():parser = argparse.ArgumentParser()parser.add_argument('--file', default='~/code/ci_ws_j6p/slot_log.csv')parser.add_argument('--downsample', type=int, default=1)parser.add_argument('--slot-id', action='append', type=int, help='filter specific slot id, can repeat')parser.add_argument('--show-yaw', action='store_true')parser.add_argument('--save', type=str, default='')parser.add_argument('--alpha', type=float, default=0.8)parser.add_argument('--figsize', type=str, default='14,6')parser.add_argument('--verbose', action='store_true')parser.add_argument('--max-points', type=int, default=1000, help='max number of points (corners) to plot from the start')parser.add_argument('--yaw-interval', type=int, default=10, help='interval for drawing yaw arrows')parser.add_argument('--yaw-scale', type=float, default=0.6, help='arrow length scale for yaw visualization')parser.add_argument('--verify', action='store_true', help='enable local->global reprojection error statistics')parser.add_argument('--error-csv', type=str, default='', help='optional output csv for per-point errors')args = parser.parse_args()slot_filter = set(args.slot_id) if args.slot_id else Nonepath = args.fileif path.startswith('~/'):import oshome = os.environ.get('HOME', '')path = home + path[1:]global_points, local_points, dr_traj, raw_rows = load_log(path, slot_filter, args.verbose)if not global_points:print('No valid global slot points loaded. Check file path or filters.')if not dr_traj:print('No DR trajectory points parsed.')verify_stats = None; verify_errs = []if args.verify:verify_stats, verify_errs = compute_verify_errors(raw_rows[:args.max_points]) # respect max-points limitif verify_stats:print('[verify] stats:', ', '.join(f'{k}={v:.6f}' if isinstance(v, float) else f'{k}={v}' for k,v in verify_stats.items()))else:print('[verify] no data for stats')if args.error_csv and verify_errs:with open(args.error_csv, 'w', newline='') as ef:w = csv.writer(ef)w.writerow(['timestamp_ns','slot_id','dx','dy','err','global_x','global_y','gx_est','gy_est'])w.writerows(verify_errs)print(f'[verify] wrote error detail csv: {args.error_csv}')w, h = [float(x) for x in args.figsize.split(',')]fig, (ax1, ax2) = plt.subplots(1, 2, figsize=(w, h))max_p = max(1, args.max_points)if len(global_points) > max_p: global_points = global_points[:max_p]if len(local_points) > max_p: local_points = local_points[:max_p]if len(dr_traj) > max_p: dr_traj = dr_traj[:max_p]plot_global(ax1, global_points, dr_traj, args.downsample, args.show_yaw, args.alpha, args.yaw_interval, args.yaw_scale, verify_stats)plot_local(ax2, local_points, dr_traj, args.downsample, args.alpha, args.show_yaw, args.yaw_interval, args.yaw_scale)fig.tight_layout()if args.save:fig.savefig(args.save, dpi=150)print(f'Saved figure to {args.save}')else:plt.show()if __name__ == '__main__':main()
开启误差验证python3 scripts/plot_slot_log.py --file ~/Documents/ci_ws_j6p/slot_log.csv --max-points 2000 --show-yaw --yaw-interval 8 --yaw-scale 0.7 --verify --error-csv slot_error.csv
3.分析输出的误差结果 slot_error.csv，其中有多种情况
从前 200 行样例可以看到以下规律：误差 err 随时间快速增大：早期每组角点 err 在 6<del>8 米量级，随后出现 3</del>5 米，再到 7+、甚至 8+ 米以上以及超过 9 米（slot_id 16）。
同一时间戳下同一个 slot 的 4 个角点，误差大小相近但存在符号对称：
例如 timestamp=1735671612167000064, slot_id=49 的四点：dx 分别约 +1.88 +1.90 -0.56 -0.58，dy 全部大正数 6.8~8.0。说明 X 方向存在一对正/负，Y 方向同向整体偏移。
这是典型的旋转后再平移时某一项符号弄反造成的模式：一个轴的局部坐标加减被颠倒，导致角点在 X 上出现成对偏差（正负对称），而 Y 累积偏移统一方向。
时间继续往后，dx 与 dy 的绝对值整体增大且仍保持类似对称性（dx 一正一负或多正多负，dy 多为同向较大负或正），说明误差并非随机噪声，而是系统性转换公式错误（sign error）。
不同 slot_id（49,50,16）均呈现类似结构性误差，排除了单一车位数据异常，指向公共转换逻辑问题。
根因定位
你的 Python 脚本中局部坐标重投影公式曾为： gx = dr_x + clx + sly gy = dr_y - slx + cly 标准 2D 旋转 R( yaw ) 对点 (lx, ly) 的全局变换应为： gx = dr_x + clx - sly gy = dr_y + slx + cly 也就是 gy 第一项应为 +slx 而不是 -slx，gx 第二项应为 -sly 而不是 +sly。两个符号均相反。 错误的双符号会：将真实旋转的正交分量“镜像”，把一个角点沿两个轴同时错误反射，导致一组角点在一个方向呈现对称偏差（dx 正负出现）而另一方向趋于统一偏移（dy 同号）。
误差随车辆姿态 yaw 变化会放大，因为错误项与真实旋转的幅度相关（跟 sin/cos 分量叠加）。
因此 slot_error.csv 中大而稳定的系统性误差与 dx 对称模式正是该符号错误的直接结果。此场景为转换坐标系有问题，将转换公式修改后就可以得到问题解决。
此时又发现问题观察得知误差持续发散且四个角点均从小到大发散，且可以看到z值会影响到发散效果，此时认为是转换局部时上游用了2D结果进行转换而定位模块用了3D转换，此时进行处理拍扁了定位转换的SE3。
// 仅保留 2D (x, y, yaw) 信息，忽略 pitch/roll/z，构造平面 SE3const double x = dr_pose.translation().x();const double y = dr_pose.translation().y();// 提取 yaw = atan2(r10, r00)const Eigen::Matrix3d& R = dr_pose.so3().matrix();const double yaw = std::atan2(R(1, 0), R(0, 0));const double c = std::cos(yaw);const double s = std::sin(yaw);Eigen::Matrix3d Rz;Rz << c, -s, 0, s, c, 0, 0, 0, 1;Sophus::SE3 yaw_only(Sophus::SO3(Rz), Eigen::Vector3d(x, y, 0.0));