import numpy as np from scipy.spatial.transform import Rotation as R from scipy.linalg import svd import os import utils import config def rotation_matrix_to_rpy(R): # 确保输入是一个有效的旋转矩阵 if not np.allclose(np.dot(R, R.T), np.eye(3)) or not np.isclose(np.linalg.det(R), 1.0): raise ValueError("输入不是一个有效的旋转矩阵") _pitch = -np.arcsin(R[2, 0]) if np.abs(_pitch - np.pi / 2) < 1e-6: # 处理接近90度的情况 _yaw = 0 _roll = np.arctan2(R[0, 1], R[0, 2]) elif np.abs(_pitch + np.pi / 2) < 1e-6: # 处理接近-90度的情况 _yaw = 0 _roll = -np.arctan2(R[0, 1], R[0, 2]) else: _yaw = np.arctan2(R[1, 0], R[0, 0]) _roll = np.arctan2(R[2, 1], R[2, 2]) return _roll, _pitch, _yaw def average_rotation_matrices(rotation_matrices, weights=None): # 将旋转矩阵转换为四元数 quaternions = [R.from_matrix(matrix).as_quat() for matrix in rotation_matrices] if weights is None: # 如果没有提供权重，则使用等权重 weights = np.ones(len(quaternions)) / len(quaternions) else: # 确保权重之和为1 weights = np.array(weights) / np.sum(weights) # 对四元数进行加权平均 avg_quaternion = np.zeros(4) for quat, weight in zip(quaternions, weights): avg_quaternion += weight * quat # 归一化四元数以确保它是单位四元数 avg_quaternion /= np.linalg.norm(avg_quaternion) # 将平均后的四元数转换回旋转矩阵 avg_rotation_matrix = R.from_quat(avg_quaternion).as_matrix() return avg_rotation_matrix def addQ(q1, q2): return np.quaternion(q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w) def calc_RT(lidar, camera, MAX_ITERS, lidarToCamera): global iteration_counter, pkg_loc, translation_sum, rotation_sum, rotation_avg_by_mult, rmse_avg if iteration_counter == 0: with open(pkg_loc + "/log/avg_values.txt", "w") as clean_file: pass translation_sum = np.zeros((3, 1), dtype=float) rotation_sum = np.ones(4) rotation_avg_by_mult = np.eye(3, dtype=float) rmse_avg = np.ones(3) num_points = lidar.shape[1] print("Number of points:", num_points) mu_lidar = np.mean(lidar, axis=0) mu_camera = np.mean(camera, axis=0) if iteration_counter == 0: print("mu_lidar:\n", mu_lidar) print("mu_camera:\n", mu_camera) # lidar_centered = lidar - mu_lidar[:, np.newaxis] # camera_centered = camera - mu_camera[:, np.newaxis] lidar_centered = lidar - mu_lidar camera_centered = camera - mu_camera if iteration_counter == 0: print("lidar_centered:\n", lidar_centered) print("camera_centered:\n", camera_centered) # cov = camera_centered @ lidar_centered.T ERROR H = np.dot(lidar_centered.T, camera_centered) print(H) U, _, Vt = np.linalg.svd(H) # rotation = U @ Vt ERROR rotation = np.dot(Vt, U.T) if np.linalg.det(rotation) < 0: # rotation = U @ np.diag([1, 1, -1]) @ Vt rotation = np.dot(Vt, U.T) # translation = mu_camera - rotation @ mu_lidar translation = mu_camera.reshape(3, 1) - np.dot(rotation, mu_lidar.reshape(3, 1)) # averaging translation and rotation translation_sum += translation temp_q = R.from_matrix(rotation).as_quat() if iteration_counter == 0: rotation_sum = temp_q else: rotation_sum = rotation_sum + temp_q # averaging rotations by multiplication if iteration_counter == 0: rotation_avg_by_mult = rotation else: rotation_avg_by_mult = average_rotation_matrices([rotation_avg_by_mult, rotation]) # ea = R.from_matrix(rotation).as_euler('zyx') ea = R.from_matrix(rotation).as_euler('xyz', degrees=False) combine_ea = utils.combine_rpy(config.roll, config.pitch, config.yaw, ea[0], ea[1], ea[2]) print("Rotation matrix:\n", rotation) print("Rotation in Euler :\n", ea) print("Combin Rotation in Euler :\n", combine_ea) print("Translation:\n", translation) # eltwise_error = np.sum((camera - (rotation @ lidar + translation[:, np.newaxis])) ** 2, axis=0) ERROR eltwise_error = np.sum((camera - (np.dot(lidar, rotation) + translation[:, np.newaxis])) ** 2, axis=0) error = np.sqrt(np.mean(eltwise_error)) print("RMSE:", error) rmse_avg += error T = np.eye(4) T[:3, :3] = rotation T[:3, 3] = np.squeeze(translation) print("Rigid-body transformation:\n", T) iteration_counter += 1 if iteration_counter % 1 == 0: with open(pkg_loc + "/log/avg_values.txt", "a") as log_avg_values: print("--------------------------------------------------------------------") print(f"After {iteration_counter} iterations") print("--------------------------------------------------------------------") print("Average translation is:\n", translation_sum / iteration_counter) log_avg_values.write(f"{iteration_counter}\n") log_avg_values.write(str(translation_sum / iteration_counter) + "\n") # rotation_sum_normalized = np.array([getattr(rotation_sum, dim) for dim in ['x', 'y', 'z', 'w']]) / iteration_counter rotation_sum_normalized = rotation_sum / iteration_counter mag = np.linalg.norm(rotation_sum_normalized) rotation_sum_normalized /= mag rotation_avg = R.from_quat(rotation_sum_normalized).as_matrix() print("Average rotation is:\n", rotation_avg) # final_rotation = rotation_avg @ lidarToCamera ERROR final_rotation = np.dot(lidarToCamera, rotation_avg) final_angles = R.from_matrix(final_rotation).as_euler('xyz') combine_final_angles = utils.combine_rpy(config.roll, config.pitch, config.yaw, \ final_angles[0], final_angles[1], final_angles[2]) log_avg_values.write("{:.8f} {:.8f} {:.8f}\n".format(*rotation_avg[0])) log_avg_values.write("{:.8f} {:.8f} {:.8f}\n".format(*rotation_avg[1])) log_avg_values.write("{:.8f} {:.8f} {:.8f}\n".format(*rotation_avg[2])) T_avg = np.eye(4) T_avg[:3, :3] = rotation_avg T_avg[:3, 3] = np.squeeze(translation_sum / iteration_counter) print("Average transformation is:\n", T_avg) print("Final rotation is:\n", final_rotation) print("Final ypr is:\n", final_angles) print("Final Combine ypr is:\n", combine_final_angles) print(f"Average RMSE is: {rmse_avg / iteration_counter}") # eltwise_error_temp = np.sum((camera - (rotation_avg @ lidar + (translation_sum / iteration_counter)[:, np.newaxis])) ** 2, axis=0) eltwise_error_temp = np.sum((camera - (np.dot(lidar, rotation_avg) + (translation_sum / iteration_counter)[:, np.newaxis])) ** 2, axis=0) error_temp = np.sqrt(np.mean(eltwise_error_temp)) print(f"RMSE on average transformation is: {error_temp}") log_avg_values.write("{:.8f}\n".format(error_temp)) return T # Global variables initialization iteration_counter = 0 pkg_loc = "./record" translation_sum = np.zeros((3, 1), dtype=float) rotation_sum = np.ones(4) rotation_avg_by_mult = np.eye(3, dtype=float) rmse_avg = np.ones(3) if __name__ == "__main__": all_points = utils.read_from_json(os.path.dirname(__file__) + '\\record\\points2.json') # records = utils.read_from_json(os.path.dirname(__file__) + '\\record\\record.json') carmera = np.asarray(all_points[0]) lidar = np.asarray(all_points[1]) lidarToCamera = R.from_euler('xyz', [config.roll, config.pitch, config.yaw], degrees=False).as_matrix() calc_RT(lidar, carmera, 50, lidarToCamera)