def vis_collection(fn, save = True, save_dir = None):
collection = numpy.load(fn)['arr_0']
if save_dir is None:
save_dir = '{}_rendered/'.format(os.path.basename(fn))
if not os.path.isdir(save_dir):
os.mkdir(save_dir)
else:
raise RuntimeError("remove the old rendering")
map_file = 'data/map/wean.dat'
mo = map_parser.map_obj(map_file)
fig = plt.figure(num = 1, figsize = (20, 20))
pc = particle_filter.particle_collection(10, mo, fig_handle = fig)
for entry_idx in range(collection.shape[2]):
pose_coords = collection[:,:, entry_idx]
print "plotting entry ", entry_idx
pc.show(x = pose_coords[:, 0], y = pose_coords[:, 1])
plt.savefig('{}/entry_{}.png'.format(save_dir, entry_idx))
cmd = 'ffmpeg -start_number 0 -i {}/entry_%d.png -c:v libx264 -y {}/movie.avi'.format(save_dir,
save_dir)
os.system(cmd)
def vis_collection(fn, save=True, save_dir=None):
collection = numpy.load(fn)['arr_0']
if save_dir is None:
save_dir = '{}_rendered/'.format(os.path.basename(fn))
if not os.path.isdir(save_dir):
os.mkdir(save_dir)
else:
raise RuntimeError("remove the old rendering")
map_file = 'data/map/wean.dat'
mo = map_parser.map_obj(map_file)
fig = plt.figure(num=1, figsize=(20, 20))
pc = particle_filter.particle_collection(10, mo, fig_handle=fig)
for entry_idx in range(collection.shape[2]):
pose_coords = collection[:, :, entry_idx]
print "plotting entry ", entry_idx
pc.show(x=pose_coords[:, 0], y=pose_coords[:, 1])
plt.savefig('{}/entry_{}.png'.format(save_dir, entry_idx))
cmd = 'ffmpeg -start_number 0 -i {}/entry_%d.png -c:v libx264 -y {}/movie.avi'.format(
save_dir, save_dir)
os.system(cmd)
def main():
fig = plt.figure(num=1, figsize=(10, 10))
# sf1 = fig.add_subplot(2,1,1)
# sf2 = fig.add_subplot(2,1,2)
# thread_pool = Pool(16)
map_file = 'data/map/wean.dat'
mo = map_parser.map_obj(map_file)
logfile_fn = 'data/log/robotdata1.log'
import datetime
ts = str(datetime.datetime.now()).split()[1]
record_fn = 'xy_record_{}_{}.npz'.format(os.path.basename(logfile_fn), ts)
log = logparse.logparse(logfile_fn)
n_particles = 1000
#usually true
use_cpp_observation_model = True
use_cpp_motion_model = True
#usually false
observation_model_off = False
vis_motion_model = False
start_idx = 0 if not vis_motion_model else 60
do_both = False
display_period = 2
print "creating particle collection of {} particles".format(n_particles)
pc = particle_collection(n_particles=n_particles,
map_obj=mo,
nbr_theta=360,
fig_handle=fig)
print "created particle collection"
have_moved = True
first_obs_at_pos = True
num_new_motions = 0
num_new_observations = 0
odom_control_gen = motion_model.odometry_control_generator()
mm = motion_model.motion_model()
obs_model = obssensemodels.observation_model(
map_obj=mo, cpp_motion_model=mm.cpp_motion_model)
obs_view = obssensemodels.observation_view(
fig_handle=fig, map_obj=mo, cpp_map_obj=obs_model.cpp_map_obj)
#mo.show()
#print "showing pc"
pc.show()
pose = pc.particles[0].pose
# changing cpp params won't affect these...
# mo.vis_z_expected(pose)
# obs_model.vis_p_z_given_x_u(pose)
# pdb.set_trace()
#todo remove start idx
num_total_observations = 0
for (l_idx, line) in enumerate(log.lines[start_idx:]):
line = line.split()
print "line {} / {}".format(l_idx + 1, len(log.lines))
if isobservation(line):
num_total_observations += 1
num_new_motions += have_moved
pose = numpy.array([
np.float64(line[1]),
np.float64(line[2]),
np.float64(line[3])
])
# if l_idx>65:
# pdb.set_trace()
u, last_odom_theta = odom_control_gen.calculate_u_and_theta(pose)
u_norm = numpy.linalg.norm(u[:2])
have_moved = numpy.linalg.norm(u[:2]) > 1e-6
first_obs_at_pos = first_obs_at_pos or have_moved
u_arctan = numpy.arctan2(u[1], u[0])
print "computing motion model.."
print "use_cpp_motion_model: {}".format(use_cpp_motion_model)
print l_idx
# if have_moved:
# pdb.set_trace()
for p in pc.particles:
mm.update(p,
u,
u_norm,
u_arctan,
last_odom_theta,
use_cpp_motion_model=use_cpp_motion_model,
vis_motion_model=vis_motion_model)
#pass
if isobservation(line):
# combine 1.1. and 1.2 as P(Z |X) = func(map_obj, cur_pose)
laser_pose_offset = (np.float64(line[4]) - np.float64(line[1]),
np.float64(line[5]) - np.float64(line[2]),
np.float64(line[6]) - np.float64(line[3]))
offset_norm = numpy.linalg.norm(laser_pose_offset[:2])
offset_arctan = numpy.arctan2(laser_pose_offset[1],
laser_pose_offset[0])
faux_last_odom_theta = np.float64(line[3])
laser = []
laser_start = 7
n_in_wall = 0
for i in range(180):
laser.append(np.float64(line[i + laser_start]))
#pose = pc.particles[0].pose
#mo.vis_z_expected(pose)
#obs_view.vis_pose_and_laser(pose, laser)
#print (pose)
if first_obs_at_pos:
num_new_observations += first_obs_at_pos
first_obs_at_pos = False
# n_particles = len(pc.particles)
# obs_update_args = zip(pc.particles,
# [obs_model] * n_particles,
# [laser_pose_offset] * n_particles,
# [laser] * n_particles)
# pc.particles = thread_pool.map(obs_update, obs_update_args)
# IF not parallelizing
print "updating weights..."
# pdb.set_trace()
py_weights = []
if use_cpp_observation_model or do_both:
print "using cpp observation model "
poses = numpy.array(
[copy.deepcopy(p.pose) for p in pc.particles])
update_particle_weights_func = obs_model.cpp_observation_model.update_particle_weights
# pdb.set_trace()
weights = update_particle_weights_func(
poses,
numpy.array(laser_pose_offset, dtype=numpy.float64),
numpy.array([offset_norm, offset_arctan],
dtype=numpy.float64),
numpy.array(laser, dtype=numpy.float64),
faux_last_odom_theta)
if observation_model_off:
weights[weights != 0] = 1
# pdb.set_trace()
if (weights.shape != (len(pc.particles), )):
raise RuntimeError("cpp weights wrong dim!")
for (p_idx, p) in enumerate(pc.particles):
p.weight *= weights[p_idx]
if not use_cpp_observation_model or do_both:
print "using python version"
for p_idx, p in enumerate(pc.particles):
one_weight = obs_model.get_weight(
p.pose,
laser_pose_offset,
offset_norm,
offset_arctan,
faux_last_odom_theta=faux_last_odom_theta,
laser=laser)
py_weights.append(one_weight)
if observation_model_off:
p.weight = 1 if one_weight > 0 else 0
else:
p.weight *= one_weight
# py_weights = numpy.array(py_weights)
# pdb.set_trace()
new_weights = pc.get_weights()
print "max weight: {}".format(new_weights.max())
max_pose = pc.particles[np.argmax(new_weights)].pose.copy()
print "max weight location: {}".format(max_pose)
# pose_debug = np.array([ 3975, 4130, numpy.pi ])
# print "weight of {} is {} ".format( pose_debug,
# obs_model.get_weight(pose_debug,
# laser_pose_offset,
# offset_norm,
# offset_arctan,
# faux_last_odom_theta = faux_last_odom_theta,
# laser = laser))
obs_view.vis_pose_and_laser(max_pose, laser)
# pdb.set_trace()
#obs_view.vis_pose_and_laser(pose_debug, laser)
#max_pose_new = max_pose
#max_pose_new[2] -= numpy.pi/2
#pose_debug_new = pose_debug
#pose_debug_new[2] -= numpy.pi/2
#vw1 = obs_model.get_vec_point_wise_weight(max_pose_new, laser)
#vw2 = obs_model.get_vec_point_wise_weight(pose_debug_new, laser)
#pdb.set_trace()
# elif not ismotion(line):
# raise RuntimeError("unknown line type!!!11!!!1")
if (num_new_motions > 0) and (num_new_observations > 0):
num_new_motions = 0
num_new_observations = 0
print "resampling..."
try:
pc.resample()
except AssertionError:
vis_history.vis_collection(record_fn)
print "resampled"
#update stuff
if l_idx % display_period == 0:
print "updating display..."
pc.show()
print "updated"
if l_idx % 1 == 0:
numpy.savez_compressed(record_fn, pc.xy_record)
pc.last_scatter.remove()
vis_history.vis_collection(record_fn)
def main():
fig = plt.figure(num = 1, figsize = (10, 10))
# sf1 = fig.add_subplot(2,1,1)
# sf2 = fig.add_subplot(2,1,2)
# thread_pool = Pool(16)
map_file = 'data/map/wean.dat'
mo = map_parser.map_obj(map_file)
logfile_fn = 'data/log/robotdata1.log'
import datetime
ts = str(datetime.datetime.now()).split()[1]
record_fn = 'xy_record_{}_{}.npz'.format(os.path.basename(logfile_fn), ts)
log = logparse.logparse(logfile_fn)
n_particles = 1000
#usually true
use_cpp_observation_model = True
use_cpp_motion_model = True
#usually false
observation_model_off = False
vis_motion_model = False
start_idx = 0 if not vis_motion_model else 60
do_both = False
display_period = 2
print "creating particle collection of {} particles".format(n_particles)
pc = particle_collection(n_particles = n_particles,
map_obj = mo,
nbr_theta = 360,
fig_handle = fig)
print "created particle collection"
have_moved = True
first_obs_at_pos = True
num_new_motions = 0
num_new_observations = 0
odom_control_gen = motion_model.odometry_control_generator()
mm = motion_model.motion_model()
obs_model = obssensemodels.observation_model(map_obj = mo, cpp_motion_model = mm.cpp_motion_model)
obs_view = obssensemodels.observation_view(fig_handle = fig, map_obj = mo,
cpp_map_obj = obs_model.cpp_map_obj)
#mo.show()
#print "showing pc"
pc.show()
pose = pc.particles[0].pose
# changing cpp params won't affect these...
# mo.vis_z_expected(pose)
# obs_model.vis_p_z_given_x_u(pose)
# pdb.set_trace()
#todo remove start idx
num_total_observations = 0
for (l_idx, line) in enumerate(log.lines[start_idx:]):
line = line.split()
print "line {} / {}".format(l_idx + 1, len(log.lines))
if isobservation(line):
num_total_observations += 1
num_new_motions += have_moved
pose = numpy.array([np.float64(line[1]), np.float64(line[2]), np.float64(line[3])])
# if l_idx>65:
# pdb.set_trace()
u, last_odom_theta = odom_control_gen.calculate_u_and_theta(pose)
u_norm = numpy.linalg.norm(u[:2])
have_moved = numpy.linalg.norm(u[:2]) > 1e-6
first_obs_at_pos = first_obs_at_pos or have_moved
u_arctan = numpy.arctan2(u[1], u[0])
print "computing motion model.."
print "use_cpp_motion_model: {}".format(use_cpp_motion_model)
print l_idx
# if have_moved:
# pdb.set_trace()
for p in pc.particles:
mm.update(p, u, u_norm, u_arctan, last_odom_theta,
use_cpp_motion_model = use_cpp_motion_model,
vis_motion_model = vis_motion_model)
#pass
if isobservation(line):
# combine 1.1. and 1.2 as P(Z |X) = func(map_obj, cur_pose)
laser_pose_offset = (np.float64(line[4]) - np.float64(line[1]),
np.float64(line[5]) - np.float64(line[2]),
np.float64(line[6]) - np.float64(line[3]))
offset_norm = numpy.linalg.norm(laser_pose_offset[:2])
offset_arctan = numpy.arctan2(laser_pose_offset[1], laser_pose_offset[0])
faux_last_odom_theta = np.float64(line[3])
laser = [ ]
laser_start = 7
n_in_wall = 0
for i in range(180):
laser.append(np.float64(line[i + laser_start]))
#pose = pc.particles[0].pose
#mo.vis_z_expected(pose)
#obs_view.vis_pose_and_laser(pose, laser)
#print (pose)
if first_obs_at_pos:
num_new_observations += first_obs_at_pos
first_obs_at_pos = False
# n_particles = len(pc.particles)
# obs_update_args = zip(pc.particles,
# [obs_model] * n_particles,
# [laser_pose_offset] * n_particles,
# [laser] * n_particles)
# pc.particles = thread_pool.map(obs_update, obs_update_args)
# IF not parallelizing
print "updating weights..."
# pdb.set_trace()
py_weights = []
if use_cpp_observation_model or do_both:
print "using cpp observation model "
poses = numpy.array([copy.deepcopy(p.pose) for p in pc.particles])
update_particle_weights_func = obs_model.cpp_observation_model.update_particle_weights
# pdb.set_trace()
weights = update_particle_weights_func(poses,
numpy.array(laser_pose_offset,
dtype = numpy.float64),
numpy.array([offset_norm, offset_arctan],
dtype=numpy.float64),
numpy.array(laser, dtype = numpy.float64),
faux_last_odom_theta)
if observation_model_off:
weights[weights != 0] = 1
# pdb.set_trace()
if (weights.shape != (len(pc.particles),)):
raise RuntimeError("cpp weights wrong dim!")
for (p_idx, p) in enumerate(pc.particles):
p.weight *= weights[p_idx]
if not use_cpp_observation_model or do_both:
print "using python version"
for p_idx, p in enumerate(pc.particles):
one_weight = obs_model.get_weight(p.pose,
laser_pose_offset,
offset_norm,
offset_arctan,
faux_last_odom_theta = faux_last_odom_theta,
laser = laser)
py_weights.append(one_weight)
if observation_model_off:
p.weight = 1 if one_weight > 0 else 0
else:
p.weight *= one_weight
# py_weights = numpy.array(py_weights)
# pdb.set_trace()
new_weights = pc.get_weights()
print "max weight: {}".format(new_weights.max())
max_pose = pc.particles[np.argmax(new_weights)].pose.copy()
print "max weight location: {}".format( max_pose )
# pose_debug = np.array([ 3975, 4130, numpy.pi ])
# print "weight of {} is {} ".format( pose_debug,
# obs_model.get_weight(pose_debug,
# laser_pose_offset,
# offset_norm,
# offset_arctan,
# faux_last_odom_theta = faux_last_odom_theta,
# laser = laser))
obs_view.vis_pose_and_laser(max_pose, laser)
# pdb.set_trace()
#obs_view.vis_pose_and_laser(pose_debug, laser)
#max_pose_new = max_pose
#max_pose_new[2] -= numpy.pi/2
#pose_debug_new = pose_debug
#pose_debug_new[2] -= numpy.pi/2
#vw1 = obs_model.get_vec_point_wise_weight(max_pose_new, laser)
#vw2 = obs_model.get_vec_point_wise_weight(pose_debug_new, laser)
#pdb.set_trace()
# elif not ismotion(line):
# raise RuntimeError("unknown line type!!!11!!!1")
if (num_new_motions > 0) and (num_new_observations > 0):
num_new_motions = 0
num_new_observations = 0
print "resampling..."
try:
pc.resample()
except AssertionError:
vis_history.vis_collection(record_fn)
print "resampled"
#update stuff
if l_idx % display_period == 0:
print "updating display..."
pc.show()
print "updated"
if l_idx % 1 == 0:
numpy.savez_compressed(record_fn, pc.xy_record)
pc.last_scatter.remove()
vis_history.vis_collection(record_fn)