diff --git a/examples/Makefile b/examples/Makefile
index 36201b6..bdeda94 100644
--- a/examples/Makefile
+++ b/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
diff --git a/examples/logdemo.py b/examples/logdemo.py
index 4c980e9..79b0dc0 100755
--- a/examples/logdemo.py
+++ b/examples/logdemo.py
@@ -36,13 +36,14 @@ along with this code. If not, see .
# 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():
@@ -76,11 +77,12 @@ 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
diff --git a/examples/logdemocv.py b/examples/logdemocv.py
new file mode 100755
index 0000000..136171f
--- /dev/null
+++ b/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 .
+'''
+
+# 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 ' % 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()
diff --git a/examples/pltslamshow.py b/examples/pltslamshow.py
index 230c8d7..26625b1 100644
--- a/examples/pltslamshow.py
+++ b/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,15 +142,16 @@ 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):