企业官网建设流程全解析

本篇是《机器人炼金术》01 期的完整版：包含直觉建立、完整推导、核心代码逐句解释、编写逻辑、四种实现方式的实测对比、以及位姿表示在工业界的全部典型应用场景。可独立阅读，无需参考任何其他文章。

一、问题定义：机器人每时每刻都在回答"我在哪、朝哪去"

机械臂末端在哪、朝向哪边；无人机在空中什么姿态；人形机器人躯干相对地面歪了多少——这些问题全是同一个形式：一个刚体相对另一个刚体的位姿（position + orientation，位置 + 朝向）。

位置好办，三个数 (x, y, z) 是全部。麻烦的是朝向。

朝向为什么难？因为"转"不是三个独立数字能老实描述的东西。你可以用三个欧拉角，但飞机俯仰 90° 时偏航和滚转突然变成同一个动作（万向锁）；你可以用四元数，四个数很优雅，但两个四元数"相加平均"得到的不是中间姿态。

本文要解决的具体问题是：给机器人系统选一种位姿表示，使得它 (a) 能无歧义地描述任意姿态 (b) 能被插值和求导 (c) 能进入优化器做数值更新 (d) 实现代价可控。这四个要求会淘汰掉大部分"看起来能用"的方案，下面逐一展开。

二、直觉层：9 个数，只有 3 个自由度

最诚实的出发点是旋转矩阵：一个 3×3 的正交矩阵，9 个数。它满足两个约束：

• RᵀR = I（列向量互相垂直且为单位长）—— 6 个独立方程
• det(R) = +1（不许把空间翻成镜像）—— 1 个方程

这些约束不独立，实际留给旋转的自由度是3。不必背这个减法，直接做实验：对旋转矩阵的 9 个元素做数值扰动，看哪些方向"动得了"。

# 实验代码（配套脚本 se3_demo.py 的验证 [1]） np.random.seed(0) Rm = exp_so3(np.array([0.3, -1.2, 2.0])) # 造一个合法旋转矩阵 eps = 1e-7 cols = [] for i in range(3): # 对 3 个旋转参数分别扰动 d = np.zeros(3); d[i] = eps cols.append((Rm @ exp_so3(d) - Rm @ exp_so3(-d)) / (2 * eps)) J = np.stack([c.reshape(-1) for c in cols], axis=1) # 9x3 数值雅可比 print(np.linalg.matrix_rank(J, tol=1e-6)) # 输出 3

逐句解释：

• 第 2 行：exp_so3把旋转向量变成矩阵（3.2 节详述），先造一个合法的旋转矩阵作为实验对象。
• 第 4~6 行：对 3 个旋转自由度分别加正负微小扰动，用中心差分算"矩阵 9 个元素随 3 个参数的变化率"。
• 第 7 行：把三个方向的变化率拼成 9×3 的雅可比矩阵。
• 第 8 行：矩阵的秩是 3——9 维空间里只有 3 个方向能动，其余 6 个方向一推就被正交性弹回来。

这个冗余不是浪费，是矩阵在替你守住"这是一个旋转"的承诺：随手写一个 9 维向量多半根本不是旋转；但随手写一个接近正交的矩阵，它仍然是（近似的）旋转。这个性质后面做优化时有大用。

左乘还是右乘：新手第一大坑

同样是"再绕 z 轴转 10°"，有两种完全不同的含义：

实测验证（配套脚本验证 [4]）：只加旋转扰动时，左乘会顺带改变平移（实测平移变化 [0.002462, 0.020649, 0]），右乘不会（[0, 0, 0]）。

记忆法：矩阵从右往左读，越靠右的动作发生在"更本体的"坐标系里。这不是数学趣味——它直接决定状态估计里"为什么 IMU 融合几乎总用右扰动"（扰动只该影响姿态，不该把已估计好的位置带偏），第五节场景 2 展开。

三、进阶层：SE(3) 与李代数

3.1 把位姿当成一个群

齐次变换矩阵把旋转和平移打包成 4×4：T 的上 3×3 块是旋转 R、右列是平移 t、底行 [0 0 0 1]，记作 T ∈ SE(3)。

SE(3) 是一个李群：既是群（可乘可逆），又是光滑流形（可求导、可优化）。拆开讲：

• 群：两个位姿复合还是位姿（T₁T₂），逆还是位姿（T⁻¹）。
• 流形：位姿的集合不是平地，是弯曲曲面。在曲面上做"线性插值""加法更新"都会滑出去——除非沿曲面的切空间走。

李代数 se(3) 就是 SE(3) 在单位元处的切空间，exp 映射负责从切空间"走回"曲面，log 反之。工程上要记三条铁律：

铁律 1：指数映射有闭式解（Rodrigues 公式），几行代码搞定，这就是机器人库从不存欧拉角的原因——存旋转向量或矩阵，更新时一步 exp 就回曲面。

铁律 2：log 在接近 π 时病态。实测：正常角度 exp→log 往返误差 ~1e-15，接近 π 时精度骤降且有符号翻转（+π 与 −π 是同一旋转）。优化器姿态突然跳变 180°，十有八九撞上了这里（3.6 节完整复盘）。

铁律 3：参数残差 ≠ 位姿残差。实测验证：|φ| 较大时 log(exp(ξ)) 的参数值可能与 ξ 不同，但回代后的位姿完全一致。优化时残差必须定义在位姿或李代数差上，不是原始参数向量上——这是自研优化代码的高频翻车点。

3.2 核心代码逐句解释

exp_so3：从旋转向量到旋转矩阵（Rodrigues 公式）

def exp_so3(w): # (1) theta = np.linalg.norm(w) # (2) if theta < 1e-12: # (3) return np.eye(3) + hat(w) # (3a) k = w / theta # (4) K = hat(k) # (5) return (np.eye(3) # (6) + np.sin(theta) * K # (6a) + (1 - np.cos(theta)) * (K @ K)) # (6b)

• (1)输入 w 是三维旋转向量：方向=旋转轴，长度=旋转角（rad），即李代数元素 [φ]× 的向量形式。
• (2)取模长得旋转角 θ=‖φ‖，旋转的"大小"与"轴"从这里分离。
• (3)小角度保护：exp 级数 I+[φ]×+½[φ]ײ+… 在 θ→0 时只剩一阶项 (3a)。不保护则 (4) 除零，且噪声搅乱结果。阈值 1e-12 下近似误差 ~1e-24，完全可忽略。
• (4)单位轴 k = w/θ。
• (5)hat 把向量变反对称矩阵 [k]×，让"叉积"变"矩阵乘法"：[k]×v = k×v。全部推导的枢纽。
• (6)Rodrigues 公式：exp(θ[k]×) = I + sinθ·[k]× + (1−cosθ)·[k]ײ。注意这不是级数截断，是精确闭式解——反对称矩阵幂满足 K³=−K，级数每一项都归并进这三项。这也是机器人学库普遍用它而不用通用矩阵指数的原因：快一个数量级，且没有特征值分解的数值副作用。

exp_se3：从 twist 到 4×4 位姿

def exp_se3(xi): # (1) rho, phi = xi[:3], xi[3:] # (2) theta = np.linalg.norm(phi) # (3) J = (np.eye(3) # (4) + (1 - np.cos(theta)) / theta**2 * K # (4a) + (theta - np.sin(theta)) / theta**3 * (K @ K)) # (4b) T[:3, :3] = exp_so3(phi) # (5) T[:3, 3] = J @ rho # (6) return T

• (1)输入 xi 是 twist（六维速度）：前 3 维平移 ρ，后 3 维旋转 φ。顺序约定坑：GTSAM/本代码平移在前，Sophus 旋转在前，对接外部库先核对此事，错序是高频 bug。
• (3)旋转部分模长 θ 仍是总旋转角。
• (4)矩阵 J 是 SE(3) 左雅可比 J_l(θ)，见 (4a)(4b)：回答"绕固定轴转 θ 的过程中平移积累多少"。系数来自对 ∫₀¹ exp(s[φ]×)ds 逐项积分——平移不是把 ρ 抄进第四列，因为旋转和平移同时发生（完整推导见 3.5）。
• (5)旋转块复用 exp_so3。
• (6)平移块 = J_l(θ)ρ。直觉：ρ 是"随旋转发生的直线运动"，J 把它"扭"成螺旋线。SE(3) 元素的几何本质是螺旋运动（Chasles 定理：任何刚体位移 = 一次螺旋）。

3.3 编写逻辑：这两个函数面对的约束

写这两个函数时的决策过程，比函数本身更值得学：

1. 约束 a：零重依赖。目标读者 clone 即跑，所以只允许 numpy——这直接排除了"直接调 Sophus/GTSAM"的省事路线。
2. 约束 b：教学透明。每行必须能对应到公式，出问题逐行可查——这排除了"用 scipy 一行解决"的路线（对比见 3.4）。
3. 小角度分支不是可选项。第一版没有它，θ=1e-13 的测试直接除零崩溃。数值代码的边界分支永远不是"防御性编程的装饰"，是正确性的一部分。
4. 输出约定：SE(3) 用 4×4 矩阵而非 (R,t) 元组，因为下游的复合运算 T₁T₂ 和求逆 T⁻¹ 对矩阵是原生的，对元组要拆装——数据结构选型跟着最高频操作走。

3.4 四种实现方式实测对比

同一功能"旋转向量 → 旋转矩阵"，四种写法的实测数据（200 个随机旋转向量，角度 0~3 rad，scipy 1.17 / numpy 2.3 / Python 3.11）：

Python 层耗时被解释器开销主导，四种差距不大；C++ 环境里差距拉大一个数量级*——Rodrigues 是十几条浮点指令，expm 是特征分解级别的重活。1 kHz 控制环每周期几十次变换，C++ 里这是真实成本。

每种实现的区别与边界：

• ② 为什么崩：矩阵指数级数 Σ Kⁿ/n! 只在 ‖K‖ 小时收敛快。反对称阵的范数就是旋转角，θ>π 时 8 项远不够，误差按 θⁿ/n! 爆炸——实测 2.3e3 的误差就是这个来源。所有正规库都用闭式解或帕德逼近，没人用裸级数。
• ③ vs ①：expm 内部走帕德逼近+缩放平方，为一般矩阵设计；对反对称阵这些 machinery 全是浪费。但遇到非反对称的一般矩阵指数（如时变线性化系统的 exp(At)），③ 是正确工具。工具没有高下，只有场景。
• ④ vs ①：性能无明显差异（Python 层），但④久经测试、双覆盖/万向锁边界都处理过。生产用库，教学手写一遍不可省——只会调库的人读不懂 VIO 源码，也写不出流形优化。

最终选用及理由：本文手写（①）服务于零依赖+教学透明的目标；从下一期起涉及批量变换时切换到④，并重跑对比表。

3.5 完整推导：exp_se3 里的 J 矩阵从哪来

免费渠道通常只给公式，这里给出完整推导链，让你能自己重推。

出发点：twist ξ = [ρ; φ] 对应矩阵 [ξ]∧（上块 [φ]× 和 ρ，底行 0）。设机器人从原点出发以恒定 body 速度 ξ 运动，位姿满足微分方程Ṫ = T·[ξ]∧，初值 T(0)=I。拆成旋转和平移两个方程：

• Ṙ = R·[φ]×：标准 SO(3) 指数方程，解 R = exp(θ[k]×)（Rodrigues）。
• ṫ = R(t)·ρ：把 R 代入后对时间积分。用变量代换 s∈[0,1]（body 速度恒定时旋转按比例累积）：

t = ∫₀¹ exp(s[φ]×)·ρ ds

把被积函数按级数展开并逐项积分：

t = Σₙ [φ]×ⁿ/(n+1)! · ρ

利用反对称矩阵幂循环 K³=−K，级数按 n mod 3 归并成三类，归并需要两个三角恒等式（可用 e^(iθ) 实部/虚部验证）：

• sinθ/θ = Σ (−1)ⁿθ²ⁿ/(2n+1)!
• (1−cosθ)/θ² = Σ (−1)ⁿθ²ⁿ/(2n+2)!

最终归并出J_l(θ) = I + (1−cosθ)/θ²·[φ]× + (θ−sinθ)/θ³·[φ]ײ——正是代码 (4a)(4b) 两行。

三个常见错法（重点）：

1. 把 [φ]× 当单位轴矩阵用：公式里的 K 含模长 θ。若先归一化了轴，系数必须换成等价变体。两种写法都对，混用必错。
2. θ²、θ³ 直接除：θ→0 时爆炸，必须走小角度分支（J_l → I + ½[φ]×）。
3. J_l 和 J_r 混用：J_r(θ) = J_l(−θ)。积分定义（速度在 body 系）出 J_l；世界系定义速度要用 J_r。错用的症状：小角度都对，大角度位置越走越歪。

3.6 log 的边界病态：一次真实的实现翻车

理论说完，看一次真实的数值事故——它揭示的方法论比结论更值钱。

第一版实现（教科书写法）：θ = arccos((tr(R)−1)/2)，轴由 vee(R−Rᵀ) = 2sinθ·k 恢复，接近 π 时特判走 R+I = 2kkᵀ 秩 1 分支。写完跑边界压力测试（θ∈[π−1e-6, π) 采样 5 万例）：

最差位姿回代误差: 0.0036 rad （预期 ~1e-9）

误差超预期7 个数量级。抓最差样本看中间量：

输入 θ = π - 1e-6（落在普通分支，未触发特判） cosθ 真值: -0.9999999999995001 矩阵算出: -0.9999999999994993 ← 差 8e-16（正常舍入） log 返回: 模长 3.1390（应为 3.14159，差 2.6e-3）

定位误差放大链条：

1. arccos 在 x→−1 处导数发散：|d arccos/dx| = 1/√(1−x²) = 1/sinθ。本例 sinθ≈10⁻⁶，角度误差 ≈ 8×10⁻¹⁶/10⁻⁶ = 8×10⁻¹⁰ rad。
2. 轴恢复系数 θ/(2sinθ) 的分子分母都用了病态的 θ：δθ 传导给 sinθ 一个 cosθ·δθ 的误差，相对 sinθ=10⁻⁶ 就是8×10⁻⁴ 的相对误差——正是实测量级。

教训：误差不在 arccos 一步产生，而是在"病态量的下游每一步"被复利。修分支阈值治标不治本（阈值放宽到 1e-3，θ=π−1e-4 时同样的戏码重演，只是误差小一点）。

根治：换全程良态的参数化路径（Shepperd 四元数方法）

tr = np.trace(Rm) if tr > 0: # (S1) s = 2.0 * np.sqrt(tr + 1.0) # s = 4*qw qw = 0.25 * s qx = (Rm[2,1] - Rm[1,2]) / s qy = (Rm[0,2] - Rm[2,0]) / s qz = (Rm[1,0] - Rm[0,1]) / s else: # (S2) i = int(np.argmax(np.diag(Rm))) # 选最大对角元分支 j, k = (i+1) % 3, (i+2) % 3 s = 2.0 * np.sqrt(1.0 + Rm[i,i] - Rm[j,j] - Rm[k,k]) # s = 4*q_i qw = (Rm[k,j] - Rm[j,k]) / s q[i] = 0.25 * s q[j] = (Rm[j,i] + Rm[i,j]) / s q[k] = (Rm[k,i] + Rm[i,k]) / s qv = np.array([qx, qy, qz]) theta = 2.0 * np.arctan2(np.linalg.norm(qv), abs(qw)) # (S3) axis = qv / np.linalg.norm(qv) * (1.0 if qw >= 0 else -1.0) # (S4)

• (S1)tr(R) = 4qw²−1，trace>0 意味着 |qw|>1/2，走 qw 主导分支：分母离零最远。各 qᵢ 由反对称差 R₂₁−R₁₂ = 4qw·qx 类公式恢复——分子小，但分母更大，商良态。
• (S2)trace≤0 时 |qw|≤1/2，改从三个 qᵢ 里选最大的做分母（4qᵢ² = 1+Rᵢᵢ−Rⱼⱼ−Rₖₖ，选最大 Rᵢᵢ 保证根号内不出现危险的小数相减）。Shepperd 方法的核心：永远用最大的四元数分量当除数。
• (S3)half-angle 公式 θ = 2·atan2(‖qv‖, |qw|)。arctan2 全定义域条件数良好，且 qv = sin(θ/2)·k 直接编码"半角正弦×轴"，不经过任何病态中间量。
• (S4)四元数双覆盖：q 与 −q 是同一旋转。若 qw<0，这个 q 表示"绕 −k 转 θ"，轴要翻回来，否则 θ∈[0,π] 配错误轴向会给出镜像旋转。

修复后实测：

[boundary] 20000 例 θ∈[π-0.05,π)：位姿回代 100% 通过 [normal-zone] 20000 例正常区：往返最大误差 1.26e-15（旧实现 1.38e-11） [sign-flip] θ=π-1e-7 时 log 返回 [3.141593, 0, 0] —— ±π 双值，位姿等价

正常区精度也提升 4 个数量级——良态化不是只修边界，是全区间受益。

剩下的双值性是数学给的：±π 处 k 与 −k 表示同一旋转（SO(3) 基本群是 ℤ₂），任何实现都消不掉。工程后果三条：优化器符号跳变（loss 周期性尖刺，尖刺间隔=旋转累计到 π 的时刻）、多旋转平均要先统一半球（否则接近 π 的旋转正负抵消，均值趋零）、slerp 不处理双覆盖会走远弧（相机从朝前到朝后，动画绕远路转 350° 而不是 10°）。

四、三种位姿表示的完整对比

实测（验证 [5]）：绕倾斜轴从单位阵转到 90°，比较"取一半"时各方法偏离真实半程姿态的角度——矩阵线性混合(SVD投影后) 0.000°，欧拉角插值 10.660°，四元数 nlerp 0.000°。欧拉角误差按 1−cos(θ/2) 增长，多轴耦合时任何涉及"平滑转向"的功能（云台、动画、轨迹插值）必须走四元数或流形插值。

五、应用场景：位姿表示具体解决什么问题

以下五个场景按"概念→具体问题→现象→根因→解法→验证"完整展开。

场景 1：ROS/tf2 坐标变换——"到位但姿态歪 10°"

• 具体问题：机械臂要抓零件，规划器输出的末端位姿转换到基座标系后总是歪。
• 现象：位置准确（误差 <1mm），姿态稳定偏 10° 左右。
• 根因：链路上两个环节的欧拉角顺序约定不同（一个 ZYX 一个 XYZ），或左乘右乘用反。顺序错 → 绕中间轴的旋转顺序颠倒 → 特定姿态下误差恰好是"轴交换"的量级。
• 解法：全链路统一为四元数或矩阵传递；打印中间位姿时附上 det(R) 和 ‖RᵀR−I‖ 做健康检查（不是 1 和 0 说明上游把矩阵当普通数组操作了，误差会随复合指数放大）。
• 验证：用一个已知姿态（如绕 z 转 90°）走完整条变换链，输出应逐位复现。

场景 2：IMU 姿态融合——为什么用右扰动

• 具体问题：IMU 以 200Hz 输出角速度增量（李代数侧），融合器维护全局姿态（李群侧），每次更新用 δφ 修正。
• 具体解决的是什么：更新量应该只影响姿态。若用左扰动 R←exp(δφ)R（绕世界轴转），位置平移分量也会被带偏——实测左乘引起平移变化 0.02m 而右乘为 0。
• 现象对比：左扰动实现的融合器，静止时位置估计漂移比右扰动版本大数倍，且漂移方向与旋转相关。
• 解法：误差状态卡尔曼滤波（ESKF）的标准做法：R ← R·exp(δφ)，δφ 作为误差状态。这是 VIO/SLAM 库的事实标准。

场景 3：优化器里的流形更新——"越优化越远"

• 具体问题：手写位姿图优化，把位姿存成 9 维向量用普通高斯牛顿更新。
• 现象：前几次迭代 loss 下降，之后开始震荡甚至发散；检查发现矩阵不再正交（det 偏离 1）。
• 根因：加法更新把估计值推出了流形——在曲面上做线性外推必然滑出去。
• 解法：位姿存矩阵，更新量存李代数：R ← R·exp(δξ)，δξ∈R⁶ 是优化变量。Ceres/g2o/GTSAM 的流形优化全是这个结构。
• 验证：每次迭代后断言 ‖RᵀR−I‖ < 1e-10。

场景 4：轨迹插值——云台运动绕远路

• 具体问题：相机云台从朝前平滑转到朝后。
• 现象：用欧拉角线性插值，云台绕远路转 350°；或路径抖动。
• 根因：欧拉角不是流形上的坐标，其线性组合没有几何意义；多轴耦合时中间姿态偏离大圆路径（实测单轴就有 10.66° 偏差）。
• 解法：四元数 slerp（处理双覆盖：q₀·q₁<0 时翻转 q₁），或李代数插值 exp(log(R₁ᵀR₂)/2)·R₁。
• 验证：中间帧用 arccos(tr(Rᵀ·R_target)/2) 检查角度应单调。

场景 5：手眼标定——AX=XB 的结构从哪来

• 具体问题：求相机相对机械臂法兰的安装位姿 X。
• 现象：标定残差大、每次标定结果差异大。
• 根因：AX=XB 里 A、B、X 全是 SE(3) 元素，朴素地把 12 个数当未知量线性求解，忽略了 X 的流形结构，噪声敏感。
• 解法：在李代数上求解（对数线性化）或先用四元数解旋转部分再解平移；多组数据平均时必须先统一半球（见 3.6 双值性）。
• 验证：标定后用新姿态做重投影，残差应 <2px。这是番外案例 A 的主题，届时完整实操。

六、可复现代码清单

全部仅依赖 numpy/scipy，实测输出已写入本文各节。

七、小结与下一篇

• 位姿表示的选型逻辑：交互看欧拉角、运行看四元数、优化看流形，三者用 exp/log 打通
• 数值代码的方法论：病态 → 实测误差量级 → 抓最差样本 → 定位放大链条 → 换参数化路径 → 全区间回归
• 下一篇（02 期）：《李群与李代数：为什么工程师必须懂 so(3) 的指数映射》——扰动模型的雅可比、J_l⁻¹ 闭式解、在 Ceres 残差块里的具体写法

附录代码

""" 文 01 付费版配套：log_so3 在 θ→π 边界的压力测试 依赖：numpy 运行：python3 se3_boundary_test.py """ import numpy as np np.set_printoptions(precision=6, suppress=True) def hat(w): return np.array([[0.0, -w[2], w[1]], [w[2], 0.0, -w[0]], [-w[1], w[0], 0.0]]) def vee(W): return np.array([W[2, 1], W[0, 2], W[1, 0]]) def exp_so3(w): theta = np.linalg.norm(w) if theta < 1e-12: return np.eye(3) + hat(w) k = w / theta K = hat(k) return np.eye(3) + np.sin(theta) * K + (1 - np.cos(theta)) * (K @ K) def log_so3(Rm): """Shepperd 四元数路径（与主文件一致），全程良态。""" tr = np.trace(Rm) if tr > 0: s = 2.0 * np.sqrt(tr + 1.0) qw = 0.25 * s qx = (Rm[2, 1] - Rm[1, 2]) / s qy = (Rm[0, 2] - Rm[2, 0]) / s qz = (Rm[1, 0] - Rm[0, 1]) / s else: i = int(np.argmax(np.diag(Rm))) j, k = (i + 1) % 3, (i + 2) % 3 s = 2.0 * np.sqrt(1.0 + Rm[i, i] - Rm[j, j] - Rm[k, k]) qw = (Rm[k, j] - Rm[j, k]) / s q = [0.0, 0.0, 0.0] q[i] = 0.25 * s q[j] = (Rm[j, i] + Rm[i, j]) / s q[k] = (Rm[k, i] + Rm[i, k]) / s qx, qy, qz = q qv = np.array([qx, qy, qz]) n = np.linalg.norm(qv) if n < 1e-12: return 2.0 * qv theta = 2.0 * np.arctan2(n, abs(qw)) axis = qv / n * (1.0 if qw >= 0 else -1.0) return theta * axis def test_boundary(n=20000): """θ ∈ [π-0.05, π) 随机采样，判据是位姿回代一致（容忍双值）""" rng = np.random.default_rng(42) fails = [] param_flips = 0 for _ in range(n): w = rng.normal(size=3) w = w / np.linalg.norm(w) * rng.uniform(np.pi - 0.05, np.pi) out = log_so3(exp_so3(w)) if not np.allclose(exp_so3(out), exp_so3(w), atol=1e-9): fails.append((w, out)) # 参数级双值：out 与 w 方向相反也算合法（位姿相同） if out @ w < 0 and np.allclose(exp_so3(out), exp_so3(w), atol=1e-9): param_flips += 1 assert not fails, f"{len(fails)} 例位姿回代失败" print(f"[boundary] {n} 例边界采样：位姿回代 100% 通过；" f"其中 {param_flips} 例参数符号翻转（±π 双值，合法）") def test_sign_flip(): """双值性直接验证""" w = np.array([np.pi - 1e-7, 0.0, 0.0]) out = log_so3(exp_so3(w)) assert abs(abs(out[0]) - np.pi) < 1e-4, out assert np.allclose(exp_so3(out), exp_so3(w), atol=1e-9) print(f"[sign-flip] θ=π-1e-7 时 log 返回 {out} —— ±π 双值，位姿等价") def test_perturbation_free_zone(): """对照：远离边界的正常区，参数回代应严格一致""" rng = np.random.default_rng(7) worst = 0.0 for _ in range(20000): w = rng.normal(size=3) w = w / np.linalg.norm(w) * rng.uniform(0, np.pi - 0.1) worst = max(worst, np.linalg.norm(log_so3(exp_so3(w)) - w)) print(f"[normal-zone] 正常区 20000 例参数往返最大误差 {worst:.2e}（应 ~1e-11 量级）") def bench(): """边界分支的正常分支性能对照""" import timeit ws = [] rng = np.random.default_rng(1) for _ in range(2000): w = rng.normal(size=3) ws.append(w / np.linalg.norm(w) * rng.uniform(0, np.pi)) t = timeit.timeit(lambda: [log_so3(exp_so3(w)) for w in ws], number=50) / (50 * len(ws)) print(f"[bench] exp+log 单次往返平均 {t*1e6:.2f} µs") if __name__ == "__main__": test_boundary() test_sign_flip() test_perturbation_free_zone() bench()