Create algorithms.cpp

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 <http://www.gnu.org/licenses/>.
+*/
+
+#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);
+}
+
+
+