def testParticleFilter():
print 'Testing the ParticleFilter class ...'
# Read map
map = Map()
map.readMap('../data/map/wean.dat')
# basic tests
filter = ParticleFilter()
filter.initParticles(map)
X = [Particle(0,0,0, 0.1), Particle(0,0,0, 0.1), Particle(0,0,0, 0.1)]
print 'X after resampling', filter._resample(X)
# test for update function
pos = filter._update((5.0, 6.0, 0.0), (1, 2, 0.0), (2, 2, 0.0))
print pos # this should print something close to (6, 6, 0)
'''
ind = weighted_choice([0.4, 0, 0])
if ind != 0:
print 'FAILED weighted_choice test.'
ind = weighted_choice([0.4, .6, 0.2])
print 'ind = ', ind
'''
return
def main():
parser = argparse.ArgumentParser(description='Laser unit test')
parser.add_argument('--map', type=str, default='../data/map/wean.dat', help='Ground truth occupancy map')
parser.add_argument('--log', type=str, default='../data/log/robotdata1.log', help='Robot data log file')
parser.add_argument('--lfield', type=str, default='./config/lfield_40.csv', help='Likelihood field')
parser.add_argument('--save', type=str, default='./test/', help='Which folder to save to.')
args = parser.parse_args()
# Read in map.
map = Map()
map.readMap(args.map)
# Number of data points.
num_odometry = 0;
num_laser = 0;
# Laser scan data.
z = []
# Read in log file.
with open(args.log) as log:
for line in log:
token = line.split()
if token[0] == 'O':
num_odometry += 1
elif token[0] == 'L':
num_laser += 1
z.append(np.array(token[1:]))
# n = len(z)
# z = [z[i] for i in range(0, n-1, 3)]
# Convert z into full numpy array.
z = np.array(z, dtype='float64')
# Load likelihood field.
L = LikelihoodField(0.01, 40, 8)
L.loadField(args.lfield)
# Run filter
filter = ParticleFilter()
# filter.setSensorModel(SensorModel(map, 0.7, 0.29, 0.01))
filter.setLikelihoodField(L)
filter.run(map, z, args.save)
def main():
parser = argparse.ArgumentParser(description='Laser unit test')
parser.add_argument('--map', type=str, default='./data/map/wean.dat', help='Ground truth occupancy map')
parser.add_argument('--log', type=str, default='./data/log/robotdata1.log', help='Robot data log file')
args = parser.parse_args()
# Read in map.
map = Map()
map.readMap(args.map)
# Number of data points.
num_odometry = 0;
num_laser = 0;
# Laser scan data.
z = []
# Odometry data.
x = []
# Time data.
t_z = []
t_x = []
# Read in log file.
with open(args.log) as log:
for line in log:
token = line.split()
if token[0] == 'O':
num_odometry += 1
x.append(np.array(token[1:4]))
t_x.append(token[4])
elif token[0] == 'L':
num_laser += 1
z.append(np.array(token[1:187]))
t_z.append(token[187])
# Convert data into full numpy arrays.
x = np.array(x, dtype='float64')
z = np.array(z, dtype='float64')
t_x = np.array(t_x, dtype='float64')
t_z = np.array(t_z, dtype='float64')
# Clip laser readings at 3000
z = np.clip(z, -3000, 3000)
# Plot the odometry trajectory.
pl.plot(x[:,0], x[:,1])
# pl.show()
pl.show(block=False)
print z.shape[0]
# Generate a list of x, y points to plot.
for i in range(z.shape[0]):
x_o = z[i,3]
y_o = z[i,4]
t_o = z[i,5]
# Build matrix of laser angles cosines and sines for projection.
t_s = np.linspace(-np.pi/2.0, np.pi/2.0, 180, True) # scan theta
cos_s = np.cos(t_o + t_s)
sin_s = np.sin(t_o + t_s)
# Get laser measurements.
z_t = z[i, 6:]
# Project measurements into world frame.
x_z = (x_o + np.multiply(z_t, cos_s))
y_z = (y_o + np.multiply(z_t, sin_s))
pl.cla()
pl.plot(x[:,0], x[:,1])
pl.plot(x_z, y_z, ',r')
pl.draw()
time.sleep(0.01)
pl.show()
