347 lines
9.7 KiB
Python
347 lines
9.7 KiB
Python
|
import numpy as np
|
|||
|
|
import math
|
|||
|
|
|
|||
|
|
from touying.ImageReproject_python.common_types import POS, ImgEO
|
|||
|
|
|
|||
|
|
|
|||
|
|
# from common_types import POS, ImgEO
|
|||
|
|
|
|||
|
|
class DPAL:
|
|||
|
|
"""数字摄影测量算法库(Digital Photogrammetry Algorithm Library)"""
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def convert_pos_r2_map_r(pos, north_angle, r=None):
|
|||
|
|
"""将导航坐标系下的旋转矩阵转换为投影坐标系下的旋转矩阵
|
|||
|
|
|
|||
|
|
Args:
|
|||
|
|
pos: POS对象，包含位置和姿态信息
|
|||
|
|
north_angle: 子午线收敛角(弧度)
|
|||
|
|
r: 输出旋转矩阵
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
旋转矩阵
|
|||
|
|
"""
|
|||
|
|
# 计算导航坐标系下的旋转矩阵
|
|||
|
|
r_pos = DPAL.computing_r_mat_pos(pos)
|
|||
|
|
|
|||
|
|
# 子午线收敛角旋转矩阵
|
|||
|
|
mt = np.array([
|
|||
|
|
[math.cos(-north_angle), math.sin(-north_angle), 0.0],
|
|||
|
|
[-math.sin(-north_angle), math.cos(-north_angle), 0.0],
|
|||
|
|
[0.0, 0.0, 1.0]
|
|||
|
|
])
|
|||
|
|
|
|||
|
|
# 应用子午线收敛角旋转
|
|||
|
|
r_pos = np.matmul(r_pos, mt)
|
|||
|
|
|
|||
|
|
# 坐标系转换矩阵
|
|||
|
|
t = np.array([
|
|||
|
|
[0.0, 1.0, 0.0],
|
|||
|
|
[1.0, 0.0, 0.0],
|
|||
|
|
[0.0, 0.0, -1.0]
|
|||
|
|
])
|
|||
|
|
|
|||
|
|
# 计算最终旋转矩阵
|
|||
|
|
if r is None:
|
|||
|
|
r = np.matmul(t, np.matmul(r_pos, t))
|
|||
|
|
else:
|
|||
|
|
r[:] = np.matmul(t, np.matmul(r_pos, t))
|
|||
|
|
|
|||
|
|
return r
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def pos2map_eo(pos, north_angle, sys_er_r, eo, angle_type=0):
|
|||
|
|
"""将POS转换为投影坐标系下的EO
|
|||
|
|
|
|||
|
|
Args:
|
|||
|
|
pos: POS对象，包含位置和姿态信息
|
|||
|
|
north_angle: 子午线收敛角(弧度)
|
|||
|
|
sys_er_r: 系统误差旋转矩阵
|
|||
|
|
eo: 输出的EO对象
|
|||
|
|
angle_type: 角度类型，0为国际转角，1为中国转角
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
None
|
|||
|
|
"""
|
|||
|
|
eo.name = pos.name
|
|||
|
|
eo.Xs = pos.L
|
|||
|
|
eo.Ys = pos.B
|
|||
|
|
eo.Zs = pos.H
|
|||
|
|
|
|||
|
|
# 计算旋转矩阵
|
|||
|
|
r = DPAL.convert_pos_r2_map_r(pos, north_angle)
|
|||
|
|
|
|||
|
|
# 应用系统误差
|
|||
|
|
r = np.matmul(sys_er_r, r)
|
|||
|
|
|
|||
|
|
# 根据角度类型计算外方位元素
|
|||
|
|
if angle_type == 0:
|
|||
|
|
DPAL.rotation2_angle(eo, r)
|
|||
|
|
else:
|
|||
|
|
DPAL.rotation2_angle_cn(eo, r)
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def rotation2_angle(eo, r):
|
|||
|
|
"""从旋转矩阵分解出外方位角元素(国际转角系统)
|
|||
|
|
|
|||
|
|
Args:
|
|||
|
|
eo: 输出的EO对象
|
|||
|
|
r: 旋转矩阵
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
None
|
|||
|
|
"""
|
|||
|
|
eo.omega = math.atan2(-r[2, 1], r[2, 2])
|
|||
|
|
eo.phi = math.asin(r[2, 0])
|
|||
|
|
eo.kappa = math.atan2(-r[1, 0], r[0, 0])
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def rotation2_angle_cn(eo_cn, r):
|
|||
|
|
"""从旋转矩阵分解出外方位角元素(中国转角系统)
|
|||
|
|
|
|||
|
|
Args:
|
|||
|
|
eo_cn: 输出的EO对象
|
|||
|
|
r: 旋转矩阵
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
None
|
|||
|
|
"""
|
|||
|
|
eo_cn.phi = math.atan2(-r[2, 0], r[2, 2])
|
|||
|
|
eo_cn.omega = math.asin(-r[2, 1])
|
|||
|
|
eo_cn.kappa = math.atan2(r[0, 1], r[1, 1])
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def eo2eo_cn(eo, eo_cn):
|
|||
|
|
"""国际转角系统的EO转换为中国转角系统的EO
|
|||
|
|
|
|||
|
|
Args:
|
|||
|
|
eo: 国际转角系统的EO对象
|
|||
|
|
eo_cn: 输出的中国转角系统的EO对象
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
None
|
|||
|
|
"""
|
|||
|
|
eo_cn.Xs = eo.Xs
|
|||
|
|
eo_cn.Ys = eo.Ys
|
|||
|
|
eo_cn.Zs = eo.Zs
|
|||
|
|
|
|||
|
|
# 计算旋转矩阵
|
|||
|
|
r = DPAL.computing_r_mat(eo)
|
|||
|
|
|
|||
|
|
# 分解为中国转角
|
|||
|
|
DPAL.rotation2_angle_cn(eo_cn, r)
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def eo_cn2eo(eo_cn, eo):
|
|||
|
|
"""中国转角系统的EO转换为国际转角系统的EO
|
|||
|
|
|
|||
|
|
Args:
|
|||
|
|
eo_cn: 中国转角系统的EO对象
|
|||
|
|
eo: 输出的国际转角系统的EO对象
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
None
|
|||
|
|
"""
|
|||
|
|
eo.Xs = eo_cn.Xs
|
|||
|
|
eo.Ys = eo_cn.Ys
|
|||
|
|
eo.Zs = eo_cn.Zs
|
|||
|
|
|
|||
|
|
# 计算旋转矩阵
|
|||
|
|
r = DPAL.computing_r_mat_cn(eo_cn)
|
|||
|
|
|
|||
|
|
# 分解为国际转角
|
|||
|
|
DPAL.rotation2_angle(eo, r)
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def eo_cn2eo_zyx(eo_cn, eo_zyx):
|
|||
|
|
"""中国转角系统的EO转换为顺序为zyx转角的EO
|
|||
|
|
|
|||
|
|
Args:
|
|||
|
|
eo_cn: 中国转角系统的EO对象
|
|||
|
|
eo_zyx: 输出的zyx转角系统的EO对象
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
None
|
|||
|
|
"""
|
|||
|
|
eo_zyx.Xs = eo_cn.Xs
|
|||
|
|
eo_zyx.Ys = eo_cn.Ys
|
|||
|
|
eo_zyx.Zs = eo_cn.Zs
|
|||
|
|
|
|||
|
|
# 计算旋转矩阵
|
|||
|
|
r = DPAL.computing_r_mat_cn(eo_cn)
|
|||
|
|
rt = r.T
|
|||
|
|
|
|||
|
|
# 分解为zyx转角
|
|||
|
|
eo_temp = ImgEO()
|
|||
|
|
DPAL.rotation2_angle(eo_temp, rt)
|
|||
|
|
eo_zyx.kappa = -eo_temp.kappa
|
|||
|
|
eo_zyx.phi = -eo_temp.phi
|
|||
|
|
eo_zyx.omega = -eo_temp.omega
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def eo2eo_zyx(eo, eo_zyx):
|
|||
|
|
"""国际转角系统的EO转换为顺序为zyx转角的EO
|
|||
|
|
|
|||
|
|
Args:
|
|||
|
|
eo: 国际转角系统的EO对象
|
|||
|
|
eo_zyx: 输出的zyx转角系统的EO对象
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
None
|
|||
|
|
"""
|
|||
|
|
eo_zyx.Xs = eo.Xs
|
|||
|
|
eo_zyx.Ys = eo.Ys
|
|||
|
|
eo_zyx.Zs = eo.Zs
|
|||
|
|
|
|||
|
|
# 计算旋转矩阵
|
|||
|
|
r = DPAL.computing_r_mat(eo)
|
|||
|
|
rt = r.T
|
|||
|
|
|
|||
|
|
# 分解为zyx转角
|
|||
|
|
eo_temp = ImgEO()
|
|||
|
|
DPAL.rotation2_angle(eo_temp, rt)
|
|||
|
|
eo_zyx.kappa = -eo_temp.kappa
|
|||
|
|
eo_zyx.phi = -eo_temp.phi
|
|||
|
|
eo_zyx.omega = -eo_temp.omega
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def computing_k_mat(io, k=None, k_type=0):
|
|||
|
|
"""根据内方位元素计算内参矩阵
|
|||
|
|
|
|||
|
|
Args:
|
|||
|
|
io: 内方位元素对象
|
|||
|
|
k: 输出内参矩阵
|
|||
|
|
k_type: 内参类型，0为左上角为原点，向右X向下Y；非0为中心为原点，向右X向下Y
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
内参矩阵
|
|||
|
|
"""
|
|||
|
|
if k_type != 0:
|
|||
|
|
k_mat = np.array([
|
|||
|
|
[-io.f, 0.0, 0.0],
|
|||
|
|
[0.0, -io.f, 0.0],
|
|||
|
|
[0.0, 0.0, 1.0]
|
|||
|
|
])
|
|||
|
|
else:
|
|||
|
|
k_mat = np.array([
|
|||
|
|
[-io.f, 0.0, io.cx],
|
|||
|
|
[0.0, io.f, io.cy],
|
|||
|
|
[0.0, 0.0, 1.0]
|
|||
|
|
])
|
|||
|
|
|
|||
|
|
if k is None:
|
|||
|
|
return k_mat
|
|||
|
|
else:
|
|||
|
|
k[:] = k_mat
|
|||
|
|
return k
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def computing_r_mat_pos(pos, r_pos=None):
|
|||
|
|
"""根据POS元素计算导航坐标系下的旋转矩阵
|
|||
|
|
|
|||
|
|
Args:
|
|||
|
|
pos: POS对象
|
|||
|
|
r_pos: 输出旋转矩阵
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
旋转矩阵
|
|||
|
|
"""
|
|||
|
|
# 计算各轴旋转矩阵
|
|||
|
|
r_roll = np.array([
|
|||
|
|
[1.0, 0.0, 0.0],
|
|||
|
|
[0.0, math.cos(pos.roll), math.sin(pos.roll)],
|
|||
|
|
[0.0, -math.sin(pos.roll), math.cos(pos.roll)]
|
|||
|
|
])
|
|||
|
|
|
|||
|
|
r_pitch = np.array([
|
|||
|
|
[math.cos(pos.pitch), 0.0, -math.sin(pos.pitch)],
|
|||
|
|
[0.0, 1.0, 0.0],
|
|||
|
|
[math.sin(pos.pitch), 0.0, math.cos(pos.pitch)]
|
|||
|
|
])
|
|||
|
|
|
|||
|
|
r_yaw = np.array([
|
|||
|
|
[math.cos(pos.yaw), math.sin(pos.yaw), 0.0],
|
|||
|
|
[-math.sin(pos.yaw), math.cos(pos.yaw), 0.0],
|
|||
|
|
[0.0, 0.0, 1.0]
|
|||
|
|
])
|
|||
|
|
|
|||
|
|
# 计算最终旋转矩阵
|
|||
|
|
if r_pos is None:
|
|||
|
|
return np.matmul(r_roll, np.matmul(r_pitch, r_yaw))
|
|||
|
|
else:
|
|||
|
|
r_pos[:] = np.matmul(r_roll, np.matmul(r_pitch, r_yaw))
|
|||
|
|
return r_pos
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def computing_r_mat(eo, r=None):
|
|||
|
|
"""根据外方位元素(国际转角)计算旋转矩阵
|
|||
|
|
|
|||
|
|
Args:
|
|||
|
|
eo: 外方位元素对象
|
|||
|
|
r: 输出旋转矩阵
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
旋转矩阵
|
|||
|
|
"""
|
|||
|
|
# 计算各轴旋转矩阵
|
|||
|
|
r_omega = np.array([
|
|||
|
|
[1.0, 0.0, 0.0],
|
|||
|
|
[0.0, math.cos(eo.omega), math.sin(eo.omega)],
|
|||
|
|
[0.0, -math.sin(eo.omega), math.cos(eo.omega)]
|
|||
|
|
])
|
|||
|
|
|
|||
|
|
r_phi = np.array([
|
|||
|
|
[math.cos(eo.phi), 0.0, -math.sin(eo.phi)],
|
|||
|
|
[0.0, 1.0, 0.0],
|
|||
|
|
[math.sin(eo.phi), 0.0, math.cos(eo.phi)]
|
|||
|
|
])
|
|||
|
|
|
|||
|
|
r_kappa = np.array([
|
|||
|
|
[math.cos(eo.kappa), math.sin(eo.kappa), 0.0],
|
|||
|
|
[-math.sin(eo.kappa), math.cos(eo.kappa), 0.0],
|
|||
|
|
[0.0, 0.0, 1.0]
|
|||
|
|
])
|
|||
|
|
|
|||
|
|
# 计算最终旋转矩阵
|
|||
|
|
if r is None:
|
|||
|
|
return np.matmul(r_kappa, np.matmul(r_phi, r_omega))
|
|||
|
|
else:
|
|||
|
|
r[:] = np.matmul(r_kappa, np.matmul(r_phi, r_omega))
|
|||
|
|
return r
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def computing_r_mat_cn(eo_cn, r=None):
|
|||
|
|
"""根据外方位元素(中国转角)计算旋转矩阵
|
|||
|
|
|
|||
|
|
Args:
|
|||
|
|
eo_cn: 外方位元素对象
|
|||
|
|
r: 输出旋转矩阵
|
|||
|
|
|
|||
|
|
Returns:
|
|||
|
|
旋转矩阵
|
|||
|
|
"""
|
|||
|
|
# 计算各轴旋转矩阵
|
|||
|
|
r_phi = np.array([
|
|||
|
|
[math.cos(eo_cn.phi), 0.0, math.sin(eo_cn.phi)],
|
|||
|
|
[0.0, 1.0, 0.0],
|
|||
|
|
[-math.sin(eo_cn.phi), 0.0, math.cos(eo_cn.phi)]
|
|||
|
|
])
|
|||
|
|
|
|||
|
|
r_omega = np.array([
|
|||
|
|
[1.0, 0.0, 0.0],
|
|||
|
|
[0.0, math.cos(eo_cn.omega), math.sin(eo_cn.omega)],
|
|||
|
|
[0.0, -math.sin(eo_cn.omega), math.cos(eo_cn.omega)]
|
|||
|
|
])
|
|||
|
|
|
|||
|
|
r_kappa = np.array([
|
|||
|
|
[math.cos(eo_cn.kappa), math.sin(eo_cn.kappa), 0.0],
|
|||
|
|
[-math.sin(eo_cn.kappa), math.cos(eo_cn.kappa), 0.0],
|
|||
|
|
[0.0, 0.0, 1.0]
|
|||
|
|
])
|
|||
|
|
|
|||
|
|
# 计算最终旋转矩阵
|
|||
|
|
if r is None:
|
|||
|
|
return np.matmul(r_kappa, np.matmul(r_omega, r_phi))
|
|||
|
|
else:
|
|||
|
|
r[:] = np.matmul(r_kappa, np.matmul(r_omega, r_phi))
|
|||
|
|
return r
|