def main():
# Bozo filter for input args
if len(argv) < 4:
print('Usage: %s <dataset> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0
# Allocate byte array to receive map updates
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Load the data from the file
timestamps, lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display, named by dataset
display = SlamShow(MAP_SIZE_PIXELS,
MAP_SIZE_METERS * 1000 / MAP_SIZE_PIXELS, dataset)
# Pose will be modified in our threaded code
pose = [0, 0, 0]
# Launch the data-collection / update thread
thread = Thread(target=threadfunc,
args=(robot, slam, timestamps, lidars,
odometries if use_odometry else None, mapbytes,
pose))
thread.daemon = True
thread.start()
# Loop forever,displaying current map and pose
while True:
# Display map and robot pose
display.displayMap(mapbytes)
display.setPose(*pose)
# Refresh the display, exiting gracefully if user closes it
if not display.refresh():
exit(0)
def main():
# Bozo filter for input args
if len(argv) < 4:
print('Usage: %s <dataset> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0
# Allocate byte array to receive map updates
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Load the data from the file
timestamps, lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Set up a SLAM display, named by dataset
viz = MapVisualizer(MAP_SIZE_PIXELS, MAP_SIZE_METERS, dataset)
# Pose will be modified in our threaded code
pose = [0,0,0]
# Launch the data-collection / update thread
thread = Thread(target=threadfunc, args=(robot, slam, timestamps, lidars, odometries if use_odometry else None, mapbytes, pose))
thread.daemon = True
thread.start()
# Loop forever,displaying current map and pose
while True:
# Display map and robot pose, exiting gracefully if user closes it
if not viz.display(pose[0]/1000., pose[1]/1000., pose[2], mapbytes):
exit(0)
def main():
# Bozo filter for input args
if len(argv) < 3:
print('Usage: %s <dataset> <use_odometry> [random_seed]' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0
# Load the data from the file, ignoring timestamps
_, lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Report what we're doing
nscans = len(lidars)
print('Processing %d scans with%s odometry / with%s particle filter...' % \
(nscans, \
'' if use_odometry else 'out', '' if seed else 'out'))
progbar = ProgressBar(0, nscans, 80)
# Start with an empty trajectory of positions
trajectory = []
# Start timing
start_sec = time()
# Loop over scans
for scanno in range(nscans):
if use_odometry:
# Convert odometry to velocities
velocities = robot.computePoseChange(odometries[scanno])
# Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocities)
else:
# Update SLAM with lidar alone
slam.update(lidars[scanno])
# Get new position
x_mm, y_mm, theta_degrees = slam.getpos()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
# Tame impatience
progbar.updateAmount(scanno)
stdout.write('\r%s' % str(progbar))
stdout.flush()
# Report elapsed time
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' %
(nscans, elapsed_sec, nscans / elapsed_sec))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get final map
slam.getmap(mapbytes)
# Pickle the map to a file
pklname = dataset + '.map'
print('Writing map to file ' + pklname)
pickle.dump(mapbytes, open(pklname, 'wb'))
def main():
# Bozo filter for input args
if len(argv) < 3:
print('Usage: %s <dataset> <use_odometry> <random_seed>' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0
# Load the data from the file
lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(URG04(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Report what we're doing
nscans = len(lidars)
print('Processing %d scans with%s odometry / with%s particle filter...' % \
(nscans, \
'' if use_odometry else 'out', '' if seed else 'out'))
progbar = ProgressBar(0, nscans, 80)
# Start with an empty trajectory of positions
trajectory = []
# Start timing
start_sec = time()
# Loop over scans
for scanno in range(nscans):
if use_odometry:
# Convert odometry to velocities
velocities = robot.computeVelocities(odometries[scanno])
# Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocities)
else:
# Update SLAM with lidar alone
slam.update(lidars[scanno])
# Get new position
x_mm, y_mm, theta_degrees = slam.getpos()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
# Tame impatience
progbar.updateAmount(scanno)
stdout.write('\r%s' % str(progbar))
stdout.flush()
# Report elapsed time
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' % (nscans, elapsed_sec, nscans/elapsed_sec))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get final map
slam.getmap(mapbytes)
# Put trajectory into map as black pixels
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0;
# Save map and trajectory as PGM file
pgm_save('%s.pgm' % dataset, mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))
def main():
# Bozo filter for input args
if len(argv) < 3:
print('Usage: %s <dataset> <npoints>' % argv[0])
print('Example: %s <exp2> 100000' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
npoints = int(argv[2])
# Load the Lidar scan data from the log file
lidarscans, _ = load_data('.', dataset)
# Load the map from the PGM file
mapbytes, mapsize_pixels = pgm_load('%s.pgm' % dataset)
# Map is square
mapsize_pixels = mapsize_pixels[0]
# Create a Map object from the bytes; call it mymap to avoid name collision with Python's built-in map function
mymap = Map(mapsize_pixels, MAP_SIZE_METERS, mapbytes)
# Create an empty Scan object with URG04_360 laser parameters
scan = Scan(URG04())
# Create a progresss bar to report what we're doing
nscans = len(lidarscans)
print('Processing %d points for %d scans...' % (npoints, nscans))
progbar = ProgressBar(0, nscans, 80)
# Start timing
start_sec = time()
# Precompute map size in mm
mapsize_mm = mapsize_pixels * MAP_SIZE_METERS
# Create a random-number generator for points
rand = Random()
# Loop over scans
for scanno in range(nscans):
# Update the Scan object with the Lidar scan values
scan.update(lidarscans[scanno], HOLE_WIDTH_MM)
# Test a lot of points on this new scan and the pre-loaded map
for pointno in range(npoints):
point = Position(rand.uniform(0,mapsize_mm), rand.uniform(0,mapsize_mm), rand.uniform(-180,180))
distanceScanToMap(mymap, scan, point)
# Tame impatience
progbar.updateAmount(scanno)
stdout.write('\r%s' % str(progbar))
stdout.flush()
# Report results
elapsed_sec = time() - start_sec
totalpoints = nscans * npoints
print('\nProcessed %d points in %f seconds = %d points / sec' % \
(totalpoints, elapsed_sec, int(totalpoints / elapsed_sec)))
def main():
# Bozo filter for input args
if len(argv) < 3:
print('Usage: %s <dataset> <use_odometry> <random_seed>' % argv[0])
print('Example: %s exp2 1 9999' % argv[0])
exit(1)
# Grab input args
dataset = argv[1]
use_odometry = True if int(argv[2]) else False
seed = int(argv[3]) if len(argv) > 3 else 0
# Load the data from the file
lidars, odometries = load_data('.', dataset)
# Build a robot model if we want odometry
robot = Rover() if use_odometry else None
# Create a CoreSLAM object with laser params and optional robot object
slam = RMHC_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed) \
if seed \
else Deterministic_SLAM(MinesLaser(), MAP_SIZE_PIXELS, MAP_SIZE_METERS)
# Report what we're doing
nscans = len(lidars)
print('Processing %d scans with%s odometry / with%s particle filter...' % \
(nscans, \
'' if use_odometry else 'out', '' if seed else 'out'))
progbar = ProgressBar(0, nscans, 80)
# Start with an empty trajectory of positions
trajectory = []
# Start timing
start_sec = time()
# Loop over scans
for scanno in range(nscans):
if use_odometry:
# Convert odometry to velocities
velocities = robot.computeVelocities(odometries[scanno])
# Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocities)
else:
# Update SLAM with lidar alone
slam.update(lidars[scanno])
# Get new position
x_mm, y_mm, theta_degrees = slam.getpos()
# Add new position to trajectory
trajectory.append((x_mm, y_mm))
# Tame impatience
progbar.updateAmount(scanno)
stdout.write('\r%s' % str(progbar))
stdout.flush()
# Report elapsed time
elapsed_sec = time() - start_sec
print('\n%d scans in %f sec = %f scans / sec' %
(nscans, elapsed_sec, nscans / elapsed_sec))
# Create a byte array to receive the computed maps
mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
# Get final map
slam.getmap(mapbytes)
# Put trajectory into map as black pixels
for coords in trajectory:
x_mm, y_mm = coords
x_pix = mm2pix(x_mm)
y_pix = mm2pix(y_mm)
mapbytes[y_pix * MAP_SIZE_PIXELS + x_pix] = 0
# Save map and trajectory as PNG file
image = Image.frombuffer('L', (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS), mapbytes,
'raw', 'L', 0, 1)
image.save('%s.png' % dataset)
