From aa7500d5b716f2076b24161849676c0b904cff17 Mon Sep 17 00:00:00 2001 From: "Simon D. Levy" Date: Sun, 7 Sep 2014 21:12:03 -0400 Subject: [PATCH] Create algorithms.py --- python/breezyslam/algorithms.py | 306 ++++++++++++++++++++++++++++++++ 1 file changed, 306 insertions(+) create mode 100644 python/breezyslam/algorithms.py diff --git a/python/breezyslam/algorithms.py b/python/breezyslam/algorithms.py new file mode 100644 index 0000000..3d14a93 --- /dev/null +++ b/python/breezyslam/algorithms.py @@ -0,0 +1,306 @@ +''' +BreezySLAM: Simple, efficient SLAM in Python + +algorithms.py: SLAM algorithms + +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 . +''' + +# distanceScanToMap is implemented as a C extension for efficiency +from pybreezyslam import distanceScanToMap + +import pybreezyslam + +import math +import time + +# Basic params +_DEFAULT_MAP_QUALITY = 50 # out of 255 +_DEFAULT_HOLE_WIDTH_MM = 600 + +# Random mutation hill-climbing (RMHC) params +_DEFAULT_SIGMA_XY_MM = 100 +_DEFAULT_SIGMA_THETA_DEGREES = 20 +_DEFAULT_MAX_SEARCH_ITER = 1000 + +# CoreSLAM class ------------------------------------------------------------------------------------------------------ + +class CoreSLAM(object): + ''' + CoreSLAM is an abstract class that uses the classes Position, Map, Scan, and Laser + to run variants of the simple CoreSLAM (tinySLAM) algorithm described in + + @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} + } + + + Implementing classes should provide the method + + _updateMapAndPointcloud(scan_mm, velocities) + + to update the map and point-cloud (particle cloud). + ''' + + def __init__(self, laser, map_size_pixels, map_size_meters, + map_quality=_DEFAULT_MAP_QUALITY, hole_width_mm=_DEFAULT_HOLE_WIDTH_MM): + ''' + Creates a CoreSLAM object suitable for updating with new Lidar and odometry data. + laser is a Laser object representing the specifications of your Lidar unit + map_size_pixels is the size of the square map in pixels + map_size_meters is the size of the square map in meters + quality from 0 through 255 determines integration speed of scan into map + hole_width_mm determines width of obstacles (walls) + ''' + + # Initialize parameters + self.map_quality = map_quality + self.hole_width_mm = hole_width_mm + + # Store laser for later + self.laser = laser + + # Initialize velocities (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry + self.velocities = (0, 0, 0) + + # Initialize a scan for computing distance to map, and one for updating map + self.scan_for_distance = pybreezyslam.Scan(laser, 1) + self.scan_for_mapbuild = pybreezyslam.Scan(laser, 3) + + # Initialize the map + self.map = pybreezyslam.Map(map_size_pixels, map_size_meters) + + def update(self, scans_mm, velocities): + ''' + Updates the scan and odometry, and calls the the implementing class's _updateMapAndPointcloud method with + the specified velocities. + + scan_mm is a list of Lidar scan values, whose count is specified in the scan_size + attribute of the Laser object passed to the CoreSlam constructor + velocities is a tuple of velocities (dxy_mm, dtheta_degrees, dt_seconds) for odometry + ''' + + # Build a scan for computing distance to map, and one for updating map + self._scan_update(self.scan_for_mapbuild, scans_mm) + self._scan_update(self.scan_for_distance, scans_mm) + + # Update velocities + velocity_factor = (1 / velocities[2]) if (velocities[2] > 0) else 0 + new_dxy_mm = velocities[0] * velocity_factor + new_dtheta_degrees = velocities[1] * velocity_factor + self.velocities = (new_dxy_mm, new_dtheta_degrees, 0) + + # Implementing class updates map and pointcloud + self._updateMapAndPointcloud(velocities) + + def getmap(self, mapbytes): + ''' + Fills bytearray mapbytes with map pixels, where bytearray length is square of map size passed + to CoreSLAM.__init__(). + ''' + self.map.get(mapbytes) + + + def __str__(self): + + return 'CoreSLAM: %s \n map quality = %d / 255 \n hole width = %7.0f mm' % \ + (str(self.map), self.map_quality, self.hole_width_mm) + + def __repr__(self): + + return self.__str__() + + + def _scan_update(self, scan, lidar): + + scan.update(scans_mm=lidar, hole_width_mm=self.hole_width_mm, velocities=self.velocities) + + +# SinglePositionSLAM class --------------------------------------------------------------------------------------------- + +class SinglePositionSLAM(CoreSLAM): + ''' + SinglePositionSLAM is an abstract class that implements CoreSLAM using a point-cloud + with a single point (position). Implementing classes should provide the method + + _getNewPosition(self, start_position) + + to compute a new position based on searching from a starting position. + ''' + + def __init__(self, laser, map_size_pixels, map_size_meters, + map_quality=_DEFAULT_MAP_QUALITY, hole_width_mm=_DEFAULT_HOLE_WIDTH_MM): + + CoreSLAM.__init__(self, laser, map_size_pixels, map_size_meters, + map_quality, hole_width_mm) + + # Initialize the position (x, y, theta) + init_coord_mm = 500 * map_size_meters # center of map + self.position = pybreezyslam.Position(init_coord_mm, init_coord_mm, 0) + + def _updateMapAndPointcloud(self, velocities): + ''' + Updates the map and point-cloud (particle cloud). Called automatically by CoreSLAM.update() + velocities is a tuple of the form (dxy_mm, dtheta_degrees, dt_seconds). + ''' + + # Start at current position + start_pos = self.position.copy() + + # Add effect of velocities + start_pos.x_mm += velocities[0] * self._costheta() + start_pos.y_mm += velocities[0] * self._sintheta() + start_pos.theta_degrees = start_pos.theta_degrees + velocities[1] + + # Add offset from laser + start_pos.x_mm += self.laser.offset_mm * self._costheta() + start_pos.y_mm += self.laser.offset_mm * self._sintheta() + + # Get new position from implementing class + new_position = self._getNewPosition(start_pos) + + # Update the map with this new position + self.map.update(self.scan_for_mapbuild, new_position, self.map_quality, self.hole_width_mm) + + # Update the current position with this new position, adjusted by laser offset + self.position = new_position.copy() + self.position.x_mm -= self.laser.offset_mm * self._costheta() + self.position.y_mm -= self.laser.offset_mm * self._sintheta() + + def getpos(self): + ''' + Returns current position as a tuple (x_mm, y_mm, theta_degrees) + ''' + return (self.position.x_mm, self.position.y_mm, self.position.theta_degrees) + + + def _costheta(self): + + return math.cos(self._thetaradians()) + + def _sintheta(self): + + return math.sin(self._thetaradians()) + + def _thetaradians(self): + + return math.radians(self.position.theta_degrees) + +# RMHC_SLAM class ------------------------------------------------------------------------------------------------------ + +class RMHC_SLAM(SinglePositionSLAM): + ''' + RMHC_SLAM implements the _getNewPosition() method of SinglePositionSLAM using Random-Mutation Hill-Climbing + search. Uses its own internal pseudorandom-number generator for efficiency. + ''' + + def __init__(self, laser, map_size_pixels, map_size_meters, + map_quality=_DEFAULT_MAP_QUALITY, hole_width_mm=_DEFAULT_HOLE_WIDTH_MM, + random_seed=None, sigma_xy_mm=_DEFAULT_SIGMA_XY_MM, sigma_theta_degrees=_DEFAULT_SIGMA_THETA_DEGREES, + max_search_iter=_DEFAULT_MAX_SEARCH_ITER): + ''' + Creates a RMHCSlam object suitable for updating with new Lidar and odometry data. + laser is a Laser object representing the specifications of your Lidar unit + map_size_pixels is the size of the square map in pixels + map_size_meters is the size of the square map in meters + quality from 0 through 255 determines integration speed of scan into map + hole_width_mm determines width of obstacles (walls) + random_seed supports reproducible results; defaults to system time if unspecified + sigma_xy_mm specifies the standard deviation in millimeters of the normal distribution of + the (X,Y) component of position for RMHC search + sigma_theta_degrees specifies the standard deviation in degrees of the normal distribution of + the rotational component of position for RMHC search + max_search_iter specifies the maximum number of iterations for RMHC search + ''' + + SinglePositionSLAM.__init__(self, laser, map_size_pixels, map_size_meters, + map_quality, hole_width_mm) + + if not random_seed: + random_seed = int(time.time()) & 0xFFFF + + self.randomizer = pybreezyslam.Randomizer(random_seed) + + self.sigma_xy_mm = sigma_xy_mm + self.sigma_theta_degrees = sigma_theta_degrees + self.max_search_iter = max_search_iter + + def update(self, scan_mm, velocities=None): + + if not velocities: + + velocities = (0, 0, 0) + + CoreSLAM.update(self, scan_mm, velocities) + + def _getNewPosition(self, start_position): + ''' + Implements the _getNewPosition() method of SinglePositionSLAM. Uses Random-Mutation Hill-Climbing + search to look for a better position based on a starting position. + ''' + + # RMHC search is implemented as a C extension for efficiency + return pybreezyslam.rmhcPositionSearch( + start_position, + self.map, + self.scan_for_distance, + self.laser, + self.sigma_xy_mm, + self.sigma_theta_degrees, + self.max_search_iter, + self.randomizer) + + def _random_normal(self, mu, sigma): + + return mu + self.randomizer.rnor() * sigma + + # Deterministic_SLAM class ------------------------------------------------------------------------------------ + +class Deterministic_SLAM(SinglePositionSLAM): + ''' + Deterministic_SLAM implements the _getNewPosition() method of SinglePositionSLAM by simply + copying the search-start position. + ''' + + def __init__(self, laser, map_size_pixels, map_size_meters, + map_quality=_DEFAULT_MAP_QUALITY, hole_width_mm=_DEFAULT_HOLE_WIDTH_MM): + ''' + Creates a Deterministic_Slam object suitable for updating with new Lidar and odometry data. + laser is a Laser object representing the specifications of your Lidar unit + map_size_pixels is the size of the square map in pixels + map_size_meters is the size of the square map in meters + quality from 0 through 255 determines integration speed of scan into map + hole_width_mm determines width of obstacles (walls) + ''' + + SinglePositionSLAM.__init__(self, laser, map_size_pixels, map_size_meters, + map_quality, hole_width_mm) + + def _getNewPosition(self, start_position): + ''' + Implements the _getNewPosition() method of SinglePositionSLAM. Returns a copy of the starting position. + ''' + + return start_position.copy() + +