def normalise_bout(coord):
""" Reset a bout to be facing upward and start from 0,0
Parameters
----------
bout
Returns
-------
"""
dir_init = angle_mean(coord[:2, 2])
coord[:, :2] = (coord[:, :2] - coord[:1, :2]) @ rot_mat(dir_init + np.pi)
coord[:, 2] -= dir_init
coord[:, 2] = reduce_to_pi(coord[:, 2])
return coord
def get_position(self):
if len(self.acc_tracking.stored_data) == 0 or not np.isfinite(
self.acc_tracking.stored_data[-1].f0_x
):
o = self._output_type(np.nan, np.nan, np.nan)
return o
past_coords = self.acc_tracking.stored_data[-1]
t = self.acc_tracking.times[-1]
if not self.calibrator.cam_to_proj is None:
projmat = np.array(self.calibrator.cam_to_proj)
if projmat.shape != (2, 3):
projmat = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]])
x, y = projmat @ np.array([past_coords.f0_x, past_coords.f0_y, 1.0])
theta = np.arctan2(
*(
projmat[:, :2]
@ np.array(
[np.cos(past_coords.f0_theta), np.sin(past_coords.f0_theta)]
)[::-1]
)
)
else:
x, y, theta = past_coords.f0_x, past_coords.f0_y, past_coords.f0_theta
c_values = np.array((y, x, theta))
if self.change_thresholds is not None:
if self.past_values is None:
self.past_values = np.array(c_values)
else:
deltas = c_values - self.past_values
deltas[2] = reduce_to_pi(deltas[2])
sel = np.abs(deltas) > self.change_thresholds
self.past_values[sel] = c_values[sel]
c_values = self.past_values
logout = self._output_type(*c_values)
self.log.update_list(t, logout)
return c_values
def _tail_trace_core_ls(img, start_x, start_y, disp_x, disp_y, num_points, tail_length):
"""Tail tracing based on min (or max) detection on arches. Wrapped by
trace_tail_angular_sweep.
Parameters
----------
img :
start_x :
start_y :
disp_x :
disp_y :
num_points :
tail_length :
Returns
-------
"""
# Define starting angle based on tail dimensions:
start_angle = np.arctan2(disp_x, disp_y)
# Initialise first angle arch, tail sum and angle list:
pi2 = np.pi / 2
lin = np.linspace(-pi2 + start_angle, pi2 + start_angle, 25)
tail_sum = 0.0
angles = np.zeros(num_points + 1)
# Create vector of intensities along the arch:
intensity_vect = np.zeros(len(lin), dtype=np.int16)
seglen = tail_length / num_points
for j in range(num_points): # Cycle on segments
# Transform arch angles in x and y coordinates:
xs = start_x + seglen * np.sin(lin)
ys = start_y + seglen * np.cos(lin)
# fill the vector of intensities along the arch
for i in range(len(xs)):
yp = int(ys[i])
xp = int(xs[i])
if (
img.shape[1] > xp >= 0 and 0 <= yp < img.shape[0]
): # check image boundaries
intensity_vect[i] = img[yp, xp]
# Find minimum or maximum of the arch.
# This switch is much faster than inverting the entire image.
ident = np.argmax(intensity_vect)
if not np.isnan(lin[ident]):
new_angle = lin[ident]
else:
new_angle = angles[j]
new_angle = reduce_to_pi(new_angle)
# skip the first angle for the tail sum
if j > 0:
tail_sum += reduce_to_pi(new_angle - angles[j])
angles[j + 1] = new_angle
# The point found will be the starting point of the next arc
start_x = xs[ident]
start_y = ys[ident]
# Create an 120 deg angle depending on the previous one:
lin = np.linspace(new_angle - pi2, new_angle + pi2, 20)
angles[0] = tail_sum
return angles