本文整理汇总了Python中map.Map.display方法的典型用法代码示例。如果您正苦于以下问题：Python Map.display方法的具体用法？Python Map.display怎么用？Python Map.display使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类map.Map的用法示例。
在下文中一共展示了Map.display方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: ParticleFilter
# 需要导入模块: from map import Map [as 别名]
# 或者: from map.Map import display [as 别名]
class ParticleFilter(object):
"""
This is the main particle filter object.
"""
def __init__(self, map_file='../data/map/wean.dat'):
self.map = Map(map_file)
#self.map.display_gaussian([], 'Gaussian Blurred Map')
self._number_particles = 1000
self.particles = list()
self._particle_probabilities = []
for _ in range(0, self._number_particles):
print 'Initializing particle', _
particle = Particle(self.map)
#particle.x = 4080 + np.random.normal(scale=35)
#particle.y = 3980 + np.random.normal(scale=15)
#particle.theta = math.pi + 0.1 + np.random.normal(scale=.1)
self.particles.append(particle)
self._particle_probabilities.append(1)
self._motion_model = MotionModel(0.001, 0.001, 0.1, 0.1)
self._sensor_model = SensorModel(self.map)
self._ranges = []
def log(self, file_handle):
line = list()
for particle in self.particles:
loc = str(particle.x) + ',' + str(particle.y)
line.append(loc)
file_handle.write(';'.join(line))
file_handle.write('\n')
def display(self):
self.map.display(self.particles, ranges=self._ranges)
def update(self, line):
"""
Update step.
Reading is a single reading (i.e. line) from the log file
Could be either an odometry or laser reading
"""
measurement_type = line[0] # O = odometry, L = laser scan
measurements = np.fromstring(line[2:], sep=' ')
odometry = measurements[0:3]
time_stamp = measurements[-1] # last variable
for particle in self.particles:
self._motion_model.update(particle, odometry, time_stamp)
if measurement_type == "L":
odometry_laser = measurements[3:6] # coordinates of the laser in standard odometry frame
self._ranges = measurements[6:-1] # exclude last variable, the time stamp
self._particle_probabilities = list() # unnormalized sensor model probabilities of the particles
for particle in self.particles:
self._particle_probabilities.append(self._sensor_model.get_probability(particle, self._ranges))
argsorted_probabilities = np.argsort(-np.asarray(self._particle_probabilities))
self.particles[argsorted_probabilities[0]].debug = True
self.particles[argsorted_probabilities[1]].debug = True
self.particles[argsorted_probabilities[2]].debug = True
def _resample(self):
"""
Resamples the particles given unnormalized particle probabilites
"""
particle_probabilities = np.asarray(self._particle_probabilities)/sum(self._particle_probabilities) # normalize
indices = np.random.choice(range(0, self._number_particles), size=self._number_particles, replace=True,
p=particle_probabilities)
indices.sort()
previous_index = -1
new_particles = list()
for index in indices:
if index != previous_index:
new_particles.append(self.particles[index])
else:
new_particles.append(deepcopy(self.particles[index]))
previous_index = index
self.particles = new_particles示例2: MyTestCase
# 需要导入模块: from map import Map [as 别名]
# 或者: from map.Map import display [as 别名]
#.........这里部分代码省略.........
odometry = [0, 0, 0]
self.particle.x = 0
self.particle.y = 40
self.particle.theta = math.pi/2
for _ in range(0, 100):
odometry[1] -= 1
motion_model.update(self.particle, odometry, 0)
self.assertEqual(self.particle.x, 100)
self.assertEqual(self.particle.y, 40)
self.assertEqual(self.particle.theta, math.pi/2)
def test_turn_left(self):
motion_model = MotionModel(0, 0, 0, 0)
odometry = [0, 0, 0]
self.particle.x = 0
self.particle.y = 40
self.particle.theta = 0
for _ in range(0, 10):
odometry[2] += math.pi/20
motion_model.update(self.particle, odometry, 0)
self.assertEqual(self.particle.x, 0)
self.assertEqual(self.particle.y, 40)
self.assertEqual(self.particle.theta, math.pi/2)
def test_turn_right(self):
motion_model = MotionModel(0, 0, 0, 0)
odometry = [0, 0, 0]
self.particle.x = 0
self.particle.y = 40
self.particle.theta = 0
for _ in range(0, 10):
odometry[2] -= math.pi/20
motion_model.update(self.particle, odometry, 0)
self.assertEqual(self.particle.x, 0)
self.assertEqual(self.particle.y, 40)
self.assertEqual(self.particle.theta, -math.pi/2)
def test_forward_back(self):
motion_model = MotionModel(0, 0, 0, 0)
odometry = [0, 0, 0]
self.particle.x = 0
self.particle.y = 40
self.particle.theta = 0
for _ in range(0, 10):
odometry[0] += 1
motion_model.update(self.particle, odometry, 0)
for _ in range(0, 10):
odometry[2] += math.pi/10
motion_model.update(self.particle, odometry, 0)
for _ in range(0, 10):
odometry[0] += 1
motion_model.update(self.particle, odometry, 0)
self.assertEqual(self.particle.x, 0)
self.assertEqual(self.particle.y, 40)
self.assertEqual(self.particle.theta, math.pi)
def test_circle(self):
motion_model = MotionModel(0, 0, 0, 0)
odometry = [0, 0, 0]
self.particle.x = 0
self.particle.y = 40
self.particle.theta = math.pi/2
for _ in range(0, 4):
odometry[0] += 10*math.sqrt(2)/2
odometry[1] -= 10*(1-math.sqrt(2)/2)
odometry[2] -= math.pi/4
motion_model.update(self.particle, odometry, 0)
#self.map.display([self.particle], title='Zero Variance Upper Circle')
self.assertAlmostEqual(self.particle.x, 20)
self.assertEqual(self.particle.y, 40)
self.assertEqual(self.particle.theta, -math.pi/2)
def test_distribution_line(self):
motion_model = MotionModel(0.1, 0.1, 0.1, 0.1)
odometry = [0, 0, 0]
p, = plt.plot([], [], 'r.')
for _ in range(0, 3):
odometry[0] += 1
x = list()
y = list()
for particle in self.particles:
motion_model.update(particle, odometry, 0)
x.append(particle.x)
y.append(particle.y)
self.map.display(self.particles, title='Line w/ motion model')
def test_distribution_circle(self):
motion_model = MotionModel(0.01, 0.01, 0.01, 0.01)
odometry = [0, 0, 0]
for particle in self.particles:
particle.x = 0
particle.y = 40
particle.theta = math.pi/2
for _ in range(0, 4):
odometry[0] += 10*math.sqrt(2)/2
odometry[1] -= 10*(1-math.sqrt(2)/2)
odometry[2] -= math.pi/4
for particle in self.particles:
motion_model.update(particle, odometry, 0)
self.map.display(self.particles, title='Upper Circle w/ Motion Model')