我正在尝试计算用无人机拍摄的图像中心像素的真实世界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
}
我试图在这个帖子中使用Beta描述的方法:How to convert Euler angles to directional vector?但我似乎误解了一些东西。
我不确定最后添加矢量组件时使用的角度。我猜这可能是我的问题。我的Coordinate类只使用long和lat,但是我必须以某种方式添加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)