教程三：breezyslam建图log2pgm

如果想要slam建图，而不用ros 系统的画，可以考虑使用这个breezyslam 是基于coreslam tinyslam实现的算法，

依托于激光雷达数据和里程计数据。

这个log2pgm.py的脚本是将exp.dat数据 转化为pgm格式的map。

log2pgm.py 0 9999

是不基于里程计实现的建图。接下来的研究我们将breezy slam 移植到树莓派3上，然后和rplidar ，里程发布计传感器

等结和起来，测试树莓派的slam建图效果。并持续研究exp.dat的数据的生成。

最下边的图是exp2.dat 转换后的pgm地图。

#!/usr/bin/env python3

'''
log2pgm.py : BreezySLAM Python demo.  Reads logfile with odometry and scan data
             from Paris Mines Tech and produces a .PGM image file showing robot 
             trajectory and final map.

For details see

    @inproceedings{coreslam-2010,
      author    = {Bruno Steux and Oussama El Hamzaoui},
      title     = {CoreSLAM: a SLAM Algorithm in less than 200 lines of C code},
      booktitle = {11th International Conference on Control, Automation, 
                   Vehicleics and Vision, ICARCV 2010, Singapore, 7-10 
                   December 2010, Proceedings},
      pages     = {1975-1979},
      publisher = {IEEE},
      year      = {2010}
    }

Copyright (C) 2014 Simon D. Levy

This code is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as 
published by the Free Software Foundation, either version 3 of the 
License, or (at your option) any later version.

This code is distributed in the hope that it will be useful,     
but WITHOUT ANY WARRANTY without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU Lesser General Public License 
along with this code.  If not, see <http://www.gnu.org/licenses/>.

Change log:

20-APR-2014 - Simon D. Levy - Get params from command line
05-JUN-2014 - SDL - get random seed from command line
'''

# Map size, scale
MAP_SIZE_PIXELS          = 800
MAP_SIZE_METERS          =  32

from breezyslam.algorithms import Deterministic_SLAM, RMHC_SLAM

from mines import MinesLaser, Rover, load_data
from progressbar import ProgressBar
from pgm_utils import pgm_save

from sys import argv, exit, stdout
from time import time

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 pose change (dxy_mm, dtheta_degrees, dt_seconds)
            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)

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

# Helpers ---------------------------------------------------------        

def mm2pix(mm):

    return int(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS))  

main()