使用四元数投影无人机的图像点

Question

给出坐标（lon，lat），UAV的高度和相机的欧拉角（偏航，俯仰，横滚），如何使用四元数计算照片中点的投影？

这是我的功能（弧度角）：

let alphaX = 2 * atan(36 / (2.0 * focalLength)
let alphaY = 2 * atan(24 / (2.0 * focalLength)
let dx = altitude * tan(alphaX / 2.0) 
let dy = altitude * tan(alphaY / 2.0)
let v = 
GLKVector3Make(dx, dy, altitude) // for top right corner


func quaternionMethod(v: GLKVector3, droneCoordinate: Coordinate, yaw: Float, pitch: Float, roll: Float) -> Coordinate {

    let axisQuaternion  = GLKQuaternionMakeWithVector3(v, 0)

    let cy = cos(yaw / 2)
    let cp = cos(pitch / 2)
    let cr = cos(roll / 2)
    let sy = sin(yaw / 2)
    let sp = sin(pitch / 2)
    let sr = sin(roll / 2)

    let eulerQuaternion = GLKQuaternionMake(cr*cp*cy+sr*sp*sy,
                                            sr*cp*cy+cr*sp*sy,
                                            cr*sp*cy+sr*cp*sy,
                                            cr*cp*sy+sr*sp*cy)

    let eulerInvers = GLKQuaternionInvert(eulerQuaternion)

    let r = GLKQuaternionMultiply(eulerQuaternion, axisQuaternion)
    let rotatedQuaternion = GLKQuaternionMultiply(r, eulerInvers)

    let rotatedVector = GLKQuaternionAxis(rotatedQuaternion)

    let rotated = GLKVector3Multiply(rotatedVector, v)

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

    return coordinate
}

我用一架无人机拍摄了一张照片，它的高度大约为100 m，俯仰角为水平方向向下40度，偏航角度为北偏-88度，位于图像中间的地面控制点。使用GCP在地图上标记。

在绘制图像的拐角点时，我的预期结果应该是这样的：

相反，使用上述函数的结果如下所示：

我怀疑旋转后将矢量分量添加到初始无人机坐标的方式有问题。我不确定要使用什么轴承。我怀疑我也应该添加向量的z分量，但是在什么角度？