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])