# 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()