refactor: replace rotation vector with body up vector for tilt input

- Replace body_attitude() with body_up_vector(): rotate world-up [0,0,1]
  by corrected world→body quaternion to get body up vector (pitch/roll only,
  no yaw). Matches DiffPhysDrone's env.R[:, 2] approach.
- Update ComputeTilt transform to use body_up_vector_np
- Update visualize_dataset.py to display Euler angles and body up vector
- Update model.py comments and disable CNN (zero output)
- Sync AGENTS.md with new architecture description

Generated by Mistral Vibe (ds-v4-flash).
Co-Authored-By: Mistral Vibe <vibe@mistral.ai>
This commit is contained in:
2026-06-04 21:02:08 +08:00
parent 8e1a98207e
commit 0a504d648e
6 changed files with 218 additions and 36 deletions

View File

@@ -24,7 +24,7 @@ import torch
# Reuse the same coordinate transforms as the training pipeline
from src.velocity_prediction.utils import (
body_attitude_np,
body_up_vector_np,
quat_to_euler_np,
world_vel_to_body_np,
quat_normalize,
@@ -199,11 +199,11 @@ def draw_pose_overlay(
]
put_text(euler_lines, origin=(10, 62), color=(0, 200, 255))
# ── Body attitude (rotation vector) ──
tilt_lines = [
f"Att: rx={tilt[0]:+.3f} ry={tilt[1]:+.3f} rz={tilt[2]:+.3f}",
# ── Body up vector (pitch & roll only) ──
up_lines = [
f"Body up: ({tilt[0]:+.3f}, {tilt[1]:+.3f}, {tilt[2]:+.3f})",
]
put_text(tilt_lines, origin=(10, 104), color=(0, 200, 255))
put_text(up_lines, origin=(10, 104), color=(0, 200, 255))
# ── Body-frame velocity ──
v_right, v_forward = v_body # [v_right, v_forward]
@@ -334,8 +334,8 @@ def create_video(
for i, frame_data in enumerate(frames):
q_raw = frame_data["pose"][3:7] # [qx, qy, qz, qw] world→odom
# True body attitude rotation vector (preserves yaw)
tilt = body_attitude_np(q_raw) # (3,)
# Body up vector (pitch & roll only, no yaw) — matches DiffPhysDrone
body_up = body_up_vector_np(q_raw) # (3,) unit vector
# Euler angles from body quaternion for display
q_body = correct_attitude(q_raw)
@@ -349,7 +349,7 @@ def create_video(
canvas=frame_data["img"],
pose=frame_data["pose"],
vel=frame_data["vel"],
tilt=tilt,
tilt=body_up,
v_body=v_body,
euler=euler_deg,
frame_idx=i,