Использование кватернионов для проецирования точек изображения с БПЛА - PullRequest
Новая
       21

Использование кватернионов для проецирования точек изображения с БПЛА

1 голос
/ 13 мая 2019

Учитывая координаты (долгота, широта), высоту БПЛА и углы Эйлера камеры (рыскание, угол наклона, крен), как рассчитать проекцию точек на фотографии с использованием кватернионов?

Это моя функция (углы в радианах): 

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 м и с углом наклона 40 градусов вниз от горизонтали и углом рыскания -88 градусов с севера в точке наземного контроля примерно посередине изображения. Помечено на карте с помощью GCP.

Мои ожидаемые результаты при построении угловых точек изображения должны быть примерно такими: enter image description here

Вместо этого мой результат с использованием функции выше выглядит так: enter image description here

Я подозреваю, что что-то не так с тем, как я добавляю компоненты вектора к начальной координате дрона после вращения. Я не уверен, какие подшипники использовать. Я подозреваю, что я также должен добавить z компонент вектора, но под каким углом?

...