def measurement_prob1(self, scan):
"""
Take laser scan data and compare it to particles position by use of
raycaster. Compute weight by summing up the quality of readings from each scan
:param scan: laser scan msg
:return: weight ofsettings. particle based on correct pose
"""
# take laser scan and compare it to raycaster for particles position
# number of laser scans
# scan_count = int((scan.angle_max - scan.angle_min) / scan.angle_increment) + 1
# beams = np.arange(scan.angle_min, scan.angle_max + 0.01, scan.angle_increment) # needed .01 to give first beam
# from eric m
# precomputes the coefficient and exponent base for faster normal calculations
p_hit_coeff = 1./(np.sqrt(2*np.pi*STD_DEV_HIT*STD_DEV_HIT))
p_hit_exp = np.exp(-1./(2.*STD_DEV_HIT*STD_DEV_HIT))
def p_hit(sensed_distance, raytraced_distance):
if sensed_distance > MAX_DIST:
return 0.0
return p_hit_coeff*(p_hit_exp**((sensed_distance - raytraced_distance)**2))
def p_max(sensed_distance, raytraced_distance):
if sensed_distance == MAX_DIST:
return 1.0
return 0.0
def p_rand(sensed_distance, raytraced_distance):
if sensed_distance < MAX_DIST:
return 0.0
return 1./MAX_DIST
scan_min = scan.angle_min
scan_inc = scan.angle_increment
prob = 1.0
for l in xrange(len(scan.ranges)):
sensed = scan.ranges[l]
traced = mcl_tools.map_range(self, scan_min+(l*scan_inc))
prob *= Z_HIT*p_hit(sensed, traced)\
+ Z_RAND*p_rand(sensed, traced)\
+ Z_MAX*p_max(sensed, traced)
print "probability", prob
self.set_weight(prob)
# print "particle prob ", prob
return prob
def particle_weight(scan, particle):
scan_min = scan.angle_min
# scan_inc = scan.angle_increment * 50
scan_inc = scan.angle_increment
prob = 1.0
for i in xrange(len(scan.ranges)):
if i % 50 is 0:
# print "scan: " + str(i)
sensed = scan.ranges[i]
val = scan_min + (i * scan_inc)
traced = mcl_tools.map_range(particle, val)
zhit = Z_HIT * p_hit(sensed, traced)
zmax = Z_MAX * p_max(sensed, traced)
zrand = Z_RAND * p_rand(sensed, traced)
prob *= (zhit + zmax + zrand)
return prob
def measurement_prob(self, scan):
"""
Take laser scan data and compare it to particles position by use of
raycaster. Compute weight by summing up the quality of readings from each scan
:param scan: laser scan msg
:return: weight of particle based on correct pose
"""
# take laser scan and compare it to raycaster for particles position
# number of laser scans
scan_count = int((scan.angle_max - scan.angle_min) / scan.angle_increment) + 1
beams = np.arange(scan.angle_min, scan.angle_max + 0.01, scan.angle_increment) # needed .01 to give first beam
w_sum = 0
for l in xrange(scan_count):
#print np.degrees(beams[l]), ": stage ", scan.ranges[l], mcl_tools.map_range(self, beams[l])
# had to add small value since map_range could return 0 and sigma cant be 0
w_sum += mcl_tools.gauss_prob(scan.ranges[l], (mcl_tools.map_range(self, beams[l]))+.001)
self.set_weight(w_sum)
#print "particle internal weight sum ", w_sum
return w_sum
def particle_weight(particle, scan):
global range_ts
return sum([
gaussian_p(scan[i], mcl_tools.map_range(particle, range_ts[i]))
for i in xrange(5)
])
def particle_weight(particle, scan):
global range_ts
return sum([gaussian_p(scan[i], mcl_tools.map_range(particle, range_ts[i]))
for i in xrange(5)])