# compute an average transform between original camera attitude estimate # and optimized camera attitude estimate quats = [] for i, image in enumerate(proj.image_list): if image.name in group_list[0]: print(image.name) ned, ypr, q0 = image.get_camera_pose(opt=False) ned, ypr, q1 = image.get_camera_pose(opt=True) # rx = q1 * conj(q0) conj_q0 = transformations.quaternion_conjugate(q0) rx = transformations.quaternion_multiply(q1, conj_q0) rx /= transformations.vector_norm(rx) print(' ', rx) (yaw_rad, pitch_rad, roll_rad) = transformations.euler_from_quaternion(rx, 'rzyx') print('euler (ypr):', yaw_rad * r2d, pitch_rad * r2d, roll_rad * r2d) quats.append(rx) # https://github.com/christophhagen/averaging-quaternions/blob/master/averageQuaternions.py # # Q is a Nx4 numpy matrix and contains the quaternions to average in the rows. # The quaternions are arranged as (w,x,y,z), with w being the scalar # The result will be the average quaternion of the input. Note that the signs # of the output quaternion can be reversed, since q and -q describe the same orientation def averageQuaternions(Q): # Number of quaternions to average M = Q.shape[0] print('averaging # quats:', M)
my_mat_2 = quaternion_matrix(my_r_2) my_mat_2[0:3, 3] = my_t_2 my_mat_final = np.dot(my_mat, my_mat_2) my_r_final = copy.deepcopy(my_mat_final) my_r_final[0:3, 3] = 0 my_r_final = quaternion_from_matrix(my_r_final, True) my_t_final = np.array( [my_mat_final[0][3], my_mat_final[1][3], my_mat_final[2][3]]) my_pred = np.append(my_r_final, my_t_final) my_r = my_r_final my_t = my_t_final my_euler_form_r = euler_from_quaternion(my_r) print("my_euler_form_r", my_euler_form_r) #my_euler_form_r <class 'tuple'>: (0.9198490563735781, -0.007832527911272334, -0.47081842893943104) my_euler_yaw = list(my_euler_form_r)[2] my_rotation_matrix_from_euler = euler_matrix( my_euler_form_r[0], my_euler_form_r[1], my_euler_form_r[2]) # [[0.90679773 0.18969227 - 0.37647672 0.] # [-0.41995766 0.48440958 - 0.76745223 0.] # [0.03678917 0.85402822 0.51892423 0.] # [0. 0. 0. 1.]] x = np.dot(my_rotation_matrix_from_euler, np.array([[1, 0, 0, 1]]).transpose()).flatten() y = np.dot(my_rotation_matrix_from_euler, np.array([[0, 1, 0, 1]]).transpose()).flatten() z = np.dot(my_rotation_matrix_from_euler, np.array([[0, 0, 1, 1]]).transpose()).flatten()