如何将无人机相机的欧拉角和高度转换为像心坐标?

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

我正在尝试计算无人机拍摄的图像中心像素的真实世界 3D 坐标。我有无人机的经度、纬度、高度和欧拉角(偏航角、俯仰角和滚转角)。假设地面是平坦的(海平面,因为那是无人机的绝对高度)。

现在我只看图像的中心,所以滚动角可能是不必要的,但我仍然有它以备将来使用。

如果可能的话,我对图像中心的经度、纬度、高度 (0) 感兴趣。

我试过用欧拉角制作一个旋转矩阵,并将其与无人机和地面之间的向量相乘。然后将结果添加到无人机的初始位置向量,然后添加到长度,直到它到达海平面。但这似乎并没有给出预期的结果。

编辑: 我一直在研究这个问题并进行了一些更新,但我仍然没有得到正确的解决方案。我将我的结果绘制在地图上并与已知位置进行比较,我可以看出我的结果是错误的。代码:

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
}

我已经尝试使用 Beta 在此线程中描述的方法:How to convert Euler angles to directional vector? 但我似乎误解了一些东西。

我不确定最后添加矢量分量时使用什么角度。我猜这可能是我的问题。我的坐标 class 仅使用经度和纬度,但我是否必须以某种方式添加 z 分量以使其正确?在那种情况下我应该以什么角度添加它?

我想我有一个解决方案,它还没有经过适当的测试,但它似乎适用于我的小型测试集。添加向量分量 在 x 轴和 y 轴的角度解决了我的问题

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