def example2():
from pyigrf.pyigrf import GetIGRF
from scipy import arange, arcsin, rad2deg
xlat = -11.95
xlon = 283.13
year = 2004.75
altlim = [90, 500.]
altstp = 10.
altbins = arange(altlim[0], altlim[1] + altstp, altstp)
for i in range(len(altbins)):
bn, be, bd, xl, icode = GetIGRF(xlat, xlon, altbins[i], year)
# Horizontal component
bh = (bn**2 + be**2)**.5
# Total component
ba = (bn**2 + be**2 + bd**2)**.5
# Dip angle
dip = rad2deg(arcsin(bd / ba))
# Declination angle
dec = rad2deg(arcsin(be / bh))
print(i, altbins[i], bn, be, bd, xl, icode)
print(bh, ba, dip, dec)
print('')
def xyzplot(theta, phi, x, y, z, vmin=-70000, vmax=70000, cmap="bwr"):
"""
plots the x,y, and z components of the magnetic field in three separate color plots, over
a world map in cylindrical equidistant projection, given point coordinate vectors theta and phi
(work in other projections is in progress).
"""
from matplotlib import pyplot
from mpl_toolkits.basemap import Basemap
base = Basemap(
projection="cyl",
llcrnrlat=90 - scipy.rad2deg(max(theta)),
urcrnrlat=90 - scipy.rad2deg(min(theta)),
llcrnrlon=scipy.rad2deg(min(phi)),
urcrnrlon=scipy.rad2deg(max(phi)),
# llcrnrlat=-90, urcrnrlat=90,llcrnrlon=-180,urcrnrlon=180,
resolution="l",
)
# xtrans=numpy.rad2deg(phi)-180
# ytrans=numpy.rad2deg(theta)-90
# xtrans=numpy.rad2deg(phi)
# ytrans=90-numpy.rad2deg(theta)
ytrans = 90 - numpy.rad2deg(theta)
xtrans = numpy.rad2deg(phi)
fig = pyplot.figure(figsize=(10, 13))
axis1 = fig.add_subplot(311)
axis1.set_title("X")
axis2 = fig.add_subplot(312)
axis2.set_title("Y")
axis3 = fig.add_subplot(313)
axis3.set_title("Z")
base.drawcoastlines(ax=axis1)
base.drawcoastlines(ax=axis2)
base.drawcoastlines(ax=axis3)
pyplot.set_cmap(cmap)
# axis1.pcolor(xtrans,ytrans,numpy.rot90(x),vmin=vmin,vmax=vmax)
# axis2.pcolor(xtrans,ytrans,numpy.rot90(y),vmin=vmin,vmax=vmax)
# cplot=axis3.pcolor(xtrans,ytrans,numpy.rot90(z),vmin=vmin,vmax=vmax)
axis1.pcolormesh(xtrans, ytrans, x.transpose(), vmin=vmin, vmax=vmax, shading="gouraud")
axis2.pcolormesh(xtrans, ytrans, y.transpose(), vmin=vmin, vmax=vmax, shading="gouraud")
cplot = axis3.pcolormesh(xtrans, ytrans, z.transpose(), vmin=vmin, vmax=vmax, shading="gouraud")
pyplot.colorbar(mappable=cplot, orientation="vertical", ax=[axis1, axis2, axis3], aspect=40, format="%1.0f nT")
# fig.show()
return fig
def get_freq_response(f0, f1, m, b, n=5000):
"""
Get frequency response for system. Returns gain, phase and frequency.
Inputs:
f0 = starting frequency
f1 = stopping frequency
m = mass of system
b = damping coefficient
Outputs:
mag_db = gain (output/input)
phase = phase shift in degrees
f = array of frequencies
"""
def transfer_func(s, m, b):
return 1.0 / (m * s + b)
f = scipy.linspace(f0, f1, n)
x = 2.0 * scipy.pi * f * 1j
y = transfer_func(x, m, b)
mag = scipy.sqrt(y.real**2 + y.imag**2)
phase = scipy.arctan2(y.imag, y.real)
phase = scipy.rad2deg(phase)
mag_db = 20.0 * scipy.log10(mag)
return mag_db, phase, f
def young(phase,
sigma_sg,
sigma_sl,
surface_tension='pore.surface_tension',
**kwargs):
r'''
Calculate contact angle using Young's equation
Notes
-----
Young's equation is: sigma_lg*Cos(theta)=sigma_sg - sigma_sl
where
sigma_lg is the liquid-gas surface tension [N/m]
sigma_sg is the solid-gas surface tension [N/m]
sigma_sl is the solid-liquid interfacial tension [J/m^2]
theta is the Young contact angle [rad]
'''
if surface_tension.split('.')[0] == 'pore':
sigma = phase[surface_tension]
sigma = phase.interpolate_data(data=sigma)
else:
sigma = phase[surface_tension]
theta = sp.arccos((sigma_sg - sigma_sl) / phase[surface_tension])
theta = sp.rad2deg(theta)
return theta
def young(phase,
sigma_sg,
sigma_sl,
surface_tension='pore.surface_tension',
**kwargs):
r'''
Calculate contact angle using Young's equation
Notes
-----
Young's equation is: sigma_lg*Cos(theta)=sigma_sg - sigma_sl
where
sigma_lg is the liquid-gas surface tension [N/m]
sigma_sg is the solid-gas surface tension [N/m]
sigma_sl is the solid-liquid interfacial tension [J/m^2]
theta is the Young contact angle [rad]
'''
if surface_tension.split('.')[0] == 'pore':
sigma = phase[surface_tension]
sigma = phase.interpolate_data(data=sigma)
else:
sigma = phase[surface_tension]
theta = sp.arccos((sigma_sg - sigma_sl)/phase[surface_tension])
theta = sp.rad2deg(theta)
return theta
def finish_up(ws1, ws3, bus_info, bus_size, g_matrix, b_matrix, input_line, line_size):
for i in range(bus_size):
bus_info[0,2] += (bus_info[0,4]*bus_info[i,4]
*(g_matrix[0,i]*np.cos(bus_info[0,5]-bus_info[i,5])
+b_matrix[0,i]*np.sin(bus_info[0,5]-bus_info[i,5])))
ws1.cell(row=i+2, column=1).value = i+1
ws1.cell(row=i+2, column=2).value = bus_info[i, 4]
bus_info[i, 5] = sp.rad2deg(bus_info[i,5])
ws1.cell(row=i + 2, column=3).value = bus_info[i, 5]
ws1.cell(row=i + 2, column=4).value = s_base*bus_info[i, 2]
ws1.cell(row=i + 2, column=5).value = -1*s_base*bus_info[i, 3]
if bus_info[i, 4] > 1.05 or bus_info[i, 4] < 0.95:
ws1.cell(row=i+2,column=6).value = "TRUE"
else:
ws1.cell(row=i + 2, column=6).value = "FALSE"
for i in range(line_size):
ws3.cell(row=i + 2, column=1).value = input_line[i, 0]
ws3.cell(row=i + 2, column=2).value = input_line[i, 1]
ws3.cell(row=i + 2, column=5).value = 0
if input_line[i,5] < ws3.cell(row=i+2, column=5).value:
ws3.cell(row=i + 2, column=6).value = "TRUE"
else:
ws3.cell(row=i + 2, column=6).value = "FALSE"
def two_d_ellipse(self,proj_vars,p,**kwargs):
"""Return the 2d projection as a matplotlib Ellipse object for the given p values
Parameters
----------
proj_vars : array that is 1 for the projection dimension, and 0 other wise
i.e. array([0,0,1,0,1]) will project 5d ellipsoid onto the plane
span by the 3rd and 5th variable.
p : the percent of points contained in the ellipsoid, either a single
value of a list of values i.e. 0.68 or [0.68,0.955],
if p is a list then a list of Ellipse objects will be returned, one for each p value
Keywords
--------
kwargs : keywords to be passed into the matplotlib Ellipse object
Return
------
ells : matplotlib Ellipse object
"""
mu,u,s=self.proj(proj_vars) #get the mean, eigenvectors, and eigenvales for projected array
try: #if a list get the length
l=len(p)
except: #if not then make it a list of length 1
l=1
p=[p]
invp=st.chi.ppf(p,self.dim) #scale it using a chi distribution (see, now we scale it)
angle=rad2deg(arctan2(u[0,1],u[0,0])) #angle the first eignevector makes with the x-axis
ells=[] #list to hold the Ellipse objects
for i in invp:
ells.append(Ellipse(xy=mu,width=s[0]*i*2,height=s[1]*i*2,angle=angle,**kwargs))#make the Ellipse objects, the *2 is needed since Ellipse takes the full axis vector
if l==1: #if only one p values was given return the Ellipse object (not as a list)
return ells[0]
else: #else return the list of Ellipse objects
return ells
def test_display_values(self):
model = load_fmu(self.coupled_name)
import scipy
val = model.get_variable_display_value("J1.phi")
val_ref = scipy.rad2deg(model.get("J1.phi"))
nose.tools.assert_almost_equal(val, val_ref)
model.simulate()
val = model.get_variable_display_value("J1.phi")
val_ref = scipy.rad2deg(model.get("J1.phi"))
nose.tools.assert_almost_equal(val, val_ref)
def getangle(A, B):
"""
When A and B are two angles around the clock returns an angle of
the line that is connecting them.
"""
x = array([cos(deg2rad(A)), sin(deg2rad(A))])
y = array([cos(deg2rad(B)), sin(deg2rad(B))])
d = y - x
return rad2deg(math.atan2(d[1], d[0]))
def getangle(A, B):
"""
When A and B are two angles around the clock returns an angle of
the line that is connecting them.
"""
x = array([cos(deg2rad(A)), sin(deg2rad(A))])
y = array([cos(deg2rad(B)), sin(deg2rad(B))])
d = y - x
return rad2deg(math.atan2(d[1], d[0]))
def test_healpix_sphere(self):
# Sphere parameters.
R = 5
# Expected outputs of healpix_sphere() applied to inputs.
if RADIANS:
sigma_a = sqrt(3 - 3 * sin(a[1]))
else:
sigma_a = sqrt(3 - 3 * sin(deg2rad(a[1])))
ha = (pi / 4 * (1 - sigma_a), pi / 4 * (2 - sigma_a))
hb = (ha[0], -ha[1])
healpix_sphere_outputs = [(0, 0), (0, pi / 4),
(0, -pi / 4), (pi / 2, 0), (-pi / 2, 0),
(-pi, 0), (-3 * pi / 4, pi / 2),
(-3 * pi / 4, -pi / 2), ha, hb]
healpix_sphere_outputs = [
tuple(R * array(p)) for p in healpix_sphere_outputs
]
# Forward projection should be correct on test points.
f = Proj(proj='healpix', R=R)
given = inputs
get = [f(*p, radians=RADIANS) for p in given]
expect = healpix_sphere_outputs
# Fuzz to allow for rounding errors:
error = 1e-12
print()
print('=' * 80)
print('HEALPix forward projection, sphere with radius R = %s' % R)
print('input (radians) / expected output (meters) / received output')
print('=' * 80)
for i in range(len(get)):
print(given[i], expect[i], get[i])
self.assertTrue(rel_err(get[i], expect[i]) < error)
# Inverse of projection of a point p should yield p.
given = get
get = [f(*q, radians=RADIANS, inverse=True) for q in given]
expect = inputs
print('=' * 80)
print('HEALPix inverse projection, sphere with radius R = %s' % R)
print('input (meters) / expected output (radians) / received output')
print('=' * 80)
for i in range(len(get)):
print(given[i], expect[i], get[i])
self.assertTrue(rel_err(get[i], expect[i]) < error)
# Inverse projection of p below should return longitude of -pi.
# Previously, it was returning a number slightly less than pi
# because of a rounding error, which got magnified by
# wrap_longitude()
p = R * array((-7 * pi / 8, 3 * pi / 8))
get = f(*p, radians=RADIANS, inverse=True)
p1 = arcsin(1 - 1.0 / 12)
if not RADIANS:
p1 = rad2deg(p1)
expect = (-PI, p1)
self.assertTrue(rel_err(get, expect) < error)
def decodeMessageSensorUDP(self, msg):
""" This is used to decode message from sensorUDP application from the android market.
The orientation field was first used, but its conventions were unclear.
So now acceleration and magnetic vectors should be used"""
data = msg.split(', ')
if data[0]=='G':
# This is GPS message
time = decimalstr2float(data[2])
latitude_deg = decimalstr2float(data[3])
longitude_deg = decimalstr2float(data[4])
altitude = decimalstr2float(data[5])
hdop = decimalstr2float(data[7]) # Horizontal dilution of precision
vdop = decimalstr2float(data[8]) # Vertical dilution of precision
print time, latitude_deg, longitude_deg, altitude, hdop, vdop
if data[0]=='O':
# \note This is no more used as orientation convention were unclear
# 'O, 146, 1366575961732, 230,1182404, -075,2031250, 001,7968750'
[ u, u, # data not used \
heading_deg, # pointing direction of top of phone \
roll_deg, # around horizontal axis, positive clockwise [-180:180] \
pitch_deg] = decimalstr2float(data[1:]) # around vertical axis [_90:90]
elevation_deg = -sp.rad2deg(sp.arctan2( \
sp.cos(sp.deg2rad(pitch_deg))*sp.cos(sp.deg2rad(roll_deg)), \
sp.sqrt(1+sp.cos(sp.deg2rad(roll_deg))**2*(sp.sin(sp.deg2rad(pitch_deg))**2-1)))) #positive up
inclinaison_deg = pitch_deg #positive clockwise
print heading_deg, roll_deg, pitch_deg, elevation_deg, inclinaison_deg
if data[0] == 'A':
# Accelerometer data
# Index and sign are adjusted to obtain x through the screen, and z down
deltaT = decimalstr2float(data[2])/1000 - self.time_acceleration
if self.filterTimeConstant == 0.0:
alpha = 1
else:
alpha = 1-sp.exp(-deltaT/self.filterTimeConstant)
self.time_acceleration = decimalstr2float(data[2])/1000
self.acceleration_raw[0] = decimalstr2float(data[3])
self.acceleration_raw[1] = decimalstr2float(data[4])
self.acceleration_raw[2] = decimalstr2float(data[5])
# Filter the data
self.acceleration_filtered +=alpha*(sp.array(self.acceleration_raw)-self.acceleration_filtered)
if data[0] == 'M':
# Magnetometer data
# Index and sign are adjusted to obtain x through the screen, and z down
deltaT = decimalstr2float(data[2])/1000-self.time_magnetic
if self.filterTimeConstant == 0.0:
alpha = 1
else:
alpha = 1-sp.exp(-deltaT/self.filterTimeConstant)
self.time_magnetic = decimalstr2float(data[2])/1000
self.magnetic_raw[0] = decimalstr2float(data[3])
self.magnetic_raw[1] = decimalstr2float(data[4])
self.magnetic_raw[2] = -decimalstr2float(data[5])# Adapt to a bug in sensorUDP?
# Filter the data
self.magnetic_filtered += alpha*(sp.array(self.magnetic_raw)-self.magnetic_filtered)
def doatest():
print("s1 is {}".format(s1_aoa))
print("s2 is {}".format(s2_aoa))
s1_est = music.Estimator(ants,music.covar(s1),nsignals=1)
s2_est = music.Estimator(ants,music.covar(s2),nsignals=1)
# s1
t1 = time()
s1_res = s1_est.doasearch()[0]
t1 = time() - t1
s1_err = sp.rad2deg(util.aoa_diff_rad(s1_res,s1_aoa))
print("s1: found {} in {}s, error {} deg".format(s1_res,t1,s1_err))
# s2
t2 = time()
s2_res = s2_est.doasearch()[0]
t2 = time() - t2
s2_err = sp.rad2deg(util.aoa_diff_rad(s2_res,s2_aoa))
print("s2: found {} in {}s, error {} deg".format(s2_res,t2,s2_err))
# both signals
bothres = est.doasearch()
print("Both signals:\n{}".format(bothres))
def E2T(Ener, dspacing):
"""Calculate The angle for an Energy in eV or KeV given a dspacing
accept, "integer", "float", monodimensional array
return corrisponding angle in degree
"""
if ((type(Ener) == float) or (type(Ener) == int)
or (type(Ener) == scipy.float64) or (type(Ener) == scipy.float32)):
if Ener > 1000:
the = scipy.rad2deg(
scipy.arcsin((1.23984E-6 / (Ener)) * 1E10 / (2 * dspacing)))
if Ener < 1000:
the = scipy.rad2deg(
scipy.arcsin(
(1.23984E-6 / (Ener * 1000)) * 1E10 / (2 * dspacing)))
else:
if Ener[0] > 1000:
the = scipy.rad2deg(
scipy.arcsin((1.23984E-6 / (Ener)) * 1E10 / (2 * dspacing)))
if Ener[0] < 1000:
the = scipy.rad2deg(
scipy.arcsin(
(1.23984E-6 / (Ener * 1000)) * 1E10 / (2 * dspacing)))
return the
def ecef2geodetic(x, y, z, degrees=True):
"""ecef2geodetic(x, y, z)
[m][m][m]
Convert ECEF coordinates to geodetic.
J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates \
to geodetic coordinates," IEEE Transactions on Aerospace and \
Electronic Systems, vol. 30, pp. 957-961, 1994."""
r = sqrt(x * x + y * y)
Esq = a * a - b * b
F = 54 * b * b * z * z
G = r * r + (1 - esq) * z * z - esq * Esq
C = (esq * esq * F * r * r) / (pow(G, 3))
S = cbrt(1 + C + sqrt(C * C + 2 * C))
P = F / (3 * pow((S + 1 / S + 1), 2) * G * G)
Q = sqrt(1 + 2 * esq * esq * P)
r_0 = -(P * esq * r) / (1 + Q) + sqrt(0.5 * a * a*(1 + 1.0 / Q) - \
P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r)
U = sqrt(pow((r - esq * r_0), 2) + z * z)
V = sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z)
Z_0 = b * b * z / (a * V)
h = U * (1 - b * b / (a * V))
lat = arctan((z + e1sq * Z_0) / r)
lon = arctan2(y, x)
return rad2deg(lat), rad2deg(lon), z
def ecef2geodetic(x, y, z, degrees=True):
"""ecef2geodetic(x, y, z)
[m][m][m]
Convert ECEF coordinates to geodetic.
J. Zhu, "Conversion of Earth-centered Earth-fixed coordinates \
to geodetic coordinates," IEEE Transactions on Aerospace and \
Electronic Systems, vol. 30, pp. 957-961, 1994."""
r = sqrt(x * x + y * y)
Esq = a * a - b * b
F = 54 * b * b * z * z
G = r * r + (1 - esq) * z * z - esq * Esq
C = (esq * esq * F * r * r) / (pow(G, 3))
S = cbrt(1 + C + sqrt(C * C + 2 * C))
P = F / (3 * pow((S + 1 / S + 1), 2) * G * G)
Q = sqrt(1 + 2 * esq * esq * P)
r_0 = -(P * esq * r) / (1 + Q) + sqrt(0.5 * a * a*(1 + 1.0 / Q) - \
P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r)
U = sqrt(pow((r - esq * r_0), 2) + z * z)
V = sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z)
Z_0 = b * b * z / (a * V)
h = U * (1 - b * b / (a * V))
lat = arctan((z + e1sq * Z_0) / r)
lon = arctan2(y, x)
return rad2deg(lat), rad2deg(lon), z
def young(phase,
sigma_sg,
sigma_sl,
pore_surface_tension='pore.surface_tension',
**kwargs):
r'''
Calculate contact angle using Young's equation
Notes
-----
Young's equation is:
.. math::
\gamma_\mathrm{LG} \cos \theta_\mathrm{C} \ = \gamma_\mathrm{SG} - \gamma_\mathrm{SL}
'''
theta = sp.arccos((sigma_sg - sigma_sl)/phase[pore_surface_tension])
theta = sp.rad2deg(theta)
return theta
def test_distortion(self):
epsilon = 10e-4
# Conformal projections should have maximum angular distortion
# equal to 0 and linear distortion equal to 1.0.
mad, ld, ad = distortion(merc, lam, phi)[1:]
self.assertAlmostEqual(mad, 0.0, places=6)
self.assertAlmostEqual(ld, 1.0, places=6)
# Area preserving projections should have area distortion
# equal to 1.0.
mad, ld, ad = distortion(cea, lam, phi)[1:]
self.assertAlmostEqual(ad, 1.0, places=6)
# Degrees mode output should agree with radians mode output.
get = distortion(cea_ed, lam_deg, phi_deg)
expect = list(distortion(cea_e, lam, phi))
# Entry 1 is an angular measurement.
expect[1] = rad2deg(expect[1])
for i in range(len(expect)):
self.assertAlmostEqual(get[i], expect[i])
def make_initial_catalogue(path):
from pywindow.catalogue import sky_to_cartesian
rng = scipy.random.RandomState(seed=42)
rmin, rmax = 2000., 3000.
size = 100000
catalogue = Catalogue()
distance = rng.uniform(2000., 3000., size=size)
ramin, ramax, decmin, decmax = 0., 30., -15, 15.
u1, u2 = rng.uniform(size=(2, size))
cmin = scipy.sin(scipy.deg2rad(decmin))
cmax = scipy.sin(scipy.deg2rad(decmax))
ra = ramin + u1 * (ramax - ramin)
dec = 90. - scipy.rad2deg(scipy.arccos(cmin + u2 * (cmax - cmin)))
catalogue['Position'] = sky_to_cartesian(distance, ra, dec)
catalogue['Weight'] = catalogue.ones()
decfrac = scipy.diff(scipy.sin(scipy.deg2rad([decmin, decmax])), axis=0)
rafrac = scipy.diff(scipy.deg2rad([ramin, ramax]), axis=0)
area = decfrac * rafrac
catalogue['NZ'] = catalogue['Weight'].sum() / (area *
(rmax - rmin)) / distance**2
catalogue.to_fits(path)
def align_image_brute_force(image, target, search_strategy, plot=False, write_files=False, PF=None):
if PF is None:
PF = PatternFinder(partitions=10)
target_center = sp.array(target.shape[:2]) / 2. - 0.5
im_center = sp.array(image.shape[:2]) / 2. - 0.5
#Initialize transformation between image and target as identity
T = transform.AffineTransform(matrix=sp.array([[1,0,0],[0,1,0],[0,0,1]]))
best_value = None
logger = logging.getLogger('stackalign')
for nr, search_phase in enumerate(search_strategy):
logger.info("\nSearch phase {0}".format(nr))
best_angle = sp.rad2deg(T.rotation)
angle_range = sp.linspace(
search_phase["angle_range"][0] - best_angle,
search_phase["angle_range"][1] - best_angle,
search_phase["angle_range"][2]
)
best_coord = sp.array([int(im_center[0]+T.translation[0]),
int(im_center[1]+T.translation[1])])
logger.debug(f"best so far: x,y=({best_coord[0]},{best_coord[1]}), r={best_angle:0.3f}º")
T, value = find_pattern_rotated(PF, target, image,
rescale=search_phase["rescale"],
rotations=angle_range,
roi_center_rc=best_coord,
roi_size_hw=search_phase["roi_hw"],
plot=plot,
progress=tqdm)
# TODO: Check if this can be done more efficiently
# image_rescaled = transform.rescale(image,search_phase["rescale"])
# Print parameters
logger.info(print_parameters(T, value))
return T, value
def makecircles(peakArray, wavelength, cellSize,name):
circlePlot, ax= plt.subplots()
filename = name + '.png'
f.write( "--------------------- \n" )
f.write( name + ':\n' )
f.write(' Index 2Theta \n')
datalimit = sp.array([0])
for peak in peakArray:
dhkl = cubicDSpacing(cellSize, peak)
angle = sp.arcsin( wavelength / ( 2 * dhkl ) )
radius = sp.tan(2 * angle)
f.write( str(peak) + str( 2 * sp.rad2deg(angle) ) + '\n' )
circle = plt.Circle((0,0), radius, fill=False)
ax.add_artist(circle)
if radius > datalimit:
datalimit = radius
ax.set_aspect('equal', adjustable='datalim')
graphLimits = datalimit * 1.3 # this magic number is just a format thing
plt.xlim(-graphLimits, graphLimits)
plt.ylim(-graphLimits, graphLimits)
circlePlot.savefig(filename)
f.write( "--------------------- \n\n" )
return (x>0 and x<image.width and y>0 and y<image.height)
width = 640; lon_0 = 270; lat_0 = 80
pixelPerRadians = 640
height=480
radius = pixelPerRadians
max_length = 0
cam = JpegStreamCamera('http://192.168.43.1:8080/videofeed')#640 * 480
mobile = mobileState.mobileState()
while True:
mobile.checkUpdate()
if mobile.isToUpdate:
mobile.computeRPY()
image = cam.getImage().rotate(-sp.rad2deg(mobile.roll), fixed = False)
m = Basemap(width=image.width,height=image.height,projection='aeqd',
lat_0=sp.rad2deg(mobile.pitch),lon_0=sp.rad2deg(mobile.yaw), rsphere = radius)
# fill background.
#m.drawmapboundary(fill_color='aqua')
# draw coasts and fill continents.
#m.drawcoastlines(linewidth=0.5)
#m.fillcontinents(color='coral',lake_color='aqua')
# 20 degree graticule.
# m.drawparallels(np.arange(-90,90,30))
#m.drawmeridians(np.arange(-180,180,30))
# draw a black dot at the center.
#xpt, ypt = m(heading_deg, elevation_deg)
#m.plot([xpt],[ypt],'ko')
# draw the title.
#plt.title('Azimuthal Equidistant Projection')
def track(self):
print "Press right mouse button to pause or play"
print "Use left mouse button to select target"
print "Target color must be different from background"
print "Target must have width larger than height"
print "Target can be upside down"
#Parameters
isUDPConnection = False # Currently switched manually in the code
display = True
displayDebug = True
useBasemap = False
maxRelativeMotionPerFrame = 2 # How much the target can moved between two succesive frames
pixelPerRadians = 320
radius = pixelPerRadians
referenceImage = '../ObjectTracking/kite_detail.jpg'
scaleFactor = 0.5
isVirtualCamera = True
useHDF5 = False
# Open reference image: this is used at initlalisation
target_detail = Image(referenceImage)
# Get RGB color palette of target (was found to work better than using hue)
pal = target_detail.getPalette(bins = 2, hue = False)
# Open video to analyse or live stream
#cam = JpegStreamCamera('http://192.168.1.29:8080/videofeed')#640 * 480
if isVirtualCamera:
#cam = VirtualCamera('../../zenith-wind-power-read-only/KiteControl-Qt/videos/kiteFlying.avi','video')
#cam = VirtualCamera('/media/bat/DATA/Baptiste/Nautilab/kite_project/robokite/ObjectTracking/00095.MTS', 'video')
#cam = VirtualCamera('output.avi', 'video')
cam = VirtualCamera('../Recording/Videos/Flying kite images (for kite steering unit development)-YTMgX1bvrTo.flv','video')
virtualCameraFPS = 25
else:
cam = JpegStreamCamera('http://192.168.43.1:8080/videofeed')#640 * 480
#cam = Camera()
# Get a sample image to initialize the display at the same size
img = cam.getImage().scale(scaleFactor)
print img.width, img.height
# Create a pygame display
if display:
if img.width>img.height:
disp = Display((27*640/10,25*400/10))#(int(2*img.width/scaleFactor), int(2*img.height/scaleFactor)))
else:
disp = Display((810,1080))
#js = JpegStreamer()
# Initialize variables
previous_angle = 0 # target has to be upright when starting. Target width has to be larger than target heigth.
previous_coord_px = (0, 0) # Initialized to top left corner, which always exists
previous_dCoord = previous_coord_px
previous_dAngle = previous_angle
angles = []
coords_px = []
coord_px = [0, 0]
angle = 0
target_elevations = []
target_bearings = []
times = []
wasTargetFoundInPreviousFrame = False
i_frame = 0
isPaused = False
selectionInProgress = False
th = [100, 100, 100]
skycolor = Color.BLUE
timeLastTarget = 0
# Prepare recording
recordFilename = datetime.datetime.utcnow().strftime("%Y%m%d_%Hh%M_")+ 'simpleTrack'
if useHDF5:
try:
os.remove(recordFilename + '.hdf5')
except:
print('Creating file ' + recordFilename + '.hdf5')
""" The following line is used to silence the following error (according to http://stackoverflow.com/questions/15117128/h5py-in-memory-file-and-multiprocessing-error)
#000: ../../../src/H5F.c line 1526 in H5Fopen(): unable to open file
major: File accessability
minor: Unable to open file"""
h5py._errors.silence_errors()
recordFile = h5py.File(recordFilename + '.hdf5', 'a')
hdfSize = 0
dset = recordFile.create_dataset('kite', (2,2), maxshape=(None,7))
imset = recordFile.create_dataset('image', (2,img.width,img.height,3 ), maxshape=(None, img.width, img.height, 3))
else:
try:
os.remove(recordFilename + '.csv')
except:
print('Creating file ' + recordFilename + '.csv')
recordFile = file(recordFilename + '.csv', 'a')
csv_writer = csv.writer(recordFile)
csv_writer.writerow(['Time (s)', 'x (px)', 'y (px)', 'Orientation (rad)', 'Elevation (rad)', 'Bearing (rad)', 'ROT (rad/s)'])
# Launch a thread to get UDP message with orientation of the camera
mobile = mobileState.mobileState()
if isUDPConnection:
a = threading.Thread(None, mobileState.mobileState.checkUpdate, None, (mobile,))
a.start()
# Loop while not canceled by user
t0 = time.time()
previousTime = t0
while not(display) or disp.isNotDone():
t = time.time()
deltaT = (t-previousTime)
FPS = 1.0/deltaT
#print 'FPS =', FPS
if isVirtualCamera:
deltaT = 1.0/virtualCameraFPS
previousTime = t
i_frame = i_frame + 1
timestamp = datetime.datetime.utcnow()
# Receive orientation of the camera
if isUDPConnection:
mobile.computeRPY([2, 0, 1], [-1, 1, 1])
ctm = np.array([[sp.cos(mobile.roll), -sp.sin(mobile.roll)], \
[sp.sin(mobile.roll), sp.cos(mobile.roll)]]) # Coordinate transform matrix
if useBasemap:
# Warning this really slows down the computation
m = Basemap(width=img.width, height=img.height, projection='aeqd',
lat_0=sp.rad2deg(mobile.pitch), lon_0=sp.rad2deg(mobile.yaw), rsphere = radius)
# Get an image from camera
if not isPaused:
img = cam.getImage()
img = img.resize(int(scaleFactor*img.width), int(scaleFactor*img.height))
if display:
# Pause image when right button is pressed
dwn = disp.rightButtonDownPosition()
if dwn is not None:
isPaused = not(isPaused)
dwn = None
if display:
# Create a layer to enable user to make a selection of the target
selectionLayer = DrawingLayer((img.width, img.height))
if img:
if display:
# Create a new layer to host information retrieved from video
layer = DrawingLayer((img.width, img.height))
# Selection is a rectangle drawn while holding mouse left button down
if disp.leftButtonDown:
corner1 = (disp.mouseX, disp.mouseY)
selectionInProgress = True
if selectionInProgress:
corner2 = (disp.mouseX, disp.mouseY)
bb = disp.pointsToBoundingBox(corner1, corner2)# Display the temporary selection
if disp.leftButtonUp: # User has finished is selection
selectionInProgress = False
selection = img.crop(bb[0], bb[1], bb[2], bb[3])
if selection != None:
# The 3 main colors in the area selected are considered.
# Note that the selection should be included in the target and not contain background
try:
selection.save('../ObjectTracking/'+ 'kite_detail_tmp.jpg')
img0 = Image("kite_detail_tmp.jpg") # For unknown reason I have to reload the image...
pal = img0.getPalette(bins = 2, hue = False)
except: # getPalette is sometimes bugging and raising LinalgError because matrix not positive definite
pal = pal
wasTargetFoundInPreviousFrame = False
previous_coord_px = (bb[0] + bb[2]/2, bb[1] + bb[3]/2)
if corner1 != corner2:
selectionLayer.rectangle((bb[0], bb[1]), (bb[2], bb[3]), width = 5, color = Color.YELLOW)
# If the target was already found, we can save computation time by
# reducing the Region Of Interest around predicted position
if wasTargetFoundInPreviousFrame:
ROITopLeftCorner = (max(0, previous_coord_px[0]-maxRelativeMotionPerFrame/2*width), \
max(0, previous_coord_px[1] -height*maxRelativeMotionPerFrame/2))
ROI = img.crop(ROITopLeftCorner[0], ROITopLeftCorner[1], \
maxRelativeMotionPerFrame*width, maxRelativeMotionPerFrame*height, \
centered = False)
if display :
# Draw the rectangle corresponding to the ROI on the complete image
layer.rectangle((previous_coord_px[0]-maxRelativeMotionPerFrame/2*width, \
previous_coord_px[1]-maxRelativeMotionPerFrame/2*height), \
(maxRelativeMotionPerFrame*width, maxRelativeMotionPerFrame*height), \
color = Color.GREEN, width = 2)
else:
# Search on the whole image if no clue of where is the target
ROITopLeftCorner = (0, 0)
ROI = img
'''#Option 1
target_part0 = ROI.hueDistance(color=(142,50,65)).invert().threshold(150)
target_part1 = ROI.hueDistance(color=(93,16,28)).invert().threshold(150)
target_part2 = ROI.hueDistance(color=(223,135,170)).invert().threshold(150)
target_raw_img = target_part0+target_part1+target_part2
target_img = target_raw_img.erode(5).dilate(5)
#Option 2
target_img = ROI.hueDistance(imgModel.getPixel(10,10)).binarize().invert().erode(2).dilate(2)'''
# Find sky color
sky = (img-img.binarize()).findBlobs(minsize=10000)
if sky:
skycolor = sky[0].meanColor()
# Option 3
target_img = ROI-ROI # Black image
# Loop through palette of target colors
if display and displayDebug:
decomposition = []
i_col = 0
for col in pal:
c = tuple([int(col[i]) for i in range(0,3)])
# Search the target based on color
ROI.save('../ObjectTracking/'+ 'ROI_tmp.jpg')
img1 = Image('../ObjectTracking/'+ 'ROI_tmp.jpg')
filter_img = img1.colorDistance(color = c)
h = filter_img.histogram(numbins=256)
cs = np.cumsum(h)
thmax = np.argmin(abs(cs- 0.02*img.width*img.height)) # find the threshold to have 10% of the pixel in the expected color
thmin = np.argmin(abs(cs- 0.005*img.width*img.height)) # find the threshold to have 10% of the pixel in the expected color
if thmin==thmax:
newth = thmin
else:
newth = np.argmin(h[thmin:thmax]) + thmin
alpha = 0.5
th[i_col] = alpha*th[i_col]+(1-alpha)*newth
filter_img = filter_img.threshold(max(40,min(200,th[i_col]))).invert()
target_img = target_img + filter_img
#print th
i_col = i_col + 1
if display and displayDebug:
[R, G, B] = filter_img.splitChannels()
white = (R-R).invert()
r = R*1.0/255*c[0]
g = G*1.0/255*c[1]
b = B*1.0/255*c[2]
tmp = white.mergeChannels(r, g, b)
decomposition.append(tmp)
# Get a black background with with white target foreground
target_img = target_img.threshold(150)
target_img = target_img - ROI.colorDistance(color = skycolor).threshold(80).invert()
if display and displayDebug:
small_ini = target_img.resize(int(img.width/(len(pal)+1)), int(img.height/(len(pal)+1)))
for tmp in decomposition:
small_ini = small_ini.sideBySide(tmp.resize(int(img.width/(len(pal)+1)), int(img.height/(len(pal)+1))), side = 'bottom')
small_ini = small_ini.adaptiveScale((int(img.width), int(img.height)))
toDisplay = img.sideBySide(small_ini)
else:
toDisplay = img
#target_img = ROI.hueDistance(color = Color.RED).threshold(10).invert()
# Search for binary large objects representing potential target
target = target_img.findBlobs(minsize = 500)
if target: # If a target was found
if wasTargetFoundInPreviousFrame:
predictedTargetPosition = (width*maxRelativeMotionPerFrame/2, height*maxRelativeMotionPerFrame/2) # Target will most likely be close to the center of the ROI
else:
predictedTargetPosition = previous_coord_px
# If there are several targets in the image, take the one which is the closest of the predicted position
target = target.sortDistance(predictedTargetPosition)
# Get target coordinates according to minimal bounding rectangle or centroid.
coordMinRect = ROITopLeftCorner + np.array((target[0].minRectX(), target[0].minRectY()))
coord_px = ROITopLeftCorner + np.array(target[0].centroid())
# Rotate the coordinates of roll angle around the middle of the screen
rot_coord_px = np.dot(ctm, coord_px - np.array([img.width/2, img.height/2])) + np.array([img.width/2, img.height/2])
if useBasemap:
coord = sp.deg2rad(m(rot_coord_px[0], img.height-rot_coord_px[1], inverse = True))
else:
coord = localProjection(rot_coord_px[0]-img.width/2, img.height/2-rot_coord_px[1], radius, mobile.yaw, mobile.pitch, inverse = True)
target_bearing, target_elevation = coord
# Get minimum bounding rectangle for display purpose
minR = ROITopLeftCorner + np.array(target[0].minRect())
contours = target[0].contour()
contours = [ ROITopLeftCorner + np.array(contour) for contour in contours]
# Get target features
angle = sp.deg2rad(target[0].angle()) + mobile.roll
angle = sp.deg2rad(unwrap180(sp.rad2deg(angle), sp.rad2deg(previous_angle)))
width = target[0].width()
height = target[0].height()
# Check if the kite is upside down
# First rotate the kite
ctm2 = np.array([[sp.cos(-angle+mobile.roll), -sp.sin(-angle+mobile.roll)], \
[sp.sin(-angle+mobile.roll), sp.cos(-angle+mobile.roll)]]) # Coordinate transform matrix
rotated_contours = [np.dot(ctm2, contour-coordMinRect) for contour in contours]
y = [-tmp[1] for tmp in rotated_contours]
itop = np.argmax(y) # Then looks at the points at the top
ibottom = np.argmin(y) # and the point at the bottom
# The point the most excentered is at the bottom
if abs(rotated_contours[itop][0])>abs(rotated_contours[ibottom][0]):
isInverted = True
else:
isInverted = False
if isInverted:
angle = angle + sp.pi
# Filter the data
alpha = 1-sp.exp(-deltaT/self.filterTimeConstant)
if not(isPaused):
dCoord = np.array(previous_dCoord)*(1-alpha) + alpha*(np.array(coord_px) - previous_coord_px) # related to the speed only if cam is fixed
dAngle = np.array(previous_dAngle)*(1-alpha) + alpha*(np.array(angle) - previous_angle)
else :
dCoord = np.array([0, 0])
dAngle = np.array([0])
#print coord_px, angle, width, height, dCoord
# Record important data
times.append(timestamp)
coords_px.append(coord_px)
angles.append(angle)
target_elevations.append(target_elevation)
target_bearings.append(target_bearing)
# Export data to controller
self.elevation = target_elevation
self.bearing = target_bearing
self.orientation = angle
dt = time.time()-timeLastTarget
self.ROT = dAngle/dt
self.lastUpdateTime = t
# Save for initialisation of next step
previous_dCoord = dCoord
previous_angle = angle
previous_coord_px = (int(coord_px[0]), int(coord_px[1]))
wasTargetFoundInPreviousFrame = True
timeLastTarget = time.time()
else:
wasTargetFoundInPreviousFrame = False
if useHDF5:
hdfSize = hdfSize+1
dset.resize((hdfSize, 7))
imset.resize((hdfSize, img.width, img.height, 3))
dset[hdfSize-1,:] = [time.time(), coord_px[0], coord_px[1], angle, self.elevation, self.bearing, self.ROT]
imset[hdfSize-1,:,:,:] = img.getNumpy()
recordFile.flush()
else:
csv_writer.writerow([time.time(), coord_px[0], coord_px[1], angle, self.elevation, self.bearing, self.ROT])
if display :
if target:
# Add target features to layer
# Minimal rectange and its center in RED
layer.polygon(minR[(0, 1, 3, 2), :], color = Color.RED, width = 5)
layer.circle((int(coordMinRect[0]), int(coordMinRect[1])), 10, filled = True, color = Color.RED)
# Target contour and centroid in BLUE
layer.circle((int(coord_px[0]), int(coord_px[1])), 10, filled = True, color = Color.BLUE)
layer.polygon(contours, color = Color.BLUE, width = 5)
# Speed vector in BLACK
layer.line((int(coord_px[0]), int(coord_px[1])), (int(coord_px[0]+20*dCoord[0]), int(coord_px[1]+20*dCoord[1])), width = 3)
# Line giving angle
layer.line((int(coord_px[0]+200*sp.cos(angle)), int(coord_px[1]+200*sp.sin(angle))), (int(coord_px[0]-200*sp.cos(angle)), int(coord_px[1]-200*sp.sin(angle))), color = Color.RED)
# Line giving rate of turn
#layer.line((int(coord_px[0]+200*sp.cos(angle+dAngle*10)), int(coord_px[1]+200*sp.sin(angle+dAngle*10))), (int(coord_px[0]-200*sp.cos(angle + dAngle*10)), int(coord_px[1]-200*sp.sin(angle+dAngle*10))))
# Add the layer to the raw image
toDisplay.addDrawingLayer(layer)
toDisplay.addDrawingLayer(selectionLayer)
# Add time metadata
toDisplay.drawText(str(i_frame)+" "+ str(timestamp), x=0, y=0, fontsize=20)
# Add Line giving horizon
#layer.line((0, int(img.height/2 + mobile.pitch*pixelPerRadians)),(img.width, int(img.height/2 + mobile.pitch*pixelPerRadians)), width = 3, color = Color.RED)
# Plot parallels
for lat in range(-90, 90, 15):
r = range(0, 361, 10)
if useBasemap:
# \todo improve for high roll
l = m (r, [lat]*len(r))
pix = [np.array(l[0]), img.height-np.array(l[1])]
else:
l = localProjection(sp.deg2rad(r), \
sp.deg2rad([lat]*len(r)), \
radius, \
lon_0 = mobile.yaw, \
lat_0 = mobile.pitch, \
inverse = False)
l = np.dot(ctm, l)
pix = [np.array(l[0])+img.width/2, img.height/2-np.array(l[1])]
for i in range(len(r)-1):
if isPixelInImage((pix[0][i],pix[1][i]), img) or isPixelInImage((pix[0][i+1],pix[1][i+1]), img):
layer.line((pix[0][i],pix[1][i]), (pix[0][i+1], pix[1][i+1]), color=Color.WHITE, width = 2)
# Plot meridians
for lon in range(0, 360, 15):
r = range(-90, 91, 10)
if useBasemap:
# \todo improve for high roll
l = m ([lon]*len(r), r)
pix = [np.array(l[0]), img.height-np.array(l[1])]
else:
l= localProjection(sp.deg2rad([lon]*len(r)), \
sp.deg2rad(r), \
radius, \
lon_0 = mobile.yaw, \
lat_0 = mobile.pitch, \
inverse = False)
l = np.dot(ctm, l)
pix = [np.array(l[0])+img.width/2, img.height/2-np.array(l[1])]
for i in range(len(r)-1):
if isPixelInImage((pix[0][i],pix[1][i]), img) or isPixelInImage((pix[0][i+1],pix[1][i+1]), img):
layer.line((pix[0][i],pix[1][i]), (pix[0][i+1], pix[1][i+1]), color=Color.WHITE, width = 2)
# Text giving bearing
# \todo improve for high roll
for bearing_deg in range(0, 360, 30):
l = localProjection(sp.deg2rad(bearing_deg), sp.deg2rad(0), radius, lon_0 = mobile.yaw, lat_0 = mobile.pitch, inverse = False)
l = np.dot(ctm, l)
layer.text(str(bearing_deg), ( img.width/2+int(l[0]), img.height-20), color = Color.RED)
# Text giving elevation
# \todo improve for high roll
for elevation_deg in range(-60, 91, 30):
l = localProjection(0, sp.deg2rad(elevation_deg), radius, lon_0 = mobile.yaw, lat_0 = mobile.pitch, inverse = False)
l = np.dot(ctm, l)
layer.text(str(elevation_deg), ( img.width/2 ,img.height/2-int(l[1])), color = Color.RED)
#toDisplay.save(js)
toDisplay.save(disp)
if display :
toDisplay.removeDrawingLayer(1)
toDisplay.removeDrawingLayer(0)
recordFile.close()
def bm29A(self):
"""define an A attribute with monochromator angle position
corrisponding to each energy point of the spectra """
self.A = scipy.rad2deg(
scipy.arcsin((1.23984E-6 / (self.E)) * 1E10 / (2 * self.dspac)))
# Relative error function.
def rel_err(get, expect):
a = euclidean(get, expect)
b = norm(expect)
if b == 0:
return a
else:
return a/b
sphere = WGS84_ASPHERE_RADIANS
ellipsoid = WGS84_ELLIPSOID_RADIANS
ellipsoid_deg = WGS84_ELLIPSOID
lam = pi/3
phi = pi/5
lam_deg, phi_deg = rad2deg([lam, phi])
# Mercator projection:
merc = Proj(ellipsoid=sphere, proj='merc')
# Lambert cylindrical equal area projection:
cea = Proj(ellipsoid=sphere, proj='cea')
cea_e = Proj(ellipsoid=ellipsoid, proj='cea')
cea_ed = Proj(ellipsoid=ellipsoid_deg, proj='cea')
class MyTestCase(unittest.TestCase):
def setUp(self):
pass
def test_fff_coeffs(self):
epsilon = 10e-4
E, F, G = fff_coeffs(merc, lam, phi)
def bm29A(self):
"""define an A attribute with monochromator angle position
corrisponding to each energy point of the spectra """
self.A = scipy.rad2deg(scipy.arcsin((1.23984E-6/(self.E))*1E10/(2*self.dspac)))
def xyzcontour(
theta,
phi,
x,
y,
z,
vmin=None,
vmax=None,
cmap="bwr",
projection="robin",
mode="xyz",
units="nT",
time=None,
string="{0}",
regular=True,
resolution=200,
):
"""
plots scalar fields x,y,z in a world map.
options:
· vmin,vmax : maximum and minimum color scale limits. if set to None, they will be automatically chosen so as to cover the entire data range + be symmetric around zero.
· cmap : matplotlib colormap to use
· projection : map projection. currently only cylindrical equirectangular ("cyl") and robinson ("robin") projections are supported.
· mode: toggles between plotting components normally ("xyz") and treating the last one as an intensity ("dif"), not very polished yet
· units: just the colorbar label
· time: time for title format string
· string: format string, where argument {0} is X,Y,Z and argument {1} is str(time)
· regular: whether input coordinates are a regular grid (True, i.e. are vectors of coordinates of shape (n,) (m,) and x,y,z are arrays of shape (n,m), or they are just a set of points (False, i.e. coordinates are of shape (n,) (n,) and x,y,z are arrays of shape (n,)). If set to False, a regular grid (latitude x longitude) will be constructed before plotting, which will be S L O W ~ A S ~ H E C K.
· resolution: resolution for the regular grid constructed when input is not regular. Default is 200x200 points.
"""
from matplotlib import pyplot, colors
from mpl_toolkits.basemap import Basemap
if projection == "cyl":
base = Basemap(
projection="cyl",
llcrnrlat=90 - scipy.rad2deg(max(theta)),
urcrnrlat=90 - scipy.rad2deg(min(theta)),
llcrnrlon=scipy.rad2deg(min(phi)),
urcrnrlon=scipy.rad2deg(max(phi)),
# llcrnrlat=-90, urcrnrlat=90,llcrnrlon=-180,urcrnrlon=180,
resolution="l",
)
elif projection == "robin":
base = Basemap(projection="robin", lon_0=0.0)
else:
raise Exception("bad projection :'(")
if not regular:
phinew = scipy.linspace(-numpy.pi, numpy.pi, resolution)
thetanew = scipy.linspace(0.01, numpy.pi - 0.01, resolution)
thetagrid, phigrid = scipy.meshgrid(thetanew, phinew, indexing="xy")
x = scipy.interpolate.griddata((theta, phi), x, (thetagrid, phigrid), method="linear")
y = scipy.interpolate.griddata((theta, phi), y, (thetagrid, phigrid), method="linear")
z = scipy.interpolate.griddata((theta, phi), z, (thetagrid, phigrid), method="linear")
x[numpy.isnan(x)] = 0.0
y[numpy.isnan(y)] = 0.0
z[numpy.isnan(z)] = 0.0
phi = phinew
theta = thetanew
xtrans = numpy.rad2deg(phi)
ytrans = 90 - numpy.rad2deg(theta)
fig = pyplot.figure(figsize=(10, 13))
axis1 = fig.add_subplot(311)
axis1.set_title(string.format("X", str(time)))
axis2 = fig.add_subplot(312)
axis2.set_title(string.format("Y", str(time)))
axis3 = fig.add_subplot(313)
axis3.set_title(string.format("Z", str(time)))
for a in (axis1, axis2, axis3):
base.drawcoastlines(ax=a)
base.drawparallels(numpy.arange(-60.0, 90.0, 30.0), ax=a)
base.drawmeridians(numpy.arange(0.0, 420.0, 60.0), labels=[0, 0, 0, 1], fontsize=10, ax=a)
base.drawmapboundary(ax=a)
if mode == "dif":
xycmap = colors.LinearSegmentedColormap(
"crisisperrotini",
segmentdata={
"red": [(0.0, 0.0, 0.0), (0.5, 1.0, 1.0), (0.75, 1.0, 1.0), (1.0, 0.0, 0.0)],
"green": [(0.0, 0.0, 0.0), (0.5, 1.0, 1.0), (1.0, 0.0, 0.0)],
"blue": [(0.0, 0.0, 0.0), (0.25, 1.0, 1.0), (0.5, 1.0, 1.0), (1.0, 0.0, 0.0)],
},
)
zcmap = cmap
else:
xycmap = zcmap = cmap
xx, yy = numpy.meshgrid(xtrans, ytrans)
if not vmin or not vmax:
xmax = numpy.max(numpy.abs(x))
ymax = numpy.max(numpy.abs(y))
zmax = numpy.max(numpy.abs(z))
m = base.contourf(xx, yy, x.transpose(), 31, latlon=True, ax=axis1, vmin=-xmax, vmax=xmax, cmap=xycmap)
cbar = base.colorbar(mappable=m, ax=axis1)
cbar.set_label(units)
m = base.contourf(xx, yy, y.transpose(), 31, latlon=True, ax=axis2, vmin=-ymax, vmax=ymax, cmap=xycmap)
cbar = base.colorbar(mappable=m, ax=axis2)
cbar.set_label(units)
m = base.contourf(xx, yy, z.transpose(), 31, latlon=True, ax=axis3, vmin=-zmax, vmax=zmax, cmap=zcmap)
cbar = base.colorbar(mappable=m, ax=axis3)
cbar.set_label(units)
else:
base.contourf(xx, yy, x.transpose(), 31, latlon=True, ax=axis1, vmin=vmin, vmax=vmax, cmap=xycmap)
base.contourf(xx, yy, y.transpose(), 31, latlon=True, ax=axis2, vmin=vmin, vmax=vmax, cmap=xycmap)
base.contourf(xx, yy, z.transpose(), 31, latlon=True, ax=axis3, vmin=vmin, vmax=vmax, cmap=zcmap)
return fig
a = euclidean(get, expect)
b = norm(expect)
if b == 0:
return a
else:
return a/b
# Define lon-lat input points to test.
RADIANS = False # Work in radians (True) or degrees (False)?
if RADIANS:
PI = pi
else:
PI = 180
phi_0 = arcsin(2.0/3)
if not RADIANS:
phi_0 = rad2deg(phi_0)
a = (0, PI/3)
b = (0, -PI/3)
inputs = [
(0, 0),
(0, phi_0),
(0, -phi_0),
(PI/2, 0),
(-PI/2, 0),
(-PI, 0),
(-PI, PI/2),
(-PI, -PI/2),
a,
b,
]
def xyz2llz(x, y, z):
"""
convert earth-centered, earth-fixed (ECEF) cartesian x, y, z to latitude, longitude, and altitude
code is based on:
https://www.mathworks.com/matlabcentral/fileexchange/7941-convert-cartesian--ecef--coordinates-to-lat--lon--alt?focused=5062924&tab=function
Parameters
----------
x: float
x-coordinate normalized to the radius of Earh
y: float
y-coordinate normalized to the radius of Earh
z: float
z-coordinate normalized to the radius of Earh
Returns
-------
lat: float
latitude (deg)
lon: float
longitude (deg)
depth: float
depth (km)
"""
import numpy as np
import math
from scipy import deg2rad, rad2deg
# World Geodetic System 1984
# WGS 84
#
erad = np.float64(
6378137.0) # Radius of the Earth in meters (equatorial radius, WGS84)
rad = 1 # sphere radius
e = np.float64(8.1819190842622e-2)
# convert to radius
x = x * erad / rad
y = y * erad / rad
z = z * erad / rad
b = np.sqrt(erad * erad * (1 - e * e))
ep = np.sqrt((erad * erad - b * b) / (b * b))
p = np.sqrt(x * x + y * y)
th = np.arctan2(erad * z, b * p)
lon = np.arctan2(y, x)
lat = np.arctan2((z + ep * ep * b * np.sin(th) * np.sin(th) * np.sin(th)),
(p - e * e * erad * np.cos(th) * np.cos(th) * np.cos(th)))
N = erad / np.sqrt(1.0 - e * e * np.sin(lat) * np.sin(lat))
alt = p / np.cos(lat) - N
lon = lon % (math.pi * 2.0)
lon = rad2deg(lon)
if lon > 180.0:
lon -= 360.0
lat = rad2deg(lat)
alt = -1 * (alt) / 1000.0 # depth as negative alt
return lat, lon, alt
def purcell_filling_angle(physics, phase, network, r_toroid,
surface_tension='pore.surface_tension',
contact_angle='pore.contact_angle',
diameter='throat.diameter',
Pc=1e3,
**kwargs):
r"""
Calculate the filling angle (alpha) for a given capillary pressure
Parameters
----------
network : OpenPNM Network Object
The Network on which to apply the calculation
sigma : dict key (string)
The dictionary key containing the surface tension values to be used. If
a pore property is given, it is interpolated to a throat list.
theta : dict key (string)
The dictionary key containing the contact angle values to be used. If
a pore property is given, it is interpolated to a throat list.
throat_diameter : dict key (string)
The dictionary key containing the throat diameter values to be used.
r_toroid : float or array_like
The radius of the toroid surrounding the pore
Notes
-----
This approach accounts for the converging-diverging nature of many throat
types. Advancing the meniscus beyond the apex of the toroid requires an
increase in capillary pressure beyond that for a cylindical tube of the
same radius. The details of this equation are described by Mason and
Morrow [1]_, and explored by Gostick [2]_ in the context of a pore network
model.
!!! Takes mean contact angle and surface tension !!!
"""
from scipy import ndimage
entity = diameter.split('.')[0]
if surface_tension.split('.')[0] == 'pore' and entity == 'throat':
sigma = phase[surface_tension]
sigma = phase.interpolate_data(data=sigma)
else:
sigma = phase[surface_tension]
if contact_angle.split('.')[0] == 'pore' and entity == 'throat':
theta = phase[contact_angle]
theta = phase.interpolate_data(data=theta)
else:
theta = phase[contact_angle]
# Mason and Morrow have the definitions switched
theta = 180 - theta
theta = _sp.deg2rad(theta)
rt = network[diameter]/2
R = r_toroid
ratios = rt/R
a_max = theta - np.arcsin((np.sin(theta))/(1 + ratios))
def purcell_pressure(ratio, fill_angle, theta, sigma, R):
# Helper function
a_max = theta - np.arcsin((np.sin(theta))/(1+ratio))
fill_angle[fill_angle > a_max] = a_max
r_men = R*(1+(ratio)-_sp.cos(fill_angle))/_sp.cos(theta-fill_angle)
Pc = 2*sigma/r_men
return Pc
fill_angle = _sp.deg2rad(np.linspace(-30, 150, 1001))
alpha = np.zeros_like(ratios)
for T, ratio in enumerate(ratios):
mask = np.zeros_like(fill_angle, dtype=bool)
nudge = 100
all_Pc = purcell_pressure(ratio, fill_angle, theta[T], sigma[T], R)
if Pc > all_Pc.max():
# Target Pc out of range
lowest = fill_angle[np.argwhere(all_Pc == all_Pc.max())[0][0]]
else:
while np.sum(mask) == 0:
plus_mask = all_Pc < Pc + nudge
minus_mask = all_Pc > Pc - nudge
mask = np.logical_and(plus_mask, minus_mask)
if np.sum(mask) == 0:
nudge += 10
regions = ndimage.find_objects(ndimage.label(mask)[0])
rx = [np.mean(fill_angle[regions[r]]) for r in range(len(regions))]
root_x = np.asarray(rx)
lowest = np.min(root_x)
alpha[T] = lowest
logger.info('Filling angles calculated for Pc: '+str(Pc))
physics['throat.alpha_max'] = a_max
return _sp.rad2deg(alpha)
for i in range(0, len(DELTA)):
taxaD = (DELTA[i, 0] - DELTA1[i, 0]) / DELTA[i, 0]
if abs(taxaD) < Vcorrecao:
MENSAGEIM1 = "Criterio de convergencia"
MENSAGEM2 = "Teste convergiu para o teste do vetor das correções"
break
DELTA1 = DELTA # Armazena a variavel para o teste
#Teste para o vetor atualizado
maximo = CCE.max() # busca o maior valor nos fatores de correção
Mang = max(
[abs(number) for number in [maximo.omega, maximo.kappa, maximo.phi]])
Mcoord = max([abs(number) for number in [maximo.X, maximo.Y, maximo.Z]])
Maps = max([
abs(number) for number in [maximo.a0, maximo.b0, maximo.b1, maximo.c0]
])
if (Mang < sc.rad2deg(Lang) and Mcoord < Lcoord and Maps < Laps):
MENSAGEIM1 = "Criterio de convergencia"
MENSAGEM2 = "Atingiu as correções minimas"
break
if iteracao == limite:
MENSAGEIM1 = "Criterio de convergencia"
MENSAGEM2 = "Atingiu o limite de iterações"
contador = contador + 1
# -------------------------- Posteriores --------------------------
# Matriz estatisticas das equações
QXX = inv(N + NC + WXX) # Matriz cofator dos Parametros
WXX = inv(QXX) # Matriz peso dos Parametros
QC = ACT.dot(QC.dot(np.transpose(ACT))) # Matriz cofator das Restrições
WC = inv(QC) # Matriz peso das Restrições
W = np.array(W).astype(np.float64)
def decodeMessageSensorUDP(self, msg):
""" This is used to decode message from sensorUDP application from the android market.
The orientation field was first used, but its conventions were unclear.
So now acceleration and magnetic vectors should be used"""
data = msg.split(', ')
if data[0] == 'G':
# This is GPS message
time = decimalstr2float(data[2])
latitude_deg = decimalstr2float(data[3])
longitude_deg = decimalstr2float(data[4])
altitude = decimalstr2float(data[5])
hdop = decimalstr2float(
data[7]) # Horizontal dilution of precision
vdop = decimalstr2float(data[8]) # Vertical dilution of precision
print time, latitude_deg, longitude_deg, altitude, hdop, vdop
if data[0] == 'O':
# \note This is no more used as orientation convention were unclear
# 'O, 146, 1366575961732, 230,1182404, -075,2031250, 001,7968750'
[
u,
u, # data not used \
heading_deg, # pointing direction of top of phone \
roll_deg, # around horizontal axis, positive clockwise [-180:180] \
pitch_deg
] = decimalstr2float(data[1:]) # around vertical axis [_90:90]
elevation_deg = -sp.rad2deg(sp.arctan2( \
sp.cos(sp.deg2rad(pitch_deg))*sp.cos(sp.deg2rad(roll_deg)), \
sp.sqrt(1+sp.cos(sp.deg2rad(roll_deg))**2*(sp.sin(sp.deg2rad(pitch_deg))**2-1)))) #positive up
inclinaison_deg = pitch_deg #positive clockwise
print heading_deg, roll_deg, pitch_deg, elevation_deg, inclinaison_deg
if data[0] == 'A':
# Accelerometer data
# Index and sign are adjusted to obtain x through the screen, and z down
deltaT = decimalstr2float(data[2]) / 1000 - self.time_acceleration
if self.filterTimeConstant == 0.0:
alpha = 1
else:
alpha = 1 - sp.exp(-deltaT / self.filterTimeConstant)
self.time_acceleration = decimalstr2float(data[2]) / 1000
self.acceleration_raw[0] = decimalstr2float(data[3])
self.acceleration_raw[1] = decimalstr2float(data[4])
self.acceleration_raw[2] = decimalstr2float(data[5])
# Filter the data
self.acceleration_filtered += alpha * (
sp.array(self.acceleration_raw) - self.acceleration_filtered)
if data[0] == 'M':
# Magnetometer data
# Index and sign are adjusted to obtain x through the screen, and z down
deltaT = decimalstr2float(data[2]) / 1000 - self.time_magnetic
if self.filterTimeConstant == 0.0:
alpha = 1
else:
alpha = 1 - sp.exp(-deltaT / self.filterTimeConstant)
self.time_magnetic = decimalstr2float(data[2]) / 1000
self.magnetic_raw[0] = decimalstr2float(data[3])
self.magnetic_raw[1] = decimalstr2float(data[4])
self.magnetic_raw[2] = -decimalstr2float(
data[5]) # Adapt to a bug in sensorUDP?
# Filter the data
self.magnetic_filtered += alpha * (sp.array(self.magnetic_raw) -
self.magnetic_filtered)
def Compute_Coupler(Resonator_ID):
""" Computes Line coupler length and Aux coupler length, if an Aux coupler is needed. Uses this to compute Resonator Length (which is length of the meander excluding the coupler).
Adds resonator and through line Eeff and Port_Z to table of computed parameters"""
Coupler_Length = 0.0
Aux_Coupler_Length = 0.0
_Length = 0.0
b = beta(Resonator_ID)
Freq, Design_Q, Geometry = Load_Freq_Q_Geo(Resonator_ID)
Coupler_Zone = Mask_DB.Get_Mask_Data("SELECT Coupler_Zone FROM Resonators Where resonator_id = " + str(Resonator_ID) ,'one')[0]
### Coupler Zone shall be greater than a coupler_offset which yields Coupler_Zone_Q_Limit
Coupler_Zone_Q_Limit = float(pow(10,10))
### in general "Coupler Zone Q" is the Q attained using a coupler_offset = coupler_zone
Sim = Mask_DB.Get_Simulation_Data(Geometry, "CouplerSweep")
######## Extract Resonator and Throughline Eeff #####
Sim.values('Eeff')
Eeff = sp.absolute(Sim.interp(Freq,Parameter_Value = Sim.Pmax))
Resonator_Eeff = Eeff[2]
Through_Line_Eeff = Eeff[1]
Sim.values('Port_Z0')
Port_Z0 = sp.absolute(Sim.interp(Freq,Parameter_Value = Sim.Pmax))
Resonator_Impedance = Port_Z0[2]
Through_Line_Impedance = Port_Z0[1]
########
Sim.values('Port_S')
try:
offset = Sim.optvalues(Freq,Q2S(Coupler_Zone_Q_Limit),2,0)[0] #May fail if S31 at Sim.Pmax yelds Q < Coupler_Zone_Q_Limit
except:
offset = Sim.Pmax
print("Function %s in module %s: For Resonator_ID = %i, insufficient Coupler_Offset in Simulation to attain Coupler Zone Q > %i" % (__name__, __file__,Resonator_ID,Coupler_Zone_Q_Limit))
if offset > Coupler_Zone:
print("Function %s in module %s: For Resonator_ID = %i, Coupler_Zone Increased to make Coupler Zone Q closer to %i" % (__name__, __file__,Resonator_ID,Coupler_Zone_Q_Limit))
offset = Coupler_Zone
delta_L = Sim.Pmax - Coupler_Zone #is a Length, um
if delta_L <= 0:
warnings.warn('Coupler_Zone is greater than Coupler_Offset')
delta_coupler_phase = -2.0 * b * delta_L #2 is becasue 1/2-wave resonator, phase is in radians
if Design_Q < Sim.Qmin(Freq): #Condition where an Aux is needed.
Sim = Mask_DB.Get_Simulation_Data(Geometry, "AuxCouplerSweep")
Sim.values('Port_S')
Coupler_Length = Coupler_Zone
if Design_Q < Sim.Qmin(Freq): #Condition where an Aux at its maximum length it not sufficient
print("Function %s in module %s: Design_Q is lower than minimum acheivable Q with Pad Coupler for Resonator_ID = %i. Using Max Coupler Pad Length" % (__name__, __file__,Resonator_ID))
####### CAUTION ########
Design_Q = Sim.Qmin(Freq)
########################
Aux_Coupler_Length = Sim.optvalues(Freq,Q2S(Design_Q),2,0)[0]
_Length = Aux_Coupler_Length
else:
Coupler_Length = Sim.optvalues(Freq,Q2S(Design_Q),2,0)[0]
_Length = Coupler_Length
#compute resonator length
Coupler_Phase_Change = sp.absolute(sp.angle(Sim.interp(Freq,Parameter_Value = _Length)[2,2], deg = False)) + delta_coupler_phase #radians
Resonator_Phase_Length = 2.0*sp.pi - Coupler_Phase_Change
#Note: Computed Resonator Length has been shortened by presence of coupler. phase change of Coupler has been subtracted from vacuum lenght or resonator.
Resonator_Length = Resonator_Phase_Length/(2*b)
#This is the "midpoint", as determined by phase = pi, along the resonator length where the current is maximal. NOTE: THis is along the meandered portion of the resonator. The couple has been substracted
Max_Current_Length = sp.around((Resonator_Phase_Length-sp.pi)/(2*b),decimals=3)
#print(Sim.Current_Attribute,{"Coupler_Zone" : Coupler_Zone, "Design_Q" : Design_Q, "Aux_Coupler_Length" : Aux_Coupler_Length, "Coupler_Length" : Coupler_Length, "Coupler_Phase_Change" : Coupler_Phase_Change})
Mask_DB.Update_Computed_Parameters(Resonator_ID, {"Coupler_Zone" : Coupler_Zone, "Design_Q" : Design_Q, "Aux_Coupler_Length" : Aux_Coupler_Length, "Coupler_Length" : Coupler_Length, "Coupler_Phase_Change" : Coupler_Phase_Change, "Resonator_Length" : Resonator_Length,"Resonator_Eeff":Resonator_Eeff, "Through_Line_Eeff":Through_Line_Eeff, "Resonator_Impedance":Resonator_Impedance,"Through_Line_Impedance":Through_Line_Impedance,"Max_Current_Length" : Max_Current_Length} )
return (Coupler_Zone,Design_Q,Aux_Coupler_Length,Coupler_Length, sp.rad2deg(Coupler_Phase_Change),Resonator_Length)
lon_0 = 270
lat_0 = 80
pixelPerRadians = 640
height = 480
radius = pixelPerRadians
max_length = 0
cam = JpegStreamCamera('http://192.168.43.1:8080/videofeed') #640 * 480
mobile = mobileState.mobileState()
mobile.open()
while True:
mobile.checkUpdate()
if mobile.isToUpdate:
mobile.computeRPY()
image = cam.getImage().rotate(-sp.rad2deg(mobile.roll), fixed=False)
m = Basemap(width=image.width,
height=image.height,
projection='aeqd',
lat_0=sp.rad2deg(mobile.pitch),
lon_0=sp.rad2deg(mobile.yaw),
rsphere=radius)
# fill background.
#m.drawmapboundary(fill_color='aqua')
# draw coasts and fill continents.
#m.drawcoastlines(linewidth=0.5)
#m.fillcontinents(color='coral',lake_color='aqua')
# 20 degree graticule.
# m.drawparallels(np.arange(-90,90,30))
#m.drawmeridians(np.arange(-180,180,30))
# draw a black dot at the center.
def flowprandtlmeyer(**flow):
"""
Prandtl-Meyer function for expansion waves.
This function accepts a given set of specific heat ratios and
an input of either Mach number, Mach angle or Prandtl-Meyer angle.
Inputs can be a single scalar or an array_like data structure.
Parameters
----------
gamma : array_like, optional
Specific heat ratio. Values must be greater than 1.
M : array_like
Mach number. Values must be greater than or equal to 1.
nu : array_like
Prandtl-Meyer angle [degrees]. Values must be
0 <= M <= 90*(sqrt((g+1)/(g-1))-1).
mu : array_like
Mach angle [degrees]. Values must be 0 <= M <= 90.
Returns
-------
out : (M, nu, mu)
Tuple of Mach number, Prandtl-Meyer angle, Mach angle.
Examples
--------
>>> flowprandtlmeyer(M=5)
(5.0, 76.920215508538789, 11.536959032815489)
"""
#parse the input
gamma, flow, mtype, itype = _flowinput(flow)
#calculate gamma-ratios for use in the equations
l = sp.sqrt((gamma-1)/(gamma+1))
#preshape mach array
M = sp.empty(flow.shape, sp.float64)
#use prandtl-meyer relation to solve for the mach number
if mtype in ["mach", "m"]:
if (flow < 1).any():
raise Exception("Mach number inputs must be real numbers greater" \
" than or equal to 1.")
M = flow
elif mtype in ["mu", "machangle"]:
if (flow < 0).any() or (flow > 90).any():
raise Exception("Mach angle inputs must be real numbers" \
" 0 <= M <= 90.")
M = 1 / sp.sin(sp.deg2rad(flow))
elif mtype in ["nu", "pm", "pmangle"]:
if (flow < 0).any() or (flow > 90*((1/l)-1)).any():
raise Exception("Prandtl-Meyer angle inputs must be real" \
" numbers 0 <= M <= 90*(sqrt((g+1)/(g-1))-1).")
M[:] = 2 #initial guess for the solution
for _ in xrange(_AETB_iternum):
b = sp.sqrt(M**2 - 1)
f = -sp.deg2rad(flow) + (1/l) * sp.arctan(l*b) - sp.arctan(b)
g = b*(1 - l**2) / (M*(1 + (l**2)*(b**2))) #derivative
M = M - (f / g) #Newton-Raphson
else:
raise Exception("Keyword input must be an acceptable string to" \
" select input parameter.")
#normal shock relations
b = sp.sqrt(M**2 - 1)
V = (1/l) * sp.arctan(l*b) - sp.arctan(b)
U = sp.arcsin(1 / M)
return from_ndarray(itype, M, sp.rad2deg(V), sp.rad2deg(U))
def test_healpix_sphere(self):
# Sphere parameters.
R = 5
# Expected outputs of healpix_sphere() applied to inputs.
if RADIANS:
sigma_a = sqrt(3 - 3*sin(a[1]))
else:
sigma_a = sqrt(3 - 3*sin(deg2rad(a[1])))
ha = (pi/4*(1 - sigma_a), pi/4*(2 - sigma_a))
hb = (ha[0], -ha[1])
healpix_sphere_outputs = [
(0, 0),
(0, pi/4),
(0, -pi/4),
(pi/2, 0),
(-pi/2, 0),
(-pi, 0),
(-3*pi/4, pi/2),
(-3*pi/4, -pi/2),
ha,
hb
]
healpix_sphere_outputs = [tuple(R*array(p)) for p in
healpix_sphere_outputs]
# Forward projection should be correct on test points.
f = Proj(proj='healpix', R=R)
given = inputs
get = [f(*p, radians=RADIANS) for p in given]
expect = healpix_sphere_outputs
# Fuzz to allow for rounding errors:
error = 1e-12
print()
print('='*80)
print('HEALPix forward projection, sphere with radius R = %s' % R)
print('input (radians) / expected output (meters) / received output')
print('='*80)
for i in range(len(get)):
print(given[i], expect[i], get[i])
self.assertTrue(rel_err(get[i], expect[i]) < error)
# Inverse of projection of a point p should yield p.
given = get
get = [f(*q, radians=RADIANS, inverse=True) for q in given]
expect = inputs
print('='*80)
print('HEALPix inverse projection, sphere with radius R = %s' % R)
print('input (meters) / expected output (radians) / received output')
print('='*80)
for i in range(len(get)):
print(given[i], expect[i], get[i])
self.assertTrue(rel_err(get[i], expect[i]) < error)
# Inverse projection of p below should return longitude of -pi.
# Previously, it was returning a number slightly less than pi
# because of a rounding error, which got magnified by
# wrap_longitude()
p = R*array((-7*pi/8, 3*pi/8))
get = f(*p, radians=RADIANS, inverse=True)
p1 = arcsin(1 - 1.0/12)
if not RADIANS:
p1 = rad2deg(p1)
expect = (-PI, p1)
self.assertTrue(rel_err(get, expect) < error)
def ephem_doponly(maindir, tleoff=10.):
"""
This function will output a dictionary that can be used to remove the
frequency offset.
Args:
maindir (:obj:'str'): Directory that holds the digital rf and metadata.
tleoff (:obj:'float'): Offset of the tle from the actual data.
Returns:
outdict (dict[str, obj]): Output data dictionary::
{
't': Time in posix,
'dop1': Doppler frequency of 150 MHz channel from TLE ,
'dop2': Doppler frequency of 400 MHz channel from TLE ,
}
"""
#%% Get Ephem info
# Assuming this will stay the same
ut0 = 25567.5
e2p = 3600. * 24 #ephem day to utc seconds
sitepath = os.path.expanduser(os.path.join(maindir,
'metadata/config/site'))
sitemeta = drf.DigitalMetadataReader(sitepath)
sdict = sitemeta.read_latest()
sdict1 = list(sdict.values())[0]
infopath = os.path.expanduser(os.path.join(maindir, 'metadata/info'))
infometa = drf.DigitalMetadataReader(infopath)
idict = infometa.read_latest()
idict1 = list(idict.values())[0]
passpath = os.path.expanduser(os.path.join(maindir, 'metadata/pass/'))
passmeta = drf.DigitalMetadataReader(passpath)
pdict = passmeta.read_latest()
pdict1 = list(pdict.values())[0]
rtime = (pdict1['rise_time'] - ut0) * e2p
tsave = list(pdict.keys())[0]
Dop_bw = pdict1['doppler_bandwidth']
t = sp.arange(0, (Dop_bw.shape[0] + 1) * 10, 10.) + rtime
t = t.astype(float)
obsLoc = ephem.Observer()
obsLoc.lat = sdict1['latitude']
obsLoc.long = sdict1['longitude']
satObj = ephem.readtle(idict1['name'], idict1['tle1'][1:-1],
idict1['tle2'][1:-1])
tephem = (t - rtime) * ephem.second + pdict1['rise_time']
sublat = sp.zeros_like(tephem)
sublon = sp.zeros_like(tephem)
for i, itime in enumerate(tephem):
obsLoc.date = itime
satObj.compute(obsLoc)
sublat[i] = sp.rad2deg(satObj.sublat)
sublon[i] = sp.rad2deg(satObj.sublong)
# XXX Extend t vector because for the most part the velocities at the edge
# are not changing much so to avoid having the interpolation extrapolate.
# If extrapolation used then error messages that the time was off
t[-1] = t[-1] + 600
t[-2] = t[-2] + 500
t[0] = t[0] - 240
tdop = (t[0:(len(t) - 1)] + t[1:len(t)]) / 2.0
tdop[0] = tdop[0] - 35.0
# XXX Used this to line up inital TLE times
tdop = tdop - tleoff
tephem = (tdop - rtime) * ephem.second + pdict1['rise_time']
sublat = sp.zeros_like(tephem)
sublon = sp.zeros_like(tephem)
for i, itime in enumerate(tephem):
obsLoc.date = itime
satObj.compute(obsLoc)
sublat[i] = sp.rad2deg(satObj.sublat)
sublon[i] = sp.rad2deg(satObj.sublong)
return ({
"t": t,
'tsave': tsave,
"dop1": sp.interpolate.interp1d(tdop, Dop_bw[:, 0], kind="cubic"),
"dop2": sp.interpolate.interp1d(tdop, Dop_bw[:, 1], kind="cubic"),
'sublat': sp.interpolate.interp1d(tdop, sublat, kind="cubic"),
'sublon': sp.interpolate.interp1d(tdop, sublon, kind="cubic"),
'site_latitude': float(sdict1['latitude']),
'site_longitude': float(sdict1['longitude'])
})
def gprint(G, mtype="obs", bend=5, curve=5, R=1, layout=None, scale=5):
"""
Prints out an automatically layout compressed dbn repesentation of
the graph in TikZ/Latex format
"""
output = StringIO.StringIO()
BE = set()
n = len(G)
if not layout:
g = dict2graph(ecj.cloneBfree(G))
layout = g.layout_fruchterman_reingold(maxiter=50000, coolexp=1.1)
#layout = g.layout_graphopt(niter=50000, node_charge=0.08)
layout.center([0, 0])
layout.scale(float(1 / scipy.absolute(layout.coords).max()))
layout.scale(R)
cc = scipy.round_(array(layout.coords), decimals=4)
else:
g = dict2graph(ecj.cloneBfree(G))
cc = array(layout.coords)
paintSCC(g, colors)
for i in range(0, n):
node = g.vs[i]['label']
rc = g.vs[i]["color"]
print >>output, "{ \\definecolor{mycolor}{RGB}{"\
+str(rc[0])+","+str(rc[1])+","+str(rc[2])+"}"
mcolor = "fill = {rgb: red,"+str(rc[0])+"; green,"+str(rc[1])+\
"; blue,"+str(rc[2])+"}"
print >>output, "\\node["+mtype+", fill=mycolor] ("+node+") at ("+\
str(cc[i][0])+","+str(cc[i][1])+") {"+node+"};}"
for i in range(0, n):
v = g.vs[i]['label']
ll = [v + '/' + u for u in G[v]]
for l in ll:
a, b = l.split('/')
if G[a][b].intersection([(edge_type['bidirected'], 0)]):
if not (BE.intersection([(a, b)])
or BE.intersection([(b, a)])):
print >>output,' \\draw[pilip, on layer=back] ('+\
a+') -- ('+b+');'
if G[a][b].intersection([(edge_type['directed'], 1)]):
if a == b:
dff = cc[g.vs['label'].index(a)] - scipy.mean(cc, 0)
ang = scipy.arctan2(dff[1], dff[0])
ang_a = scipy.rad2deg(ang)
print >>output,"\\path[overlay,draw,pil] ("+a+")" +\
" .. controls +("+ "%.5f" % (bend+ang_a) +\
":"+ fstr % (2*curve)+"mm) and +("+\
"%.5f" % (ang_a-bend)+\
":"+"%.5f" % (2*curve)+"mm) .. ("+b+");"
else:
dff = cc[g.vs['label'].index(b)] \
- cc[g.vs['label'].index(a)]
ang = scipy.arctan2(dff[1], dff[0])
ang_a = scipy.rad2deg(ang)
ang_b = ang_a + 180
print >>output,"\\path[overlay,draw,pil] ("+a+")" +\
" .. controls +("+\
"%.5f" % (bend+ang_a) +\
":"+fstr % (curve)+"mm) and +("+\
fstr % (ang_b-bend)+\
":"+fstr % (curve)+"mm) .. ("+b+");"
return output
b = norm(expect)
if b == 0:
return a
else:
return a / b
# Define lon-lat input points to test.
RADIANS = False # Work in radians (True) or degrees (False)?
if RADIANS:
PI = pi
else:
PI = 180
phi_0 = arcsin(2.0 / 3)
if not RADIANS:
phi_0 = rad2deg(phi_0)
a = (0, PI / 3)
b = (0, -PI / 3)
inputs = [
(0, 0),
(0, phi_0),
(0, -phi_0),
(PI / 2, 0),
(-PI / 2, 0),
(-PI, 0),
(-PI, PI / 2),
(-PI, -PI / 2),
a,
b,
]
for in_data_file, param in data_file_dict.items():
fignum = param['fignum']
plt.figure(fignum, figsize=(10, 7))
with open(in_data_file, 'rb') as f:
data_list = pickle.load(f)
for i, data in enumerate(data_list):
angle = data['angle']
velocity = data['velocity']
cum_density = data['cum_density']
print('{}/{}: velocity = {:0.2f}'.format(i + 1, len(data_list),
velocity))
# Transform to degrees for plotting
angle_deg = scipy.rad2deg(angle)
cum_density_deg = cum_density / scipy.rad2deg(1)
plt.plot(scipy.rad2deg(angle_deg), cum_density_deg, 'b')
plt.grid(True)
plt.xlabel('angle (deg)')
plt.ylabel('(prob/deg)')
plt.title('exit probability density, D={:0.0f}, R={:0.0f}'.format(
param['D'], param['R']))
if save_figs:
plt.savefig('exit_prob_vs_angle.png', bbox_inches='tight')
plt.show()
def purcell(target,
r_toroid,
surface_tension='pore.surface_tension',
contact_angle='pore.contact_angle',
diameter='throat.diameter'):
r"""
Computes the throat capillary entry pressure assuming the throat is a
toroid.
Parameters
----------
target : OpenPNM Object
The object for which these values are being calculated. This
controls the length of the calculated array, and also provides
access to other necessary thermofluid properties.
r_toroid : float or array_like
The radius of the toroid surrounding the pore
surface_tension : dict key (string)
The dictionary key containing the surface tension values to be used.
If a pore property is given, it is interpolated to a throat list.
contact_angle : dict key (string)
The dictionary key containing the contact angle values to be used.
If a pore property is given, it is interpolated to a throat list.
diameter : dict key (string)
The dictionary key containing the throat diameter values to be used.
Notes
-----
This approach accounts for the converging-diverging nature of many throat
types. Advancing the meniscus beyond the apex of the toroid requires an
increase in capillary pressure beyond that for a cylindical tube of the
same radius. The details of this equation are described by Mason and
Morrow [1]_, and explored by Gostick [2]_ in the context of a pore network
model.
References
----------
.. [1] G. Mason, N. R. Morrow, Effect of contact angle on capillary
displacement curvatures in pore throats formed by spheres. J.
Colloid Interface Sci. 168, 130 (1994).
.. [2] J. Gostick, Random pore network modeling of fibrous PEMFC gas
diffusion media using Voronoi and Delaunay tessellations. J.
Electrochem. Soc. 160, F731 (2013).
"""
network = target.project.network
phase = target.project.find_phase(target)
element, sigma, theta = _get_key_props(phase=phase,
diameter=diameter,
surface_tension=surface_tension,
contact_angle=contact_angle)
r = network[diameter] / 2
R = r_toroid
alpha = theta - 180 + \
_sp.rad2deg(_sp.arcsin(_sp.sin(_sp.radians(theta))/(1+r/R)))
value = (-2*sigma/r) * \
(_sp.cos(_sp.radians(theta - alpha)) /
(1 + R/r*(1 - _sp.cos(_sp.radians(alpha)))))
if diameter.split('.')[0] == 'throat':
value = value[phase.throats(target.name)]
else:
value = value[phase.pores(target.name)]
return value
def flowprandtlmeyer(**flow):
"""
Prandtl-Meyer function for expansion waves.
This function accepts a given set of specific heat ratios and
an input of either Mach number, Mach angle or Prandtl-Meyer angle.
Inputs can be a single scalar or an array_like data structure.
Parameters
----------
gamma : array_like, optional
Specific heat ratio. Values must be greater than 1.
M : array_like
Mach number. Values must be greater than or equal to 1.
nu : array_like
Prandtl-Meyer angle [degrees]. Values must be
0 <= M <= 90*(sqrt((g+1)/(g-1))-1).
mu : array_like
Mach angle [degrees]. Values must be 0 <= M <= 90.
Returns
-------
out : (M, nu, mu)
Tuple of Mach number, Prandtl-Meyer angle, Mach angle.
Examples
--------
>>> flowprandtlmeyer(M=5)
(5.0, 76.920215508538789, 11.536959032815489)
"""
#parse the input
gamma, flow, mtype, itype = _flowinput(flow)
#calculate gamma-ratios for use in the equations
l = sp.sqrt((gamma - 1) / (gamma + 1))
#preshape mach array
M = sp.empty(flow.shape, sp.float64)
#use prandtl-meyer relation to solve for the mach number
if mtype in ["mach", "m"]:
if (flow < 1).any():
raise Exception("Mach number inputs must be real numbers greater" \
" than or equal to 1.")
M = flow
elif mtype in ["mu", "machangle"]:
if (flow < 0).any() or (flow > 90).any():
raise Exception("Mach angle inputs must be real numbers" \
" 0 <= M <= 90.")
M = 1 / sp.sin(sp.deg2rad(flow))
elif mtype in ["nu", "pm", "pmangle"]:
if (flow < 0).any() or (flow > 90 * ((1 / l) - 1)).any():
raise Exception("Prandtl-Meyer angle inputs must be real" \
" numbers 0 <= M <= 90*(sqrt((g+1)/(g-1))-1).")
M[:] = 2 #initial guess for the solution
for _ in xrange(_AETB_iternum):
b = sp.sqrt(M**2 - 1)
f = -sp.deg2rad(flow) + (1 / l) * sp.arctan(l * b) - sp.arctan(b)
g = b * (1 - l**2) / (M * (1 + (l**2) * (b**2))) #derivative
M = M - (f / g) #Newton-Raphson
else:
raise Exception("Keyword input must be an acceptable string to" \
" select input parameter.")
#normal shock relations
b = sp.sqrt(M**2 - 1)
V = (1 / l) * sp.arctan(l * b) - sp.arctan(b)
U = sp.arcsin(1 / M)
return from_ndarray(itype, M, sp.rad2deg(V), sp.rad2deg(U))
def gprint(G, mtype="obs", bend=5, curve=5, R=1, layout=None, scale=5):
"""
Prints out an automatically layout compressed dbn repesentation of
the graph in TikZ/Latex format
"""
output = StringIO.StringIO()
BE = set()
n = len(G)
if not layout:
g = dict2graph(ecj.cloneBfree(G))
layout = g.layout_fruchterman_reingold(maxiter=50000, coolexp=1.1)
# layout = g.layout_graphopt(niter=50000, node_charge=0.08)
layout.center([0, 0])
layout.scale(float(1 / scipy.absolute(layout.coords).max()))
layout.scale(R)
cc = scipy.round_(array(layout.coords), decimals=4)
else:
g = dict2graph(ecj.cloneBfree(G))
cc = array(layout.coords)
paintSCC(g, colors)
for i in range(0, n):
node = g.vs[i]['label']
rc = g.vs[i]["color"]
print >>output, "{ \\definecolor{mycolor}{RGB}{"\
+ str(rc[0]) + "," + str(rc[1]) + "," + str(rc[2]) + "}"
mcolor = "fill = {rgb: red," + str(rc[0]) + "; green," + str(rc[1]) +\
"; blue," + str(rc[2]) + "}"
print >>output, "\\node[" + mtype + ", fill=mycolor] (" + node + ") at (" +\
str(cc[i][0]) + "," + str(cc[i][1]) + ") {" + node + "};}"
for i in range(0, n):
v = g.vs[i]['label']
ll = [v + '/' + u for u in G[v]]
for l in ll:
a, b = l.split('/')
if G[a][b].intersection([(edge_type['bidirected'], 0)]):
if not(BE.intersection([(a, b)]) or BE.intersection([(b, a)])):
print >>output, ' \\draw[pilip, on layer=back] (' +\
a + ') -- (' + b + ');'
if G[a][b].intersection([(edge_type['directed'], 1)]):
if a == b:
dff = cc[g.vs['label'].index(a)] - scipy.mean(cc, 0)
ang = scipy.arctan2(dff[1], dff[0])
ang_a = scipy.rad2deg(ang)
print >>output, "\\path[overlay,draw,pil] (" + a + ")" +\
" .. controls +(" + "%.5f" % (bend + ang_a) +\
":" + fstr % (2 * curve) + "mm) and +(" +\
"%.5f" % (ang_a - bend) +\
":" + "%.5f" % (2 * curve) + "mm) .. (" + b + ");"
else:
dff = cc[g.vs['label'].index(b)] \
- cc[g.vs['label'].index(a)]
ang = scipy.arctan2(dff[1], dff[0])
ang_a = scipy.rad2deg(ang)
ang_b = ang_a + 180
print >>output, "\\path[overlay,draw,pil] (" + a + ")" +\
" .. controls +(" +\
"%.5f" % (bend + ang_a) +\
":" + fstr % (curve) + "mm) and +(" +\
fstr % (ang_b - bend) +\
":" + fstr % (curve) + "mm) .. (" + b + ");"
return output
def track(self):
print "Press right mouse button to pause or play"
print "Use left mouse button to select target"
print "Target color must be different from background"
print "Target must have width larger than height"
print "Target can be upside down"
#Parameters
isUDPConnection = False # Currently switched manually in the code
display = True
displayDebug = True
useBasemap = False
maxRelativeMotionPerFrame = 2 # How much the target can moved between two succesive frames
pixelPerRadians = 320
radius = pixelPerRadians
referenceImage = '../ObjectTracking/kite_detail.jpg'
scaleFactor = 0.5
isVirtualCamera = True
useHDF5 = False
# Open reference image: this is used at initlalisation
target_detail = Image(referenceImage)
# Get RGB color palette of target (was found to work better than using hue)
pal = target_detail.getPalette(bins=2, hue=False)
# Open video to analyse or live stream
#cam = JpegStreamCamera('http://192.168.1.29:8080/videofeed')#640 * 480
if isVirtualCamera:
#cam = VirtualCamera('../../zenith-wind-power-read-only/KiteControl-Qt/videos/kiteFlying.avi','video')
#cam = VirtualCamera('/media/bat/DATA/Baptiste/Nautilab/kite_project/robokite/ObjectTracking/00095.MTS', 'video')
#cam = VirtualCamera('output.avi', 'video')
cam = VirtualCamera(
'../Recording/Videos/Flying kite images (for kite steering unit development)-YTMgX1bvrTo.mp4',
'video')
virtualCameraFPS = 25
else:
cam = JpegStreamCamera(
'http://192.168.43.1:8080/videofeed') #640 * 480
#cam = Camera()
# Get a sample image to initialize the display at the same size
img = cam.getImage().scale(scaleFactor)
print img.width, img.height
# Create a pygame display
if display:
if img.width > img.height:
disp = Display(
(27 * 640 / 10, 25 * 400 / 10)
) #(int(2*img.width/scaleFactor), int(2*img.height/scaleFactor)))
else:
disp = Display((810, 1080))
#js = JpegStreamer()
# Initialize variables
previous_angle = 0 # target has to be upright when starting. Target width has to be larger than target heigth.
previous_coord_px = (
0, 0) # Initialized to top left corner, which always exists
previous_dCoord = previous_coord_px
previous_dAngle = previous_angle
angles = []
coords_px = []
coord_px = [0, 0]
angle = 0
target_elevations = []
target_bearings = []
times = []
wasTargetFoundInPreviousFrame = False
i_frame = 0
isPaused = False
selectionInProgress = False
th = [100, 100, 100]
skycolor = Color.BLUE
timeLastTarget = 0
# Prepare recording
recordFilename = datetime.datetime.utcnow().strftime(
"%Y%m%d_%Hh%M_") + 'simpleTrack'
if useHDF5:
try:
os.remove(recordFilename + '.hdf5')
except:
print('Creating file ' + recordFilename + '.hdf5')
""" The following line is used to silence the following error (according to http://stackoverflow.com/questions/15117128/h5py-in-memory-file-and-multiprocessing-error)
#000: ../../../src/H5F.c line 1526 in H5Fopen(): unable to open file
major: File accessability
minor: Unable to open file"""
h5py._errors.silence_errors()
recordFile = h5py.File(
os.path.join(os.getcwd(), 'log', recordFilename + '.hdf5'),
'a')
hdfSize = 0
dset = recordFile.create_dataset('kite', (2, 2),
maxshape=(None, 7))
imset = recordFile.create_dataset('image',
(2, img.width, img.height, 3),
maxshape=(None, img.width,
img.height, 3))
else:
try:
os.remove(recordFilename + '.csv')
except:
print('Creating file ' + recordFilename + '.csv')
recordFile = file(
os.path.join(os.getcwd(), 'log', recordFilename + '.csv'), 'a')
csv_writer = csv.writer(recordFile)
csv_writer.writerow([
'Time (s)', 'x (px)', 'y (px)', 'Orientation (rad)',
'Elevation (rad)', 'Bearing (rad)', 'ROT (rad/s)'
])
# Launch a thread to get UDP message with orientation of the camera
mobile = mobileState.mobileState()
if isUDPConnection:
mobile.open()
# Loop while not canceled by user
t0 = time.time()
previousTime = t0
while not (display) or disp.isNotDone():
t = time.time()
deltaT = (t - previousTime)
FPS = 1.0 / deltaT
#print 'FPS =', FPS
if isVirtualCamera:
deltaT = 1.0 / virtualCameraFPS
previousTime = t
i_frame = i_frame + 1
timestamp = datetime.datetime.utcnow()
# Receive orientation of the camera
if isUDPConnection:
mobile.computeRPY([2, 0, 1], [-1, 1, 1])
ctm = np.array([[sp.cos(mobile.roll), -sp.sin(mobile.roll)], \
[sp.sin(mobile.roll), sp.cos(mobile.roll)]]) # Coordinate transform matrix
if useBasemap:
# Warning this really slows down the computation
m = Basemap(width=img.width,
height=img.height,
projection='aeqd',
lat_0=sp.rad2deg(mobile.pitch),
lon_0=sp.rad2deg(mobile.yaw),
rsphere=radius)
# Get an image from camera
if not isPaused:
img = cam.getImage()
img = img.resize(int(scaleFactor * img.width),
int(scaleFactor * img.height))
if display:
# Pause image when right button is pressed
dwn = disp.rightButtonDownPosition()
if dwn is not None:
isPaused = not (isPaused)
dwn = None
if display:
# Create a layer to enable user to make a selection of the target
selectionLayer = DrawingLayer((img.width, img.height))
if img:
if display:
# Create a new layer to host information retrieved from video
layer = DrawingLayer((img.width, img.height))
# Selection is a rectangle drawn while holding mouse left button down
if disp.leftButtonDown:
corner1 = (disp.mouseX, disp.mouseY)
selectionInProgress = True
if selectionInProgress:
corner2 = (disp.mouseX, disp.mouseY)
bb = disp.pointsToBoundingBox(
corner1,
corner2) # Display the temporary selection
if disp.leftButtonUp: # User has finished is selection
selectionInProgress = False
selection = img.crop(bb[0], bb[1], bb[2], bb[3])
if selection != None:
# The 3 main colors in the area selected are considered.
# Note that the selection should be included in the target and not contain background
try:
selection.save('../ObjectTracking/' +
'kite_detail_tmp.jpg')
img0 = Image(
"kite_detail_tmp.jpg"
) # For unknown reason I have to reload the image...
pal = img0.getPalette(bins=2, hue=False)
except: # getPalette is sometimes bugging and raising LinalgError because matrix not positive definite
pal = pal
wasTargetFoundInPreviousFrame = False
previous_coord_px = (bb[0] + bb[2] / 2,
bb[1] + bb[3] / 2)
if corner1 != corner2:
selectionLayer.rectangle((bb[0], bb[1]),
(bb[2], bb[3]),
width=5,
color=Color.YELLOW)
# If the target was already found, we can save computation time by
# reducing the Region Of Interest around predicted position
if wasTargetFoundInPreviousFrame:
ROITopLeftCorner = (max(0, previous_coord_px[0]-maxRelativeMotionPerFrame/2*width), \
max(0, previous_coord_px[1] -height*maxRelativeMotionPerFrame/2))
ROI = img.crop(ROITopLeftCorner[0], ROITopLeftCorner[1], \
maxRelativeMotionPerFrame*width, maxRelativeMotionPerFrame*height, \
centered = False)
if display:
# Draw the rectangle corresponding to the ROI on the complete image
layer.rectangle((previous_coord_px[0]-maxRelativeMotionPerFrame/2*width, \
previous_coord_px[1]-maxRelativeMotionPerFrame/2*height), \
(maxRelativeMotionPerFrame*width, maxRelativeMotionPerFrame*height), \
color = Color.GREEN, width = 2)
else:
# Search on the whole image if no clue of where is the target
ROITopLeftCorner = (0, 0)
ROI = img
'''#Option 1
target_part0 = ROI.hueDistance(color=(142,50,65)).invert().threshold(150)
target_part1 = ROI.hueDistance(color=(93,16,28)).invert().threshold(150)
target_part2 = ROI.hueDistance(color=(223,135,170)).invert().threshold(150)
target_raw_img = target_part0+target_part1+target_part2
target_img = target_raw_img.erode(5).dilate(5)
#Option 2
target_img = ROI.hueDistance(imgModel.getPixel(10,10)).binarize().invert().erode(2).dilate(2)'''
# Find sky color
sky = (img - img.binarize()).findBlobs(minsize=10000)
if sky:
skycolor = sky[0].meanColor()
# Option 3
target_img = ROI - ROI # Black image
# Loop through palette of target colors
if display and displayDebug:
decomposition = []
i_col = 0
for col in pal:
c = tuple([int(col[i]) for i in range(0, 3)])
# Search the target based on color
ROI.save('../ObjectTracking/' + 'ROI_tmp.jpg')
img1 = Image('../ObjectTracking/' + 'ROI_tmp.jpg')
filter_img = img1.colorDistance(color=c)
h = filter_img.histogram(numbins=256)
cs = np.cumsum(h)
thmax = np.argmin(
abs(cs - 0.02 * img.width * img.height)
) # find the threshold to have 10% of the pixel in the expected color
thmin = np.argmin(
abs(cs - 0.005 * img.width * img.height)
) # find the threshold to have 10% of the pixel in the expected color
if thmin == thmax:
newth = thmin
else:
newth = np.argmin(h[thmin:thmax]) + thmin
alpha = 0.5
th[i_col] = alpha * th[i_col] + (1 - alpha) * newth
filter_img = filter_img.threshold(
max(40, min(200, th[i_col]))).invert()
target_img = target_img + filter_img
#print th
i_col = i_col + 1
if display and displayDebug:
[R, G, B] = filter_img.splitChannels()
white = (R - R).invert()
r = R * 1.0 / 255 * c[0]
g = G * 1.0 / 255 * c[1]
b = B * 1.0 / 255 * c[2]
tmp = white.mergeChannels(r, g, b)
decomposition.append(tmp)
# Get a black background with with white target foreground
target_img = target_img.threshold(150)
target_img = target_img - ROI.colorDistance(
color=skycolor).threshold(80).invert()
if display and displayDebug:
small_ini = target_img.resize(
int(img.width / (len(pal) + 1)),
int(img.height / (len(pal) + 1)))
for tmp in decomposition:
small_ini = small_ini.sideBySide(tmp.resize(
int(img.width / (len(pal) + 1)),
int(img.height / (len(pal) + 1))),
side='bottom')
small_ini = small_ini.adaptiveScale(
(int(img.width), int(img.height)))
toDisplay = img.sideBySide(small_ini)
else:
toDisplay = img
#target_img = ROI.hueDistance(color = Color.RED).threshold(10).invert()
# Search for binary large objects representing potential target
target = target_img.findBlobs(minsize=500)
if target: # If a target was found
if wasTargetFoundInPreviousFrame:
predictedTargetPosition = (
width * maxRelativeMotionPerFrame / 2,
height * maxRelativeMotionPerFrame / 2
) # Target will most likely be close to the center of the ROI
else:
predictedTargetPosition = previous_coord_px
# If there are several targets in the image, take the one which is the closest of the predicted position
target = target.sortDistance(predictedTargetPosition)
# Get target coordinates according to minimal bounding rectangle or centroid.
coordMinRect = ROITopLeftCorner + np.array(
(target[0].minRectX(), target[0].minRectY()))
coord_px = ROITopLeftCorner + np.array(
target[0].centroid())
# Rotate the coordinates of roll angle around the middle of the screen
rot_coord_px = np.dot(
ctm, coord_px -
np.array([img.width / 2, img.height / 2])) + np.array(
[img.width / 2, img.height / 2])
if useBasemap:
coord = sp.deg2rad(
m(rot_coord_px[0],
img.height - rot_coord_px[1],
inverse=True))
else:
coord = localProjection(
rot_coord_px[0] - img.width / 2,
img.height / 2 - rot_coord_px[1],
radius,
mobile.yaw,
mobile.pitch,
inverse=True)
target_bearing, target_elevation = coord
# Get minimum bounding rectangle for display purpose
minR = ROITopLeftCorner + np.array(target[0].minRect())
contours = target[0].contour()
contours = [
ROITopLeftCorner + np.array(contour)
for contour in contours
]
# Get target features
angle = sp.deg2rad(target[0].angle()) + mobile.roll
angle = sp.deg2rad(
unwrap180(sp.rad2deg(angle),
sp.rad2deg(previous_angle)))
width = target[0].width()
height = target[0].height()
# Check if the kite is upside down
# First rotate the kite
ctm2 = np.array([[sp.cos(-angle+mobile.roll), -sp.sin(-angle+mobile.roll)], \
[sp.sin(-angle+mobile.roll), sp.cos(-angle+mobile.roll)]]) # Coordinate transform matrix
rotated_contours = [
np.dot(ctm2, contour - coordMinRect)
for contour in contours
]
y = [-tmp[1] for tmp in rotated_contours]
itop = np.argmax(y) # Then looks at the points at the top
ibottom = np.argmin(y) # and the point at the bottom
# The point the most excentered is at the bottom
if abs(rotated_contours[itop][0]) > abs(
rotated_contours[ibottom][0]):
isInverted = True
else:
isInverted = False
if isInverted:
angle = angle + sp.pi
# Filter the data
alpha = 1 - sp.exp(-deltaT / self.filterTimeConstant)
if not (isPaused):
dCoord = np.array(previous_dCoord) * (
1 - alpha) + alpha * (
np.array(coord_px) - previous_coord_px
) # related to the speed only if cam is fixed
dAngle = np.array(previous_dAngle) * (
1 - alpha) + alpha * (np.array(angle) -
previous_angle)
else:
dCoord = np.array([0, 0])
dAngle = np.array([0])
#print coord_px, angle, width, height, dCoord
# Record important data
times.append(timestamp)
coords_px.append(coord_px)
angles.append(angle)
target_elevations.append(target_elevation)
target_bearings.append(target_bearing)
# Export data to controller
self.elevation = target_elevation
self.bearing = target_bearing
self.orientation = angle
dt = time.time() - timeLastTarget
self.ROT = dAngle / dt
self.lastUpdateTime = t
# Save for initialisation of next step
previous_dCoord = dCoord
previous_angle = angle
previous_coord_px = (int(coord_px[0]), int(coord_px[1]))
wasTargetFoundInPreviousFrame = True
timeLastTarget = time.time()
else:
wasTargetFoundInPreviousFrame = False
if useHDF5:
hdfSize = hdfSize + 1
dset.resize((hdfSize, 7))
imset.resize((hdfSize, img.width, img.height, 3))
dset[hdfSize - 1, :] = [
time.time(), coord_px[0], coord_px[1], angle,
self.elevation, self.bearing, self.ROT
]
imset[hdfSize - 1, :, :, :] = img.getNumpy()
recordFile.flush()
else:
csv_writer.writerow([
time.time(), coord_px[0], coord_px[1], angle,
self.elevation, self.bearing, self.ROT
])
if display:
if target:
# Add target features to layer
# Minimal rectange and its center in RED
layer.polygon(minR[(0, 1, 3, 2), :],
color=Color.RED,
width=5)
layer.circle(
(int(coordMinRect[0]), int(coordMinRect[1])),
10,
filled=True,
color=Color.RED)
# Target contour and centroid in BLUE
layer.circle((int(coord_px[0]), int(coord_px[1])),
10,
filled=True,
color=Color.BLUE)
layer.polygon(contours, color=Color.BLUE, width=5)
# Speed vector in BLACK
layer.line((int(coord_px[0]), int(coord_px[1])),
(int(coord_px[0] + 20 * dCoord[0]),
int(coord_px[1] + 20 * dCoord[1])),
width=3)
# Line giving angle
layer.line((int(coord_px[0] + 200 * sp.cos(angle)),
int(coord_px[1] + 200 * sp.sin(angle))),
(int(coord_px[0] - 200 * sp.cos(angle)),
int(coord_px[1] - 200 * sp.sin(angle))),
color=Color.RED)
# Line giving rate of turn
#layer.line((int(coord_px[0]+200*sp.cos(angle+dAngle*10)), int(coord_px[1]+200*sp.sin(angle+dAngle*10))), (int(coord_px[0]-200*sp.cos(angle + dAngle*10)), int(coord_px[1]-200*sp.sin(angle+dAngle*10))))
# Add the layer to the raw image
toDisplay.addDrawingLayer(layer)
toDisplay.addDrawingLayer(selectionLayer)
# Add time metadata
toDisplay.drawText(str(i_frame) + " " + str(timestamp),
x=0,
y=0,
fontsize=20)
# Add Line giving horizon
#layer.line((0, int(img.height/2 + mobile.pitch*pixelPerRadians)),(img.width, int(img.height/2 + mobile.pitch*pixelPerRadians)), width = 3, color = Color.RED)
# Plot parallels
for lat in range(-90, 90, 15):
r = range(0, 361, 10)
if useBasemap:
# \todo improve for high roll
l = m(r, [lat] * len(r))
pix = [np.array(l[0]), img.height - np.array(l[1])]
else:
l = localProjection(sp.deg2rad(r), \
sp.deg2rad([lat]*len(r)), \
radius, \
lon_0 = mobile.yaw, \
lat_0 = mobile.pitch, \
inverse = False)
l = np.dot(ctm, l)
pix = [
np.array(l[0]) + img.width / 2,
img.height / 2 - np.array(l[1])
]
for i in range(len(r) - 1):
if isPixelInImage(
(pix[0][i], pix[1][i]), img) or isPixelInImage(
(pix[0][i + 1], pix[1][i + 1]), img):
layer.line((pix[0][i], pix[1][i]),
(pix[0][i + 1], pix[1][i + 1]),
color=Color.WHITE,
width=2)
# Plot meridians
for lon in range(0, 360, 15):
r = range(-90, 91, 10)
if useBasemap:
# \todo improve for high roll
l = m([lon] * len(r), r)
pix = [np.array(l[0]), img.height - np.array(l[1])]
else:
l= localProjection(sp.deg2rad([lon]*len(r)), \
sp.deg2rad(r), \
radius, \
lon_0 = mobile.yaw, \
lat_0 = mobile.pitch, \
inverse = False)
l = np.dot(ctm, l)
pix = [
np.array(l[0]) + img.width / 2,
img.height / 2 - np.array(l[1])
]
for i in range(len(r) - 1):
if isPixelInImage(
(pix[0][i], pix[1][i]), img) or isPixelInImage(
(pix[0][i + 1], pix[1][i + 1]), img):
layer.line((pix[0][i], pix[1][i]),
(pix[0][i + 1], pix[1][i + 1]),
color=Color.WHITE,
width=2)
# Text giving bearing
# \todo improve for high roll
for bearing_deg in range(0, 360, 30):
l = localProjection(sp.deg2rad(bearing_deg),
sp.deg2rad(0),
radius,
lon_0=mobile.yaw,
lat_0=mobile.pitch,
inverse=False)
l = np.dot(ctm, l)
layer.text(
str(bearing_deg),
(img.width / 2 + int(l[0]), img.height - 20),
color=Color.RED)
# Text giving elevation
# \todo improve for high roll
for elevation_deg in range(-60, 91, 30):
l = localProjection(0,
sp.deg2rad(elevation_deg),
radius,
lon_0=mobile.yaw,
lat_0=mobile.pitch,
inverse=False)
l = np.dot(ctm, l)
layer.text(str(elevation_deg),
(img.width / 2, img.height / 2 - int(l[1])),
color=Color.RED)
#toDisplay.save(js)
toDisplay.save(disp)
if display:
toDisplay.removeDrawingLayer(1)
toDisplay.removeDrawingLayer(0)
recordFile.close()
kite_base = Image('http://www.winds-up.com/images/annonces/7915_1.jpg').resize(50, 50).invert()
disp = Display()
i_loop = 0
#pid = PID.PID(1, 1, 0.1)
offset = sp.pi/3*0
kite_model = kiteModel()
dX = 0*kite_model.X
while disp.isNotDone():
setpoint = sp.pi/1.7*sp.sin(2*sp.pi/7*time.time())+offset
i_loop = i_loop +1
order = 0+0*sp.randn(1)/5+2.0*disp.mouseX/background.width-1
#error = X[0] -setpoint
#order = sp.randn(1)/100 + pid.computeCorrection(error, dX[0]/dt-0)
#pid.incrementTime(error, dt)
dt = 0.1
kite_model.update(order, dt)
print kite_model.X
kite = kite_base.rotate(sp.rad2deg(kite_model.X[0]), fixed=False).invert()
#kite.save(disp)d
#background.blit(kite, (799,0)).save(disp)
toDisplay = background.blit(kite.invert(), (max(-kite.width +1, min(background.width-1, int(kite_model.X[1]+background.width/2-kite.width/2))), max(-kite.height+1, min(background.height-1, int(background.height-kite_model.X[2]-200)))), mask = kite.binarize())
toDisplay.drawText(str(i_loop*dt), 0, 0, color = Color.RED, fontsize=60)
toDisplay.save(disp)
time.sleep(dt)
#pid = PID.PID(1, 1, 0.1)
offset = sp.pi / 3 * 0
kite_model = kiteModel()
dX = 0 * kite_model.X
while disp.isNotDone():
setpoint = sp.pi / 1.7 * sp.sin(2 * sp.pi / 7 * time.time()) + offset
i_loop = i_loop + 1
order = 0 + 0 * sp.randn(
1) / 5 + 2.0 * disp.mouseX / background.width - 1
#error = X[0] -setpoint
#order = sp.randn(1)/100 + pid.computeCorrection(error, dX[0]/dt-0)
#pid.incrementTime(error, dt)
dt = 0.1
kite_model.update(order, dt)
print kite_model.X
kite = kite_base.rotate(sp.rad2deg(kite_model.X[0]),
fixed=False).invert()
#kite.save(disp)d
#background.blit(kite, (799,0)).save(disp)
toDisplay = background.blit(
kite.invert(),
(max(
-kite.width + 1,
min(
background.width - 1,
int(kite_model.X[1] + background.width / 2 -
kite.width / 2))),
max(
-kite.height + 1,
min(background.height - 1,
