| 6 | from scipy.stats import trim_mean |
| 7 | |
| 8 | def average_rotation_from_3x3n(rotations): |
| 9 | |
| 10 | N = len(rotations) |
| 11 | |
| 12 | # find global rotation from quaternion |
| 13 | |
| 14 | quaternions = np.array([Rot.from_matrix(rot).as_quat() for rot in rotations]) |
| 15 | # change sign to all positive for w |
| 16 | quaternions = np.array([q if q[3] > 0 else -q for q in quaternions]) |
| 17 | |
| 18 | # avg_quaternion = np.mean(quaternions, axis=0) |
| 19 | # avg_quaternion /= np.linalg.norm(avg_quaternion) |
| 20 | |
| 21 | # 初始化众数（从第一个四元数开始） |
| 22 | mode_quaternion = quaternions[0] |
| 23 | |
| 24 | # # 迭代寻找众数 |
| 25 | # for _ in range(10): # 设定最大迭代次数 |
| 26 | # distances = np.linalg.norm(quaternions - mode_quaternion, axis=1) # 计算每个四元数到 mode_quaternion 的欧氏距离 |
| 27 | # weights = np.exp(-distances**2) # 根据距离生成权重，高斯加权 |
| 28 | # mode_quaternion = np.average(quaternions, axis=0, weights=weights) # 加权平均 |
| 29 | # mode_quaternion /= np.linalg.norm(mode_quaternion) # 归一化为单位四元数 |
| 30 | |
| 31 | R_avg = Rot.from_quat(mode_quaternion).as_matrix() |
| 32 | |
| 33 | return R_avg |
| 34 | |
| 35 | def rad_error(R_est, R_gt): |
| 36 | cosvalue = np.trace(R_est @ R_gt.T) - 1 |