def __init__(self, map_file, num, alpha, log_file_path, r_step, s_search):
print 'Intializing particle filter'
# TODO: load the map first
self.visuvalize = visuvalize.Visuvalize(map_file, r_step, s_search)
self.rotation_step = int(r_step)
# initialize set of particles
self.particles = []
# read logs and based on the data propogate or update
self.d = read_log_data.DataReader(log_file_path)
# number of particles
self.num = num
# alphas for motion model
self.alpha = alpha
# initial odom
self.initial_odom = np.array([0, 0, 0])
# check if the robot has moved to do resmapling
self.isMoved = False
self.start = True
self.laser_count = 0
self.z_test = []
# read distance table
self.z_required = np.load('distance_table.npy', mmap_mode='r')
# generate random particles
self.generate_particles = False
def __init__(self, map_file, num, alpha, log_file_path, r_step, s_search):
print 'Intializing particle filter'
# TODO: load the map first
self.visuvalize = visuvalize.Visuvalize(map_file, r_step, s_search)
self.rotation_step = int(r_step)
# initialize set of particles
self.particles = []
# read logs and based on the data propogate or update
self.d = read_log_data.DataReader(log_file_path)
# number of particles
self.num = num
# alphas for motion model
self.alpha = alpha
# initial odom
self.initial_odom = np.array([0, 0, 0])
# check if the robot has moved to do resmapling
self.isMoved = False
self.start = True
self.laser_count = 0
self.z_test = []
# read distance table
self.z_required = np.load('distance_table.npy', mmap_mode='r')
# generate random particles
self.generate_particles = False
# laser data
self.ll = []
#img count
self.img_count = 0
# for using vectorization
self.particles_weight = np.ones(self.num) * 1.0 / self.num
self.particles_pose = np.ndarray(shape=(self.num, 3), dtype=float)
# initialize the motion model and mesurement model
self.motion_model = motion_model.MotionModel(alpha, [0, 0, 0],
'normal')
# measeurement model
self.measurement_model = measurement_model.MeasurementModel(
self.visuvalize.global_map, self.visuvalize.distance_table,
self.rotation_step)
def __init__(self, map_file, num, alpha, log_file_path, r_step, s_search):
print 'Intializing particle filter'
# TODO: load the map first
self.visuvalize = visuvalize.Visuvalize(map_file, r_step, s_search)
self.rotation_step = int(r_step)
# initialize set of particles
self.particles = []
# read logs and based on the data propogate or update
self.d = read_log_data.DataReader(log_file_path)
# number of particles
self.num = num
# alphas for motion model
self.alpha = alpha
# initial odom
self.initial_odom = np.array([0, 0, 0])
# check if the robot has moved to do resmapling
self.isMoved = False
import ray_casting
import measurement_model
import cv2
import numpy
import visuvalize
map_file_path = "/home/baby/gatech_spring18/stat_techniques_robotics/lab/lab_particlefilter/data/map/wean.dat"
# log data path
log_path = "/home/baby/gatech_spring18/stat_techniques_robotics/lab/lab_particlefilter/data/log/robotdata1.log"
# distance table rotation step (1,2,3 etc) while creating distacle table
rotation_step = 10
step_search = 0.5
visuvalize = visuvalize.Visuvalize(map_file_path, rotation_step, step_search)
measurement_model = measurement_model.MeasurementModel(
visuvalize.global_map, visuvalize.distance_table, rotation_step)
while True:
print 'Give the pose to search'
x = float(raw_input("x: "))
y = float(raw_input("y: "))
th = float(raw_input("th: "))
index = measurement_model.convertPoseToIndex([x, y, th])
print 'index:', index
distance = measurement_model.table.queryTable(index[0], index[1], index[2])
x_new = x + distance * ma.cos(ma.radians(index[2] * 10))
y_new = y + distance * ma.sin(ma.radians(index[2] * 10))
index_new = measurement_model.convertPoseToIndex([x_new, y_new, th])