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);
/**
* 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 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);
/**
* 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