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:
0
visualize/__init__.py
Normal file
0
visualize/__init__.py
Normal 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,
|
||||
|
||||
Reference in New Issue
Block a user