Restored OpenCV version

examples/Makefile

@@ -33,6 +33,9 @@ all: log2pgm Log2PGM.class
 movie:
 	./logdemo.py exp1 1 9999
 
+cvmovie:
+	./logdemocv.py exp1 1 9999
+
 pytest: 
 	./log2pgm.py $(DATASET) $(USE_ODOMETRY) $(RANDOM_SEED)
 	$(VIEWER) $(DATASET).pgm ~/Desktop/$(DATASET).pgm

examples/logdemo.py

@@ -36,13 +36,14 @@ along with this code.  If not, see <http://www.gnu.org/licenses/>.
 
 # Map size, scale
 MAP_SIZE_PIXELS          = 800
-MAP_SIZE_METERS          =  32
+MAP_SIZE_METERS          = 32
 
 from breezyslam.algorithms import Deterministic_SLAM, RMHC_SLAM
 from mines import MinesLaser, Rover, load_data
 from pltslamshow import SlamShow
 
 from sys import argv, exit
+from time import time
 
 def main():
 	    
@@ -77,10 +78,11 @@ def main():
     # Set up a SLAM display
     display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
 
+    start_time = time()
+    
     # Loop over scans    
     for scanno in range(nscans):
    
-
         if use_odometry:
                   
             # Convert odometry to velocities
@@ -98,17 +100,20 @@ def main():
         x_mm, y_mm, theta_degrees = slam.getpos()    
 
         # Get new map
-        slam.getmap(mapbytes)
+        #slam.getmap(mapbytes)
 
         # Display map and robot pose
-        display.displayMap(mapbytes)
+        #display.displayMap(mapbytes)
 
-        display.setPose(x_mm, y_mm, theta_degrees)   
+        #display.setPose(x_mm, y_mm, theta_degrees)   
 
         # Exit gracefully if user closes display
         if not display.refresh():
             exit(0)
 
+        curr_time = time()
+        print(curr_time - start_time)
+        start_time = curr_time
  
         # XXX Add delay for real-time plot
     

examples/logdemocv.py

@@ -0,0 +1,123 @@
+#!/usr/bin/env python
+
+'''
+logdemo.py : BreezySLAM Python demo.  Reads logfile with odometry and scan data
+             from Paris Mines Tech and displays showing robot pose and map in 
+             real time.
+             
+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, 
+                   Robotics and Vision, ICARCV 2010, Singapore, 7-10 
+                   December 2010, Proceedings},
+      pages     = {1975-1979},
+      publisher = {IEEE},
+      year      = {2010}
+    }
+                 
+Copyright (C) 2016 Simon D. Levy and Matt Lubas
+
+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/>.
+'''
+
+# 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 cvslamshow import SlamShow
+
+from sys import argv, exit
+
+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)
+    
+    # Create a byte array to receive the computed maps
+    mapbytes = bytearray(MAP_SIZE_PIXELS * MAP_SIZE_PIXELS)
+
+    # Set up a SLAM display
+    display = SlamShow(MAP_SIZE_PIXELS, MAP_SIZE_METERS*1000/MAP_SIZE_PIXELS, 'SLAM')
+    
+    # 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()    
+
+        print(scanno, x_mm, y_mm, theta_degrees)
+
+        # Get current map    
+        slam.getmap(mapbytes)
+
+        # Display map and robot pose
+        display.displayMap(mapbytes)
+        display.displayRobot((x_mm, y_mm, theta_degrees))
+
+        # Exit gracefully if user closes display
+        key = display.refresh()
+        if key != None and (key&0x1A):
+            exit(0)
+ 
+        # XXX Add delay for real-time plot
+    
+            
+# Helpers ---------------------------------------------------------        
+
+def mm2pix(mm):
+        
+    return int(mm / (MAP_SIZE_METERS * 1000. / MAP_SIZE_PIXELS))  
+    
+                    
+main()

examples/pltslamshow.py

@@ -91,7 +91,7 @@ class SlamShow(object):
 
         mapimg = np.reshape(np.frombuffer(mapbytes, dtype=np.uint8), (self.map_size_pixels, self.map_size_pixels))
  
-        plt.imshow(mapimg)
+        #plt.imshow(mapimg)
         plt.set_cmap('gray')
 
         # Interleave the grayscale map bytes into the color bytes
@@ -135,7 +135,6 @@ class SlamShow(object):
 
         self.vehicle=self.ax.arrow(x_mm, y_mm, dx, dy, head_width=ROBOT_WIDTH_MM, head_length=ROBOT_HEIGHT_MM, fc='r', ec='r')
 
-
     def refresh(self):                   
 
         # If we have a new figure, something went wrong (closing figure failed)
@@ -143,16 +142,17 @@ class SlamShow(object):
             return False
 
         # Redraw current objects without blocking
-        plt.draw()
+        #plt.draw()
 
         # Refresh display, setting flag on window close or keyboard interrupt
         try:
-            plt.pause(.0001)
-
+            plt.pause(.01) # Arbitrary pause to force redraw
             return True
         except:
             return False
 
+        return True
+    
     # Converts millimeters to pixels
     def mm2pix(self, mm):
         return int(mm / float(self.map_scale_mm_per_pixel))