yaw pitch roll from quaternions

[Tom2L]

Hi all,
I'm still trying to recreate the capture track option of the plan mission in the metashape python API. I think i'm very close but i need to transform the quaternions data from the .path file to yaw pitch roll.
I'm able to capture an image of my model using the below code :

Code: [Select]

doc = Metashape.Document()
doc.open(path=r"C:\Users\Thomas\Documents\projet_x2.psx")
chunk = doc.chunk
position = Metashape.Vector((4.522160467, 50.68980499, 132.3963475)) # position vector X, Y, Z in chunk.crs 'WGS 84 + EGM96 height (EPSG::9707)'
orientation = Metashape.Vector((276.698,60.522,0)) # orientation vector Yaw, Pitch, Roll in chunk.crs

#calculate relevant transform matrix t
position = chunk.crs.unproject(position)  # position in ECEF
orientation = chunk.crs.geogcs.localframe(position).rotation().t() * Metashape.Utils.ypr2mat(orientation) # orientation matrix in ECEF
#orientation = chunk.crs.geogcs.localframe(position).rotation().t() * quaternion_rotation_matrix(0.3773313583,0.3340252627,0.5725150359,0.646741605) # orientation matrix in ECEF
transform = Metashape.Matrix.Translation(position) * Metashape.Matrix.Rotation(orientation)
transform = chunk.transform.matrix.inv() * transform * Metashape.Matrix.Diag((1, -1, -1, 1))
cameraT = Metashape.Matrix.Translation(transform.translation()) * Metashape.Matrix.Rotation(transform.rotation()) # 4x4 transform matrix 3 translations and 3 rotations

image = chunk.model.renderImage(cameraT, chunk.sensors[0].calibration)
image.save(r"C:\Users\Thomas\Documents\render.jpg")Unfortunately, the .path file contain only quaternions infos as qX, qY, qZ, qW.

In order to automate my process, i'm trying to convert the quaternions into yaw pitch roll, knowing that the metashape notation of those is [-yaw,pitch, roll] from traditional [roll, pitch,yaw] euler angles.
I try to do so with a custom def:

Code: [Select]

def euler_from_quaternion(x, y, z, w):
    """
    Convert a quaternion into euler angles (roll, pitch, yaw)
    roll is rotation around x in radians (counterclockwise)
    pitch is rotation around y in radians (counterclockwise)
    yaw is rotation around z in radians (counterclockwise)
    """
    t0 = +2.0 * (w * x + y * z)
    t1 = +1.0 - 2.0 * (x * x + y * y)
    roll_x = math.atan2(t0, t1)*(180/math.pi)

    t2 = +2.0 * (w * y - z * x)
    t2 = +1.0 if t2 > +1.0 else t2
    t2 = -1.0 if t2 < -1.0 else t2
    pitch_y = math.asin(t2)*(180/math.pi)

    t3 = +2.0 * (w * z + x * y)
    t4 = +1.0 - 2.0 * (y * y + z * z)
    yaw_z = math.atan2(t3, t4)*(180/math.pi)

    return -yaw_z, roll_x, pitch_y   # in degrees

print(euler_from_quaternion(0.3773313583,0.3340252627,0.5725150359,0.646741605)) # quaternions corresponding to (276.698,60.522,0)
Unfortunately, i do not manage to get the correct values : i end up with (-83.03244255204716, 60.5215387197955, 1.81473867143059e-05) from my custom function instead of (-276.968, 60.522, 0.00) that i get from the animation panel of the GUI metashape application.
I suppose that the pitch and roll values are correct (only approximation), but my yaw value seem false. So, how is yaw computed into the software ?
Is there maybe a way to convert quaternions data to world rotation matrix without converting it to yaw, pitch, roll ?
 

[Tom2L]

UPDATE:
I manage to make it work :
my custom function is :

Code: [Select]

# create function to transform quaternion into rotation matrix ---------------------------------------------------------
def euler_from_quaternion(x, y, z, w):
    """
    Convert a quaternion into euler angles (roll, pitch, yaw)
    roll is rotation around x in radians (counterclockwise)
    pitch is rotation around y in radians (counterclockwise)
    yaw is rotation around z in radians (counterclockwise)
    """
    t0 = +2.0 * (w * x + y * z)
    t1 = +1.0 - 2.0 * (x * x + y * y)
    roll_x = math.atan2(t0, t1)*(180/math.pi)

    t2 = +2.0 * (w * y - z * x)
    t2 = +1.0 if t2 > +1.0 else t2
    t2 = -1.0 if t2 < -1.0 else t2
    pitch_y = math.asin(t2)*(180/math.pi)

    t3 = +2.0 * (w * z + x * y)
    t4 = +1.0 - 2.0 * (y * y + z * z)
    yaw_z = math.atan2(t3, t4)*(180/math.pi)

    return -yaw_z, roll_x, -pitch_y   # in degrees in fact it's alpha nu kappa (weird)
and my code to capture image is :

Code: [Select]

doc = Metashape.Document()
doc.open(path=r"C:\Users\Thomas\Documents\projet_x2.psx")
chunk = doc.chunk
position = Metashape.Vector((4.522160467, 50.68980499, 132.3963475)) # position vector X, Y, Z in chunk.crs 'WGS 84 + EGM96 height (EPSG::9707)'
orientation = Metashape.Vector(euler_from_quaternion(0.3773313583,0.3340252627,0.5725150359,0.646741605)) # orientation vector Yaw, Pitch, Roll in chunk.crs
#calculate relevant transform matrix t
position = chunk.crs.unproject(position)  # position in ECEF
orientation = chunk.crs.geogcs.localframe(position).rotation().t() * Metashape.Utils.ypr2mat(orientation) # orientation matrix in ECEF
transform = Metashape.Matrix.Translation(position) * Metashape.Matrix.Rotation(orientation)
transform = chunk.transform.matrix.inv() * transform * Metashape.Matrix.Diag((1, -1, -1, 1))
cameraT = Metashape.Matrix.Translation(transform.translation()) * Metashape.Matrix.Rotation(transform.rotation()) # 4x4 transform matrix 3 translations and 3 rotations

image = chunk.model.renderImage(cameraT, chunk.sensors[0].calibration)
image.save(r"C:\Users\Thomas\Documents\render.jpg")

What is weird is that what the api call yaw pitch roll correspond to the alpha nu kappa of the GUI application.
Anyway, i get the same world transformation matrix in both case.
Hope it would help someone !