def deflect(self,xim,yim,lens):
"""
Calculate deflection following Keeton,Mao,Witt 2000.
Parameters:
xim, yim
2D Arrays of image coordinates we're going to lens,
probably generated by np.meshgrid.
lens
A lens object; we use this to shift the coordinate system
to be centered on the lens.
"""
ximage,yimage = xim.copy(), yim.copy()
ximage -= lens.x['value']; yimage -= lens.y['value']
if not np.isclose(lens.PA['value'], 0.):
r,theta = cart2pol(ximage,yimage)
ximage,yimage = pol2cart(r,theta-(lens.PA['value']*deg2rad))
# KMW2000, altered for our coordinate convention.
g,thg = self.shear['value'], (self.shearangle['value']-lens.PA['value'])*deg2rad
dxs = -g*np.cos(2*thg)*ximage - g*np.sin(2*thg)*yimage
dys = -g*np.sin(2*thg)*ximage + g*np.cos(2*thg)*yimage
if not np.isclose(lens.PA['value'], 0.):
r,theta = cart2pol(dxs,dys)
dxs,dys = pol2cart(r,theta+(lens.PA['value']*deg2rad))
self.deflected_x = dxs; self.deflected_y = dys
def deflect(self,xim,yim,Dd,Ds,Dds):
"""
Follow Kormann+1994 for the lensing deflections.
Parameters:
xim, yim
2D Arrays of image coordinates we're going to lens,
probably generated by np.meshgrid.
Dd, Ds, Dds
Distances to the lens, source and between the source
and lens (units don't matter as long as they're the
same). Can't be calculated only from lens due to source
distances.
"""
if self._altered: # Only redo if something is new.
ximage, yimage = xim.copy(), yim.copy() # for safety.
f = 1. - self.e['value']
fprime = np.sqrt(1. - f**2.)
# K+94 parameterizes in terms of LOS velocity dispersion and then
# basically the Einstein radius.
sigma = ((self.M['value']*Ds*G*Msun*c**2.)/(4*np.pi**2. * Dd*Dds*Mpc))**(1/4.)
Xi0 = 4*np.pi * (sigma/c)**2. * (Dd*Dds/Ds)
# Flip units, the recenter and rotate grid to lens center and major axis
ximage *= arcsec2rad; yimage *= arcsec2rad
ximage -= (self.x['value']*arcsec2rad)
yimage -= (self.y['value']*arcsec2rad)
if not np.isclose(self.PA['value'], 0.):
r,theta = cart2pol(ximage,yimage)
ximage,yimage = pol2cart(r,theta-(self.PA['value']*deg2rad))
phi = np.arctan2(yimage,ximage)
# Calculate the deflections, account for e=0 (the SIS), which has
# cancelling infinities. K+94 eq 27a.
if np.isclose(f, 1.):
dxs = -(Xi0/Dd)*np.cos(phi)
dys = -(Xi0/Dd)*np.sin(phi)
else:
dxs = -(Xi0/Dd)*(np.sqrt(f)/fprime)*np.arcsinh(np.cos(phi)*fprime/f)
dys = -(Xi0/Dd)*(np.sqrt(f)/fprime)*np.arcsin(np.sin(phi)*fprime)
# Rotate and shift back to sky frame
if not np.isclose(self.PA['value'], 0.):
r,theta = cart2pol(dxs,dys)
dxs,dys = pol2cart(r,theta+(self.PA['value']*deg2rad))
dxs *= rad2arcsec; dys *= rad2arcsec
self.deflected_x = dxs
self.deflected_y = dys
self._altered = False
def azim_proj(pos):
"""
Computes the Azimuthal Equidistant Projection of input point in 3D Cartesian Coordinates.
Imagine a plane being placed against (tangent to) a globe. If
a light source inside the globe projects the graticule onto
the plane the result would be a planar, or azimuthal, map
projection.
:param pos: position in 3D Cartesian coordinates
:return: projected coordinates using Azimuthal Equidistant Projection
"""
[r, elev, az] = cart2sph(pos[0], pos[1], pos[2])
return pol2cart(az, m.pi / 2 - elev)
def azim_proj(pos):
"""
Computes the Azimuthal Equidistant Projection of input point in 3D Cartesian Coordinates.
Imagine a plane being placed against (tangent to) a globe. If
a light source inside the globe projects the graticule onto
the plane the result would be a planar, or azimuthal, map
projection.
:param pos: position in 3D Cartesian coordinates
:return: projected coordinates using Azimuthal Equidistant Projection
"""
[r, elev, az] = cart2sph(pos[0], pos[1], pos[2])
return pol2cart(az, np.pi / 2 - elev)
def set_heading(self, custom_angle_of_attack=None):
if custom_angle_of_attack:
print('custom')
self.this_hdg_tgt = self.heading_tgt_rad + (
self.sweep_direction * np.deg2rad(custom_angle_of_attack))
else:
print('not custon')
self.this_hdg_tgt = self.heading_tgt_rad + (
self.sweep_direction * np.deg2rad(self.angle_of_attack_degs))
self.sweep_direction *= -1
self.x_tgt, self.y_tgt = pol2cart(self.mission_vel_tgt,
self.this_hdg_tgt)
def update(self, steering):
"""
Update the positions and headings
steering: vector of (x, y) steering
"""
self.accelerations = steering * self.dt
# old heading
_, old_headings = cart2pol(self.velocities)
new_velocities = self.velocities + self.accelerations * self.dt
# new velocity in polar coordinates
speeds, new_headings = cart2pol(new_velocities)
# calculate heading diff
headings_diff = np.radians(
180 -
(180 - np.degrees(new_headings) + np.degrees(old_headings)) % 360)
# if the heading diff is too big, we cut the turn at max_turn or min_turn
# use np.where
new_headings = np.where(headings_diff < -self.max_turn,
old_headings - self.max_turn, new_headings)
new_headings = np.where(self.max_turn < headings_diff,
old_headings + self.max_turn, new_headings)
# if the speed is too slow or too big, we adjust it
# use np.where
speeds = np.where(speeds < self.min_speed, self.min_speed, speeds)
speeds = np.where(speeds > self.max_speed, self.max_speed, speeds)
# set the new velocity
# modify pol2cart
self.velocities = pol2cart(speeds, new_headings)
self.headings = new_headings
# move
self.positions += self.velocities * self.dt
self.adjust_positions_to_boundaries()
def azim_proj(pos): [r, elev, az] = cart2sph(pos[0], pos[1], pos[2]) return pol2cart(float(az), m.pi/2.0 - float(elev))
def test_cart_pol_conv():
print(ut.pol2cart((2, np.pi / 2)))
print(ut.cart2pol((5, 5)))
def generate_sweep_mission(
args_in,
):
"""
generates a parametised mission that flies out with a target heading, then flies back to the tack off location in a
zig-zag fashion but with a heading that is opposite to the outbound direction (i.e. plus pi), this means that on the
return path the heading direction non-aligned with the craft's velocity velocity
purpose is to test whether images on the outbound path are more familiar than those on the inbound path
:param args_in:
:return:
"""
# initialise instruction vars
instructions = {}
instruction_cnt = 0
################# new instruction
instructions[instruction_cnt] = Arming_state(
# instruction_type='take_off',
# instruction_args={'to_altitude_tgt':args_in.height},
timeout=90.0
)
instruction_cnt += 1
################# new instruction
instructions[instruction_cnt] = Take_off_state(
# instruction_type='hold',
to_altitude_tgt=args_in.outbound_hgt,
yaw_type='pos',
heading_tgt_rad=args_in.hdg_out,
)
instruction_cnt += 1
# if we have an offset
if abs(args_in.x_offset) > 1e-6:
# calculate the offset in local coordinates based on the target heading
x_offset_setpoint = np.cos(args_in.hdg_out) * args_in.x_offset
y_offset_setpoint = np.sin(args_in.hdg_out) * args_in.x_offset
extra_travel_time = 5 + (args_in.x_offset * args_in.vel_tgt) + 2 # extra time to settle at the starting point
else:
x_offset_setpoint = 0
y_offset_setpoint = 0
extra_travel_time = 5
################# new instruction, hover at departure point
instructions[instruction_cnt] = Waypoint_state(
state_label='Hovering at Offset',
waypoint_type='hold',
timeout=5 + extra_travel_time,
xy_type='pos',
x_setpoint=x_offset_setpoint,
y_setpoint=y_offset_setpoint,
z_type='pos',
z_setpoint=args_in.outbound_hgt,
yaw_type='pos',
yaw_setpoint=args_in.hdg_out,
coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
)
instruction_cnt += 1
################# new instruction, Head out
x_tgt, y_tgt = pol2cart(args_in.vel_tgt, args_in.hdg_out)
if args_in.outbound_configuration == 'fixed_heading':
instructions[instruction_cnt] = Waypoint_state(
state_label='Head_out', # do not chnage this label - used for analysis
waypoint_type='vel',
timeout=args_in.duration_out,
xy_type='vel',
x_setpoint=x_tgt,
y_setpoint=y_tgt,
z_type='pos',
z_setpoint=args_in.outbound_hgt,
yaw_type='pos',
yaw_setpoint=args_in.hdg_out,
coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
)
elif args_in.outbound_configuration == 'sin_wave_2':
instructions[instruction_cnt] = Sin_wave_2(
state_label='Head_out',
mission_z_tgt=args_in.outbound_hgt,
mission_vel_tgt=args_in.vel_tgt,
heading_tgt_rad=args_in.hdg_out,
timeout=args_in.duration_out,
# inhibit_turning=INHIBIT_TURNING_MASK(args_in.inhibit_turning),
# max_nest_dist=args_in.max_nest_distance,
sin_freq=args_in.outbound_sin_freq,
)
else:
raise AttributeError ('unknown mission config {}'.format(args.outbound_configuration))
instruction_cnt += 1
if args_in.flip_images:
################# new instruction, hover at return point
instructions[instruction_cnt] = Hold_pos_state(
state_label='Turn around time', # do not chnage this label - used for analysis
xy_type='vel',
x_setpoint=0.0,
y_setpoint=0.0,
z_setpoint=args_in.mission_hgt,
yaw_setpoint=args_in.hdg_out + np.pi,
timeout=args_in.t_turnaround,
)
instruction_cnt += 1
else:
################# new instruction, return to departure point
instructions[instruction_cnt] = Waypoint_state(
state_label='Return to start',
waypoint_type='pos',
timeout=args_in.duration_out + 5,
xy_type='pos',
x_setpoint=x_offset_setpoint,
y_setpoint=y_offset_setpoint,
z_type='pos',
z_setpoint=args_in.outbound_hgt,
yaw_type='pos',
yaw_setpoint=args_in.hdg_out,
coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
)
instruction_cnt += 1
instructions[instruction_cnt] = Waypoint_state(
state_label='Hovering at Offset',
waypoint_type='hold',
timeout=5,
xy_type='pos',
x_setpoint=x_offset_setpoint,
y_setpoint=y_offset_setpoint,
z_type='pos',
z_setpoint=args_in.outbound_hgt,
yaw_type='pos',
yaw_setpoint=args_in.hdg_out,
coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
)
instruction_cnt += 1
args_in.hdg_out = args_in.hdg_out + np.pi # flip by 180 to counteract the presumed flip in the sidesweep part
################# new ins truction, depart at fixed heading
if args_in.mission_type == 'active':
instructions[instruction_cnt] = Sweep_wave_familiarity(
state_label='Sweep_state', # do not chnage this label - used for analysis
timeout=args_in.duration_out * args_in.multiplier,
mission_z_tgt=args_in.mission_hgt,
heading_tgt_rad=args_in.hdg_out + np.pi,
mission_vel_tgt=args_in.vel_tgt,
amplitude=args_in.amplitude,
cwssim_thresh=args_in.cwssim_thresh,
close_to_end_thresh_high=args_in.close_to_end_thresh_high,
close_to_end_thresh_low=args_in.close_to_end_thresh_low,
slow_down_at_end=args_in.termination_slowdown,
angle_of_attack_degs=args_in.angle_of_attack,
search_angle_of_attack_degs=args_in.search_angle_of_attack,
slowdown_factor=args_in.slowdown_factor,
t_familiar=args_in.t_familiar,
hdg_offset=args_in.hdg_in_offset,
)
elif args_in.mission_type == 'passive':
instructions[instruction_cnt] = Sweep_wave(
state_label='Sweep_state',
timeout=args_in.duration_out * args_in.multiplier,
mission_z_tgt=args_in.mission_hgt,
heading_tgt_rad=args_in.hdg_out + np.pi,
mission_vel_tgt=args_in.vel_tgt,
amplitude=args_in.amplitude,
)
else:
raise ValueError('unkown mission type: {}'.format(args_in.mission_type))
instruction_cnt += 1
################# new instruction, hover at final state
instructions[instruction_cnt] = Hold_pos_state(
state_label='Hovering at completion',
yaw_setpoint=args_in.hdg_out + np.pi,
timeout=20,
)
instruction_cnt += 1
################# new instruction, hover at departure point
instructions[instruction_cnt] = Waypoint_state(
state_label='Return_home',
waypoint_type='pos',
timeout=60,
xy_type='pos',
x_setpoint=0,
y_setpoint=0,
z_type='pos',
z_setpoint=args_in.mission_hgt,
yaw_type='pos',
yaw_setpoint=args_in.hdg_out,
coordinate_frame=PositionTarget.FRAME_LOCAL_NED,
)
instruction_cnt += 1
################# landing instructions
instructions[instruction_cnt] = Landing_state(
instruction_type='land',
timeout=30,
)
instruction_cnt += 1
################# landing instructions
instructions[instruction_cnt] = Post_run_state(
instruction_type='post_run',
timeout=30,
)
instruction_cnt += 1
# todo - check that instruction keys do not have nay numerical gaps (perhaps do this in pyx4)
return instructions
def points_synthesis(fileName,
labD,
C,
imS,
labS,
rotangle,
rotangle_real,
l_real,
id,
dir,
n_jobs=1,
display=False):
global _cellsD, _cellsborderD, _cellsS, _cellsborderS, _imS, _imD
height, width, depth = imS.shape
cellsS = labS # already reshaped
cellsD = labD.reshape((height, width)).astype('int64')
allborderS = (morphological_gradient(cellsS, size=3) > 0)
cellsinsideS = cellsS.copy()
cellsinsideS[allborderS] = -1
cellsinsideS[0, :] = -1
cellsinsideS[:, 0] = -1
cellsinsideS[cellsinsideS.shape[0] - 1, :] = -1
cellsinsideS[:, cellsinsideS.shape[1] - 1] = -1
cellsborderS = cellsS - cellsinsideS - 1 # -1 to compensate the previous -1s, this is to allow for one of the labels to be 0
allborderD = (morphological_gradient(cellsD, size=3) > 0)
cellsinsideD = cellsD.copy()
cellsinsideD[allborderD] = -1
cellsinsideD[0, :] = -1
cellsinsideD[:, 0] = -1
cellsinsideD[cellsinsideD.shape[0] - 1, :] = -1
cellsinsideD[:, cellsinsideD.shape[1] - 1] = -1
cellsborderD = cellsD - cellsinsideD - 1
alllabelsD = np.unique(labD)
cellsborderDwhite = cellsborderD.astype('uint8')
cellsborderDwhite[cellsborderDwhite < 255] = 0
cellsborderDwhite = 255 - cellsborderDwhite
#multiprocessing
_cellsD = cellsD
_cellsborderD = cellsborderD
_cellsS = cellsS
_cellsborderS = cellsborderS
_imS = imS
centriolePos = readPixelPosition(fileName) #np.array(np.load(fileName))
_centriolPosition = np.empty((alllabelsD.shape[0], 2))
_centriolPosition.fill(np.nan)
_centriolPosition[
np.array([np.argwhere(id == i)[0][0]
for i in centriolePos[:, 0]]), :] = centriolePos[:, 1:3]
parameters = [(int(alllabelsD[i]), float(rotangle_real[i] - rotangle[i]))
for i in range(0, alllabelsD.shape[0], 1)]
parametersCentriole = list(
np.concatenate((parameters, np.copy(_centriolPosition)), axis=1))
if n_jobs > 1:
pool = Pool(n_jobs)
xyc = list(pool.imap(_wrapingPix, parametersCentriole))
pool.close()
pool.join()
else:
xyc = list(map(_wrapingPix, parametersCentriole))
_cellsD = None
_cellsborderD = None
_cellsS = None
_cellsborderS = None
_imS = None
if display:
imD = np.dstack(
(cellsborderDwhite, cellsborderDwhite, cellsborderDwhite))
results = list(map(_warp, parameters))
for i in range(0, alllabelsD.shape[0], 1):
if not (results[i] is None):
yxmaskD, values = results[i]
imD[yxmaskD[:, 0], yxmaskD[:, 1], :] = values
for i in range(0, alllabelsD.shape[0], 1):
sellabel = alllabelsD[i]
yxmaskD = np.argwhere(cellsD == sellabel)
try:
shift = 4
center = tuple((C[i][::-1] / 2**(-shift)).astype(int))
axes = tuple((np.sqrt(l_real[i])[::-1] /
2**(-shift)).astype(int)) # half size
angle = int((180 * rotangle[i] / np.pi))
p0 = tuple(((C[i][::-1] +
pol2cart(rotangle[i], np.sqrt(l_real[i][1]))) /
2**(-shift)).astype(int))
cv2.ellipse(imD,
center,
axes,
angle,
0,
360, (100, 0, 0, 150),
thickness=1,
lineType=cv2.CV_AA,
shift=shift)
cv2.circle(imD,
tuple((C[i][::-1] / 2**(-shift)).astype(int)),
int(1 / 2**(-shift)), (200, 0, 0),
thickness=2,
lineType=cv2.CV_AA,
shift=shift)
if not (xyc[i] is None) and not np.isnan(xyc[i][0]):
cv2.circle(imD, (np.floor(xyc[i][1]).astype(int),
np.floor(xyc[i][0]).astype(int)),
3, (230, 138, 0),
thickness=1,
lineType=8,
shift=0)
except np.linalg.LinAlgError as e:
print('np.linalg.LinAlgError (image_synthesis): %s' % e)
Image.fromarray(imD, 'RGB').save(dir + 'fullwarping.tif')
return imD, xyc
def update_postion(self, distance, orientation):
x_dist, y_dist = utils.pol2cart(orientation, distance)
self.pos_x += int(x_dist // self.resolution)
self.pos_y += int(y_dist // self.resolution)
