From d107617dd9cfa0f25b40baf7e3ebb48819c82a6c Mon Sep 17 00:00:00 2001 From: "Simon D. Levy" Date: Sun, 7 Sep 2014 20:55:39 -0400 Subject: [PATCH] Create algorithms.hpp --- cpp/algorithms.hpp | 303 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 303 insertions(+) create mode 100644 cpp/algorithms.hpp diff --git a/cpp/algorithms.hpp b/cpp/algorithms.hpp new file mode 100644 index 0000000..6428707 --- /dev/null +++ b/cpp/algorithms.hpp @@ -0,0 +1,303 @@ +/** +* +* algorithms.hpp - C++ header for 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 . +*/ + +#include +#include + +#include +#include +using namespace std; + +class Position; +class Map; +class Scan; +class Laser; + +/** +* 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 +* +* void updateMapAndPointcloud(int * scan_mm, Velocities & velocities) +* +* to update the map and point-cloud (particle cloud). +* +*/ +class CoreSLAM +{ + +public: + + /** + * Computes distance between a scan and map, given hypothetical position, to support particle filtering. + * @param scan the scan + * @param map the map + * @param position the position + * @return distance in arbitrary units, or -1 for infinity + */ + static int distanceScanToMap(Scan & scan, Map & map, Position & position); + + /** + * Retrieves the current map. + * @param mapbytes a byte array big enough to hold the map (map_size_pixels * map_size_pixels) + */ + void getmap(unsigned char * mapbytes); + + /** + * Updates the scan and odometry, and calls the the implementing class's updateMapAndPointcloud method with + * the specified velocities. + * + * @param scan_mm Lidar scan values, whose count is specified in the scan_size + * attribute of the Laser object passed to the CoreSlam constructor + * @param velocities velocities for odometry + */ + void update(int * scan_mm, Velocities & velocities); + + + /** + * The quality of the map (0 through 255); default = 50 + */ + int map_quality; + + /** + * The width in millimeters of each "hole" in the map (essentially, wall width); + * default = 600 + */ + double hole_width_mm; + +protected: + + /** + * Creates a CoreSLAM object. + * @param laser a Laser object containing parameters for your Lidar equipment + * @param map_size_pixels the size of the desired map (map is square) + * @param map_size_meters the size of the area to be mapped, in meters + * @return a new CoreSLAM object + */ + CoreSLAM(Laser & laser, int map_size_pixels, double map_size_meters); + + /** + * Deallocates this CoreSLAM object. + */ + ~CoreSLAM(void); + + /** + * A pointer to the current map + */ + Map * map; + + /** + * A pointer to the laser model + */ + Laser * laser; + + /** + * A scan for building the map + */ + Scan * scan_for_mapbuild; + + /** + * A scan for use in distanceScanToMap() + */ + Scan * scan_for_distance; + + /** + * The current velocities from odometry + */ + class Velocities * velocities; + + /** + * Updates the map and point-cloud (particle cloud). Called automatically by CoreSLAM::update() + * @param velocities velocities for odometry + */ + virtual void updateMapAndPointcloud(Velocities & velocities) = 0; + +private: + + Scan * scan_create(int span); + + void scan_update(Scan * scan, int * scan_mm); + +}; // CoreSLAM + + +/** +* SinglePositionSLAM is an abstract class that implements CoreSLAM using a point-cloud +* with a single point (particle, position). Implementing classes should provide the method +* +* Position & getNewPosition(Position & start_position) +* +* to compute a new position based on searching from a starting position. +*/ +class SinglePositionSLAM : public CoreSLAM +{ + +public: + + /** + * Returns the current position. + * @return the current position as a Position object. + */ + Position & getpos(void); + +protected: + + /** + * Creates a SinglePositionSLAM object. + * @param laser a Laser object containing parameters for your Lidar equipment + * @param map_size_pixels the size of the desired map (map is square) + * @param map_size_meters the size of the area to be mapped, in meters + * @return a new SinglePositionSLAM object + */ + SinglePositionSLAM(Laser & laser, int map_size_pixels, double map_size_meters); + + + /** + * Updates the map and point-cloud (particle cloud). Called automatically by CoreSLAM::update() + * @param velocities velocities for odometry + */ + void updateMapAndPointcloud(Velocities & velocities); + + /** + * Returns a new position based on searching from a starting position. Called automatically by + * SinglePositionSLAM::updateMapAndPointcloud() + * @param start_pos the starting position + */ + virtual Position getNewPosition(Position & start_pos) = 0; + + +private: + + Position position; + + double init_coord_mm(void); + + double theta_radians(void); + + double costheta(void); + + double sintheta(void); + +}; + +/** +* RMHC_SLAM implements SinglePositionSLAM using random-mutation hill-climbing search on the starting position. +*/ +class RMHC_SLAM : public SinglePositionSLAM +{ + +public: + + /** + * Creates an RMHC_SLAM object. + * @param laser a Laser object containing parameters for your Lidar equipment + * @param map_size_pixels the size of the desired map (map is square) + * @param map_size_meters the size of the area to be mapped, in meters + * @param random_seed seed for psuedorandom number generator in particle filter + * @return a new CoreSLAM object + */ + RMHC_SLAM(Laser & laser, + int map_size_pixels, + double map_size_meters, + unsigned random_seed); + + ~RMHC_SLAM(void); + + /** + * Updates the scan, and calls the the implementing class's updateMapAndPointcloud method with zero velocities + * (no odometry). + * @param scan_mm Lidar scan values, whose count is specified in the scan_size + * attribute of the Laser object passed to the CoreSlam constructor + */ + void update(int * scan_mm); + + /** + * The standard deviation in millimeters of the Gaussian distribution of + * the (X,Y) component of position in the particle filter; default = 100 + */ + double sigma_xy_mm; + + /** + * The standard deviation in degrees of the Gaussian distribution of + * the angular rotation component of position in the particle filter; default = 20 + */ + double sigma_theta_degrees; + + /** + * The maximum number of iterations for particle-filter search; default = 1000 + */ + int max_search_iter; + +protected: + + /** + * Returns a new position based on RMHC search from a starting position. Called automatically by + * SinglePositionSLAM::updateMapAndPointcloud() + * @param start_position the starting position + */ + Position getNewPosition(Position & start_position) ; + +private: + + // Pseudorandom-number generator + void * randomizer; + +}; // RMHC_SLAM + +/** +* Deterministic_SLAM implements SinglePositionSLAM using by returning the starting position instead of searching +* on it; i.e., using odometry alone. +*/ +class Deterministic_SLAM : public SinglePositionSLAM +{ + +public: + + /** + * Creates a Deterministic_SLAM object. + * @param laser a Laser object containing parameters for your Lidar equipment + * @param map_size_pixels the size of the desired map (map is square) + * @param map_size_meters the size of the area to be mapped, in meters + * @return a new CoreSLAM object + */ + Deterministic_SLAM(Laser & laser, int map_size_pixels, double map_size_meters); + +protected: + + /** + * Returns a new position identical to the starting position. Called automatically by + * SinglePositionSLAM::updateMapAndPointcloud() + * @param start_pos the starting position + */ + Position getNewPosition(Position & start_position) ; + +}; // Deterministic_SLAM