Как преобразовать углы Эйлера и высоту камеры беспилотника в координаты центра изображения? - PullRequest
Новая
       42

Как преобразовать углы Эйлера и высоту камеры беспилотника в координаты центра изображения?

2 голосов
/ 04 апреля 2019

Я пытаюсь вычислить 3D-координату реального мира центрального пикселя изображения, полученного с помощью дрона.У меня есть долгота, широта, высота и углы Эйлера (рыскание, тангаж и крен) дрона.Предполагается, что земля ровная (уровень моря, так как это абсолютная высота дрона).

Пока я смотрю только на центр изображения, поэтому угол наклона может быть ненужным, ноУ меня все еще есть это для будущего использования.

Если возможно, меня интересуют lon, lat, altitude (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
}

Я пытался использовать метод, описанный Бета в этой теме: Как преобразовать углы Эйлера в вектор направления? , но я, кажется, что-то неправильно понимаю.

Я не уверен, какой угол использовать при добавлении векторных компонентов в конце.И я предполагаю, что это может быть моей проблемой.Мой класс Coordinate использует только long и lat, но мне нужно как-то добавить компонент z, чтобы он был правильным?И под каким углом я должен добавить это в этом случае?

1 Ответ

0 голосов
/ 24 апреля 2019

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

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