def closest_point(self, f_x, f_y, s_x, s_y, obs_bearing):
'''
Calculate the closest point on the line to the feature
The feature is the point (probably not on the line)
The line is defined by a point (state x and y) and direction (heading)
This probably won't return a point that is behind the x-y-bearing.
Input:
f_x float (feature's x coordinate)
f_y float (feature's y coordinate)
s_x float (robot state's x)
s_y float (robot state's y)
obs_bearing float (robot state's heading)
'''
origin_to_feature = (f_x - s_x, f_y - s_y, 0.0,)
line_parallel = unit((cos(obs_bearing), sin(obs_bearing), 0.0))
# origin_to_feature dot line_parallel = magnitude of otf along line
magmag = dot_product(origin_to_feature, line_parallel)
if magmag < 0:
return (s_x, s_y)
scaled_line = scale(line_parallel, magmag)
scaled_x = scaled_line[0]
scaled_y = scaled_line[1]
return (float(s_x + scaled_x), float(s_y + scaled_y))
def closest_point(self, f_x, f_y, s_x, s_y, obs_bearing):
'''
Calculate the closest point on the line to the feature
The feature is the point (probably not on the line)
The line is defined by a point (state x and y) and direction (heading)
This probably won't return a point that is behind the x-y-bearing.
Input:
f_x float (feature's x coordinate)
f_y float (feature's y coordinate)
s_x float (robot state's x)
s_y float (robot state's y)
obs_bearing float (robot state's heading)
'''
origin_to_feature = (
f_x - s_x,
f_y - s_y,
0.0,
)
line_parallel = unit((cos(obs_bearing), sin(obs_bearing), 0.0))
# origin_to_feature dot line_parallel = magnitude of otf along line
magmag = dot_product(origin_to_feature, line_parallel)
if magmag < 0:
return (s_x, s_y)
scaled_line = scale(line_parallel, magmag)
scaled_x = scaled_line[0]
scaled_y = scaled_line[1]
return (float(s_x + scaled_x), float(s_y + scaled_y))
def generate_plan(self, start, goal):
# debug = None
debug = rospy.loginfo
if debug is not None:
debug('potential generate plane\n'+str(goal))
deck = deque()
deck.append(start)
next_ = deepcopy(start)
distance = (math.pow(next_.position.x-goal.position.x, 2)
+ math.pow(next_.position.y-goal.position.y, 2))
if distance < .01:
debug('the start distance is very small')
return []
debug('dist: %f' % (distance,))
step_size = 0.2
count = max(10, int(1.0/step_size))
while(distance > .01 and count >= 0):
debug('start calculating obs_force')
obs_force = self.calc_potential(next_)
debug('end calculating obs_force')
goal_force = self.goal_force(next_, goal)
total_force = addv(obs_force, goal_force)
total_force = unit(total_force)
debug('%s\n%s\n%s' % (obs_force, goal_force, total_force))
dx = total_force[0]*step_size
dy = total_force[1]*step_size
debug('d: %f , %f' % (dx, dy,))
new_pose = Pose()
new_pose.position.x = next_.position.x+dx
new_pose.position.y = next_.position.y+dy
new_pose.orientation = heading_to_quaternion(math.atan2(dy, dx))
deck.append(new_pose)
next_ = deepcopy(new_pose)
distance = (math.pow(next_.position.x-goal.position.x, 2)
+ math.pow(next_.position.y-goal.position.y, 2))
debug('dist: %f' % (distance,))
count += -1
if (distance > .01):
debug('count break')
else:
deck.append(goal)
return list(deck)
def goal_force(self, pose, goal):
dx = goal.position.x - pose.position.x
dy = goal.position.y - pose.position.y
weight = 5.0
farce = (dx, dy, 0)
farce = unit(farce)
farce = scale(farce, weight)
return farce
def off_axis_error(self, location, goal):
"""
calc error normal to axis defined by the goal position and direction
input: two nav_msgs.msg.Odometry, current best location estimate and
goal
output: double distance along the axis
axis is defined by a vector from the unit circle aligned with the goal
heading
relative position is the vector from the goal x, y to the location x, y
distance is defined by subtracting the parallel vector from the total
relative position vector
example use:
see calc_errors above
"""
relative_position_x = (location.pose.pose.position.x -
goal.pose.pose.position.x)
relative_position_y = (location.pose.pose.position.y -
goal.pose.pose.position.y)
# relative position of the best estimate position and the goal
# vector points from the goal to the location
## relative_position = (relative_position_x, relative_position_y, 0.0)
goal_heading = quaternion_to_heading(goal.pose.pose.orientation)
goal_vector_x = math.cos(goal_heading)
goal_vector_y = math.sin(goal_heading)
# vector in the direction of the goal heading, axis of desired motion
goal_vector = (goal_vector_x, goal_vector_y, 0.0)
along_axis_error = self.along_axis_error(location, goal)
along_axis_vec = scale(unit(goal_vector), along_axis_error)
new_rel_x = relative_position_x - along_axis_vec[0]
new_rel_y = relative_position_y - along_axis_vec[1]
new_rel_vec = (new_rel_x, new_rel_y, 0.0)
error_magnitude = math.sqrt(new_rel_x*new_rel_x +
new_rel_y*new_rel_y)
if error_magnitude < .0001:
return 0.0
if cross_product(goal_vector, new_rel_vec)[2] >= 0.0:
return error_magnitude
else:
return -error_magnitude
def construct_problem_mixed(spline_template,
observed_accel_timestamps,
observed_accel_orientations,
observed_accel_readings,
observed_frame_timestamps,
observed_frame_orientations,
observed_features,
imu_to_camera=np.eye(3),
camera_matrix=np.eye(3),
feature_tolerance=1e-2,
gravity_magnitude=9.8,
max_bias_magnitude=.1):
if len(observed_features) < 5:
raise InsufficientObservationsError()
# Sanity checks
assert isinstance(spline_template, spline.SplineTemplate)
assert len(observed_accel_orientations) == len(observed_accel_readings)
assert len(observed_accel_timestamps) == len(observed_accel_readings)
assert len(observed_frame_timestamps) == len(observed_frame_orientations)
assert all(0 <= f.frame_id < len(observed_frame_timestamps) for f in observed_features)
assert np.ndim(observed_accel_timestamps) == 1
assert np.ndim(observed_frame_timestamps) == 1
# Compute offsets
position_offset = 0
position_len = spline_template.control_size
gravity_offset = position_offset + position_len
accel_bias_offset = gravity_offset + 3
structure_offset = accel_bias_offset + 3
track_ids = set(f.track_id for f in observed_features)
num_aux_vars = 1 # one extra variable representing the objective
num_frames = len(observed_frame_timestamps)
num_tracks = max(track_ids) + 1
num_vars = structure_offset + num_tracks * 3 + num_aux_vars
# Make sure each track has at least one observation
counts_by_frame = np.zeros(num_frames, int)
counts_by_track = np.zeros(num_tracks, int)
for f in observed_features:
counts_by_frame[f.frame_id] += 1
counts_by_track[f.track_id] += 1
if not np.all(counts_by_frame > 0):
raise InsufficientObservationsError(
'These frames had zero features: ' + ','.join(map(str, np.flatnonzero(counts_by_frame == 0))))
if not np.all(counts_by_track > 0):
raise InsufficientObservationsError(
'These tracks had zero features: ' + ','.join(map(str, np.flatnonzero(counts_by_track == 0))))
# Track IDs should be exactly 0..n-1
assert all(track_id < num_tracks for track_id in track_ids)
# Initialize the problem
objective = utils.unit(num_vars-1, num_vars) # the last variable is the objective we minimize
problem = socp.SocpProblem(objective)
# Construct accel constraints
print 'Constructing constraints for %d accel readings...' % len(observed_accel_readings)
accel_coefficients = spline_template.coefficients_d2(observed_accel_timestamps)
accel_j_blocks = []
accel_r_blocks = []
for r, a, c in zip(observed_accel_orientations, observed_accel_readings, accel_coefficients):
amat = spline.diagify(c, 3)
j = np.zeros((3, num_vars))
j[:, :position_len] = np.dot(r, amat)
j[:, gravity_offset:gravity_offset+3] = r
j[:, accel_bias_offset:accel_bias_offset+3] = np.eye(3)
accel_j_blocks.append(j)
accel_r_blocks.append(a)
# Form the least squares objective || J*x + r ||^2
accel_j = np.vstack(accel_j_blocks)
accel_r = np.hstack(accel_r_blocks)
# Form the quadratic objective: x' J' J x + b' x + c <= objective ("objective" is the variable we minimize)
accel_c = np.dot(accel_r, accel_r)
accel_b = -2. * np.dot(accel_j.T, accel_r)
accel_b[-1] = -1.
# Convert to an SOCP objective
problem.add_constraint(*soc_constraint_from_quadratic_constraint(accel_j, accel_b, accel_c))
# Construct gravity constraints
a_gravity = np.zeros((3, num_vars))
a_gravity[:, gravity_offset:gravity_offset+3] = np.eye(3)
d_gravity = gravity_magnitude
problem.add_constraint(a=a_gravity, d=d_gravity)
# Construct accel bias constraints
a_bias = np.zeros((3, num_vars))
a_bias[:, accel_bias_offset:accel_bias_offset+3] = np.eye(3)
d_bias = max_bias_magnitude
problem.add_constraint(a=a_bias, d=d_bias)
# Construct vision constraints
print 'Constructing constraints for %d features...' % len(observed_features)
pos_coefficients = spline_template.coefficients(observed_frame_timestamps)
pos_multidim_coefs = [spline.diagify(x, 3) for x in pos_coefficients]
for feature in observed_features:
r = observed_frame_orientations[feature.frame_id]
pmat = pos_multidim_coefs[feature.frame_id]
point_offset = structure_offset + feature.track_id*3
assert point_offset + 3 <= num_vars, 'track id was %d, num vars was %d' % (feature.track_id, num_vars)
k_rc_r = np.dot(camera_matrix, np.dot(imu_to_camera, r))
ymat = np.zeros((3, num_vars))
ymat[:, :position_len] = -np.dot(k_rc_r, pmat)
ymat[:, point_offset:point_offset+3] = k_rc_r
a_feature = ymat[:2] - np.outer(feature.position, ymat[2])
c_feature = ymat[2] * feature_tolerance
problem.add_constraint(a=a_feature, c=c_feature)
return problem
