# 姿态到四元数转换说明

本文档说明如何将姿态表示（欧拉角与旋转向量 Rodrigues/Axis-Angle）转换为四元数 (qx, qy, qz, qw)，包含原理、公式、边界情况与一个完整的计算示例，并给出现有脚本 `src/scripts/euler_to_quaternion.py` 的使用方法。

## 基本约定
- 四元数记为 q = [x, y, z, w]，其中 w 为实部，(x, y, z) 为虚部（与 ROS、OpenCV 常见约定一致）。
- 旋转向量 r = (rx, ry, rz) 的方向为旋转轴，模长 |r| 为旋转角（弧度）。
- 欧拉角使用“内在旋转”约定，支持顺序 xyz（默认）与 zyx。

---

## 1) 旋转向量 (Rodrigues) → 四元数
给定旋转向量 r = (rx, ry, rz)：
1. 计算旋转角与单位旋转轴：
   - 角度：θ = |r| = sqrt(rx^2 + ry^2 + rz^2)
   - 轴：u = r / θ = (ux, uy, uz)
2. 由轴角到四元数：
   - s = sin(θ/2), c = cos(θ/2)
   - q = (ux·s, uy·s, uz·s, c)
3. 数值稳定性：若 θ 非常小（例如 < 1e-12），可近似认为 q ≈ (0, 0, 0, 1)。
4. 归一化：实际实现中会做一次单位化 q ← q / ||q||，以避免数值误差。

> 注：四元数 q 与 -q 表示同一旋转（双覆表示）。

### 示例（完整计算过程）
已知 r = (-1.433, 0.114, -0.430)（单位：rad）：
- θ = |r| ≈ 1.5004615956
- u = r / θ ≈ (-0.955039439, 0.075976620, -0.286578478)
- s = sin(θ/2) ≈ 0.6818076141
- c = cos(θ/2) ≈ 0.7315315286
- q = (ux·s, uy·s, uz·s, c)
    ≈ (-0.6511531610, 0.0518014378, -0.1953913882, 0.7315315286)

因此，对应四元数为：
- qx,qy,qz,qw ≈ -0.65115316, 0.05180144, -0.19539139, 0.73153153

---

## 2) 欧拉角 → 四元数
对于欧拉角 (rx, ry, rz)，若输入单位为弧度：
- 以 xyz 顺序为例（依次绕自身 x、y、z 轴）：
  1. 构造三个轴角四元数：
     - qx = (sin(rx/2), 0, 0, cos(rx/2))
     - qy = (0, sin(ry/2), 0, cos(ry/2))
     - qz = (0, 0, sin(rz/2), cos(rz/2))
  2. 乘法顺序为 q = qx ⊗ qy ⊗ qz（内在旋转，右乘积）。
- 对于 zyx 顺序：q = qz ⊗ qy ⊗ qx。
- 最后同样进行单位化。

> 提示：不同库/软件可能有“外在 vs 内在”、“左乘 vs 右乘”、“轴顺序”等差异，需确保与使用方约定一致。

---

## 3) 边界与常见问题
- 零角或极小角：直接返回单位四元数 (0,0,0,1) 或采用泰勒展开近似。
- 符号一致性：q 与 -q 表示同一旋转，批量处理时常将 w 约束为非负以保证连续性（可选）。
- 单位：所有角度必须使用弧度；若源数据是度，请先转弧度（乘以 π/180）。
- 数值稳定：建议在输出前做单位化，避免浮点累积误差。

---

## 4) 使用脚本批量/单次转换
脚本路径：`src/scripts/euler_to_quaternion.py`

- 单次旋转向量 → 四元数：
  ```bash
  python src/scripts/euler_to_quaternion.py --mode rotvec --single RX RY RZ
  # 例如：
  python src/scripts/euler_to_quaternion.py --mode rotvec --single -1.433 0.114 -0.430
  ```

- CSV 批量（默认欧拉角，若为旋转向量请加 --mode rotvec）：
  ```bash
  python src/scripts/euler_to_quaternion.py \
    --in input.csv --out output.csv \
    --rx rx --ry ry --rz rz \
    --mode rotvec
  ```

- 欧拉角（内在旋转顺序 xyz 或 zyx）：
  ```bash
  python src/scripts/euler_to_quaternion.py \
    --mode euler --order xyz --single RX RY RZ
  ```

> 如果输入 CSV 同时包含位姿中的位置列（x,y,z），脚本支持可选的单位转换 `--mm-to-m`（将毫米换算为米，并额外输出 x_m,y_m,z_m 列）。

---

## 5) 本次 12 组旋转向量的结果
以下 12 组 (rx, ry, rz)（单位：rad）对应的四元数 (qx, qy, qz, qw)：

1. -0.65115316,  0.05180144, -0.19539139, 0.73153153
2. -0.71991924, -0.00710155, -0.17753865, 0.67092912
3. -0.44521470, -0.25512818, -0.41269388, 0.75258039
4. -0.72304324,  0.09631938, -0.32264833, 0.60318248
5. -0.67311368,  0.06894426, -0.11793097, 0.72681287
6. -0.73524204, -0.19261515, -0.19652833, 0.61943132
7. -0.75500427, -0.08296268, -0.09788718, 0.64304265
8. -0.88627353, -0.08089799, -0.15658968, 0.42831579
9. -0.62408775, -0.13051614, -0.11718879, 0.76141106
10. -0.67818166, -0.10516535, -0.18696062, 0.70289090
11. -0.77275040, -0.19297175, -0.05741665, 0.60193194
12. -0.66493346,  0.09013744, -0.16351565, 0.72318833

---

## 6) 参考实现
脚本 `euler_to_quaternion.py` 中的核心函数：
- `rotvec_to_quaternion(rx, ry, rz)`：实现了第 1 节所述的 Rodrigues → 四元数转换，并在小角度与归一化方面做了稳健处理。
- `euler_to_quaternion(rx, ry, rz, order, degrees)`：实现了第 2 节所述的欧拉角 → 四元数转换，支持 xyz/zyx 两种顺序与度/弧度输入。

如需将结果保存为 CSV 或用于后续手眼标定、TF 发布等，可直接复用该脚本的命令行接口。

---
