Skip to content
Snippets Groups Projects
Commit f11d1eeb authored by Davis, Duane T's avatar Davis, Duane T
Browse files

Added a ray_trace method to the map classes 

Also corrected a misplaced wall in GeometryMap.circleWorldMapFactory()
parent bf27697a
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/cs4313_utilities/geometry.py
+ 43
− 0
View file @ f11d1eeb
......@@ -27,6 +27,13 @@ class Geometry(object):
self.center = ( 0.0, 0.0 )
def __repr__(self):
''' Returns a formal summary string for the object
@return "Geometry: (ctrx, ctry)
'''
return "Geometry: (%f, %f)"%(self.center[0], self.center[1])
def distance(self, point):
''' Computes the Euclidean distance from the Geometry to a point. If
the test point is in or on the Geometry object, 0.0 will be returned
......@@ -118,6 +125,13 @@ class Point(Geometry):
self.center = ( point[0], point[1] )
def __repr__(self):
''' Returns a formal summary string for the object
@return "Point: (ctrx, ctry)
'''
return "Point: (%f, %f)"%(self.center[0], self.center[1])
class Circle(Geometry):
''' Class for defining a circle with its center point and radius
......@@ -146,6 +160,13 @@ class Circle(Geometry):
self.radius = abs(radius)
def __repr__(self):
''' Returns a formal summary string for the object
@return "Point: (ctrx, ctry)
'''
return "Circle: (%f, %f), %f"%(self.center[0], self.center[1], self.radius)
def distance(self, point):
''' Computes the Euclidean distance from the Circle to a point. If
the test point is in or on the Circle object, 0.0 will be returned
......@@ -306,6 +327,14 @@ class LineSegment(Geometry):
self.phi, self.rho = rm.normal_form_parameters(end1, end2)
def __repr__(self):
''' Returns a formal summary string for the object
@return "Geometry: (ctrx, ctry)
'''
return "Segment: (%f, %f)-(%f, %f)"\
%(self.end1[0], self.end1[1], self.end2[0], self.end2[1])
def distance(self, point):
''' Determines the distance from this LineSegment to a point
specified by Cartesian coordinates. If the point is contained
......@@ -445,6 +474,20 @@ class Polygon(Geometry):
((1.0 / (6.0 * center_A)) * center_y) )
def __repr__(self):
''' Returns a formal summary string for the object
@return "Geometry: (ctrx, ctry)
'''
result = "Polygon: "
if len(self.edges) > 0:
for edge in self.edges:
result += "(%f, %f)-"%(edge[0][0], edge[0][1])
result += "(%f, %f)"%(self.edges[0][0][0], self.edges[0][0][1])
else:
result += "no edges"
return result
def distance(self, point):
''' Computes the Euclidean distance from the Polygon to a point. If
the test point is in or on the Polygon object, 0.0 will be returned
......
src/cs4313_utilities/maps.py
+ 133
− 60
View file @ f11d1eeb
......@@ -22,14 +22,14 @@ class Map(object):
_alpha_obst_sens: alpha_obstacle parameter for the sensor model
_alpha_rand_sens: alpha_random parameter for the sensor model
_alpha_max_sens: alpha_max parameter for the sensor model
_sigma_hit: Gaussian standard deviation for the sensor model hit
_lamda_rand: exponential rate parameter for the sensor model random return
_sigma_hit_rm: Gaussian standard deviation for the sensor model hit
_lamda_rand_rm: exponential rate parameter for the sensor model random return
_alpha_hit_like: alpha_hit parameter for the likelihood field model
_alpha_rand_like: alpha_rand parameter for the likelihood field model
_sigma_like: Gaussian standard deviation for the likelihood field model
Public methods:
set_sensor_model_params: sets sensor model parameters
set_ranger_model_params: sets the range sensor model parameters
set_likelihood_field_params: sets the likelihood field model parameters
Abstract public methods:
......@@ -38,54 +38,59 @@ class Map(object):
is_free_space: Boolean method that returns True if a test state is in free space
is_free_space_path: Boolean method that returns True if a straight test path is in free space
nearest_obstacle: computes the distance between a point and the nearest obstacle
ray_trace: identifies the first intersection of a ray with a mapped obstacle
distance: provides a scalar direct-path distance between two map states
apply_sensor_model: applies the range-sensor model for a state hypothesis
apply_ranger_model: applies the range-sensor model for a state hypothesis
apply_likelihood_model: applies the likelihood field model for a state hypothesis
'''
def __init__(self, alpha_hit_sens=0.75, alpha_obst_sens=0.2, \
alpha_rand_sens=0.025, alpha_max_sens=0.025, z_max=8.0, \
sigma_hit=0.25, lamda_rand=0.5, \
def __init__(self, z_max=8.0, alpha_hit_rm=0.75, alpha_obst_rm=0.2, \
alpha_rand_rm=0.025, alpha_max_rm=0.025, \
sigma_hit_rm=0.25, lamda_rand_rm=0.5, \
alpha_hit_like=0.9, alpha_rand_like=0.05, sigma_like=0.5):
''' Class initializer
@alpha_hit_sens: alpha_hit parameter for sensor model
@alpha_obst_sens: alpha_obstacle parameter for sensor model
@alpha_rand_sens: alpha_random parameter for sensor model
@alpha_max_sens: alpha_max parameter for sensor model
@z_max: maximum sensor range
@sigma_hit: Gaussian standard deviation for sensor model hit
@lamda_rand: exponential rate parameter for sensor model random return
@alpha_hit_rm: alpha_hit parameter for sensor model
@alpha_obst_rm: alpha_obstacle parameter for sensor model
@alpha_rand_rm: alpha_random parameter for sensor model
@alpha_max_rm: alpha_max parameter for sensor model
@param z_max: maximum range of the LIDAR sensor
@alpha_hit_rm: alpha_hit parameter for sensor model
@alpha_obst_rm: alpha_obstacle parameter for sensor model
@alpha_rand_rm: alpha_random parameter for sensor model
@alpha_max_rm: alpha_max parameter for sensor model
@sigma_hit_rm: Gaussian standard deviation for sensor model hit
@lamda_rand_rm: exponential rate parameter for sensor model random return
@alpha_hit_like: alpha_hit parameter for likelihood field model
@alpha_rand_like: alpha_rand parameter for likelihood field model
@sigma_like: Gaussian standard deviation for likelihood field model
'''
self.set_sensor_model_params(alpha_hit_sens, alpha_obst_sens, \
alpha_rand_sens, alpha_max_sens, \
z_max, sigma_hit, lamda_rand)
self.set_ranger_model_params(alpha_hit_rm, alpha_obst_rm, \
alpha_rand_rm, alpha_max_rm, \
z_max, sigma_hit_rm, lamda_rand_rm)
self.set_likelihood_field_params(alpha_hit_like, alpha_rand_like, \
z_max, sigma_like)
def set_sensor_model_params(self, alpha_hit, alpha_obst, alpha_rand, \
alpha_max, z_max, sigma, lamda):
''' Sets model parameters for the range-based sensor model
NOTE: This model is not currently implemented
@alpha_hit: alpha_hit parameter
@alpha_obst: alpha_obstacle parameter
@alpha_rand: alpha_random parameter
@alpha_max: alpha_max parameter
@z_max: maximum sensor range
@sigma: Gaussian standard deviation for hit
@lamda: exponential rate parameter for random return
def set_ranger_model_params(self, alpha_hit, alpha_obst,
alpha_rand, alpha_max, \
z_max, sigma, lamda):
''' Sets model parameters for the likelihood field model
@param alpha_hit: alpha_hit parameter
@param alpha_obst: alpha_obstacle parameter
@param alpha_rand: alpha_random parameter
@param alpha_max: alpha_max parameter
@param z_max: maximum sensor range
@param sigma: p_hit Gaussian sigma
@sigma: lamda: p_obst exponential distribution lamda
'''
alpha_total = alpha_hit + alpha_obst + alpha_rand + alpha_max
self._alpha_hit_sens = alpha_hit / alpha_total
self._alpha_obst_sens = alpha_obst / alpha_total
self._alpha_rand_sens = alpha_rand / alpha_total
self._alpha_max_sens = alpha_max / alpha_total
self._alpha_hit_rm = alpha_hit / alpha_total
self._alpha_obst_rm = alpha_obst / alpha_total
self._alpha_rand_rm = alpha_rand / alpha_total
self._alpha_max_rm = alpha_max / alpha_total
self._z_max = z_max
self._sigma_sens = sigma
self._lamda_sens = lamda
self._sigma_hit_rm = sigma
self._lamda_obst_rm = lamda
def set_likelihood_field_params(self, alpha_hit, alpha_rand, \
......@@ -97,8 +102,6 @@ class Map(object):
@sigma: likelihood field Gaussian standard deviation
'''
alpha_total = alpha_hit + alpha_rand
if alpha_total < 1.0:
alpha_total = 1.0
self._alpha_hit_like = alpha_hit / alpha_total
self._alpha_rand_like = alpha_rand / alpha_total
self._z_max = z_max
......@@ -111,8 +114,8 @@ class Map(object):
def set_goal(self, goal, goal_bias=0.0):
''' Resets the goal position and sampling bias
@param new goal position
@param new probabilistic bias to the goal for sampling
@param goal: new goal position
@param goal_bias: new probabilistic bias to the goal for sampling
'''
return None
......@@ -126,6 +129,15 @@ class Map(object):
return None
def ray_trace(self, origin, bearing):
''' Identifies the first intersection of a ray with a mapped obstacle
@param origin: Cartesian coordinates of the origin of the ray
@param bearing: direction (Radians) of the ray
@return the first intersection with a mapped object (or None)
'''
return None
def is_free_space(self, state):
''' Inheriting classes should override this method stub to compute a
Boolean value based on whether or not the test value is in free space
......@@ -162,7 +174,7 @@ class Map(object):
return math.hypot((state2[0] - state1[0]), (state2[1] - state1[1]))
def apply_sensor_model(self, state, sensor):
def apply_ranger_model(self, state, sensor):
''' Applies the range sensor model for a particular state hypothesis
and an actual sensor reading
NOTE: This model is not yet implemented and will return 1.0 for now
......@@ -199,12 +211,12 @@ class GeometryMap(Map):
Inherited protected member variables (Map):
_z_max: maximum sensor range
_alpha_hit_sens: alpha_hit parameter for the sensor model
_alpha_obst_sens: alpha_obstacle parameter for the sensor model
_alpha_rand_sens: alpha_random parameter for the sensor model
_alpha_max_sens: alpha_max parameter for the sensor model
_sigma_hit: Gaussian standard deviation for the sensor model hit
_lamda_rand: exponential rate parameter for the sensor model random return
_alpha_hit_rm: alpha_hit parameter for the sensor model
_alpha_obst_rm: alpha_obstacle parameter for the sensor model
_alpha_rand_rm: alpha_random parameter for the sensor model
_alpha_max_rm: alpha_max parameter for the sensor model
_sigma_hit_rm: Gaussian standard deviation for the sensor model hit
_lamda_rand_rm: exponential rate parameter for the sensor model random return
_alpha_hit_like: alpha_hit parameter for the likelihood field model
_alpha_rand_like: alpha_rand parameter for the likelihood field model
_sigma_like: Gaussian standard deviation for the likelihood field model
......@@ -213,6 +225,7 @@ class GeometryMap(Map):
Implementations of parent class abstract methods:
set_goal: resets the goal position and the sampling bias
sample: returns a random free-space location
ray_trace: identifies the first intersection of a ray with a mapped obstacle
is_free_space: Boolean method that returns True if a test state is in free space
is_free_space_path: Boolean method that returns True if a straight test path is in free space
distance: provides a scalar direct-path distance between two map states
......@@ -229,10 +242,12 @@ class GeometryMap(Map):
set_likelihood_field_params: sets the likelihood field model parameters
'''
SENSOR_MODEL_PTS = 20 # Downsample the sensor model for efficiency
def __init__(self, sw_corner, ne_corner, geometries, goal=None, goal_bias=0.0, \
alpha_hit_sens=0.75, alpha_obst_sens=0.2, \
alpha_rand_sens=0.025, alpha_max_sens=0.025, z_max=8.0, \
sigma_hit=0.25, lamda_rand=0.5, \
z_max=8.0, alpha_hit_rm=0.75, alpha_obst_rm=0.2, \
alpha_rand_rm=0.025, alpha_max_rm=0.025, \
sigma_hit_rm=0.25, lamda_rand_rm=0.5, \
alpha_hit_like=0.9, alpha_rand_like=0.05, sigma_like=0.5):
''' Initializes rectangle corners and geometries. Corners are specified
using Cartesian coordinates. Polygon objects must be of type
......@@ -243,19 +258,20 @@ class GeometryMap(Map):
@param geometries: iterable object containing geometries
@param goal: Cartesian coordinates of the goal position (default None)
@param goal_bias: probabilistic sampling bias towards the goal (default 0.0)
@alpha_hit_sens: alpha_hit parameter for sensor model
@alpha_obst_sense: alpha_obstacle parameter for sensor model
@alpha_rand_sens: alpha_random parameter for sensor model
@alpha_max_sens: alpha_max parameter for sensor model
@sigma_hit: Gaussian standard deviation for sensor model hit
@lamda_rand: exponential rate parameter for sensor model random return
@param z_max: maximum range of the LIDAR sensor
@alpha_hit_rm: alpha_hit parameter for sensor model
@alpha_obst_rm: alpha_obstacle parameter for sensor model
@alpha_rand_rm: alpha_random parameter for sensor model
@alpha_max_rm: alpha_max parameter for sensor model
@sigma_hit_rm: Gaussian standard deviation for sensor model hit
@lamda_rand_rm: exponential rate parameter for sensor model random return
@alpha_hit_like: alpha_hit parameter for likelihood field model
@alpha_rand_like: alpha_rand parameter for likelihood field model
@sigma_like: Gaussian standard deviation for likelihood field model
'''
super(GeometryMap, self).__init__(alpha_hit_sens, alpha_obst_sens, \
alpha_rand_sens, alpha_max_sens,
z_max, sigma_hit, lamda_rand, \
super(GeometryMap, self).__init__(z_max, alpha_hit_rm, alpha_obst_rm, \
alpha_rand_rm, alpha_max_rm,
sigma_hit_rm, lamda_rand_rm, \
alpha_hit_like, alpha_rand_like, \
sigma_like)
self._sw_corner = ( sw_corner[0], sw_corner[1] )
......@@ -278,8 +294,8 @@ class GeometryMap(Map):
def set_goal(self, goal, goal_bias=0.0):
''' Resets the goal position and sampling bias
@param Cartesian coordinates of the new goal position
@param new probabilistic bias to the goal for sampling
@param goal: Cartesian coordinates of the new goal position
@param goal_bias: new probabilistic bias to the goal for sampling
'''
if goal:
self._goal = ( goal[0], goal[1] )
......@@ -307,6 +323,30 @@ class GeometryMap(Map):
return (random_x, random_y, random_theta)
def ray_trace(self, origin, bearing):
''' Identifies the first intersection of a ray with a mapped obstacle
@param origin: Cartesian coordinates of the origin of the ray
@param bearing: direction (Radians) of the ray
@return the first intersection with a mapped object (or None)
'''
# convert ray to a huge segment, offset the first end by a bit to
# make sure that it isn't already directly on an obstacle edge
end1 = ((origin[0] + rm.SMALLNUM * math.cos(bearing)), \
(origin[1] + rm.SMALLNUM * math.sin(bearing)))
end2 = ((origin[0] + rm.BIGNUM * math.cos(bearing)), \
(origin[1] + rm.BIGNUM * math.sin(bearing)))
result = None
hit_dist = rm.BIGNUM
for obstacle in self._geometries:
hit = obstacle.intersects(end1, end2)
if hit:
dist = self.distance(origin, hit)
if dist < hit_dist:
result = hit
hit_dist = dist
return result
def is_free_space(self, state):
''' Computes a Boolean value based on whether or not the test value
is in free space
......@@ -365,6 +405,38 @@ class GeometryMap(Map):
return math.hypot((state2[0] - state1[0]), (state2[1] - state1[1]))
def apply_ranger_model(self, state, sensor):
''' Applies the range sensor model for a particular state hypothesis
and an actual sensor reading
@param state: state hypothesis tuple in the form (x, y, theta)
@param sensor: sensor reading (should be a LaserData object)
@return model PDF value corresponding to P( z | x, m )
'''
# If the "state" isn't in free space, the likelihood is 0
if not self.is_free_space(state):
return 1e-9 # A very low, but nonzero number
result = 1.0
# Test scan returns against the map, down sample for efficiency
step_size = len(sensor.ranges) / GeometryMap.SENSOR_MODEL_PTS
start = step_size / 2
for step in range(GeometryMap.SENSOR_MODEL_PTS):
testReturn = round(start + step_size * step)
bearing = state[2] + sensor.rayBearing(testReturn)
ray_hit = self.ray_trace(state, bearing)
z_exp = self._z_max
if ray_hit:
z_exp = min((self._z_max, self.distance(state, ray_hit)))
result *= rm.rangeSensorModel(sensor.ranges[testReturn], z_exp, \
self._z_max, self._sigma_hit_rm, \
self._lamda_obst_rm, \
self._alpha_hit_rm, \
self._alpha_obst_rm, \
self._alpha_max_rm, \
self._alpha_rand_rm)
return result
def apply_likelihood_model(self, state, sensor):
''' Applies the likelihood field model for a particular state hypothesis
and an actual sensor reading
......@@ -376,6 +448,7 @@ class GeometryMap(Map):
if not self.is_free_space(state):
return 1e-9 # A very low, but nonzero number
result = 1.0
# For efficiency, only check interval of continuity closest pts
for interval in sensor.intervals:
rng = sensor.ranges[interval[2]]
......@@ -400,7 +473,7 @@ class GeometryMap(Map):
''' CS4313-specific static method that creates and returns a map
of the cs4313_worlds/circles.world Gazebo environment
'''
north_wall = geometry.LineSegment((-28.0, -25.0), (35.0, 35.0))
north_wall = geometry.LineSegment((-35.0, 35.0), (35.0, 35.0))
east_wall = geometry.LineSegment((35.0, 35.0), (35.0, -35.0))
south_wall = geometry.LineSegment((-35.0, -35.0), (35.0, -35.0))
west_wall = geometry.LineSegment((-35.0, -35.0), (-35.0, 35.0))
......
src/cs4313_utilities/robot_math.py
+ 66
− 67
View file @ f11d1eeb
......@@ -177,73 +177,72 @@ def likelihoodFieldModel(distance, z_max, sigma, alpha_hit, alpha_rand):
return alpha_hit * gaussian_PDF(distance, sigma) + alpha_rand / z_max
# NOTE: Model not implemented yet (commented out for now)
#def rangeSensorModel(z, z_exp, z_max, sigma, lamda, \
# alpha_hit, alpha_unexp, alpha_max, alpha_rand):
# ''' Implements the range sensor model from the CS4313 lecture material
# @param z: sensor reading (range) being checked
# @param z_exp: expected value (i.e., expected range to the obstacle)
# @param z_max: sensor's maximum range
# @param sigma: standard deviation of the hit model Gaussian
# @param lamda: rate parameter of the obstacle model exponential
# @param alpha_hit: normalized coefficient for the hit model
# @param alpha_unexp: normalized coefficient for the obstacle model
# @param alpha_max: normalized coefficient for the maximum range model
# @param alpha_rand: normalized coefficient for the random return model
# @return: PDF value for the probability that the return corresponds to the map
# '''
# return alpha_hit * rangeSensorHitModel(z, z_exp, sigma) + \
# alpha_unexp * rangeSensorObstacleModel(z, z_exp, lamda) + \
# alpha_rand * rangeSensorRandomModel(z_max) + \
# alpha_max * rangeSensorMaxRangeModel(z, z_max)
#
#
#def rangeSensorHitModel(z, z_exp, sigma):
# ''' Computes the probability that a given sensor reading is from a mapped
# obstacle using a Gaussian distribution centered on the expected range
# @param z: sensor reading (range) being checked
# @param z_exp: expected value (i.e., expected range to the obstacle)
# @param sigma: standard deviation of the Gaussian
# @return: the PDF value that the return is from a mapped obstacle
# '''
# return gaussian_PDF((z-z_exp), sigma)
#
#
#def rangeSensorObstacleModel(z, z_exp, lamda):
# ''' Computes the probability of a given sensor reading is from an
# unmapped obstacle using an exponential distribution
# @param z: sensor reading (range) being checked
# @param z_exp: expected value (i.e., expected range to the obstacle)
# @param lamda: rate parameter of the distribution
# @return: the PDF value that the return is from an unmapped obstacle
# '''
# if z > z_exp:
# return 0.0 # Can't detect an obstacle beyond the mapped one
#
# exponential = math.exp(-lamda * z)
# eta = 1.0 / (1.0 - exponential)
# return eta * lamda * exponential
#
#
#def rangeSensorRandomModel(z_max):
# ''' Computes the probability that a return is just a random return
# using a uniform distribution between 0 and the maximum range
# @param z_max: sensor's maximum range
# @return: PDF value that the return is random
# '''
# return 1.0 / z_max
#
#
#def rangeSensorMaxRangeModel(z, z_max):
# ''' Computes the probability that a return is a maximum range return
# (i.e., not object detected)
# @param z: sensor reading (range) being checked
# @param z_max: maximum range of the sensor
# @return the PDF value that the specific return was a max-range return
# '''
# if abs(z - z_max) <= SMALLNUM:
# return 1.0
# return 0.0
def rangeSensorModel(z, z_exp, z_max, sigma, lamda, \
alpha_hit, alpha_unexp, alpha_max, alpha_rand):
''' Implements the range sensor model from the CS4313 lecture material
@param z: sensor reading (range) being checked
@param z_exp: expected value (i.e., expected range to the obstacle)
@param z_max: sensor's maximum range
@param sigma: standard deviation of the hit model Gaussian
@param lamda: rate parameter of the obstacle model exponential
@param alpha_hit: normalized coefficient for the hit model
@param alpha_unexp: normalized coefficient for the obstacle model
@param alpha_max: normalized coefficient for the maximum range model
@param alpha_rand: normalized coefficient for the random return model
@return: PDF value for the probability that the return corresponds to the map
'''
return alpha_hit * rangeSensorHitModel(z, z_exp, sigma) + \
alpha_unexp * rangeSensorObstacleModel(z, z_exp, lamda) + \
alpha_rand * rangeSensorRandomModel(z_max) + \
alpha_max * rangeSensorMaxRangeModel(z, z_max)
def rangeSensorHitModel(z, z_exp, sigma):
''' Computes the probability that a given sensor reading is from a mapped
obstacle using a Gaussian distribution centered on the expected range
@param z: sensor reading (range) being checked
@param z_exp: expected value (i.e., expected range to the obstacle)
@param sigma: standard deviation of the Gaussian
@return: the PDF value that the return is from a mapped obstacle
'''
return gaussian_PDF((z-z_exp), sigma)
def rangeSensorObstacleModel(z, z_exp, lamda):
''' Computes the probability of a given sensor reading is from an
unmapped obstacle using an exponential distribution
@param z: sensor reading (range) being checked
@param z_exp: expected value (i.e., expected range to the obstacle)
@param lamda: rate parameter of the distribution
@return: the PDF value that the return is from an unmapped obstacle
'''
if z > z_exp:
return 0.0 # Can't detect an obstacle beyond the mapped one
exponential = math.exp(-lamda * z)
eta = 1.0 / (1.0 - exponential)
return eta * lamda * exponential
def rangeSensorRandomModel(z_max):
''' Computes the probability that a return is just a random return
using a uniform distribution between 0 and the maximum range
@param z_max: sensor's maximum range
@return: PDF value that the return is random
'''
return 1.0 / z_max
def rangeSensorMaxRangeModel(z, z_max):
''' Computes the probability that a return is a maximum range return
(i.e., not object detected)
@param z: sensor reading (range) being checked
@param z_max: maximum range of the sensor
@return the PDF value that the specific return was a max-range return
'''
if abs(z - z_max) <= SMALLNUM:
return 1.0
return 0.0
def projectSensorReturnToWorld(state, sensor_posit, rng, brg):
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment