How to convert Euler angles and altitude of drone camera to coordinates of image center?

Question

I'm trying to calculate the real world 3D-coordinate of the center pixel of image taken with a drone. I have the longitude, latitude, altitude, and Euler angles (yaw, pitch and roll) of the drone. The ground is assumed to be flat (sea level, since that's the absolute altitude of the drone).

For now I'm only looking at the center of the image, so the roll-angle might be unnecessary but I still have it for future use.

If possible I'm interested in the lon, lat, altitude (0) of the image center.

I've tried making a rotation matrix with the Euler angles and multiplying this with the vector between the drone and the ground. Then adding the result to the initial position vector of the drone and then adding to the length until it hits sea level. But this doesn't seem to give the desired result.

EDIT: I've been working on this problem and made some updates, but I still have not gotten it right. Im plotting my result on a map and comparing with known locations and I can tell my result is off. CODE:

func calculateImagePointCoordinate(v: GLKVector3, droneCoords: Coordinate, gimYaw: Float, gimPitch: Float, gimRoll: Float) -> Coordinate {
// v = GLKVector3 with image coordinates, example (0,0,altitude) for image center

    let radYaw      = degreesToRadians(gimYaw)
    let radPitch    = degreesToRadians(gimPitch)
    let radRoll     = degreesToRadians(gimRoll)

    let rotationMatrixRoll = GLKMatrix3Make(1, 0, 0,
                                            0, cosf(radRoll), sinf(radRoll),
                                            0, -sinf(radRoll), cosf(radRoll))

    let rotationMatrixPitch = GLKMatrix3Make(cosf(radPitch), 0, sinf(radPitch),
                                             0, 1, 0,
                                             -sinf(radPitch), 0, cosf(radPitch))

    let rotationMatrixYaw = GLKMatrix3Make(cosf(radYaw), sinf(radYaw), 0,
                                           -sinf(radYaw), cosf(radYaw), 0,
                                           0, 0, 1)

    let rollPitch = GLKMatrix3Multiply(rotationMatrixRoll, rotationMatrixPitch)
    let rollPitchYaw = GLKMatrix3Multiply(rollPitch, rotationMatrixYaw)

    let transpose = GLKMatrix3Transpose(rollPitchYaw)

    let rotatedVector   = GLKMatrix3MultiplyVector3(rollPitchYaw, v)

    let rotateBack = GLKMatrix3MultiplyVector3(transpose, rotatedVector)

    var coordinate = droneCoords.addDistance(inMeters: Double(rotateBack.x), withBearingInRadians: Double.pi / 2)
    coordinate = coordinate.addDistance(inMeters: Double(rotateBack.y), withBearingInRadians: Double.pi)

  return coordinate
}

I've tried to use the method described by Beta in this thread: How to convert Euler angles to directional vector? but I seem to misunderstanding something.

I'm not sure about what angle to use when adding the vector components in the end. And I'm guessing this might be my problem. My Coordinate class only uses long and lat, but do I have to somehow add the z component to make it correct? And at what angle should I add it in that case?

Answer 1

I think I have a solution, it's not properly tested yet but it seem to work for my small test set. Adding the vector components in the angle of the x and y-axis solved my problem

var coordinate = droneCoords.addDistance(inMeters: Double(rotateBack.x), withBearingInRadians: Double.pi)
coordinate = coordinate.addDistance(inMeters: Double(rotateBack.y), withBearingInRadians: Double.pi/2)