def enu2aer_point(
e: "ndarray",
n: "ndarray",
u: "ndarray",
deg: bool = True) -> typing.Tuple["ndarray", "ndarray", "ndarray"]:
# 1 millimeter precision for singularity
if abs(e) < 1e-3:
e = 0.0
if abs(n) < 1e-3:
n = 0.0
if abs(u) < 1e-3:
u = 0.0
r = hypot(e, n)
slantRange = hypot(r, u)
elev = atan2(u, r)
az = atan2(e, n) % tau
if deg:
az = degrees(az)
elev = degrees(elev)
return az, elev, slantRange
def e2h(ha, dec, lat):
"""
Equatorial to horizon.
Convert equatorial ha/dec coords (both in degs) to az,za (radian)
given an observer latitute (degs). Returns (az,za)
"""
ha_rad = ha * numpy.pi / 180.0
dec_rad = dec * numpy.pi / 180.0
lat_rad = lat * numpy.pi / 180.0
sh = numpy.sin(ha_rad)
ch = numpy.cos(ha_rad)
sd = numpy.sin(dec_rad)
cd = numpy.cos(dec_rad)
sp = numpy.sin(lat_rad)
cp = numpy.cos(lat_rad)
x = -ch * cd * sp + sd * cp
y = -sh * cd
z = ch * cd * cp + sd * sp
r = numpy.sqrt(x * x + y * y)
a = numpy.atan2(y, x)
if a < 0.0:
az = a + 2.0 * numpy.pi
else:
az = a
el = numpy.atan2(z, r)
return (az, numpi.pi / 2.0 - el)
def angles_from_matrix(rot_matrix):
if rot_matrix.shape == (2, 2):
theta = np.atan2(rot_matrix[1, 0], rot_matrix[0, 0])
return theta,
elif rot_matrix.shape == (3, 3):
if rot_matrix[2, 2] == 1.: # cannot use last row and column
theta = 0.
# upper-left block is 2d rotation for phi + psi, so one needs
# to be fixed
psi = 0.
phi = np.atan2(rot_matrix[1, 0], rot_matrix[0, 0])
if phi < 0:
phi += 2 * np.pi # in [0, 2pi)
else:
phi = np.atan2(rot_matrix[0, 2], -rot_matrix[1, 2])
psi = np.atan2(rot_matrix[2, 0], rot_matrix[2, 1])
theta = np.acos(rot_matrix[2, 2])
if phi < 0. or psi < 0.:
phi += np.pi
psi += np.pi
theta = -theta
return phi, theta, psi
else:
raise ValueError('shape of `rot_matrix` must be (2, 2) or (3, 3), '
'got {}'.format(rot_matrix.shape))
def vector_from_T(Tcws):
'''
Compute ego motion vector from given tranformation matrix
:Params:
Tcws: a (time, 4, 4) numpy array of transformation matrics
:Return:
ego_motion: a (time,6) numpy array of ego motion vector [yaw, pitch, roll, x, y, z]
'''
ego_motion = np.zeros((Tcws.shape[0], 6))
for i in range(Tcws.shape[0]):
sy = np.sqrt(Tcws[i][0, 0] * Tcws[i][0, 0] +
Tcws[i][1, 0] * Tcws[i][1, 0])
singular = sy < 1e-6
if not singular:
# yaw = -np.arcsin(Tcws[i][2,0]) # ccw is positive, in [-pi/2, pi/2]
yaw = np.arctan2(-Tcws[i][2, 0],
sy) # ccw is positive, in [-pi/2, pi/2]
pitch = np.arctan2(Tcws[i][2, 1],
Tcws[i][2, 2]) # small value, in [-pi/2, pi/2]
roll = np.arctan2(Tcws[i][1, 0],
Tcws[i][0, 0]) # small value, in [-pi/2, pi/2]
else:
yaw = np.atan2(-Tcws[i][2, 0], sy)
pitch = np.atan2(-Tcws[i][1, 2], Tcws[i][1, 1])
roll = 0
ego_motion[i, 0] = yaw
ego_motion[i, 1] = pitch
ego_motion[i, 2] = roll
ego_motion[i, 3:] = -Tcws[i, :3, 3]
return ego_motion
def angles_from_matrix(rot_matrix):
if rot_matrix.shape == (2, 2):
theta = np.atan2(rot_matrix[1, 0], rot_matrix[0, 0])
return theta,
elif rot_matrix.shape == (3, 3):
if rot_matrix[2, 2] == 1.: # cannot use last row and column
theta = 0.
# upper-left block is 2d rotation for phi + psi, so one needs
# to be fixed
psi = 0.
phi = np.atan2(rot_matrix[1, 0], rot_matrix[0, 0])
if phi < 0:
phi += 2 * np.pi # in [0, 2pi)
else:
phi = np.atan2(rot_matrix[0, 2], -rot_matrix[1, 2])
psi = np.atan2(rot_matrix[2, 0], rot_matrix[2, 1])
theta = np.acos(rot_matrix[2, 2])
if phi < 0. or psi < 0.:
phi += np.pi
psi += np.pi
theta = -theta
return phi, theta, psi
else:
raise ValueError('shape of `rot_matrix` must be (2, 2) or (3, 3), '
'got {}'.format(rot_matrix.shape))
def map_to_geog(self, x, y):
'''Converts x,y coordinates on the map to latitude and longitude in
degrees'''
lon = self.std_lon - atan2(y,x) / rad_per_deg
lat = (pi/2.0 - 2 * atan2(hypot(x,y)/self.map_scale,\
self.radius*self.__img_factor)) / rad_per_deg
return (lat,lon)
def cart2sph(x: float, y: float, z: float) -> typing.Tuple[float, float, float]:
"""Transform Cartesian to spherical coordinates"""
hxy = hypot(x, y)
r = hypot(hxy, z)
el = atan2(z, hxy)
az = atan2(y, x)
return az, el, r
def __cluster_orientation(self):
#currently only search along strike is implemented
ref = (self.input).references
ref1_x = ref[1, 0] - ref[0, 0]
ref1_y = ref[1, 1] - ref[0, 1]
theta1 = num.atan2(ref1_y, ref1_x)
theta2 = num.atan2(ref2_y, ref2_x)
def rECEF2LATLONG(rECEF):
r_delta_sat = np.sqrt(rECEF[0]**2.0 + rECEF[1]**2.0)
a = 6378.1363
# Equatorial radius of the Earth
b = 6356.751 * np.sign(rECEF[2])
#
E = (b * rECEF[2] - (a**2 - b**2)) / (a * r_delta_sat)
#
longitude = np.atan2(rECEF[1] / rECEF[0])
#
F = (b * rECEF[2] + (a**2.0 + b**2.0)) / (a * r_delta_sat)
P = (4.0 * (E * F + 1.0)) / 3.0
Q = 2.0 * (E**2.0 - F**2.0)
D = P**3.0 + Q**2.0
if D > 0.0:
nu = (np.sqrt(D) - Q)**(1 / 3) - (np.sqrt(D) + Q)**(1 / 3)
else:
sqrt_negative_P = np.sqrt(-P)
nu = 2.0 * sqrt_negative_P * np.cos(
1 / 3 * np.arccos(Q / (P * sqrt_negative_P)))
#
G = 0.5 * (np.sqrt(E + nu) + E)
t = np.sqrt(G**2.0 + (F - nu * G) / (2 * G - E)) - G
#
phi_gd = np.atan2(a * (1.0 - t**2.0) / (2.0 * b * t))
h_ellp = (r_delta_sat - a * t) * np.cos(phi_gd) + (rECEF[2] -
b) * np.sin(phi_gd)
#
return phi_gd, longitude, h_ellp
def getPPsimpleAngle(height=[450.e3,],mPosition=[0.,0.,0.],direction=[0.,0.,0.]):
'''get (lon,lat,h values) of piercepoints for antenna position mPosition in m, direction ITRF in m on unit sphere and for array of heights, assuming a spherical Earth'''
height=np.array(height)
stX=mPosition[0]
stY=mPosition[1]
stZ=mPosition[2]
x=np.divide(stX,(R_earth+height))
y=np.divide(stY,(R_earth+height))
z=np.divide(stZ,(R_earth+height))
c = x*x + y*y + z*z - 1.0;
dx=np.divide(direction[0],(R_earth+height))
dy=np.divide(direction[1],(R_earth+height))
dz=np.divide(direction[2],(R_earth+height))
a = dx*dx + dy*dy + dz*dz;
b = x*dx + y*dy + z*dz;
alpha = (-b + np.sqrt(b*b - a*c))/a;
pp=np.zeros(height.shape+(3,))
pp[:,0]=stX+alpha*direction[0]
pp[:,1]=stY+alpha*direction[1]
pp[:,2]=stZ+alpha*direction[2]
am=np.divide(1.,pp[:,0]*dx+pp[:,1]*dy+pp[:,2]*dz)
ppl=np.zeros(height.shape+(3,))
ppl[:,0]=np.atan2(pp[:,1],pp[:0])
ppl[:,1]=np.atan2(pp[:,2],np.sqrt(pp[:0]*pp[:,0]+pp[:1]*pp[:,1]))
ppl[:,2]=heigth
return ppl,am
def cart2sph(x: ArrayLike, y: ArrayLike,
z: ArrayLike) -> typing.Tuple[ArrayLike, ArrayLike, ArrayLike]:
"""Transform Cartesian to spherical coordinates"""
hxy = hypot(x, y)
r = hypot(hxy, z)
el = atan2(z, hxy)
az = atan2(y, x)
return az, el, r
def quaternion_to_euler(quat):
# https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
# Needs to be checked for correctness given openGL coordinate system
q0, q1, q2, q3 = quat
phi = np.atan2(2*(q0*q1 + q2*q3), 1-2*(q1**2 + q2**2))
theta = np.asin(2*(q0*q2 - q3*q1))
psi = np.atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2**2 + q3**2))
return phi, theta, psi
def cart2sph(x: "ndarray", y: "ndarray",
z: "ndarray") -> typing.Tuple["ndarray", "ndarray", "ndarray"]:
"""Transform Cartesian to spherical coordinates"""
hxy = hypot(x, y)
r = hypot(hxy, z)
el = atan2(z, hxy)
az = atan2(y, x)
return az, el, r
def p613():
from scipy.integrate import dblquad
from numpy import arctan2 as atan2
from math import pi
g = lambda x, y: (atan2(40-y, -x)-atan2(-y,30-x))/(2*pi*30*40/2.0)
return dblquad(g, 0, 40, lambda x: 0, lambda x: 30-3.0/4*x,
epsabs = 1e-10)
def cart2sph(
x: float | ndarray, y: float | ndarray, z: float | ndarray
) -> tuple[float | ndarray, float | ndarray, float | ndarray]:
"""Transform Cartesian to spherical coordinates"""
hxy = hypot(x, y)
r = hypot(hxy, z)
el = atan2(z, hxy)
az = atan2(y, x)
return az, el, r
def enu2aer(e: ndarray,
n: ndarray,
u: ndarray,
deg: bool = True) -> tuple[ndarray, ndarray, ndarray]:
"""
ENU to Azimuth, Elevation, Range
Parameters
----------
e : float
ENU East coordinate (meters)
n : float
ENU North coordinate (meters)
u : float
ENU Up coordinate (meters)
deg : bool, optional
degrees input/output (False: radians in/out)
Results
-------
azimuth : float
azimuth to rarget
elevation : float
elevation to target
srange : float
slant range [meters]
"""
# 1 millimeter precision for singularity
try:
e[abs(e) < 1e-3] = 0.0
n[abs(n) < 1e-3] = 0.0
u[abs(u) < 1e-3] = 0.0
except TypeError:
if abs(e) < 1e-3:
e = 0.0 # type: ignore
if abs(n) < 1e-3:
n = 0.0 # type: ignore
if abs(u) < 1e-3:
u = 0.0 # type: ignore
r = hypot(e, n)
slantRange = hypot(r, u)
elev = atan2(u, r)
az = atan2(e, n) % tau
if deg:
az = degrees(az)
elev = degrees(elev)
return az, elev, slantRange
def quaternion_to_euler(x, y, z, w):
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
roll = np.atan2(t0, t1)
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch = np.asin(t2)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
yaw = np.atan2(t3, t4)
return yaw, pitch, roll
def DCM2Euler321(C):
"""
C2Euler321
Q = C2Euler321(C) translates the 3x3 direction cosine matrix
C into the corresponding (3-2-1) Euler angle set.
"""
q = np.zeros(3)
q[0] = np.atan2(C[0, 1], C[0, 0])
q[1] = np.asin(-C[0, 2])
q[2] = np.atan2(C[1, 2], C[2, 2])
return q
def quaterionToEulerAngles(self, q):
"""
Args:
q -
return:
roll, pitch, yaw
"""
roll = np.atan2(2*(q[0]*q[1] + q[2]*q[3]),1-2*(q[1]**2 + q[2]**2))#phi
pitch = np.arcsin(2*(q[0]*q[1] - q[3]*q[1]))#theta
yaw = np.atan2(2*(q[0]*q[3] + q[1]*q[2]),1-2*(q[2]**2 + q[3]**2))#psi
EulerAngles = [pitch, roll, yaw]
return EulerAngles
def rot2euler(matr):
if matr[0, 0] != 1 and matr[0, 0] != -1:
psi = atan2(matr[1, 0], -matr[2, 0])
phi = atan2(matr[0, 1], matr[0, 2])
theta = atan2(sqrt(matr[2, 0]**2 + matr[1, 0]**2), matr[0, 0])
elif matr[0, 0] == 1:
theta = 0
phi = 0
psi = atan2(-matr[1, 2], matr[1, 1])
else:
theta = pi
phi = 0
psi = atan2(matr[1, 2], matr[1, 1])
return psi, theta, phi
def calcHeading(lat1deg, lon1deg, lat2deg, lon2deg):
"""
http://www.movable-type.co.uk/scripts/latlong.html
var R = 6371; // km
var dLat = (lat2-lat1).toRad();
var dLon = (lon2-lon1).toRad();
var lat1 = lat1.toRad();
var lat2 = lat2.toRad();
var a = Math.sin(dLat/2) * Math.sin(dLat/2) +
Math.sin(dLon/2) * Math.sin(dLon/2) * Math.cos(lat1) * Math.cos(lat2);
var c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
var d = R * c;
var y = Math.sin(dLon) * Math.cos(lat2);
var x = Math.cos(lat1)*Math.sin(lat2) -
Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
var brng = Math.atan2(y, x).toDeg();
"""
R = 6371 # km
lat1=((lat1deg)/180*pi)
lon1=((lon1deg)/180*pi)
lat2=((lat2deg)/180*pi)
lon2=((lon2deg)/180*pi)
dLat = (lat2-lat1)
dLon = (lon2-lon1)
a = sin(dLat/2) * sin(dLat/2) + sin(dLon/2) * sin(dLon/2) * cos(lat1) * cos(lat2)
b = 2 * atan2(sqrt(a), sqrt(1-a))
distance = R * b * 1000
y = sin(dLon) * cos(lat2)
x = cos(lat1)*sin(lat2) - sin(lat1)*cos(lat2)*cos(dLon)
brng = atan2(y, x)
lat2=((lat1deg)/180*pi)
lon2=((lon1deg)/180*pi)
lat1=((lat2deg)/180*pi)
lon1=((lon2deg)/180*pi)
dLat = (lat2-lat1)
dLon = (lon2-lon1)
y = sin(dLon) * cos(lat2)
x = cos(lat1)*sin(lat2) - sin(lat1)*cos(lat2)*cos(dLon)
final = (atan2(y, x)+ pi) % (2*pi)
return [brng/pi*180, final/pi*180, distance]
def point2unit_cylindrical(point):
ub = dot(self.axinv,(point-self.origin))
u=zeros((self.DIM,))
u[0] = sqrt(ub[0]*ub[0]+ub[1]*ub[1])
u[1] = atan2(ub[1],ub[0])*o2pi+.5
u[2] = ub[2]
return u
def RayleighPhase(t, v):
n = len(t)
theta = 2. * np.pi * v * t
ph = np.atan2((np.sum(np.sin(theta)), np.sum(np.cos(theta))))
return ph
def point2unit_cylindrical(point):
ub = dot(self.axinv, (point - self.origin))
u = zeros((self.DIM, ))
u[0] = sqrt(ub[0] * ub[0] + ub[1] * ub[1])
u[1] = atan2(ub[1], ub[0]) * o2pi + .5
u[2] = ub[2]
return u
def angle_between(u,v):
u = asarray(u)
v = asarray(v)
c = cross(u,v)
if u.shape[-1]!=2:
c = magnitudes(c)
return atan2(c,dots(u,v))
def generate(out_type='Stokes'):
################### Sample Stokes ###################
# out_type should be either 'Stokes' or 'IPA'
S0 = np.tile(np.linspace(0, 1, 100), (100, 1))
S0 *= np.random.uniform(.7, 1, (100, 100)) # add noise
# Example of false polarization signals:
# S1 and S2 are moderately high, but because the resulting AoLP has high variance, a delta mask would suppress it
S1 = np.random.uniform(-.4, .4, (100, 100))
S2 = np.random.uniform(-.4, .4, (100, 100))
# Actual polarized square
S1[25:75, 25:75] = np.random.uniform(
0.3, 0.6,
(50, 50)) # S1 is large while S1 is small so AoLP has low variance
S2[25:75, 25:75] = np.random.uniform(-0.1, 0.1, (50, 50))
S = np.stack((S0, S1, S2), -1)
###############################
if out_type == 'Stokes':
return S
elif out_type == 'IPA':
P = np.sqrt(S1**2 + S2**2)
A = 0.5 * np.atan2(S2, S1)
IPA = np.dstack((S0, P, A))
return IPA
else:
raise ValueError('Please enter out_type as \'Stokes\' or \'IPA\'')
def point2unit_spherical(point):
ub = dot(self.axinv, (point - self.origin))
u = zeros((self.DIM, ))
u[0] = sqrt(ub[0] * ub[0] + ub[1] * ub[1] + ub[2] * ub[2])
u[1] = atan2(ub[1], ub[0]) * o2pi + .5
u[2] = acos(ub[2] / u[0]) * o2pi * 2.0
return u
def rotationMatrixToEulerAngles(R):
assert (isRotationMatrix(R))
sy = np.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
singular = sy < 1e-6
if not singular:
x = np.atan2(R[2, 1], R[2, 2])
y = np.atan2(-R[2, 0], sy)
z = np.atan2(R[1, 0], R[0, 0])
else:
x = np.atan2(-R[1, 2], R[1, 1])
y = np.atan2(-R[2, 0], sy)
z = 0
return np.array([x, y, z])
def loxodrome_inverse_point(lat1: float,
lon1: float,
lat2: float,
lon2: float,
ell: Ellipsoid = None,
deg: bool = True) -> typing.Tuple[float, float]:
if deg:
lat1, lon1, lat2, lon2 = radians(lat1), radians(lon1), radians(
lat2), radians(lon2)
# compute changes in isometric latitude and longitude between points
disolat = geodetic2isometric(lat2, deg=False,
ell=ell) - geodetic2isometric(
lat1, deg=False, ell=ell)
dlon = lon2 - lon1
# compute azimuth
az12 = atan2(dlon, disolat)
cosaz12 = cos(az12)
# compute distance along loxodromic curve
if abs(cosaz12) < 1e-9: # straight east or west
dist = departure(lon2, lon1, lat1, ell, deg=False)
else:
dist = meridian_arc(lat2, lat1, deg=False, ell=ell) / abs(cos(az12))
if deg:
az12 = degrees(az12) % 360.0
return dist, az12
def get_estimate(curr_scan,trans_scan,correspondance):
"""
curr_scan: Lidar scan at current time step (n,2)
trans_scan: Transformed previous scan based on best known pose (n,2)
Returns
-------
d_pose: difference in pose between two scans
"""
d_pose = np.zeros(3)
curr_mean = np.mean(curr_scan,axis = 0)
trans_mean = np.mean(trans_scan,axis = 0)
curr_scan -= curr_mean
trans_scan -= trans_mean
corres_scan = curr_scan[correspondance,:]
assert corres_scan.shape == trans_scan.shape
W = np.dot(curr_scan.T,trans_scan.T)
u,s,vt = np.linalg.svd(W)
R = np.dot(u,vt)
d_pose[:2] = curr_mean - np.dot(R,trans_mean)
d_pose[2] = np.atan2(R[1,0],R[0,0])
return d_pose
def fitPhase( infile ) :
fin = open( infile, "r" )
xin = []
cin = []
sin = []
for line in fin :
if not line.startswith( "#" ) :
a = line.split()
x.append( float(a[0]) )
c.append( float(a[1]) )
s.append( float(a[2]) )
fin.close()
x = numpy.array(xin)
c = numpy.array(cin)
s = numpy.array(sin)
coffset = c.max() + c.min()
crange = c.max() - c.min()
soffset = s.max() + s.min()
srange = s.max() - s.min()
print "cos max,min = %.6f,%.6f" % (c.max(),c.min())
print "sin max,min = %.6f,%.6f" % (s.max(),s.min())
phase = numpy.unwrap(numpy.atan2( c-coffset, crange/srange*(s-soffset)))
def angle_between(u, v):
u = asarray(u)
v = asarray(v)
c = cross(u, v)
if u.shape[-1] != 2:
c = magnitudes(c)
return atan2(c, dots(u, v))
def point2unit_spherical(point):
ub = dot(self.axinv,(point-self.origin))
u=zeros((self.DIM,))
u[0] = sqrt(ub[0]*ub[0]+ub[1]*ub[1]+ub[2]*ub[2])
u[1] = atan2(ub[1],ub[0])*o2pi+.5
u[2] = acos(ub[2]/u[0])*o2pi*2.0
return u
def calc_bond_orientations(self, x, nsteps, nbeads, npols, nbpp):
""" calculate the bond orientations"""
self.ori = np.zeros((nsteps, nbeads), dtype=np.float32)
for step in xrange(nsteps):
k = 0
for n in xrange(npols):
for j in xrange(nbpp[n] - 1):
dr = x[step, :, k + 1] - x[step, :, k]
self.ori[step][k] = np.atan2(dr[1], dr[0])
k += 1
dr = x[step, :, k + 1] - x[step, :, k]
self.ori[step][k] = np.atan2(dr[1], dr[0])
k += 1
return
def distance(origin, destination):
"""
Calculates both distance and bearing
"""
lat1, lon1 = origin
lat2, lon2 = destination
if lat1>1000:
(lat1,lon1)=dm2dd(lat1,lon1)
(lat2,lon2)=dm2dd(lat2,lon2)
print 'converted to from ddmm to dd.ddd'
radius = 6371 # km
print lat2, lat1
dlat = np.radians(lat2-lat1)
dlon = np.radians(lon2-lon1)
a = np.sin(dlat/2) * np.sin(dlat/2) + np.cos(np.radians(lat1)) \
* np.cos(np.radians(lat2)) * np.sin(dlon/2) * np.sin(dlon/2)
c = 2 * np.atan2(np.sqrt(a), np.sqrt(1-a))
d = radius * c
def calcBearing(lat1, lon1, lat2, lon2):
dLon = lon2 - lon1
y = np.sin(dLon) * np.cos(lat2)
x = np.cos(lat1) * np.sin(lat2) \
- np.sin(lat1) * np.cos(lat2) * np.cos(dLon)
return np.atan2(y, x)
bear= np.degrees(calcBearing(lat1, lon1, lat2, lon2))
return d,bear
def Distance(lat1, lon1, lat2, lon2):
global R
dlat = lat1 - lat2
dlon = lon1 - lon2
a = sin(dlat / 2)**2 + cos(lat1) * cos(lat2) * sin(dlon / 2)**2
c = 2 * atan2(a**0.5, (1 - a)**0.5)
return R * c
def get_phi_theta(vec):
""" returns a tupel of the phi and theta angles of the given vector
"""
try:
return (atan2(vec[1], vec[0]), acos(np.clip(vec[2] / length(vec), -1, 1))) * u.rad
except ValueError:
return (0, 0)
def ecef2aer(obs_lla, ecef_sat, ecef_obs):
"""
:Ref: Geometric Reference Systems in Geodesy by Christopher Jekeli, Ohio State University, August 2016
https://kb.osu.edu/bitstream/handle/1811/77986/Geom_Ref_Sys_Geodesy_2016.pdf?sequence=1&isAllowed=y
:param obs_lla: observations in lat, lon, height (deg, deg, m)
:param ecef_sat:
:return: array[azimuth, elevation, slant]
"""
lat, lon = obs_lla[0], obs_lla[1] # phi, lambda
trans_uvw_ecef = array(
[[-sin(lat) * cos(lon), -sin(lon),
cos(lat) * cos(lon)],
[-sin(lat) * sin(lon),
cos(lon), cos(lat) * sin(lon)], [cos(lat), 0,
sin(lat)]]) # (eq 2.153)
delta_ecef = ecef_sat - ecef_obs # (eq 2.149)
R_enz = trans_uvw_ecef.T @ delta_ecef # (eq 2.153)
r = sqrt(sum(delta_ecef**2)) # (eq 2.156)
az = atan2(R_enz[1], (R_enz[0])) # (eq 2.154)
if az < 0:
az = az + 2 * pi
el = asin(R_enz[2] / r) # (eq 2.155)
aer = array([az, el, r])
return aer
def quaternion2rotation(q):
qw, qx, qy, qz = q
c, d, e, f = qx, qy, qz, qw
g, h, k = c+c, d+d, e+e
a = c*g
l=c*h
c=c*k
m=d*h
d=d*k
e=e*k
g=f*g
h=f*h
f=f*k
mat = np.array([[1-(m+e),1-f,c+h],[l+f,1-(a+e),d-g],[c-h,d+g,1-(a+m)]])
a, f, g = mat[0][0], mat[0][1], mat[0][2]
h, k, l = mat[1][0], mat[1][1], mat[1][2]
m, n, e = mat[2][0], mat[2][1], mat[2][2]
eulerx = -np.arcsin(np.clip(l,-1,1))
eulery = 0
eulerz = 0
if .99999>np.abs(l):
eulery=np.arctan2(g,e)
eulerz=np.arctan2(h,k)
else:
eulery=np.atan2(-m,a)
eulerz=0
return [eulerx, eulery, eulerz]
def update_lm_bearing(x, P, l_px, l_x, l_y, Rk):
p_x = x[0]
p_y = x[1]
theta = x[2]
tmp0 = cos(theta)
tmp1 = l_y - p_y
tmp2 = tmp0 * tmp1
tmp3 = sin(theta)
tmp4 = l_x - p_x
tmp5 = tmp3 * tmp4
tmp6 = tmp0 * tmp4 + tmp1 * tmp3
tmp7 = -tmp2 + tmp5
tmp8 = 1 / (tmp6**2 + tmp7**2)
tmp9 = tmp3 * tmp6
tmp10 = tmp0 * tmp7
tmp11 = tmp8 * (tmp0 * tmp6 + tmp3 * tmp7)
yk = np.float32([l_px - atan2(tmp2 - tmp5, tmp6)])
Hk = np.float32([[tmp8 * (-tmp10 + tmp9), -tmp11, -1]])
Mk = np.float32([[1, tmp8 * (tmp10 - tmp9), tmp11]])
Rk = np.dot(Mk, np.dot(Rk, Mk.T))
S = np.dot(Hk, np.dot(P, Hk.T)) + Rk
LL = -np.dot(yk, np.dot(np.linalg.inv(S), yk)) - 0.5 * np.log(
(2 * np.pi)**3 * np.linalg.det(S))
K = np.linalg.lstsq(S, np.dot(Hk, P))[0].T
x += np.dot(K, yk)
KHk = np.dot(K, Hk)
P = np.dot((np.eye(len(x)) - KHk), P)
return x, P, LL
def xy2latlong(xy):
"convert xy to lat-long, taking care to prevent division by 0"
eegch=np.logical_not(np.any(np.logical_or(np.isnan(xy),np.isinf(xy)),0))
latlong=np.zeros(np.shape(xy))
latlong[0,eegch]= np.sqrt(np.sum(xy[:,eegch] ** 2,1))
latlong[1,eegch]= np.atan2(xy[1,eegch],xy[0,eegch])
latlong[:,np.logical_not(eegch)]=np.NaN
return latlong
def stabilize (img_list):
cv_images = []
for i in xrange(len(img_list)):
cv_images.append(pil_to_opencv(img_list[i]))
#prev and prev_gray should be of type Mat
prev = cv_images[0]
cv2.cvtColor(prev, prev_gray, cv2.COLOR_BGR2GRAY)
for i in xrange(1, len(cv_images)):
current = cv_images[i]
cv2.cvtColor(current, current_gray, cv2.COLOR_BGR2GRAY)
cv2.goodFeaturesToTrack(prev_gray, prev_corner, 200, 0.01, 30)
cv2.calcOpticalFlowPyrLK(prev_gray, current_gray, prev_corner, current_corner, status, err)
prev_corner2 = []
current_corner2 = []
for j in xrange(len(status)):
if(status[j]):
prev_corner2.push_back(prev_corner[j])
current_corner2.push_back(current_corner[j])
T = cv2.estimateRigidTransform(prev_corner2, current_corner2, false)
if T.data == NULL:
last_T.copyTo(T)
T.copyTo(last_T)
dx = T.at(0,2)
dy = T.at(1,2)
da = numpy.atan2(T.at(1,0), T.at(0,0))
transform = []
transform.push_back(Transform(dx, dy, da))
current.copyTo(prev)
current_gray.copyTo(prev_gray)
# accumulate transformations to get image trajectory
x = 0
y = 0
a = 0
trajectory_list = []
for k in xrange(len(transform)):
x += transform[i].dx
y += transform[i].dy
a += transform[i].da
#trajectory_list.push_back(Trajectory(x,y,a))
return img_list
def vecs_to_rot(a, b):
"""
Finds the rotation from vector a to vector b using as axis an
orthogonal vector.
"""
axis = unitize(cross(a,b))
if any(isnan(axis)):
# Parallel vectors -> No rotation, arbitrary axis.
return (1,0,0,0)
angle = atan2(dot(cross(a,b),axis), dot(a,b))
return append(axis, angle)
def inverse_k(H60, d1, a1, a2, a3, d4, d6):
p60 = H60[:3, 3]
p50 = p60 - d6 * H60[:3, 2]
x, y, z = p50
theta1 = atan2(y, x)
eta_x = sqrt(x**2 + y**2) - a1
eta_z = z - d1
d4_tilde = sqrt(a3**2 + d4**2)
s3_tilde = (eta_x**2 + eta_z**2 - d4**2 - a2**2) / (2 * a2 * d4_tilde)
c3_tilde = sqrt(1 - s3_tilde**2)
theta3_tilde = atan2(s3_tilde, c3_tilde)
theta3 = theta3_tilde - atan(a3 / d4)
theta2 = atan2(eta_x * d4_tilde * c3_tilde + eta_z * (d4_tilde * s3_tilde + a2),
eta_x * (d4_tilde * s3_tilde + a2) - eta_z * d4_tilde * c3_tilde)
R03 = matrix([[cos(theta1) * cos(theta2 + theta3), sin(theta1) * cos(theta2 + theta3), sin(theta2 + theta3)],
[sin(theta1), -cos(theta1), 0],
[cos(theta1) * sin(theta2 + theta3), sin(theta1) * sin(theta2 + theta3), -cos(theta2 + theta3)]])
rho = R03 * H60[:3, :3]
theta4 = atan(rho[1, 2] / rho[0, 2])
theta5 = atan2(cos(theta4) * rho[0, 2] + sin(theta4) * rho[1, 2],
-rho[2, 2])
theta6 = atan2(sin(theta4) * rho[0, 0] - cos(theta4) * rho[1, 0],
sin(theta4) * rho[0, 1] - cos(theta4) * rho[1, 1])
return [theta1,
theta2,
theta3,
theta4,
theta5,
theta6]
def quat2euler(r, i, j, k):
"""
Converts a quaternion to Euler angles
@type r: float
@param r: Real part of quaternion
@type i: float
@param i: Imaginary i part of quaternion
@type j: float
@param j: Imaginary j part of quaternion
@type k: float
@param k: Imaginary k part of quaternion
@rtype: tuple
@return: (phi, theta, psi) Euler angles
phi - (float) Roll angle in radians
theta - (float) Pitch angle in radians
psi - (float) Yaw angle in radians
"""
r = float(r)
i = float(i)
j = float(j)
k = float(k)
if not np.isclose(np.array(np.sqrt(r*r+i*i+j*j+k*k)), 1.0):
raise Exception("Invalid argument:\n" + \
str(r) + " " + str(i) + " " + str(j) + " " + str(k) + \
"\n" + "Valid types: norm of [r i j k] = 1")
phi = atan2(2*(r*i + j*k), 1 - 2*(i**2 + j**2))
theta = asin(2*(r*j - k*i))
psi = atan2(2*(r*k + i*j), 1 - 2*(j**2 + k**2))
return (phi, theta, psi)
def _partial_move(to_x, to_y):
required_heading = np.atan2(to_y - self.map_position[1],
to_x - self.map_position[0])
rotation = required_heading - self.heading
if np.fabs(rotation) < self._angle_margin:
rotation = 0
speed = 0 if rotation > 0 else self._speed
twist = Twist()
twist.linear.x = speed
twist.angular.z = rotation
return twist
def to_polar(x, y, theta_units="radians"):
"""
Converts cartesian x, y to polar r, theta.
"""
assert theta_units in ['radians', 'degrees'],\
"kwarg theta_units must specified in radians or degrees"
theta = atan2(y, x)
r = sqrt(x**2 + y**2)
if theta_units == "degrees":
theta = to_degrees(theta)
return r, theta
def solar_azimuth(longitude=0, latitude=0, j2000_ott = None):
"""Azimuth Angle, between sun and north pole"""
if j2000_ott is None:
jday_tt = julian_tt()
j2000_ott = j2000_offset_tt(jday_tt)
ha = hourangle(longitude, j2000_ott)
ls = Mars_Ls(j2000_ott)
dec = solar_declination(ls)*np.pi/180.
denom = (np.cos(latitude)*np.tan(dec)\
- np.sin(latitude)*np.cos(ha))
num = np.sin(ha)
if use_numpy:
az = (360+np.arctan2(num,denom)*180./np.pi) % 360.
else:
az = (360+np.atan2(num,denom)*180./np.pi) % 360.
return az
def cart2pol(x, y, z):
"""
Converts Cartesian coordinates to polar or cylindrical coordinates
@type x: float
@param x: Cartesian x coordinate
@type y: float
@param y: Cartesian y coordinate
@type z: float
@param z: Cartesian z coordinate
@rtype: tuple
@return: (r, theta, z) Polar or cylindrical coordinates
r - (float) Radial distance outward from z-axis
theta - (float) Azimuth angle counter-clockwise around z-axis
z - (float) Signed distance from xy-plane
"""
return (hypot(x, y), atan2(y, x), float(z))
def apply_gradient_filter(img, x_dir_filter):
"""Applies the gradient filter in two orthogonal directions.
Inputs:
img: image that filters should be applied onto
x_dir_filter: a filter which, which convolved with the image, returns the
gradients in the x direction. the filter is transposed for use in the
y direction. it should be a numpy array, and can have up to the same
number of channels as the image.
Returns:
tuple of (S, O, gx, gy), with
S: magnitude of the gradient
O: orientation of the gradient in radians
gx: x-direction raw gradient data
gy: y-direction raw gradient data
"""
gradient_x = scipy.ndimage.filters.convolve(img, x_dir_filter)
gradient_y = scipy.ndimage.filters.convolve(img, x_dir_filter.transpose())
S = numpy.sqrt(gradient_x**2 + gradient_y**2)
O = numpy.atan2(gradient_y, gradient_x)
return S, O, gradient_x, gradient_y
def points2domains_spherical(self,points,domains,points_outside):
u = zeros((self.DIM,))
iu = zeros((self.DIM,),dtype=int)
ndomains=-1
npoints,ndim = points.shape
for p in xrange(npoints):
ub = dot(self.axinv,(points[p]-self.origin))
u[0] = sqrt(ub[0]*ub[0]+ub[1]*ub[1]+ub[2]*ub[2])
u[1] = atan2(ub[1],ub[0])*o2pi+.5
u[2] = acos(ub[2]/u[0])*o2pi*2.0
if (u>self.umin).all() and (u<self.umax).all():
points_outside[p]=False
iu=floor( (u-self.umin)*self.odu )
iu[0] = self.gmap[0][iu[0]]
iu[1] = self.gmap[1][iu[1]]
iu[2] = self.gmap[2][iu[2]]
ndomains+=1
domains[ndomains,0] = p
domains[ndomains,1] = dot(self.dm,iu)
#end
#end
ndomains+=1
return ndomains
def higgs_to_phys(self,v=2.38711864E+02):
v2=v**2
beta=0;
self.m22_2=self.m_Hp*self.m_Hp-0.5*v2*self.lambdas[3];
self.m11_2=-0.5*lambdas[1]*v2
self.m12_2=0.5*lambdas[6]*v2
m_A2 = self.m_Hp*self.m_Hp-0.5*v2*(self.lambdas[5]-self.lambdas[4]);
s2ba = -2.*self.lambdas[6]*v2;
c2ba = -(m_A2+(self.lambdas[5]-self.lambdas[1])*v2);
#Handle special case with degenerate masses
#if np.abs(s2ba)>sqrt(DBL_EPSILON))||(abs(c2ba)>sqrt(DBL_EPSILON))) {
bma = 0.5*np.atan2(s2ba,c2ba);
self.sinba = sin(bma);
if m_A2>0:
self.m_A0 = np.sqrt(m_A2)
else:
self.m_A0=1j*np.sqrt(np.abs(m_A2))
if np.abs(self.lambdas).max()>8*np.pi:
perturbativity=False
return perturbativity
def mat_to_rot(mat):
"""
Given an rotation matrix, return an (x, y, z, angle) rotation. See
also: http://en.wikipedia.org/wiki/Rotation_matrix
TODO Support a fast batch form?
"""
# TODO Is there a faster way than eigen calculation?
# TODO Wikipedia also gives the following:
#
# A partial approach is as follows.
# x = Qzy-Qyz
# y = Qxz-Qzx
# z = Qyx-Qxy
# r = norm(x,norm(y,z))
# t = Qxx+Qyy+Qzz
# theta = atan2(r,t-1)
#
# The x, y, and z components of the axis would then be divided by r. A fully robust approach will use different code when t is negative, as with quaternion extraction. When r is zero because the angle is zero, an axis must be provided from some source other than the matrix.
# Find the eigenvector with eigenvalue 1.
val, vec = eig(mat)
i = argmin(1 - val)
axis = real(vec[:,i])
# Find the angle as Wikipedia suggests.
# TODO Surely a faster way exists.
# Find any orthogonal vector.
a = cross(axis, (1,0,0))
if any(isnan(a)):
# Just need any non-parallel
a = cross(axis, (0,0,1))
# Find the signed angle between a and rotated a.
b = dot(mat, a)
angle = atan2(dot(cross(a,b),axis), dot(a,b))
return append(axis, angle)
def xyz2rtp( x,y,z):
r=np.sqrt( x**2+y**2+z**2)
t=np.acos( z/r )
p=np.atan2( y, x )
return (r,t,p)
def define_plane_by_planar_stereonet_coordinates(x, y): dir = np.degrees(np.atan2(x,y)) l = sqrt(x**2+y**2) dip = 90 - np.degrees (2 * np.atan(l)) return [dir, dip]
def angle_between(a,b):
return atan2(cross(a,b),dot(a,b))
def phiTest(cx,cy,gradDX,gradDY,gradXcoords,gradYcoords):
for ix,xcoord in enumerate(gradXcoords):
if xcoord==cx and gradYcoords[ix]==cy:
return np.atan2(gradDY[ix],gradDX[ix])
return 0
def pix2world(self, x_y):
x, y = x_y[:,0], x_y[:,1]
r = np.sqrt(x*x + y*y)
theta = np.atan2(y, x)
return np.vstack([r, theta]).transpose()
def angle(v):
v = asarray(v)
if iscomplexobj(v):
return numpy.angle(v)
assert v.shape[-1]==2
return atan2(v[...,1],v[...,0])
def rotation(self):
return np.atan2(self.params[1, 0], self.params[1, 1])
