orientation误差表示

目录1 Orientation误差最常见方法误差旋转Python实现2 Orientation RMSE3 位置 姿态一起计算SE(3)4 Python实现SE3误差5 机器人领域常见指标6 实践建议很重要总结orientation errororientation RMSESE(3)误差在机器人 / SLAM / 控制里orientation 误差通常不能像位置一样直接做减法因为旋转属于Lie Group(SO(3))而不是普通欧式空间。因此需要用旋转误差或李代数表示来计算。我分三部分讲1️⃣ orientation 误差怎么计算2️⃣ 能不能用 RMSE3️⃣ 如何把位置 orientation一起计算李代数 SE(3)1 Orientation误差最常见方法假设真实姿态R g t R_{gt}Rgt​预测姿态R p r e d R_{pred}Rpred​它们是旋转矩阵。误差旋转R e r r R g t T R p r e d R_{err} R_{gt}^{T} R_{pred}Rerr​RgtT​Rpred​旋转误差角θ arccos ⁡ ( trace ( R e r r ) − 1 2 ) \theta \arccos\left(\frac{\text{trace}(R_{err}) - 1}{2}\right)θarccos(2trace(Rerr​)−1​)这个角度 (\theta) 就是orientation error。单位通常degree 或 radianPython实现用SciPyfromscipy.spatial.transformimportRotationasRimportnumpyasnp R_gtR.from_quat(q_gt)R_predR.from_quat(q_pred)R_errR_gt.inv()*R_pred angle_errorR_err.magnitude()# radmagnitude()就是旋转角。2 Orientation RMSE如果有 N 个姿态R M S E r o t 1 N ∑ θ i 2 RMSE_{rot} \sqrt{\frac{1}{N} \sum \theta_i^2}RMSErot​N1​∑θi2​​Pythonrot_errors[]foriinrange(N):R_errR_gt[i].inv()*R_pred[i]rot_errors.append(R_err.magnitude())rmse_rotnp.sqrt(np.mean(np.square(rot_errors)))另一种计算思路dotnp.sum(q_gt*q_pred,axis1)angles2*np.arccos(np.clip(np.abs(dot),-1,1))rmsenp.sqrt(np.mean(angles**2))四元数q ( x , y , z , w ) q (x, y, z, w)q(x,y,z,w)第一行其实是在计算q g t ⋅ q p r e d x 1 x 2 y 1 y 2 z 1 z 2 w 1 w 2 q_{gt} \cdot q_{pred} x_1 x_2 y_1 y_2 z_1 z_2 w_1 w_2qgt​⋅qpred​x1​x2​y1​y2​z1​z2​w1​w2​即四元数点积。几何意义两个单位四元数的点积满足q 1 ⋅ q 2 c o s ( θ / 2 ) q_1 \cdot q_2 cos(\theta / 2)q1​⋅q2​cos(θ/2)其中θ \thetaθ是两个旋转之间的角度误差。第二行是在计算θ 2 a r c c o s ( ∣ q 1 ⋅ q 2 ∣ ) \theta 2arccos(|q_1 \cdot q_2|)θ2arccos(∣q1​⋅q2​∣)其中绝对值是因为单位四元数的双重表示即q qq和− q -q−q表示同一个旋转。因此加上绝对值np.abs(dot)以避免得到错误的角度。第三行是在计算R M S E 1 N ∑ i 1 N θ i 2 RMSE \sqrt { \frac{1}{N} \sum_{i1}^N \theta_i^2 }RMSEN1​i1∑N​θi2​​3 位置 姿态一起计算SE(3)机器人领域更标准的方法是使用Lie Algebra。姿态属于S E ( 3 ) SE(3)SE(3)误差定义T e r r T g t − 1 T p r e d T_{err} T_{gt}^{-1} T_{pred}Terr​Tgt−1​Tpred​然后取李代数 logξ log ⁡ ( T e r r ) \xi \log(T_{err})ξlog(Terr​)得到 6 维向量ξ [ ρ ϕ ] [ x , y , z , ω x , ω y , ω z ] \xi \begin{bmatrix} \rho \\ \phi \end{bmatrix} \begin{bmatrix} x,y,z,\omega_x,\omega_y,\omega_z \end{bmatrix}ξ[ρϕ​][x,y,z,ωx​,ωy​,ωz​​]其中部分含义ρ位置误差φ旋转误差然后计算∣ ∣ ξ ∣ ∣ ||\xi||∣∣ξ∣∣或R M S E 1 N ∑ ∣ ∣ ξ i ∣ ∣ 2 RMSE \sqrt{\frac{1}{N}\sum ||\xi_i||^2}RMSEN1​∑∣∣ξi​∣∣2​4 Python实现SE3误差importnumpyasnpfromscipy.spatial.transformimportRotationasRdefse3_error(T_gt,T_pred):T_errnp.linalg.inv(T_gt) T_pred R_errT_err[:3,:3]t_errT_err[:3,3]rotvecR.from_matrix(R_err).as_rotvec()xinp.concatenate([t_err,rotvec])returnxi计算 RMSEerrs[]foriinrange(N):xise3_error(T_gt[i],T_pred[i])errs.append(np.linalg.norm(xi))rmsenp.sqrt(np.mean(np.square(errs)))5 机器人领域常见指标指标用途ATE轨迹误差RPE相对位姿误差translation RMSE位置rotation error姿态SE(3) log error联合误差例如SLAM benchmarktrajectory evaluation6 实践建议很重要通常不会直接混合position 和 orientation RMSE因为单位不同position → meter rotation → radian所以一般translation RMSE (m) rotation RMSE (deg)分别报告。如果必须合并∣ ∣ ξ ∣ ∣ ∣ ∣ t ∣ ∣ 2 λ ∣ ∣ ϕ ∣ ∣ 2 ||\xi|| \sqrt{||t||^2 \lambda ||\phi||^2}∣∣ξ∣∣∣∣t∣∣2λ∣∣ϕ∣∣2​需要权重 λ。总结orientation errorθ angle ( R g t T R p r e d ) \theta \text{angle}(R_{gt}^{T} R_{pred})θangle(RgtT​Rpred​)orientation RMSER M S E r o t 1 N ∑ θ i 2 RMSE_{rot} \sqrt{\frac{1}{N}\sum \theta_i^2}RMSErot​N1​∑θi2​​SE(3)误差ξ log ⁡ ( T g t − 1 T p r e d ) \xi \log(T_{gt}^{-1}T_{pred})ξlog(Tgt−1​Tpred​)得到[x,y,z, wx,wy,wz]