Create benchmark.py

examples/benchmark.py

@@ -0,0 +1,104 @@
+#!/usr/bin/env python
+
+'''
+benchmark.py : Benchmark breezyslam.algorithms.distanceScanToMap()  
+               with lots of random points
+                
+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/>.
+'''
+
+# Size of square mapping area
+MAP_SIZE_METERS = 40
+
+# Width of holes (obstacles, walls) in saved map
+HOLE_WIDTH_MM   = 600
+        
+from breezyslam.components import Laser, Map, Scan, Position
+from breezyslam.algorithms import distanceScanToMap
+
+from progressbar import ProgressBar
+
+from mines import URG04, load_data
+from pgm_utils import pgm_load
+
+from sys import argv, exit, stdout
+from time import time
+from random import Random
+
+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)))
+                     
+main()