def weighted_props(regprops):
'''
Return the shape properties based on the weighted moments.
'''
wmu = regprops.weighted_moments_central
# Create the inertia tensor from the moments
a = wmu[2, 0] / wmu[0, 0]
b = -wmu[1, 1] / wmu[0, 0]
c = wmu[0, 2] / wmu[0, 0]
# inertia_tensor = np.array([[a, b], [b, c]])
# The eigenvalues give the axes lengths
l1 = (a + c) / 2 + sqrt(4 * b**2 + (a - c)**2) / 2
l2 = (a + c) / 2 - sqrt(4 * b**2 + (a - c)**2) / 2
# These are the radii, not the lengths
major = 2 * np.sqrt(l1)
minor = 2 * np.sqrt(l2)
# And finally the orientation
absb = -b # need this to be > 0
if a - c == 0:
if absb > 0:
pa = -np.pi / 4.
else:
pa = np.pi / 4.
else:
pa = -0.5 * np.arctan2(2 * absb, (a - c))
pa = wrap_to_pi(pa + 0.5 * np.pi)
centroid = regprops.weighted_centroid
return np.array([centroid[0], centroid[1], major, minor, pa])
def weighted_props(regprops):
'''
Return the shape properties based on the weighted moments.
'''
wmu = regprops.weighted_moments_central
# Create the inertia tensor from the moments
a = wmu[2, 0] / wmu[0, 0]
b = -wmu[1, 1] / wmu[0, 0]
c = wmu[0, 2] / wmu[0, 0]
# inertia_tensor = np.array([[a, b], [b, c]])
# The eigenvalues give the axes lengths
l1 = (a + c) / 2 + sqrt(4 * b ** 2 + (a - c) ** 2) / 2
l2 = (a + c) / 2 - sqrt(4 * b ** 2 + (a - c) ** 2) / 2
# These are the radii, not the lengths
major = 2 * np.sqrt(l1)
minor = 2 * np.sqrt(l2)
# And finally the orientation
absb = -b # need this to be > 0
if a - c == 0:
if absb > 0:
pa = -np.pi / 4.
else:
pa = np.pi / 4.
else:
pa = - 0.5 * np.arctan2(2 * absb, (a - c))
pa = wrap_to_pi(pa + 0.5 * np.pi)
centroid = regprops.weighted_centroid
return np.array([centroid[0], centroid[1], major, minor, pa])
def turn(self):
twist = Twist()
while not self.reached():
twist.angular.z = self._ang_zvel * utils.sign(utils.wrap_to_pi(self.theta_goal - self.theta))
if not self.command(twist):
return False
self.ok = True
return True
def turn(self):
twist = Twist()
while not self.reached():
twist.angular.z = self._ang_zvel * utils.sign(utils.wrap_to_pi(self.theta_goal - self.theta))
if not self.command(twist):
return False
self.ok = True
return True
def bumper_event_callback(self, data):
if data.state == BumperEvent.PRESSED:
self.ok = False
if data.bumper == BumperEvent.LEFT:
self.theta_goal = self.theta - 3.141592*random.uniform(0.2, 1.0)
elif data.bumper == BumperEvent.RIGHT:
self.theta_goal = self.theta + 3.141592*random.uniform(0.2, 1.0)
else:
self.theta_goal = utils.wrap_to_pi(self.theta + 3.141592*random.uniform(-1.0, 1.0))
def bumper_event_callback(self, data):
if data.state == BumperEvent.PRESSED:
self.ok = False
if data.bumper == BumperEvent.LEFT:
self.theta_goal = self.theta - 3.141592*random.uniform(0.2, 1.0)
elif data.bumper == BumperEvent.RIGHT:
self.theta_goal = self.theta + 3.141592*random.uniform(0.2, 1.0)
else:
self.theta_goal = utils.wrap_to_pi(self.theta + 3.141592*random.uniform(-1.0, 1.0))
def cliff_event_callback(self, data):
if data.state == CliffEvent.CLIFF:
self.ok = False
# print "Cliff event: %s,%s"%(str(data.sensor),str(data.state))
if data.sensor == CliffEvent.LEFT:
self.theta_goal = self.theta - 3.141592*random.uniform(0.2, 1.0)
elif data.sensor == CliffEvent.RIGHT:
self.theta_goal = self.theta + 3.141592*random.uniform(0.2, 1.0)
else:
self.theta_goal = utils.wrap_to_pi(self.theta + 3.141592*random.uniform(-1.0, 1.0))
def cliff_event_callback(self, data):
if data.state == CliffEvent.CLIFF:
self.ok = False
# print "Cliff event: %s,%s"%(str(data.sensor),str(data.state))
if data.sensor == CliffEvent.LEFT:
self.theta_goal = self.theta - 3.141592*random.uniform(0.2, 1.0)
elif data.sensor == CliffEvent.RIGHT:
self.theta_goal = self.theta + 3.141592*random.uniform(0.2, 1.0)
else:
self.theta_goal = utils.wrap_to_pi(self.theta + 3.141592*random.uniform(-1.0, 1.0))
def angleCloseEnough(desiredTheta):
'''
Returns true if the robot's theta is close to the given angle (with some
smart handling of pi wraparound)
'''
theta = odom.curVals['theta']
if math.fabs(utils.wrap_to_pi(desiredTheta - theta)) < math.radians(3.0):
return True
else:
return False
def _get_desired_pose(self, idx):
"""Get the desired pose at the given path index. The desired heading
is calculated from the velocity.
Args:
idx (int): Index of point in path
Returns:
ndarray: The desired pose at the given index
"""
x, y = self._pos[idx]
_, direction = decompose(self._vel[idx])
theta = wrap_to_pi(np.arctan2(direction[1], direction[0]))
return x, y, theta
def rotate(desiredAngle):
'''
This will drive the robot to an angle, relative to its current orientation
0 degrees will keep the robot fixed.
This code was inspired by kobuki_testsuite at:
https://github.com/yujinrobot/kobuki/tree/hydro-devel/kobuki_testsuite/src/kobuki_testsuite
Positive angles = counter-clockwise motion
'''
u = 0
tolerance = .1
zvel = .8
initialAngle = odom.curVals['theta']
# print initialAngle
# print desiredAngle
desiredAngle = utils.wrap_to_pi(desiredAngle + initialAngle)
while not angleCloseEnough(desiredAngle):
currentAngle = odom.curVals['theta']
speed = zvel * utils.sign(utils.wrap_to_pi(desiredAngle - currentAngle))
#print currentAngle
publishTwist(0, speed)
stopRobot()
return
def get_control(self, pose):
"""Calculate the values for the feedback control.
Args:
pose (ndarray): The current pose of the mobile base
Returns:
tuple: Linear and angular velocity
"""
# Get the nearest point's index on the path
nearest = self._get_nearest(pose)
# Calculate the desired pose on the nearest point on the path
d_pose = self._get_desired_pose(nearest)
# Calculate the difference between x, y and theta
transformation = self._get_transform(nearest)
e = transformation.dot(pose - d_pose)
# Limit the difference of the orientation between -pi and pi
e[2] = wrap_to_pi(e[2])
# Get the linear velocity control from the nearest point on the path
linear, _ = decompose(self._vel[nearest])
# Pole-placement
a = -2
b = -3
# Gains
k2 = a * b
k3 = -(a + b)
# Angular velocity control calculation
omega1 = -k2 * e[1] - k3 * e[2]
s_dot = (linear * np.cos(e[2])) / (e[1] * self._curvature[nearest] - 1)
angular = omega1 + self._curvature[nearest] * s_dot
return linear, angular
def reached(self):
if abs(utils.wrap_to_pi(self.theta_goal - self.theta)) < radians(5.0):
return True
else:
return False
def fit_region(coords, initial_props=None,
try_fit_ellipse=True, use_ransac=False,
min_in_mask=0.8, mask=None, max_resid=None,
ransac_trials=50, beam_pix=4, max_rad=1.75,
max_eccent=3., image_shape=None, conv_hull=None,
verbose=False):
'''
Fit a circle or ellipse to the given coordinates.
'''
coords = np.array(coords).copy()
ymean = coords[:, 0].mean()
xmean = coords[:, 1].mean()
# Fitting works better when the points are near the origin
coords[:, 0] -= int(ymean)
coords[:, 1] -= int(xmean)
new_props = np.empty((5,))
if len(coords) < 3:
raise ValueError("Must have at least 3 points to fit.")
if len(coords) > 5 and try_fit_ellipse:
with catch_warnings():
filterwarnings("ignore",
r"Number of calls to function")
filterwarnings("ignore",
r"gtol=0.000000 is too small")
if use_ransac:
model, inliers = \
ransac(coords[:, ::-1], EllipseModel, 5,
beam_pix, max_trials=ransac_trials)
else:
model = EllipseModel()
model.estimate(coords[:, ::-1])
dof = len(coords) - 5
# Calculate the model residual
resid = model.residuals(coords[:, ::-1]).sum() / dof
pars = model.params.copy()
pars[0] += int(xmean)
pars[1] += int(ymean)
eccent = pars[2] / float(pars[3])
# Sometimes a < b?? If so, manually correct.
if eccent < 1:
eccent = 1. / eccent
pars[2], pars[3] = pars[3], pars[2]
pars[4] = wrap_to_pi(pars[4] + 0.5 * np.pi)
fail_conds = eccent > max_eccent
if beam_pix is not None and not fail_conds:
fail_conds = fail_conds or \
pars[3] < beam_pix
if max_resid is not None and not fail_conds:
fail_conds = fail_conds or \
resid > max_resid
if initial_props is not None and not fail_conds:
fail_conds = fail_conds or \
pars[2] > max_rad * initial_props[2] or \
not in_ellipse(initial_props[:2][::-1], pars)
if image_shape is not None and not fail_conds:
fail_conds = fail_conds or \
not in_array(pars[:2], image_shape[::-1])
# We require the entire region be inside the array. This
# shouldn't be a big issue for galaxies, but should be revisited
# when testing on galactic emission or incomplete maps.
fail_conds = fail_conds or not \
ellipse_in_array(pars, image_shape[::-1])
if mask is not None and not fail_conds:
# Make sure most of the region falls within the hole mask
fail_conds = fail_conds or \
fraction_in_mask(pars, mask) < min_in_mask
# Now make sure the region is within a region with signal.
# We define this based on the convex hull. It seems
# appropriate to just use min_in_mask, since most of it needs to
# be within the region.
if conv_hull is not None and not fail_conds:
fail_conds = fail_conds or \
fraction_in_mask(pars, conv_hull) < min_in_mask
# The center of the fit should fall within the convex hull mask
y, x = floor_int(pars[1]), floor_int(pars[0])
fail_conds = fail_conds or not conv_hull[y, x]
if fail_conds:
ellip_fail = True
else:
new_props[0] = pars[1]
new_props[1] = pars[0]
new_props[2] = pars[2]
new_props[3] = pars[3]
new_props[4] = pars[4]
ellip_fail = False
else:
ellip_fail = True
# If ellipse fitting is not allowed, or it failed, fit a circle
if ellip_fail:
with catch_warnings():
filterwarnings("ignore",
r"Number of calls to function")
filterwarnings("ignore",
r"gtol=0.000000 is too small")
if use_ransac:
model, inliers = \
ransac(coords[:, ::-1], CircleModel, 3,
beam_pix, max_trials=ransac_trials)
else:
model = CircleModel()
model.estimate(coords[:, ::-1])
dof = len(coords) - 3
# Calculate the model residual
resid = model.residuals(coords[:, ::-1]).sum() / dof
pars = model.params.copy()
pars[0] += int(xmean)
pars[1] += int(ymean)
fail_conds = False
if beam_pix is not None and not fail_conds:
fail_conds = fail_conds or \
pars[2] < beam_pix
if max_resid is not None and not fail_conds:
fail_conds = fail_conds or \
resid > max_resid
if initial_props is not None and not fail_conds:
fail_conds = fail_conds or \
pars[2] > max_rad * initial_props[2] or \
not in_circle(initial_props[:2][::-1], pars)
if image_shape is not None and not fail_conds:
fail_conds = fail_conds or \
not in_array(pars[:2], image_shape[::-1])
# We require the entire region be inside the array. This
# shouldn't be a big issue for galaxies, but should be revisited
# when testing on galactic emission or incomplete maps.
fail_conds = fail_conds or not \
circle_in_array(pars, image_shape[::-1])
if mask is not None and not fail_conds:
fail_conds = fail_conds or \
fraction_in_mask(pars, mask) < min_in_mask
if conv_hull is not None and not fail_conds:
# Needs to be in convex hull. See ellipse rejections
fail_conds = fail_conds or \
fraction_in_mask(pars, conv_hull) < min_in_mask
# The center of the fit should fall within the convex hull mask
y, x = floor_int(pars[1]), floor_int(pars[0])
fail_conds = fail_conds or not conv_hull[y, x]
if fail_conds:
if verbose:
Warning("All fitting failed.")
return None, None
new_props[0] = pars[1]
new_props[1] = pars[0]
new_props[2] = pars[2]
new_props[3] = pars[2]
new_props[4] = 0.0
props = new_props
return props, resid
def reached(self):
if abs(utils.wrap_to_pi(self.theta_goal - self.theta)) < radians(5.0):
return True
else:
return False
def _loop(self, trial_start):
self.trial = trial_start
rate = rospy.Rate(40)
# first wait for calibration to complete
print "Please calibrate the IMU, when ready, press 's'...\r"
while not rospy.is_shutdown():
print "Calibration values are: ", self.logger.cal_data, "\r"
if self.flag_start_trial:
break
rate.sleep()
print "Experiment started...\r"
self.state = "Resetting"
self.flag_start_trial = False
t_last = rospy.get_time()
self.t_reset_start = rospy.get_time()
while not rospy.is_shutdown() and not self.flag_end_program:
if self.state == "Running":
# check for stop or trial end
pose = self.logger.get_pose()
if self.check_for_stop(pose) or rospy.get_time(
) - self.t_trial_start > self.trial_dt:
print "Trial ", self.trial, " ended\r"
# save data first
self.logger.save_data(file_name="trial{}".format(
self.trial),
flag_save_comm=True)
# check if exp ends
self.trial += 1
if self.trial >= self.n_trial_total:
print "All trials finished!\r"
break
# prepare to reset
self.t_reset_start = rospy.get_time()
self.state = "Resetting"
if rospy.get_time() - t_last >= self.planner_dt:
# compute and send feedback
x_diff = self.s_g[:2] - pose[:2]
print "possition difference is: ", x_diff, "\r"
print "pose is: ", pose, "\r"
cmd = wrap_to_pi(
np.arctan2(x_diff[1], x_diff[0]) - pose[2])
# convert to right format and publish
self.publish_haptic_control(
[self.convert_feedback(cmd), 2])
# log communication
self.logger.log_comm(rospy.get_time() - self.t_trial_start,
cmd)
t_last = rospy.get_time()
# log data every loop
self.logger.log(self.t_trial_start)
elif self.state == "Resetting":
# wait for some time or user input to start the next trial
flag_start_trial = False
if self.mode == "auto":
if rospy.get_time(
) - self.t_reset_start >= self.resetting_dt:
flag_start_trial = True
else:
flag_start_trial = self.flag_start_trial
if flag_start_trial:
self.start_trial()
elif self.state == "Pausing":
# wait for user input
if self.flag_start_trial:
self.flag_start_trial = False
self.t_resume_start = rospy.get_time()
self.state = "Resuming"
elif self.state == "Resuming":
# wait for timer
if rospy.get_time() - self.t_resume_start >= self.resuming_dt:
self.start_trial()
def estimate(self, data, params0=None):
"""Estimate circle model from data using total least squares.
Parameters
----------
data : (N, 2) array
N points with ``(x, y)`` coordinates, respectively.
Returns
-------
success : bool
True, if model estimation succeeds.
"""
_check_data_dim(data, dim=2)
x = data[:, 0]
y = data[:, 1]
N = data.shape[0]
# pre-allocate jacobian for all iterations
A = np.zeros((N + 5, 2 * N), dtype=np.double)
# same for all iterations: xc, yc
A[0, :N] = -1
A[1, N:] = -1
diag_idxs = np.diag_indices(N)
def fun(params):
xyt = self.predict_xy(params[5:], params[:5])
fx = x - xyt[:, 0]
fy = y - xyt[:, 1]
return np.append(fx, fy)
def Dfun(params):
xc, yc, a, b, theta = params[:5]
t = params[5:]
ct = np.cos(t)
st = np.sin(t)
ctheta = math.cos(theta)
stheta = math.sin(theta)
# derivatives for fx, fy in the following order:
# xc, yc, a, b, theta, t_i
# fx
A[2, :N] = - ctheta * ct
A[3, :N] = stheta * st
A[4, :N] = a * stheta * ct + b * ctheta * st
A[5:, :N][diag_idxs] = a * ctheta * st + b * stheta * ct
# fy
A[2, N:] = - stheta * ct
A[3, N:] = - ctheta * st
A[4, N:] = - a * ctheta * ct + b * stheta * st
A[5:, N:][diag_idxs] = a * stheta * st - b * ctheta * ct
return A
# initial guess of parameters using a circle model
all_params0 = np.empty((N + 5, ), dtype=np.double)
if params0 is None:
xc0 = np.median(x)
yc0 = np.median(y)
r0 = np.median(np.sqrt((x - xc0)**2 + (y - yc0)**2))
params0 = (xc0, yc0, r0, r0, 0)
else:
if len(params0) != 5:
raise ValueError("params0 must have a length of 5.")
params0 = tuple(params0)
xc0 = params0[0]
yc0 = params0[1]
all_params0[:5] = params0
all_params0[5:] = np.arctan2(y - yc0, x - xc0)
pfit, pcov, infodict, errmsg, success = \
optimize.leastsq(fun, all_params0, Dfun=Dfun, col_deriv=True,
full_output=True)
self.params = pfit[:5]
self.params[2:4] = np.abs(self.params[2:4])
self.params[-1] = wrap_to_pi(self.params[-1])
# Need to multiply the fractional covariance matrix from leastsq with
# the reduced chi-square value
if len(data) > 5 and pcov is not None:
s_sq = (self.residuals(data) ** 2).sum() / (len(data) - 5)
pcov *= s_sq
self.param_errors = np.empty((len(pfit)))
# Standard errors are sqrt of the cov matrix diagonals
for i in range(len(pfit)):
self.param_errors[i] = np.sqrt(np.abs(pcov[i, i]))
else:
self.param_errors = np.array([np.NaN] * len(pfit))
# Did it work?
if success in [1, 2, 3, 4]:
return True
# If not, print fail message and return false
# print(output[-2])
return False
def _loop(self, trial_start):
trial = trial_start
rate = rospy.Rate(40)
# first wait for calibration
print "Please calibrate the IMU, when ready, press 's'...\r"
while not rospy.is_shutdown():
print "Calibration values are: ", self.logger.cal_data, "\r"
if self.flag_start_trial:
break
rate.sleep()
self.flag_start_trial = False
print "Calibration finished...\r"
if self.mode == "auto":
print "Trial will automatically start in ", self.t_pause, " seconds\r"
else:
print "Please press 's' to start trial and 'e' to end trial\r"
t_last = rospy.get_time()
t_render = self.t_render
while not rospy.is_shutdown() and not self.flag_end_program:
# save data if is saving
if self.flag_is_saving:
self.logger.log(self.t_start)
# if mode is auto, set flag based on timer
if self.mode == "auto" and rospy.get_time() - t_last > t_render:
self.flag_end_trial = True
t_last = rospy.get_time()
# check for end trial
if self.flag_end_trial:
self.logger.save_data(file_name="trial{}.txt".format(trial))
self.flag_is_saving = False
self.flag_end_trial = False
self.flag_start_trial = False
# send another feedback to remind user
# always use backward cue for this
self.publish_haptic_control([270, 0])
print "Trial ", trial, " ended\r"
trial += 1
# take a break or end experiment if reaching end of the block
if trial >= len(self.dir):
break
if trial % self.n_block == 0:
self._break(rate)
t_last = rospy.get_time()
else:
if self.mode == "auto" and rospy.get_time() - t_last > self.t_pause:
self.flag_start_trial = True
t_last = rospy.get_time()
# check for start trial
if self.flag_start_trial:
# publish haptic feedback
self.publish_haptic_control([self.dir[trial], self.mag[trial]])
# set flags
self.flag_is_saving = True
self.flag_start_trial = False
self.flag_end_trial = False
# reset logger
self.logger.reset()
# compute new render time
t_render = self.t_render + \
np.abs(wrap_to_pi(np.deg2rad(self.dir[trial]) - np.pi * 0.5)) * self.t_render_inc
print "Trial ", trial, " started, t_render is ", t_render, "...\r"
t_last = rospy.get_time()
rate.sleep()
def _loop(self, trial_start):
self.trial = trial_start
rate = rospy.Rate(40)
# first wait for calibration to complete
print "Please calibrate the IMU, when ready, press 's'...\r"
while not rospy.is_shutdown():
print "Calibration values are: ", self.logger.cal_data, "\r"
if self.flag_start_trial:
break
rate.sleep()
print "Experiment started...\r"
self.state = "Resetting"
self.flag_start_trial = False
t_last = rospy.get_time()
self.t_reset_start = rospy.get_time()
while not rospy.is_shutdown() and not self.flag_end_program:
if self.state == "Running":
# check for stop or trial end
pose = self.logger.get_pose()
if self.check_for_stop(pose) or rospy.get_time() - self.t_trial_start > self.trial_dt:
print "Trial ", self.trial, " ended\r"
# save data first
self.logger.save_data(file_name="trial{}".format(self.trial), flag_save_comm=True)
# check if exp ends
self.trial += 1
if self.trial >= self.n_trial_total:
print "All trials finished!\r"
break
# prepare to reset
self.t_reset_start = rospy.get_time()
self.state = "Resetting"
if rospy.get_time() - t_last >= self.planner_dt:
# compute and send feedback
x_diff = self.s_g[:2] - pose[:2]
print "possition difference is: ", x_diff, "\r"
print "pose is: ", pose, "\r"
cmd = wrap_to_pi(np.arctan2(x_diff[1], x_diff[0]) - pose[2])
# convert to right format and publish
self.publish_haptic_control([self.convert_feedback(cmd), 2])
# log communication
self.logger.log_comm(rospy.get_time() - self.t_trial_start, cmd)
t_last = rospy.get_time()
# log data every loop
self.logger.log(self.t_trial_start)
elif self.state == "Resetting":
# wait for some time or user input to start the next trial
flag_start_trial = False
if self.mode == "auto":
if rospy.get_time() - self.t_reset_start >= self.resetting_dt:
flag_start_trial = True
else:
flag_start_trial = self.flag_start_trial
if flag_start_trial:
self.start_trial()
elif self.state == "Pausing":
# wait for user input
if self.flag_start_trial:
self.flag_start_trial = False
self.t_resume_start = rospy.get_time()
self.state = "Resuming"
elif self.state == "Resuming":
# wait for timer
if rospy.get_time() - self.t_resume_start >= self.resuming_dt:
self.start_trial()
def fit_region(coords,
initial_props=None,
try_fit_ellipse=True,
use_ransac=False,
min_in_mask=0.8,
mask=None,
max_resid=None,
ransac_trials=50,
beam_pix=4,
max_rad=1.75,
max_eccent=3.,
image_shape=None,
conv_hull=None,
verbose=False):
'''
Fit a circle or ellipse to the given coordinates.
'''
coords = np.array(coords).copy()
ymean = coords[:, 0].mean()
xmean = coords[:, 1].mean()
# Fitting works better when the points are near the origin
coords[:, 0] -= int(ymean)
coords[:, 1] -= int(xmean)
new_props = np.empty((5, ))
if len(coords) < 3:
raise ValueError("Must have at least 3 points to fit.")
if len(coords) > 5 and try_fit_ellipse:
with catch_warnings():
filterwarnings("ignore", r"Number of calls to function")
filterwarnings("ignore", r"gtol=0.000000 is too small")
if use_ransac:
model, inliers = \
ransac(coords[:, ::-1], EllipseModel, 5,
beam_pix, max_trials=ransac_trials)
else:
model = EllipseModel()
model.estimate(coords[:, ::-1])
dof = len(coords) - 5
# Calculate the model residual
resid = model.residuals(coords[:, ::-1]).sum() / dof
pars = model.params.copy()
pars[0] += int(xmean)
pars[1] += int(ymean)
eccent = pars[2] / float(pars[3])
# Sometimes a < b?? If so, manually correct.
if eccent < 1:
eccent = 1. / eccent
pars[2], pars[3] = pars[3], pars[2]
pars[4] = wrap_to_pi(pars[4] + 0.5 * np.pi)
fail_conds = eccent > max_eccent
if beam_pix is not None and not fail_conds:
fail_conds = fail_conds or \
pars[3] < beam_pix
if max_resid is not None and not fail_conds:
fail_conds = fail_conds or \
resid > max_resid
if initial_props is not None and not fail_conds:
fail_conds = fail_conds or \
pars[2] > max_rad * initial_props[2] or \
not in_ellipse(initial_props[:2][::-1], pars)
if image_shape is not None and not fail_conds:
fail_conds = fail_conds or \
not in_array(pars[:2], image_shape[::-1])
# We require the entire region be inside the array. This
# shouldn't be a big issue for galaxies, but should be revisited
# when testing on galactic emission or incomplete maps.
fail_conds = fail_conds or not \
ellipse_in_array(pars, image_shape[::-1])
if mask is not None and not fail_conds:
# Make sure most of the region falls within the hole mask
fail_conds = fail_conds or \
fraction_in_mask(pars, mask) < min_in_mask
# Now make sure the region is within a region with signal.
# We define this based on the convex hull. It seems
# appropriate to just use min_in_mask, since most of it needs to
# be within the region.
if conv_hull is not None and not fail_conds:
fail_conds = fail_conds or \
fraction_in_mask(pars, conv_hull) < min_in_mask
# The center of the fit should fall within the convex hull mask
y, x = floor_int(pars[1]), floor_int(pars[0])
fail_conds = fail_conds or not conv_hull[y, x]
if fail_conds:
ellip_fail = True
else:
new_props[0] = pars[1]
new_props[1] = pars[0]
new_props[2] = pars[2]
new_props[3] = pars[3]
new_props[4] = pars[4]
ellip_fail = False
else:
ellip_fail = True
# If ellipse fitting is not allowed, or it failed, fit a circle
if ellip_fail:
with catch_warnings():
filterwarnings("ignore", r"Number of calls to function")
filterwarnings("ignore", r"gtol=0.000000 is too small")
if use_ransac:
model, inliers = \
ransac(coords[:, ::-1], CircleModel, 3,
beam_pix, max_trials=ransac_trials)
else:
model = CircleModel()
model.estimate(coords[:, ::-1])
dof = len(coords) - 3
# Calculate the model residual
resid = model.residuals(coords[:, ::-1]).sum() / dof
pars = model.params.copy()
pars[0] += int(xmean)
pars[1] += int(ymean)
fail_conds = False
if beam_pix is not None and not fail_conds:
fail_conds = fail_conds or \
pars[2] < beam_pix
if max_resid is not None and not fail_conds:
fail_conds = fail_conds or \
resid > max_resid
if initial_props is not None and not fail_conds:
fail_conds = fail_conds or \
pars[2] > max_rad * initial_props[2] or \
not in_circle(initial_props[:2][::-1], pars)
if image_shape is not None and not fail_conds:
fail_conds = fail_conds or \
not in_array(pars[:2], image_shape[::-1])
# We require the entire region be inside the array. This
# shouldn't be a big issue for galaxies, but should be revisited
# when testing on galactic emission or incomplete maps.
fail_conds = fail_conds or not \
circle_in_array(pars, image_shape[::-1])
if mask is not None and not fail_conds:
fail_conds = fail_conds or \
fraction_in_mask(pars, mask) < min_in_mask
if conv_hull is not None and not fail_conds:
# Needs to be in convex hull. See ellipse rejections
fail_conds = fail_conds or \
fraction_in_mask(pars, conv_hull) < min_in_mask
# The center of the fit should fall within the convex hull mask
y, x = floor_int(pars[1]), floor_int(pars[0])
fail_conds = fail_conds or not conv_hull[y, x]
if fail_conds:
if verbose:
Warning("All fitting failed.")
return None, None
new_props[0] = pars[1]
new_props[1] = pars[0]
new_props[2] = pars[2]
new_props[3] = pars[2]
new_props[4] = 0.0
props = new_props
return props, resid
def estimate(self, data, params0=None):
"""Estimate circle model from data using total least squares.
Parameters
----------
data : (N, 2) array
N points with ``(x, y)`` coordinates, respectively.
Returns
-------
success : bool
True, if model estimation succeeds.
"""
_check_data_dim(data, dim=2)
x = data[:, 0]
y = data[:, 1]
N = data.shape[0]
# pre-allocate jacobian for all iterations
A = np.zeros((N + 5, 2 * N), dtype=np.double)
# same for all iterations: xc, yc
A[0, :N] = -1
A[1, N:] = -1
diag_idxs = np.diag_indices(N)
def fun(params):
xyt = self.predict_xy(params[5:], params[:5])
fx = x - xyt[:, 0]
fy = y - xyt[:, 1]
return np.append(fx, fy)
def Dfun(params):
xc, yc, a, b, theta = params[:5]
t = params[5:]
ct = np.cos(t)
st = np.sin(t)
ctheta = math.cos(theta)
stheta = math.sin(theta)
# derivatives for fx, fy in the following order:
# xc, yc, a, b, theta, t_i
# fx
A[2, :N] = -ctheta * ct
A[3, :N] = stheta * st
A[4, :N] = a * stheta * ct + b * ctheta * st
A[5:, :N][diag_idxs] = a * ctheta * st + b * stheta * ct
# fy
A[2, N:] = -stheta * ct
A[3, N:] = -ctheta * st
A[4, N:] = -a * ctheta * ct + b * stheta * st
A[5:, N:][diag_idxs] = a * stheta * st - b * ctheta * ct
return A
# initial guess of parameters using a circle model
all_params0 = np.empty((N + 5, ), dtype=np.double)
if params0 is None:
xc0 = np.median(x)
yc0 = np.median(y)
r0 = np.median(np.sqrt((x - xc0)**2 + (y - yc0)**2))
params0 = (xc0, yc0, r0, r0, 0)
else:
if len(params0) != 5:
raise ValueError("params0 must have a length of 5.")
params0 = tuple(params0)
xc0 = params0[0]
yc0 = params0[1]
all_params0[:5] = params0
all_params0[5:] = np.arctan2(y - yc0, x - xc0)
pfit, pcov, infodict, errmsg, success = \
optimize.leastsq(fun, all_params0, Dfun=Dfun, col_deriv=True,
full_output=True)
self.params = pfit[:5]
self.params[2:4] = np.abs(self.params[2:4])
self.params[-1] = wrap_to_pi(self.params[-1])
# Need to multiply the fractional covariance matrix from leastsq with
# the reduced chi-square value
if len(data) > 5 and pcov is not None:
s_sq = (self.residuals(data)**2).sum() / (len(data) - 5)
pcov *= s_sq
self.param_errors = np.empty((len(pfit)))
# Standard errors are sqrt of the cov matrix diagonals
for i in range(len(pfit)):
self.param_errors[i] = np.sqrt(np.abs(pcov[i, i]))
else:
self.param_errors = np.array([np.NaN] * len(pfit))
# Did it work?
if success in [1, 2, 3, 4]:
return True
# If not, print fail message and return false
# print(output[-2])
return False
