From 938d1f2d80d3cc7ee114e5a5ffbe789e0ca547c8 Mon Sep 17 00:00:00 2001 From: "Simon D. Levy" Date: Sun, 7 Sep 2014 20:55:18 -0400 Subject: [PATCH] Create algorithms.cpp --- cpp/algorithms.cpp | 252 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 252 insertions(+) create mode 100644 cpp/algorithms.cpp diff --git a/cpp/algorithms.cpp b/cpp/algorithms.cpp new file mode 100644 index 0000000..911c67f --- /dev/null +++ b/cpp/algorithms.cpp @@ -0,0 +1,252 @@ +/* +algorithms.cpp - C++ CoreSLAM algorithms + +Copyright (C) 2013 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 "coreslam.h" +#include "random.h" + +#include "Position.hpp" +#include "Map.hpp" +#include "Laser.hpp" +#include "Scan.hpp" +#include "Velocities.hpp" +#include "WheeledRobot.hpp" + +#include "algorithms.hpp" + +// Local helpers ------------------------------------------------------------------------------------------------------- + +static void Position2position_t(Position & cpp_pos, struct position_t * c_pos) +{ + c_pos->x_mm = cpp_pos.x_mm; + c_pos->y_mm = cpp_pos.y_mm; + c_pos->theta_degrees = cpp_pos.theta_degrees; +} + +// CoreSLAM class ------------------------------------------------------------------------------------------------------- + +int CoreSLAM::distanceScanToMap( + Scan & scan, + Map & map, + Position & position) +{ + position_t pos_c; + Position2position_t(position, &pos_c); + + return distance_scan_to_map(map.map, scan.scan, pos_c); +} + + +CoreSLAM::CoreSLAM( + Laser & laser, + int map_size_pixels, + double map_size_meters) +{ + // Set default params + this->map_quality = DEFAULT_MAP_QUALITY; + this->hole_width_mm = DEFAULT_HOLE_WIDTH_MM; + + // Store laser for later + this->laser = new Laser(laser); + + // Initialize velocities (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry + this->velocities = new Velocities(); + + // Initialize a scan for computing distance to map, and one for updating map + this->scan_for_mapbuild = this->scan_create(3); + this->scan_for_distance = this->scan_create(1); + + // Initialize the map + this->map = new Map(map_size_pixels, map_size_meters); +} + +CoreSLAM::~CoreSLAM(void) +{ + delete this->map; + delete this->scan_for_distance; + delete this->scan_for_mapbuild; + delete this->velocities; +} + + +void CoreSLAM::update(int * scan_mm, Velocities & velocities) +{ + // Build a scan for computing distance to map, and one for updating map + this->scan_update(this->scan_for_mapbuild, scan_mm); + this->scan_update(this->scan_for_distance, scan_mm); + + // Update velocities + this->velocities->update(velocities.dxy_mm, + velocities.dtheta_degrees, + velocities.dt_seconds); + + // Implementing class updates map and pointcloud + this->updateMapAndPointcloud(velocities); +} + + +void CoreSLAM::getmap(unsigned char * mapbytes) +{ + this->map->get((char *)mapbytes); +} + +Scan * CoreSLAM::scan_create(int span) +{ + return new Scan(this->laser, span); +} + + +void +CoreSLAM::scan_update(Scan * scan, int * scan_mm) +{ + scan->update(scan_mm, this->hole_width_mm, *this->velocities); +} + +SinglePositionSLAM::SinglePositionSLAM(Laser & laser, int map_size_pixels, double map_size_meters) : +CoreSLAM(laser, map_size_pixels, map_size_meters) +{ + this->position = Position(this->init_coord_mm(), this->init_coord_mm(), 0); +} + + +// SinglePositionSLAM class ------------------------------------------------------------------------------------------- + +void SinglePositionSLAM::updateMapAndPointcloud(Velocities & velocities) +{ + // Start at current position + Position start_pos = this->position; + + // Add effect of velocities + start_pos.x_mm += velocities.dxy_mm * this->costheta(), + start_pos.y_mm += velocities.dxy_mm * this->sintheta(), + start_pos.theta_degrees += velocities.dtheta_degrees; + + // Add offset from laser + start_pos.x_mm += this->laser->laser->offset_mm * this->costheta(); + start_pos.y_mm += this->laser->laser->offset_mm * this->sintheta(); + + // Get new position from implementing class + Position new_position = this->getNewPosition(start_pos); + + // Update the map with this new position + this->map->update(*this->scan_for_mapbuild, new_position, this->map_quality, this->hole_width_mm); + + // Update the current position with this new position, adjusted by laser offset + this->position = Position(new_position); + this->position.x_mm -= this->laser->laser->offset_mm * this->costheta(); + this->position.y_mm -= this->laser->laser->offset_mm * this->sintheta(); +} + +Position & SinglePositionSLAM::getpos(void) +{ + return this->position; +} + +double SinglePositionSLAM::init_coord_mm(void) +{ + // Center of map + return 500 * this->map->map->size_meters; +} + + +double SinglePositionSLAM::theta_radians(void) +{ + return M_PI * this->position.theta_degrees / 180; +} + +double SinglePositionSLAM::costheta(void) +{ + return cos(this->theta_radians()); +} + +double SinglePositionSLAM::sintheta(void) +{ + return sin(this->theta_radians()); +} + + +// RMHC_SLAM class ------------------------------------------------------------------------------------------------------ + +RMHC_SLAM::RMHC_SLAM(Laser & laser, int map_size_pixels, double map_size_meters, unsigned random_seed) : +SinglePositionSLAM(laser, map_size_pixels, map_size_meters) +{ + this->sigma_xy_mm = DEFAULT_SIGMA_XY_MM; + this->sigma_theta_degrees = DEFAULT_SIGMA_THETA_DEGREES; + + this->max_search_iter = DEFAULT_MAX_SEARCH_ITER; + + this->randomizer = random_init(random_seed); +} + +RMHC_SLAM::~RMHC_SLAM(void) +{ + free(this->randomizer); +} + +void RMHC_SLAM::update(int * scan_mm) +{ + Velocities zero_velocities; + + CoreSLAM::update(scan_mm, zero_velocities); +} + +Position RMHC_SLAM::getNewPosition(Position & start_pos) +{ + // Search for a new position if indicated + Position likeliest_position = start_pos; + if (this->randomizer) + { + // Use C to find likeliest position + position_t start_pos_c; + Position2position_t(start_pos, &start_pos_c); + position_t c_likeliest_position = + rmhc_position_search( + start_pos_c, + this->map->map, + this->scan_for_distance->scan, + *this->laser->laser, + this->sigma_xy_mm, + this->sigma_theta_degrees, + this->max_search_iter, + this->randomizer); + + // Convert back to C++ object + likeliest_position = + Position( + c_likeliest_position.x_mm, + c_likeliest_position.y_mm, + c_likeliest_position.theta_degrees); + } + + return likeliest_position; +} + +// DeterministicSLAM class --------------------------------------------------------------------------------------------- + +Deterministic_SLAM::Deterministic_SLAM(Laser & laser, int map_size_pixels, double map_size_meters) : +SinglePositionSLAM(laser, map_size_pixels, map_size_meters) +{ +} + +Position Deterministic_SLAM::getNewPosition(Position & start_pos) +{ + return Position(start_pos.x_mm, start_pos.y_mm, start_pos.theta_degrees); +} + + +