自动驾驶-nuscenes 全局坐标与自车朝向 rotation 解读
Rotation解读
在nuscenes数据集中，自车 rotation 和全局地图原点的方向，是怎么样的，用图画出来，包括坐标，以及自车rotation 朝向是 怎么计算出来的，用哪些传感器或采集器？朝向是怎么来的。
我先把图画出来，把两个坐标系和 yaw 的定义放在一张图里。核心结论，先给你压缩成几句：
两个坐标系
- 全局地图系：原点在地图 mask 的左下角，x 向右（≈东）、y 向上（≈北）、z 朝天，单位米。每个 location 一张地图、一个独立原点，
boston-seaport和singapore-onenorth的坐标互不相干。 - 自车系：原点在后轴中点，x 车头、y 车左、z 朝上。
ego_pose 里的 translation + rotation 描述的就是 ego → global 这个变换，rotation 是 [w, x, y, z] 四元数。
yaw 怎么算：把车头单位向量 [1,0,0] 用旋转矩阵转到全局系，再取 atan2(v[1], v[0])。yaw=0 表示车头指向全局 +x。devkit 的 quaternion_yaw() 就是这么实现的，别用欧拉分解，有坡度时会偏。
朝向从哪来：不是 GPS 航向，也不是磁罗盘。nuScenes 先离线用 LiDAR 建了一张点云/反射强度地图，在线用 Velodyne HDL-32E 的当前扫描去和这张先验地图做配准（蒙特卡洛定位），位置和朝向一起解出来；IMU 和轮速/转角负责在两次匹配之间做高频推算和平滑；GNSS 只用来粗初始化，城市多径太重不参与最终位姿。官方精度在 10 cm 量级。
图里还标了坐标变换链和几个实操坑（四元数顺序、地图像素 y 翻转、每条 sample_data 各有一条 ego_pose）。
自车 rotation 与全局地图原点的方向关系
图 1 · 全局地图系与自车位姿
图 2 · 自车坐标系本身的定义
从四元数算出朝向角
# nuscenes-devkit 里的标准做法
from pyquaternion import Quaternion
import numpy as np
ep = nusc.get('ego_pose', sd['ego_pose_token'])
q = Quaternion(ep['rotation']) # [w, x, y, z]
v = q.rotation_matrix @ np.array([1, 0, 0]) # 车头单位向量 → 全局系
yaw = np.arctan2(v[1], v[0]) # 弧度
# yaw = 0 → 车头指向 x_global（图中向右）
# yaw = +π/2 → 车头指向 y_global（图中向上）注意不要直接用 q.yaw_pitch_roll[0]。当 roll / pitch 不为零（上下坡、颠簸）时，欧拉分解出的 yaw 与"车头在水平面上的投影方向"会有偏差，devkit 用投影法就是为了避开这一点。
坐标变换链
传感器系 --[calibrated_sensor]--> ego 系(t₁) --[ego_pose t₁]--> 全局地图系 --[ego_pose⁻¹ t₂]--> ego 系(t₂) --[calibrated_sensor⁻¹]--> 相机系全局系在这里的唯一作用，就是充当不同时刻、不同传感器之间的公共桥梁。点云去畸变、多帧叠加、把 3D 框投到图像上，走的都是这条链。
朝向是怎么测出来的
几个容易踩的点
- 四元数顺序是 [w, x, y, z]，不是 scipy 默认的 [x, y, z, w]。传给 Rotation.from_quat() 前必须换序。
- 全局系不是经纬度，也不是严格的 ENU。它是建图时定下的固定参考系，x/y 只是"大致"对应东/北，四张地图各有各的原点，跨 location 的坐标不可比。
- 地图 mask 是 0.1 m/像素的二值图，像素 (0,0) 在左下角；用 OpenCV 读图时行索引从上往下，y 要翻转。
- 每一条 sample_data 都有自己的 ego_pose，不是每个 sample（关键帧）一条。做运动补偿时一定要取对应传感器时间戳的那条。
- rotation 里包含 roll 和 pitch，虽然通常接近 0，但上下坡时不是。做 BEV 投影时别默认为平面。
二、实测生成朝向
分两步来:先把原理用可运行的代码演示一遍(这样数字都是真的),再给 CARLA 版本。玩具实验跑通了,数字都是真的。现在写 CARLA 版本。给你三份东西:一份图解文档,两个可直接运行的脚本。
关于原理,核心是三句话
-
结构决定有没有解,点数决定解得多准。 一个点不约束角度(它只在圆弧上滑动);一堵墙把 yaw 限制到两个解;一个墙角或一根杆子就唯一了。几万个点的作用是把每点 2 cm 的测距噪声按 1/√N 平均掉。
-
每个点对角度的敏感度正好等于它的距离。 数学上 ‖∂(R(θ)p)/∂θ‖ = ‖p‖ = r。所以 50 m 处的点在 1° 误差下横移 0.87 m,5 m 处只移 0.09 m。理论精度 σ_θ ≈ σ_range / (r_rms·√N),代入 nuScenes 量级得 ~0.0003°。
-
地面点几乎没用。 水平面绕竖直轴转动不变,对 yaw 零贡献。真正定住角度的是墙面、墙角、杆状物这些垂直结构。
实测数据(玩具脚本跑出来的,不是估计):
- 残差曲线谷底 0.0278 m,偏 5° 就涨到 1.2329 m —— 44 倍,谷又深又窄
- 只用 0–12 m 的近点解出误差 −0.0762°,用 12 m 以外误差 +0.0008°,差两个数量级
- ICP 从偏 7° 的初值出发,20 次迭代收敛到 dyaw = +0.0028°、dx = −0.0004 m
- 长直走廊:yaw 可观测(残差起伏 0.56 m),但沿墙位置不可观测(起伏 0.0005 m)
- 空旷圆形场地:yaw 残差起伏 0.0001 m,角度完全解不出来
关于 CARLA 脚本
# 终端 1
./CarlaUE4.sh -RenderOffScreen -quality-level=Low
# 终端 2
conda activate carla
python carla_scan_matching.py --town Town10HD_Opt --frames 120 --perturb-yaw 8五个阶段:autopilot 跑 120 帧用真值位姿拼出先验地图 → 刹停取一帧新扫描(只留局部坐标,把位姿藏起来)→ 固定 x,y 扫 151 个候选 yaw 画残差曲线 → 从偏 8° 的初值跑 ICP 同解三个自由度 → 和 CARLA 真值对比并输出 nuScenes 格式的四元数。
会打印原始点云表格(x/y/z/intensity/range/方位角/俯仰角)、距离直方图、高度直方图、ASCII 残差曲线、ICP 迭代过程。
三个容易踩的坑我在脚本里都处理了:fixed_delta_seconds=0.05 必须和 rotation_frequency=20 匹配否则拿不到完整 360° 扫描;全程待在 CARLA 左手系里用 get_matrix() 转换,不做任何翻转所以没有符号问题;地图必须体素降采样,不然 KDTree 建不动。
在 carla里复现
运行方式：
# 终端 1：启动 CARLA
cd ~/CARLA_0.9.15
./CarlaUE4.sh -RenderOffScreen -quality-level=Low
# 终端 2：跑脚本
conda activate carla
python carla_scan_matching.py --town Town10HD_Opt --frames 120 --perturb-yaw 8
# 想看地面点的影响（保留地面 -> 残差曲线会明显变浅）
python carla_scan_matching.py --keep-ground
# 存下点云和地图，后面自己画图
python carla_scan_matching.py --save scan.npz建议先跑 toy_scan_matching.py——不需要 CARLA,秒级出结果,两个脚本的匹配核心是逐行一致的。
toy_scan_matching.py
#!/usr/bin/env python3
"""
toy_scan_matching.py
--------------------
不依赖 CARLA、不依赖 Open3D，只用 numpy(+scipy 可选) 演示：
几万个激光点 + 一张先验地图 -> 如何解出一个 yaw 角
运行: python toy_scan_matching.py
"""
import numpy as np
try:
from scipy.spatial import cKDTree as KDTree
HAS_SCIPY = True
except ImportError:
HAS_SCIPY = False
np.random.seed(0)
D2R = np.pi / 180.0
R2D = 180.0 / np.pi
# ----------------------------------------------------------------------
# 1. 造一张"先验地图"：几栋楼 + 几根杆子，用线段表示
# ----------------------------------------------------------------------
def make_world():
segs = []
def box(cx, cy, w, h):
x0, x1 = cx - w / 2, cx + w / 2
y0, y1 = cy - h / 2, cy + h / 2
segs.extend([
[x0, y0, x1, y0], [x1, y0, x1, y1],
[x1, y1, x0, y1], [x0, y1, x0, y0],
])
box(-28, 22, 30, 20) # 左上楼
box( 26, 26, 26, 24) # 右上楼
box(-30, -26, 34, 18) # 左下楼
box( 30, -22, 22, 22) # 右下楼
box( 0, 48, 40, 12) # 远处一排
# 几根路灯杆（小方块）
for (px, py) in [(-8, 6), (9, -5), (-11, -8), (13, 12), (-2, -14)]:
box(px, py, 0.5, 0.5)
return np.array(segs, dtype=np.float64) # (S,4) 每行 x0,y0,x1,y1
def sample_map_points(segs, spacing=0.10):
"""把线段离散成稠密点云 —— 这就是离线建图的产物"""
pts = []
for x0, y0, x1, y1 in segs:
L = np.hypot(x1 - x0, y1 - y0)
n = max(int(L / spacing), 2)
t = np.linspace(0, 1, n)
pts.append(np.stack([x0 + t * (x1 - x0), y0 + t * (y1 - y0)], axis=1))
return np.concatenate(pts, axis=0)
# ----------------------------------------------------------------------
# 2. 模拟一帧激光扫描：从 (x,y,yaw) 出发向四周打光线
# ----------------------------------------------------------------------
def simulate_sweep(segs, pose, n_beams=3600, max_range=90.0,
noise_std=0.03, dropout=0.15):
x, y, yaw = pose
az = np.linspace(-np.pi, np.pi, n_beams, endpoint=False)
d = np.stack([np.cos(az + yaw), np.sin(az + yaw)], axis=1) # (R,2) 世界系方向
o = np.array([x, y])
a = segs[:, 0:2] # (S,2)
e = segs[:, 2:4] - segs[:, 0:2] # (S,2) 段向量
denom = d[:, None, 0] * e[None, :, 1] - d[:, None, 1] * e[None, :, 0] # (R,S)
ao = a[None, :, :] - o[None, None, :] # (1,S,2)
t = (ao[..., 0] * e[None, :, 1] - ao[..., 1] * e[None, :, 0]) / np.where(denom == 0, 1e-12, denom)
u = (ao[..., 0] * d[:, None, 1] - ao[..., 1] * d[:, None, 0]) / np.where(denom == 0, 1e-12, denom)
valid = (np.abs(denom) > 1e-12) & (t > 0.5) & (t < max_range) & (u >= 0) & (u <= 1)
t = np.where(valid, t, np.inf)
rng = t.min(axis=1) # (R,) 每条光线的最近命中
hit = np.isfinite(rng)
rng = rng[hit]
az_hit = az[hit]
rng = rng + np.random.randn(rng.size) * noise_std # 测距噪声
keep = np.random.rand(rng.size) > dropout # 漏检
rng, az_hit = rng[keep], az_hit[keep]
# 返回【传感器局部坐标系】下的点 —— 真实数据里存的就是这个
local = np.stack([rng * np.cos(az_hit), rng * np.sin(az_hit)], axis=1)
return local, rng, az_hit
def transform(pts, x, y, yaw):
c, s = np.cos(yaw), np.sin(yaw)
R = np.array([[c, -s], [s, c]])
return pts @ R.T + np.array([x, y])
# ----------------------------------------------------------------------
# 3. 残差函数：把扫描按候选位姿摆到地图上，看总的对不齐程度
# ----------------------------------------------------------------------
class Matcher:
def __init__(self, map_pts):
self.map_pts = map_pts
if HAS_SCIPY:
self.tree = KDTree(map_pts)
def nn_dist(self, q):
if HAS_SCIPY:
d, idx = self.tree.query(q)
return d, idx
# 退化实现（慢，仅在没有 scipy 时使用）
d = np.full(len(q), np.inf)
idx = np.zeros(len(q), dtype=int)
for i in range(0, len(q), 512):
blk = q[i:i + 512]
dist = np.linalg.norm(blk[:, None, :] - self.map_pts[None, :, :], axis=2)
idx[i:i + 512] = dist.argmin(axis=1)
d[i:i + 512] = dist.min(axis=1)
return d, idx
def residual(self, local, x, y, yaw, trunc=1.5):
q = transform(local, x, y, yaw)
d, _ = self.nn_dist(q)
return np.minimum(d, trunc).mean() # 截断均值，抗离群点
def icp(self, local, x, y, yaw, iters=40, max_corr=2.0, tol=1e-6):
hist = []
for it in range(iters):
q = transform(local, x, y, yaw)
d, idx = self.nn_dist(q)
m = d < max_corr
if m.sum() < 20:
break
A, B = q[m], self.map_pts[idx[m]]
ca, cb = A.mean(0), B.mean(0)
H = (A - ca).T @ (B - cb)
U, S, Vt = np.linalg.svd(H)
Rm = Vt.T @ U.T
if np.linalg.det(Rm) < 0:
Vt[-1] *= -1
Rm = Vt.T @ U.T
dyaw = np.arctan2(Rm[1, 0], Rm[0, 0])
t = cb - Rm @ ca
# 把增量复合到当前位姿上
nx, ny = Rm @ np.array([x, y]) + t
x, y, yaw = nx, ny, yaw + dyaw
hist.append((it, x, y, yaw, d[m].mean()))
if abs(dyaw) < tol and np.hypot(*(t)) < tol:
break
return (x, y, yaw), hist
# ----------------------------------------------------------------------
# 4. ASCII 曲线
# ----------------------------------------------------------------------
def ascii_curve(xs, ys, height=14, width=62, xlabel="", ylabel=""):
ys = np.asarray(ys, float)
lo, hi = ys.min(), ys.max()
rngy = hi - lo if hi > lo else 1.0
grid = [[" "] * width for _ in range(height)]
xi = ((np.asarray(xs) - xs[0]) / (xs[-1] - xs[0]) * (width - 1)).astype(int)
yi = ((ys - lo) / rngy * (height - 1)).astype(int)
for a, b in zip(xi, yi):
grid[height - 1 - b][a] = "*"
best = int(np.argmin(ys))
grid[height - 1 - yi[best]][xi[best]] = "@"
out = []
for r, row in enumerate(grid):
tag = f"{hi:8.4f} |" if r == 0 else (f"{lo:8.4f} |" if r == height - 1 else " |")
out.append(tag + "".join(row))
out.append(" +" + "-" * width)
out.append(" " + f"{xs[0]:<10.1f}" + " " * (width - 22) + f"{xs[-1]:>10.1f} {xlabel}")
return "\n".join(out)
def refine_min(xs, ys):
"""抛物线插值，求亚网格分辨率的极小点"""
i = int(np.argmin(ys))
if i == 0 or i == len(ys) - 1:
return xs[i]
y0, y1, y2 = ys[i - 1], ys[i], ys[i + 1]
denom = (y0 - 2 * y1 + y2)
if abs(denom) < 1e-12:
return xs[i]
delta = 0.5 * (y0 - y2) / denom
return xs[i] + delta * (xs[1] - xs[0])
def bar(v, vmax, w=40):
n = int(round(v / vmax * w)) if vmax > 0 else 0
return "█" * n
# ======================================================================
def main():
print("=" * 74)
print(" 扫描匹配演示：几万个激光点如何约束出一个 yaw 角")
print("=" * 74)
print(f" scipy KDTree: {'可用' if HAS_SCIPY else '不可用(使用慢速回退)'}")
segs = make_world()
map_pts = sample_map_points(segs, spacing=0.08)
print(f"\n[建图] 先验地图：{len(segs)} 条结构线段 -> {len(map_pts):,} 个地图点")
# ---- 真值位姿 ----
TRUE = (3.5, -2.0, 37.0 * D2R)
print(f"[真值] x={TRUE[0]:.3f} y={TRUE[1]:.3f} yaw={TRUE[2]*R2D:.4f}°")
local, rng, az = simulate_sweep(segs, TRUE)
print(f"\n[扫描] 本帧点数 {len(local):,}")
print(f" 距离范围 {rng.min():.2f} ~ {rng.max():.2f} m, 中位数 {np.median(rng):.2f} m")
print("\n--- 原始点云长什么样（传感器局部坐标系，前 8 个点）---")
print(f"{'idx':>5} {'x_local':>9} {'y_local':>9} {'range':>8} {'azimuth':>9}")
for i in range(8):
print(f"{i:>5} {local[i,0]:>9.3f} {local[i,1]:>9.3f} {rng[i]:>8.3f} {az[i]*R2D:>8.2f}°")
print("\n--- 距离分布直方图 ---")
h, edges = np.histogram(rng, bins=9)
for c, lo, hi in zip(h, edges[:-1], edges[1:]):
print(f" {lo:5.1f}-{hi:5.1f} m | {bar(c, h.max()):<40} {c:5d}")
M = Matcher(map_pts)
# ------------------------------------------------------------------
print("\n" + "=" * 74)
print(" 实验 A：固定真实 x,y，只扫 yaw —— 看残差随角度怎么变")
print("=" * 74)
yaws = np.linspace(TRUE[2] - 12 * D2R, TRUE[2] + 12 * D2R, 121)
res = np.array([M.residual(local, TRUE[0], TRUE[1], t) for t in yaws])
print(ascii_curve(yaws * R2D, res, xlabel="候选 yaw (度)"))
best = yaws[res.argmin()]
print(f"\n 残差最小处 yaw = {best*R2D:.4f}° 真值 = {TRUE[2]*R2D:.4f}°"
f" 误差 = {(best-TRUE[2])*R2D:+.4f}°")
print(f" 曲线最低点残差 {res.min():.4f} m, 偏 5° 时残差 "
f"{M.residual(local, TRUE[0], TRUE[1], TRUE[2]+5*D2R):.4f} m")
# ------------------------------------------------------------------
print("\n" + "=" * 74)
print(" 实验 B：点数越多，角度越准（同一帧随机抽样）")
print("=" * 74)
fine = np.linspace(TRUE[2] - 3 * D2R, TRUE[2] + 3 * D2R, 241)
print(" 每种点数重复 12 次随机抽样，统计解出角度的标准差")
print(f"{'使用点数':>10} {'均值误差':>12} {'标准差 σ':>12} {'谷底曲率':>12}")
for n in [5, 20, 100, 500, 2000, len(local)]:
n = min(n, len(local))
ests, ks = [], []
for _ in range(12):
sel = np.random.choice(len(local), n, replace=False)
r = np.array([M.residual(local[sel], TRUE[0], TRUE[1], t) for t in fine])
ests.append(refine_min(fine, r))
ks.append(np.gradient(np.gradient(r, fine), fine)[r.argmin()])
ests = np.array(ests)
print(f"{n:>10,} {(ests.mean()-TRUE[2])*R2D:>+11.4f}° "
f"{ests.std()*R2D:>11.4f}° {np.mean(ks):>12.1f}")
# ------------------------------------------------------------------
print("\n" + "=" * 74)
print(" 实验 C：远点比近点更能定角度（杠杆效应）")
print("=" * 74)
print(" 每段各抽 200 个点、重复 12 次，比较解出角度的稳定性")
print(f"{'点的距离段':>14} {'可用点数':>10} {'均值误差':>12} {'标准差 σ':>12}")
for lo, hi in [(0, 12), (12, 24), (24, 36), (36, 95)]:
m = (rng >= lo) & (rng < hi)
if m.sum() < 30:
print(f"{lo:>5}-{hi:<5} m {m.sum():>10,} (点太少，跳过)")
continue
idxs = np.where(m)[0]
ests = []
for _ in range(12):
sel = np.random.choice(idxs, min(200, len(idxs)), replace=False)
r = np.array([M.residual(local[sel], TRUE[0], TRUE[1], t) for t in fine])
ests.append(refine_min(fine, r))
ests = np.array(ests)
print(f"{lo:>5}-{hi:<5} m {m.sum():>10,} {(ests.mean()-TRUE[2])*R2D:>+11.4f}° "
f"{ests.std()*R2D:>11.4f}°")
print("\n 1° 的姿态误差，会让距离 r 处的点偏离 r×0.01745 米：")
for r_ in [5, 20, 50, 80]:
print(f" r = {r_:>3} m -> 偏离 {r_*1*D2R:>6.3f} m")
# ------------------------------------------------------------------
print("\n" + "=" * 74)
print(" 实验 D：完整 ICP —— 从一个错误的初值收敛回真值")
print("=" * 74)
init = (TRUE[0] + 1.2, TRUE[1] - 0.9, TRUE[2] + 7.0 * D2R)
print(f" 初值 x={init[0]:.3f} y={init[1]:.3f} yaw={init[2]*R2D:.3f}°"
f" (yaw 偏了 +7.000°)")
(fx, fy, fyaw), hist = M.icp(local, *init)
print(f"\n{'iter':>5} {'x':>9} {'y':>9} {'yaw(°)':>10} {'平均残差(m)':>14}")
for it, x, y, t, e in hist[:12]:
print(f"{it:>5} {x:>9.3f} {y:>9.3f} {t*R2D:>10.4f} {e:>14.5f}")
if len(hist) > 12:
print(f" ... 共 {len(hist)} 次迭代")
it, x, y, t, e = hist[-1]
print(f"{it:>5} {x:>9.3f} {y:>9.3f} {t*R2D:>10.4f} {e:>14.5f}")
print(f"\n 最终 x={fx:.4f} y={fy:.4f} yaw={fyaw*R2D:.4f}°")
print(f" 真值 x={TRUE[0]:.4f} y={TRUE[1]:.4f} yaw={TRUE[2]*R2D:.4f}°")
print(f" 误差 dx={fx-TRUE[0]:+.4f} m dy={fy-TRUE[1]:+.4f} m "
f"dyaw={(fyaw-TRUE[2])*R2D:+.4f}°")
# ------------------------------------------------------------------
print("\n" + "=" * 74)
print(" 实验 E：退化场景 —— 什么时候角度解不出来")
print("=" * 74)
# 长直走廊：两条平行长墙
corr = np.array([[-200, 4, 200, 4], [-200, -4, 200, -4]], dtype=float)
cpts = sample_map_points(corr, 0.08)
Mc = Matcher(cpts)
lc, _, _ = simulate_sweep(corr, (0.0, 0.0, 20 * D2R), max_range=90)
ry = np.array([Mc.residual(lc, 0, 0, t) for t in np.linspace(20*D2R-6*D2R, 20*D2R+6*D2R, 121)])
rx = np.array([Mc.residual(lc, dx, 0, 20*D2R) for dx in np.linspace(-6, 6, 121)])
print(f" 长直走廊 yaw 方向 残差起伏 = {ry.max()-ry.min():.4f} m -> 角度可观测")
print(f" 沿墙 x 方向 残差起伏 = {rx.max()-rx.min():.4f} m -> 位置不可观测")
# 空旷圆形场地
th = np.linspace(0, 2*np.pi, 400, endpoint=False)
ring = np.stack([60*np.cos(th), 60*np.sin(th)], 1)
circ = np.concatenate([ring, np.roll(ring, -1, axis=0)], axis=1)
Mo = Matcher(sample_map_points(circ, 0.08))
lo_, _, _ = simulate_sweep(circ, (0.0, 0.0, 20*D2R), max_range=90)
ro = np.array([Mo.residual(lo_, 0, 0, t) for t in np.linspace(20*D2R-6*D2R, 20*D2R+6*D2R, 121)])
print(f" 空旷圆形场 yaw 方向 残差起伏 = {ro.max()-ro.min():.4f} m -> 角度完全不可观测")
print("\n 结论：yaw 是被【非对称的垂直结构】约束住的，"
"\n 环境本身没有方向性信息时，再多的点也解不出角度。")
print("=" * 74)
if __name__ == "__main__":
main()carla_scan_matching.py
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
carla_scan_matching.py
======================
在 CARLA 里完整复现 nuScenes 的 ego_pose.rotation 是怎么被"算"出来的。
流程与真实系统一一对应：
① 建图 : 开车跑一段，用【真值位姿】把每帧点云拼到世界系 -> 先验点云地图
② 采集 : 停车，取一帧新的激光扫描（只保留传感器局部坐标，假装不知道位姿）
③ 扫角度 : 固定 x,y 只扫 yaw，画出残差曲线 -> 看角度是怎么被约束出来的
④ ICP : 从一个错误初值出发，同时解 (x, y, yaw)
⑤ 对比 : 与 CARLA 真值比对，并输出 nuScenes 格式的四元数 [w,x,y,z]
依赖：carla (0.9.15)、numpy、scipy
用法：
# 终端 1
./CarlaUE4.sh -RenderOffScreen -quality-level=Low
# 终端 2
conda activate carla
python carla_scan_matching.py --town Town10HD_Opt --frames 120 --perturb-yaw 8
"""
import argparse
import math
import sys
import time
from queue import Queue, Empty
import numpy as np
try:
from scipy.spatial import cKDTree as KDTree
except ImportError:
sys.exit("需要 scipy： pip install scipy")
try:
import carla
except ImportError:
sys.exit("找不到 carla 模块。请确认 PythonAPI 已加入 PYTHONPATH，或 pip install carla==0.9.15")
D2R = math.pi / 180.0
R2D = 180.0 / math.pi
# ======================================================================
# 通用几何 / 匹配工具（与 toy_scan_matching.py 完全一致的核心）
# ======================================================================
def transform2d(pts, x, y, yaw):
"""pts: (N,2) 局部坐标 -> 世界坐标。与 CARLA 的 get_matrix() 约定一致。"""
c, s = math.cos(yaw), math.sin(yaw)
R = np.array([[c, -s], [s, c]])
return pts @ R.T + np.array([x, y])
def voxel_downsample(pts, voxel):
"""把点云按体素栅格去重，控制地图规模。pts:(N,2) 或 (N,3)"""
keys = np.floor(pts / voxel).astype(np.int64)
_, idx = np.unique(keys, axis=0, return_index=True)
return pts[np.sort(idx)]
def refine_min(xs, ys):
"""抛物线插值，取亚网格分辨率的极小点"""
i = int(np.argmin(ys))
if i == 0 or i == len(ys) - 1:
return xs[i]
y0, y1, y2 = ys[i - 1], ys[i], ys[i + 1]
den = y0 - 2 * y1 + y2
if abs(den) < 1e-12:
return xs[i]
return xs[i] + 0.5 * (y0 - y2) / den * (xs[1] - xs[0])
class Matcher:
"""先验地图 + 残差计算 + 2D ICP（求解 x, y, yaw 三个自由度）"""
def __init__(self, map_xy):
self.map = map_xy
self.tree = KDTree(map_xy)
def residual(self, local_xy, x, y, yaw, trunc=1.0):
q = transform2d(local_xy, x, y, yaw)
d, _ = self.tree.query(q)
return float(np.minimum(d, trunc).mean())
def icp(self, local_xy, x, y, yaw, iters=60, max_corr=2.0, tol=1e-7):
hist = []
for it in range(iters):
q = transform2d(local_xy, x, y, yaw)
d, idx = self.tree.query(q)
m = d < max_corr
if m.sum() < 30:
break
A, B = q[m], self.map[idx[m]]
ca, cb = A.mean(0), B.mean(0)
H = (A - ca).T @ (B - cb)
U, S, Vt = np.linalg.svd(H)
R = Vt.T @ U.T
if np.linalg.det(R) < 0:
Vt[-1] *= -1
R = Vt.T @ U.T
dyaw = math.atan2(R[1, 0], R[0, 0])
t = cb - R @ ca
x, y = R @ np.array([x, y]) + t
yaw += dyaw
hist.append((it, x, y, yaw, float(d[m].mean()), int(m.sum())))
if abs(dyaw) < tol and np.hypot(*t) < tol:
break
return (x, y, yaw), hist
def yaw_to_quat(yaw):
"""只绕竖直轴旋转 -> nuScenes 风格四元数 [w, x, y, z]"""
return [math.cos(yaw / 2), 0.0, 0.0, math.sin(yaw / 2)]
def ascii_curve(xs, ys, height=16, width=66, xlabel=""):
ys = np.asarray(ys, float)
lo, hi = ys.min(), ys.max()
rng = hi - lo if hi > lo else 1.0
grid = [[" "] * width for _ in range(height)]
xi = ((np.asarray(xs) - xs[0]) / (xs[-1] - xs[0]) * (width - 1)).astype(int)
yi = ((ys - lo) / rng * (height - 1)).astype(int)
for a, b in zip(xi, yi):
grid[height - 1 - b][a] = "*"
b0 = int(np.argmin(ys))
grid[height - 1 - yi[b0]][xi[b0]] = "@"
out = []
for r, row in enumerate(grid):
tag = f"{hi:8.4f} |" if r == 0 else (f"{lo:8.4f} |" if r == height - 1 else " |")
out.append(tag + "".join(row))
out.append(" +" + "-" * width)
out.append(" " + f"{xs[0]:<10.2f}" + " " * max(0, width - 22) + f"{xs[-1]:>10.2f} {xlabel}")
return "\n".join(out)
def bar(v, vmax, w=38):
return "#" * (int(round(v / vmax * w)) if vmax > 0 else 0)
# ======================================================================
# CARLA 部分
# ======================================================================
def parse_lidar(data):
"""
carla.LidarMeasurement -> (local_xyzi, world_xyz, pose)
CARLA 的点存在【传感器局部系】: x 前, y 右, z 上（左手系）
"""
raw = np.frombuffer(data.raw_data, dtype=np.float32).reshape(-1, 4).copy()
local = raw[:, :3]
inten = raw[:, 3]
M = np.array(data.transform.get_matrix()) # 局部 -> 世界 的 4x4
homo = np.hstack([local, np.ones((len(local), 1))])
world = (M @ homo.T).T[:, :3]
tf = data.transform
pose = (tf.location.x, tf.location.y, tf.rotation.yaw * D2R)
return local, inten, world, pose
def filter_points(local, world, min_r, max_r, z_min_local, use_ground):
r = np.linalg.norm(local[:, :2], axis=1)
m = (r > min_r) & (r < max_r)
if not use_ground:
m &= local[:, 2] > z_min_local # 去掉地面：局部 z 太低的点
return m, r
def main():
ap = argparse.ArgumentParser()
ap.add_argument("--host", default="127.0.0.1")
ap.add_argument("--port", type=int, default=2000)
ap.add_argument("--town", default=None, help="不填则使用当前已加载的地图")
ap.add_argument("--frames", type=int, default=120, help="建图帧数")
ap.add_argument("--voxel", type=float, default=0.15, help="地图体素尺寸(m)")
ap.add_argument("--perturb-yaw", type=float, default=8.0, help="ICP 初值的角度误差(度)")
ap.add_argument("--perturb-xy", type=float, default=1.5, help="ICP 初值的位置误差(m)")
ap.add_argument("--channels", type=int, default=32)
ap.add_argument("--pps", type=int, default=600000, help="points_per_second")
ap.add_argument("--lidar-range", type=float, default=80.0)
ap.add_argument("--noise", type=float, default=0.02, help="测距噪声 std(m)")
ap.add_argument("--keep-ground", action="store_true", help="不过滤地面点（用于对比实验）")
ap.add_argument("--save", default=None, help="把点云与地图存成 .npz")
args = ap.parse_args()
client = carla.Client(args.host, args.port)
client.set_timeout(30.0)
if args.town:
print(f"[init] 加载地图 {args.town} ...")
world = client.load_world(args.town)
else:
world = client.get_world()
print(f"[init] 当前地图: {world.get_map().name}")
orig_settings = world.get_settings()
tm = client.get_trafficmanager(8000)
vehicle = lidar = None
q = Queue()
try:
s = world.get_settings()
s.synchronous_mode = True
s.fixed_delta_seconds = 0.05 # 20 Hz，与雷达转速对齐
world.apply_settings(s)
tm.set_synchronous_mode(True)
bl = world.get_blueprint_library()
vbp = bl.filter("vehicle.tesla.model3")[0]
sps = world.get_map().get_spawn_points()
for sp in np.random.permutation(sps):
vehicle = world.try_spawn_actor(vbp, sp)
if vehicle:
break
if vehicle is None:
sys.exit("车辆生成失败")
vehicle.set_autopilot(True, 8000)
tm.ignore_lights_percentage(vehicle, 100)
lbp = bl.find("sensor.lidar.ray_cast")
lbp.set_attribute("channels", str(args.channels))
lbp.set_attribute("range", str(args.lidar_range))
lbp.set_attribute("points_per_second", str(args.pps))
lbp.set_attribute("rotation_frequency", "20") # = 1 / fixed_delta_seconds
lbp.set_attribute("upper_fov", "10.0")
lbp.set_attribute("lower_fov", "-30.0")
lbp.set_attribute("noise_stddev", str(args.noise))
lbp.set_attribute("dropoff_general_rate", "0.05")
lidar = world.spawn_actor(lbp,
carla.Transform(carla.Location(x=0.0, z=1.8)),
attach_to=vehicle)
lidar.listen(q.put)
print(f"[init] LiDAR: {args.channels} 线 / {args.pps} pps / 20Hz "
f"-> 约 {args.pps // 20:,} 点每帧")
def tick():
world.tick()
try:
return q.get(timeout=5.0)
except Empty:
sys.exit("等待雷达数据超时")
for _ in range(20): # 预热，让车动起来
tick()
# ------------------------------------------------------------------
# ① 建图
# ------------------------------------------------------------------
print("\n" + "=" * 76)
print(" ① 建图：用真值位姿把每帧点云拼到世界系")
print("=" * 76)
acc = []
t0 = time.time()
for i in range(args.frames):
data = tick()
local, inten, world_xyz, pose = parse_lidar(data)
m, _ = filter_points(local, world_xyz, 3.0, args.lidar_range,
-1.4, args.keep_ground)
acc.append(world_xyz[m][:, :2])
if (i + 1) % 30 == 0:
n = sum(len(a) for a in acc)
print(f" frame {i+1:>4}/{args.frames} 累计 {n:,} 点 "
f"车位置 ({pose[0]:.1f}, {pose[1]:.1f}) yaw {pose[2]*R2D:.1f}°")
map_raw = np.concatenate(acc, axis=0)
map_xy = voxel_downsample(map_raw, args.voxel)
print(f"\n 原始累计 {len(map_raw):,} 点 -> 体素({args.voxel} m)降采样后 "
f"{len(map_xy):,} 点 用时 {time.time()-t0:.1f}s")
print(f" 地图范围 x [{map_xy[:,0].min():.1f}, {map_xy[:,0].max():.1f}] "
f"y [{map_xy[:,1].min():.1f}, {map_xy[:,1].max():.1f}]")
# ------------------------------------------------------------------
# ② 停车，取一帧待定位的扫描
# ------------------------------------------------------------------
vehicle.set_autopilot(False, 8000)
for _ in range(25):
vehicle.apply_control(carla.VehicleControl(throttle=0.0, brake=1.0))
data = tick()
local, inten, world_xyz, gt = parse_lidar(data)
gx, gy, gyaw = gt
m, r = filter_points(local, world_xyz, 3.0, args.lidar_range, -1.4, args.keep_ground)
query = local[m][:, :2] # 只保留 x,y —— 这就是"待定位的一帧"
qr = r[m]
print("\n" + "=" * 76)
print(" ② 待定位的这一帧点云")
print("=" * 76)
print(f" 原始点数 {len(local):,} -> 过滤后 {len(query):,} "
f"({'保留' if args.keep_ground else '剔除'}地面)")
print(f" CARLA 真值位姿 x={gx:.4f} y={gy:.4f} yaw={gyaw*R2D:.4f}°")
print("\n --- 原始点云打印（传感器局部坐标系，前 12 个点）---")
print(f" {'idx':>5} {'x':>9} {'y':>9} {'z':>8} {'inten':>7} "
f"{'range':>8} {'方位角':>9} {'俯仰角':>9}")
sel = np.linspace(0, len(local) - 1, 12).astype(int)
for i in sel:
px, py, pz = local[i]
rr = math.sqrt(px * px + py * py + pz * pz)
az = math.atan2(py, px) * R2D
el = math.asin(pz / rr) * R2D if rr > 0 else 0.0
print(f" {i:>5} {px:>9.3f} {py:>9.3f} {pz:>8.3f} {inten[i]:>7.3f} "
f"{rr:>8.3f} {az:>8.2f}° {el:>8.2f}°")
print("\n --- 距离分布 ---")
h, e = np.histogram(qr, bins=10)
for c, a, b in zip(h, e[:-1], e[1:]):
print(f" {a:6.1f}-{b:6.1f} m | {bar(c, h.max()):<38} {c:6d}")
print("\n --- 高度分布（局部 z，看得出地面/墙面/树冠分层）---")
h, e = np.histogram(local[:, 2], bins=10)
for c, a, b in zip(h, e[:-1], e[1:]):
print(f" {a:6.2f}-{b:6.2f} m | {bar(c, h.max()):<38} {c:6d}")
M = Matcher(map_xy)
# ------------------------------------------------------------------
# ③ 只扫 yaw
# ------------------------------------------------------------------
print("\n" + "=" * 76)
print(" ③ 固定真实 x,y，只扫 yaw —— 残差曲线")
print("=" * 76)
span = 15.0
yaws = np.linspace(gyaw - span * D2R, gyaw + span * D2R, 151)
res = np.array([M.residual(query, gx, gy, t) for t in yaws])
print(ascii_curve(yaws * R2D, res, xlabel="候选 yaw (度)"))
best = refine_min(yaws, res)
print(f"\n 残差最小的 yaw = {best*R2D:.4f}°")
print(f" CARLA 真值 = {gyaw*R2D:.4f}°")
print(f" 误差 = {(best-gyaw)*R2D:+.4f}°")
print(f" 谷底残差 {res.min():.4f} m 偏 5° 时 "
f"{M.residual(query, gx, gy, gyaw + 5*D2R):.4f} m "
f"偏 10° 时 {M.residual(query, gx, gy, gyaw + 10*D2R):.4f} m")
# 点数 -> 精度
print("\n --- 用多少个点就够了？（每档重复 8 次随机抽样）---")
fine = np.linspace(gyaw - 2 * D2R, gyaw + 2 * D2R, 161)
print(f" {'点数':>8} {'均值误差':>12} {'标准差':>12}")
for n in [10, 50, 200, 1000, 5000, len(query)]:
n = min(n, len(query))
est = []
for _ in range(8):
s_ = np.random.choice(len(query), n, replace=False)
rr = np.array([M.residual(query[s_], gx, gy, t) for t in fine])
est.append(refine_min(fine, rr))
est = np.array(est)
print(f" {n:>8,} {(est.mean()-gyaw)*R2D:>+11.4f}° {est.std()*R2D:>11.4f}°")
# 远近点对比
print("\n --- 远点 vs 近点（杠杆效应）---")
print(f" {'距离段':>14} {'点数':>9} {'角度误差':>12}")
for lo, hi in [(3, 15), (15, 30), (30, 50), (50, args.lidar_range)]:
mm = (qr >= lo) & (qr < hi)
if mm.sum() < 50:
print(f" {lo:>5}-{hi:<6.0f} m {mm.sum():>9,} 点太少")
continue
rr = np.array([M.residual(query[mm], gx, gy, t) for t in fine])
print(f" {lo:>5}-{hi:<6.0f} m {mm.sum():>9,} "
f"{(refine_min(fine, rr)-gyaw)*R2D:>+11.4f}°")
# ------------------------------------------------------------------
# ④ ICP：同时解 x, y, yaw
# ------------------------------------------------------------------
print("\n" + "=" * 76)
print(" ④ ICP：从错误初值同时解出 x, y, yaw")
print("=" * 76)
ix = gx + args.perturb_xy
iy = gy - args.perturb_xy * 0.7
iyaw = gyaw + args.perturb_yaw * D2R
print(f" 初值 x={ix:.3f} y={iy:.3f} yaw={iyaw*R2D:.3f}° "
f"(位置偏 {args.perturb_xy:.1f}m, 角度偏 {args.perturb_yaw:+.1f}°)")
(fx, fy, fyaw), hist = M.icp(query, ix, iy, iyaw)
print(f"\n {'iter':>5} {'x':>10} {'y':>10} {'yaw(°)':>10} "
f"{'残差(m)':>10} {'内点':>8}")
show = hist[:10] + ([hist[-1]] if len(hist) > 10 else [])
for it, x, y, t, e, n in show:
print(f" {it:>5} {x:>10.3f} {y:>10.3f} {t*R2D:>10.4f} {e:>10.5f} {n:>8,}")
if len(hist) > 10:
print(f" （共 {len(hist)} 次迭代）")
# ------------------------------------------------------------------
# ⑤ 结果对比 + 输出 nuScenes 格式
# ------------------------------------------------------------------
print("\n" + "=" * 76)
print(" ⑤ 与 CARLA 真值对比 —— 这就是 ego_pose 的生产过程")
print("=" * 76)
print(f" {'':<10}{'x (m)':>12}{'y (m)':>12}{'yaw (°)':>12}")
print(f" {'真值':<10}{gx:>12.4f}{gy:>12.4f}{gyaw*R2D:>12.4f}")
print(f" {'解出':<10}{fx:>12.4f}{fy:>12.4f}{fyaw*R2D:>12.4f}")
print(f" {'误差':<10}{fx-gx:>+12.4f}{fy-gy:>+12.4f}{(fyaw-gyaw)*R2D:>+12.4f}")
qgt = yaw_to_quat(gyaw)
qes = yaw_to_quat(fyaw)
print("\n 写成 nuScenes ego_pose.json 的样子：")
print(" {")
print(f' "timestamp": {int(data.timestamp * 1e6)},')
print(f' "translation": [{fx:.6f}, {fy:.6f}, 0.0],')
print(f' "rotation": [{qes[0]:.10f}, {qes[1]:.1f}, {qes[2]:.1f}, {qes[3]:.10f}]')
print(" }")
print(f"\n 真值四元数 [w,x,y,z] = "
f"[{qgt[0]:.6f}, 0.0, 0.0, {qgt[3]:.6f}]")
print(f" 解出四元数 [w,x,y,z] = "
f"[{qes[0]:.6f}, 0.0, 0.0, {qes[3]:.6f}]")
# 反解验证：和第一份文档里的算法完全一致
c, s_ = math.cos(fyaw), math.sin(fyaw)
head = np.array([c, s_])
print(f"\n 反解车头向量 R@[1,0] = [{head[0]:+.4f}, {head[1]:+.4f}] "
f"-> atan2 = {math.atan2(head[1], head[0])*R2D:.4f}°")
if args.save:
np.savez_compressed(args.save,
map_xy=map_xy, query_local=query,
gt=np.array([gx, gy, gyaw]),
est=np.array([fx, fy, fyaw]),
raw_local=local, intensity=inten)
print(f"\n 已保存 -> {args.save}")
print("=" * 76)
finally:
print("\n[cleanup] 恢复设置并销毁 actor ...")
try:
if lidar is not None:
lidar.stop()
lidar.destroy()
if vehicle is not None:
vehicle.destroy()
tm.set_synchronous_mode(False)
world.apply_settings(orig_settings)
except Exception as e:
print(" 清理时出错:", e)
if __name__ == "__main__":
main()执行结果打印：
(ad) leoking@leoking-desktop:~/carla$ python work/demo/carla_scan_matching.py --town Town10HD_Opt --frames 120 --perturb-yaw 8
[init] 加载地图 Town10HD_Opt ...
[init] 当前地图: Carla/Maps/Town10HD_Opt
[init] LiDAR: 32 线 / 600000 pps / 20Hz -> 约 30,000 点每帧
============================================================================
① 建图：用真值位姿把每帧点云拼到世界系
============================================================================
frame 30/120 累计 224,687 点 车位置 (-42.6, 137.0) yaw 0.0°
frame 60/120 累计 450,803 点 车位置 (-30.4, 137.0) yaw 0.3°
frame 90/120 累计 691,331 点 车位置 (-18.3, 137.1) yaw 0.4°
frame 120/120 累计 937,451 点 车位置 (-6.2, 137.2) yaw 0.4°
原始累计 937,451 点 -> 体素(0.15 m)降采样后 45,095 点 用时 2.8s
地图范围 x [-129.9, 69.5] y [57.7, 210.0]
============================================================================
② 待定位的这一帧点云
============================================================================
原始点数 26,669 -> 过滤后 8,000 (剔除地面)
CARLA 真值位姿 x=-2.2968 y=137.1973 yaw=0.3550°
--- 原始点云打印（传感器局部坐标系，前 12 个点）---
idx x y z inten range 方位角 俯仰角
0 -68.762 -19.374 12.597 0.748 72.541 -164.26° 10.00°
2424 16.470 16.636 2.514 0.910 23.544 45.29° 6.13°
4848 -7.414 -15.265 0.287 0.934 16.972 -115.91° 0.97°
7273 -4.851 -21.251 -1.105 0.916 21.826 -102.86° -2.90°
9697 -17.364 7.244 -1.806 0.927 18.901 157.36° -5.48°
12121 5.100 8.654 -1.655 0.960 10.180 59.49° -9.35°
14546 6.238 -4.418 -1.796 0.969 7.852 -35.31° -13.23°
16970 -3.966 -4.284 -1.795 0.976 6.107 -132.79° -17.10°
19394 -2.847 4.166 -1.804 0.979 5.359 124.35° -19.68°
21819 3.587 2.002 -1.790 0.982 4.481 29.17° -23.55°
24243 0.994 -3.313 -1.794 0.984 3.896 -73.30° -27.42°
26668 -0.585 0.004 -0.338 0.997 0.676 179.62° -30.00°
--- 距离分布 ---
7.9- 15.1 m | ################# 1097
15.1- 22.3 m | ###################################### 2473
22.3- 29.5 m | ##################################### 2387
29.5- 36.7 m | ############### 975
36.7- 43.9 m | ##### 300
43.9- 51.1 m | ### 171
51.1- 58.3 m | # 38
58.3- 65.5 m | # 59
65.5- 72.7 m | ### 187
72.7- 79.9 m | ##### 313
--- 高度分布（局部 z，看得出地面/墙面/树冠分层）---
-1.83- -0.26 m | ###################################### 20579
-0.26- 1.31 m | #### 2118
1.31- 2.88 m | #### 1953
2.88- 4.46 m | ### 1382
4.46- 6.03 m | 190
6.03- 7.60 m | 144
7.60- 9.17 m | 122
9.17- 10.74 m | 68
10.74- 12.32 m | 65
12.32- 13.89 m | 48
============================================================================
③ 固定真实 x,y，只扫 yaw —— 残差曲线
============================================================================
0.6803 | *
|*** *****
| ******* ****
| ***** ****
| **** ****
| **** ***
| *** ****
| ** ***
| ** **
| * **
| ** **
| ** **
| * *
| ** **
| ** **
0.0631 | *@*
+------------------------------------------------------------------
-14.65 15.35 候选 yaw (度)
残差最小的 yaw = 0.3601°
CARLA 真值 = 0.3550°
误差 = +0.0052°
谷底残差 0.0631 m 偏 5° 时 0.4347 m 偏 10° 时 0.5813 m
--- 用多少个点就够了？（每档重复 8 次随机抽样）---
点数 均值误差 标准差
10 +0.1019° 0.1517°
50 +0.0182° 0.0280°
200 +0.0036° 0.0104°
1,000 +0.0022° 0.0043°
5,000 +0.0039° 0.0029°
8,000 +0.0025° 0.0000°
--- 远点 vs 近点（杠杆效应）---
距离段 点数 角度误差
3-15 m 1,084 +0.0518°
15-30 m 4,972 -0.0150°
30-50 m 1,338 +0.0281°
50-80 m 606 +0.0006°
============================================================================
④ ICP：从错误初值同时解出 x, y, yaw
============================================================================
初值 x=-0.797 y=136.147 yaw=8.355° (位置偏 1.5m, 角度偏 +8.0°)
iter x y yaw(°) 残差(m) 内点
0 -0.831 136.400 8.0307 0.66668 6,510
1 -0.872 136.634 7.7115 0.62550 6,692
2 -0.909 136.819 7.4236 0.57344 6,790
3 -0.948 136.965 7.1356 0.54837 6,935
4 -0.980 137.069 6.8852 0.51536 6,984
5 -1.013 137.141 6.6618 0.49937 7,047
6 -1.045 137.194 6.4455 0.49035 7,095
7 -1.077 137.243 6.2260 0.49401 7,192
8 -1.102 137.289 6.0007 0.49044 7,250
9 -1.125 137.323 5.7724 0.48868 7,311
59 -2.280 137.199 0.3693 0.06060 7,969
（共 60 次迭代）
============================================================================
⑤ 与 CARLA 真值对比 —— 这就是 ego_pose 的生产过程
============================================================================
x (m) y (m) yaw (°)
真值 -2.2968 137.1973 0.3550
解出 -2.2801 137.1987 0.3693
误差 +0.0168 +0.0014 +0.0144
写成 nuScenes ego_pose.json 的样子：
{
"timestamp": 30834579,
"translation": [-2.280054, 137.198668, 0.0],
"rotation": [0.9999948056, 0.0, 0.0, 0.0032231498]
}
真值四元数 [w,x,y,z] = [0.999995, 0.0, 0.0, 0.003098]
解出四元数 [w,x,y,z] = [0.999995, 0.0, 0.0, 0.003223]
反解车头向量 R@[1,0] = [+1.0000, +0.0064] -> atan2 = 0.3693°
============================================================================
[cleanup] 恢复设置并销毁 actor ...
(ad) leoking@leoking-desktop:~/carla$ python work/demo/carla_scan_matching.py --keep-ground
[init] 当前地图: Carla/Maps/Town10HD_Opt
[init] LiDAR: 32 线 / 600000 pps / 20Hz -> 约 30,000 点每帧
============================================================================
① 建图：用真值位姿把每帧点云拼到世界系
============================================================================
frame 30/120 累计 706,993 点 车位置 (18.1, -64.4) yaw 180.0°
frame 60/120 累计 1,410,833 点 车位置 (6.0, -64.4) yaw -179.6°
frame 90/120 累计 2,116,132 点 车位置 (-5.7, -64.6) yaw -179.4°
frame 120/120 累计 2,822,205 点 车位置 (-15.4, -64.7) yaw -179.4°
原始累计 2,822,205 点 -> 体素(0.15 m)降采样后 196,289 点 用时 4.4s
地图范围 x [-95.2, 103.7] y [-144.0, -1.3]
============================================================================
② 待定位的这一帧点云
============================================================================
原始点数 26,845 -> 过滤后 23,566 (保留地面)
CARLA 真值位姿 x=-16.7769 y=-64.6692 yaw=-179.3987°
--- 原始点云打印（传感器局部坐标系，前 12 个点）---
idx x y z inten range 方位角 俯仰角
0 -76.856 -12.463 13.729 0.729 79.061 -170.79° 10.00°
2440 48.432 25.355 5.870 0.803 54.982 27.63° 6.13°
4880 -31.004 10.327 1.289 0.877 32.704 161.58° 2.26°
7321 -24.425 -21.571 -1.653 0.878 32.628 -138.55° -2.90°
9761 -4.637 6.231 -0.746 0.969 7.803 126.65° -5.48°
12201 8.953 6.251 -1.799 0.957 11.066 34.93° -9.35°
14642 3.900 -6.618 -1.805 0.969 7.891 -59.49° -13.23°
17082 -5.367 -2.324 -1.799 0.976 6.119 -156.59° -17.10°
19522 -1.125 4.928 -1.808 0.979 5.368 102.86° -19.68°
21963 4.068 0.772 -1.804 0.982 4.516 10.75° -23.55°
24403 0.429 -3.448 -1.802 0.984 3.914 -82.90° -27.42°
26844 -0.571 0.004 -0.330 0.997 0.660 179.62° -30.00°
--- 距离分布 ---
3.1- 10.8 m | ###################################### 11884
10.8- 18.4 m | ######### 2908
18.4- 26.1 m | ######## 2386
26.1- 33.8 m | ####### 2225
33.8- 41.5 m | ### 988
41.5- 49.2 m | ## 702
49.2- 56.9 m | ### 978
56.9- 64.6 m | ### 862
64.6- 72.3 m | # 330
72.3- 79.9 m | # 303
--- 高度分布（局部 z，看得出地面/墙面/树冠分层）---
-1.83- -0.26 m | ###################################### 20782
-0.26- 1.31 m | ### 1541
1.31- 2.88 m | ## 1329
2.88- 4.46 m | ## 1223
4.46- 6.03 m | ## 883
6.03- 7.60 m | # 442
7.60- 9.17 m | # 324
9.17- 10.74 m | 165
10.74- 12.32 m | 108
12.32- 13.89 m | 48
============================================================================
③ 固定真实 x,y，只扫 yaw —— 残差曲线
============================================================================
0.2207 | *
| ************************
| **
| ********* *
|****** ********* **
| *** **
| ** **
| *** *
| ** *
| *
| * *
| * *
| * *
| *
| * *
0.0766 | @
+------------------------------------------------------------------
-194.40 -164.40 候选 yaw (度)
残差最小的 yaw = -179.3997°
CARLA 真值 = -179.3987°
误差 = -0.0010°
谷底残差 0.0766 m 偏 5° 时 0.2156 m 偏 10° 时 0.2159 m
--- 用多少个点就够了？（每档重复 8 次随机抽样）---
点数 均值误差 标准差
10 +0.3319° 0.8637°
50 -0.0018° 0.0288°
200 +0.0005° 0.0195°
1,000 +0.0042° 0.0066°
5,000 +0.0013° 0.0023°
23,566 -0.0006° 0.0000°
--- 远点 vs 近点（杠杆效应）---
距离段 点数 角度误差
3-15 m 13,922 +0.0011°
15-30 m 4,353 -0.0130°
30-50 m 2,934 -0.0037°
50-80 m 2,357 +0.0019°
============================================================================
④ ICP：从错误初值同时解出 x, y, yaw
============================================================================
初值 x=-15.277 y=-65.719 yaw=-171.399° (位置偏 1.5m, 角度偏 +8.0°)
iter x y yaw(°) 残差(m) 内点
0 -15.269 -65.723 -171.4287 0.17448 22,175
1 -15.260 -65.728 -171.4538 0.17448 22,177
2 -15.252 -65.732 -171.4742 0.17484 22,182
3 -15.245 -65.737 -171.4947 0.17490 22,183
4 -15.236 -65.743 -171.5159 0.17444 22,178
5 -15.227 -65.748 -171.5368 0.17409 22,175
6 -15.218 -65.753 -171.5609 0.17411 22,177
7 -15.209 -65.758 -171.5878 0.17406 22,178
8 -15.200 -65.763 -171.6127 0.17402 22,179
9 -15.191 -65.767 -171.6374 0.17387 22,179
59 -14.582 -66.092 -174.7075 0.15836 22,538
（共 60 次迭代）
============================================================================
⑤ 与 CARLA 真值对比 —— 这就是 ego_pose 的生产过程
============================================================================
x (m) y (m) yaw (°)
真值 -16.7769 -64.6692 -179.3987
解出 -14.5818 -66.0922 -174.7075
误差 +2.1951 -1.4230 +4.6912
写成 nuScenes ego_pose.json 的样子：
{
"timestamp": 5167365121,
"translation": [-14.581813, -66.092202, 0.0],
"rotation": [0.0461689714, 0.0, 0.0, -0.9989336445]
}
真值四元数 [w,x,y,z] = [0.005247, 0.0, 0.0, -0.999986]
解出四元数 [w,x,y,z] = [0.046169, 0.0, 0.0, -0.998934]
反解车头向量 R@[1,0] = [-0.9957, -0.0922] -> atan2 = -174.7075°
============================================================================
[cleanup] 恢复设置并销毁 actor ...
(ad) leoking@leoking-desktop:~/carla$ python work/demo/carla_scan_matching.py --save scan.npz
[init] 当前地图: Carla/Maps/Town10HD_Opt
[init] LiDAR: 32 线 / 600000 pps / 20Hz -> 约 30,000 点每帧
============================================================================
① 建图：用真值位姿把每帧点云拼到世界系
============================================================================
frame 30/120 累计 239,643 点 车位置 (-105.7, -28.8) yaw -81.1°
frame 60/120 累计 470,130 点 车位置 (-102.1, -40.3) yaw -65.6°
frame 90/120 累计 697,181 点 车位置 (-95.5, -50.4) yaw -49.1°
frame 120/120 累计 924,452 点 车位置 (-86.6, -57.5) yaw -30.0°
原始累计 924,452 点 -> 体素(0.15 m)降采样后 26,556 点 用时 3.2s
地图范围 x [-176.7, -7.0] y [-127.8, 59.3]
============================================================================
② 待定位的这一帧点云
============================================================================
原始点数 27,006 -> 过滤后 7,523 (剔除地面)
CARLA 真值位姿 x=-84.5728 y=-58.5770 yaw=-29.2871°
--- 原始点云打印（传感器局部坐标系，前 12 个点）---
idx x y z inten range 方位角 俯仰角
0 -53.019 0.000 9.349 0.806 53.837 180.00° 10.00°
2455 22.243 -29.479 3.966 0.862 37.142 -52.96° 6.13°
4910 -6.912 31.245 1.262 0.880 32.025 102.47° 2.26°
7365 -48.184 12.193 -1.400 0.820 49.722 165.80° -1.61°
9820 0.977 17.148 -1.649 0.933 17.255 86.74° -5.48°
12275 10.827 -1.312 -1.797 0.957 11.053 -6.91° -9.35°
14730 -0.791 -7.590 -1.793 0.969 7.839 -95.95° -13.23°
17185 -6.401 0.300 -1.814 0.974 6.660 177.31° -15.81°
19640 0.520 4.993 -1.795 0.979 5.331 84.05° -19.68°
22095 4.153 -0.363 -1.817 0.982 4.548 -4.99° -23.55°
24550 -0.381 -3.437 -1.794 0.984 3.895 -96.33° -27.42°
27005 -0.583 0.004 -0.337 0.997 0.673 179.62° -30.00°
--- 距离分布 ---
7.9- 15.0 m | #### 247
15.0- 22.1 m | ########## 574
22.1- 29.3 m | #### 247
29.3- 36.4 m | ###################################### 2237
36.4- 43.5 m | ############################ 1655
43.5- 50.7 m | ################### 1106
50.7- 57.8 m | ######## 462
57.8- 65.0 m | ####### 431
65.0- 72.1 m | ####### 389
72.1- 79.2 m | ### 175
--- 高度分布（局部 z，看得出地面/墙面/树冠分层）---
-1.83- -0.26 m | ###################################### 20729
-0.26- 1.31 m | ### 1483
1.31- 2.88 m | ## 1318
2.88- 4.45 m | ## 1161
4.45- 6.02 m | ## 1131
6.02- 7.59 m | # 658
7.59- 9.16 m | # 287
9.16- 10.73 m | 124
10.73- 12.30 m | 78
12.30- 13.87 m | 37
============================================================================
③ 固定真实 x,y，只扫 yaw —— 残差曲线
============================================================================
0.8138 |*
|********** ************
| ******** ******
| **** ***
| ** ***
| ** ***
| ** **
| * **
| * *
| ** *
| ** *
| ** *
| * *
| * *
| ** *
0.0608 | *@*
+------------------------------------------------------------------
-44.29 -14.29 候选 yaw (度)
残差最小的 yaw = -29.2913°
CARLA 真值 = -29.2871°
误差 = -0.0042°
谷底残差 0.0608 m 偏 5° 时 0.6106 m 偏 10° 时 0.7656 m
--- 用多少个点就够了？（每档重复 8 次随机抽样）---
点数 均值误差 标准差
10 +0.0174° 0.1179°
50 -0.0069° 0.0306°
200 +0.0079° 0.0134°
1,000 +0.0033° 0.0056°
5,000 +0.0016° 0.0013°
7,523 +0.0017° 0.0000°
--- 远点 vs 近点（杠杆效应）---
距离段 点数 角度误差
3-15 m 247 -0.1609°
15-30 m 830 +0.0257°
30-50 m 4,947 -0.0011°
50-80 m 1,499 +0.0011°
============================================================================
④ ICP：从错误初值同时解出 x, y, yaw
============================================================================
初值 x=-83.073 y=-59.627 yaw=-21.287° (位置偏 1.5m, 角度偏 +8.0°)
iter x y yaw(°) 残差(m) 内点
0 -83.038 -59.605 -21.4190 0.68260 6,011
1 -83.013 -59.591 -21.5498 0.67442 6,016
2 -82.995 -59.573 -21.6860 0.67259 6,052
3 -82.994 -59.559 -21.8133 0.66212 6,057
4 -82.986 -59.543 -21.9358 0.65556 6,070
5 -82.981 -59.530 -22.0781 0.65427 6,103
6 -82.981 -59.513 -22.2221 0.65233 6,143
7 -82.984 -59.491 -22.3664 0.64630 6,166
8 -82.991 -59.480 -22.5117 0.64483 6,203
9 -83.003 -59.469 -22.6644 0.64060 6,237
59 -84.520 -58.640 -29.0930 0.07933 7,509
（共 60 次迭代）
============================================================================为者常成，行者常至
自由转载-非商用-非衍生-保持署名（创意共享3.0许可证）
