Merge pull request iris-ua#5 from UbiquityRobotics/feature/set_map

[slam2d] Add an interface to set a new occupancy map.

include/lama/slam2d.h

@@ -166,6 +166,11 @@ class Slam2D {
     inline void resumeMapping()
     { do_mapping_ = true; }
 
+    // Switch to a new occupancy map.
+    // The current maps (occupancy and distance) will be deleted.
+    // We also assume ownership of the map object.
+    bool setOccupancyMap(FrequencyOccupancyMap* map);
+
 private:
 
     StrategyPtr makeStrategy(const std::string& name, const VectorXd& parameters);

src/slam2d.cpp

@@ -222,6 +222,30 @@ void lama::Slam2D::useCompression(bool compression, const std::string& algorithm
     occupancy_map_->useCompression(compression, 60, algorithm);
 }
 
+bool lama::Slam2D::setOccupancyMap(FrequencyOccupancyMap* map)
+{
+    if (map == nullptr)
+        return false;
+
+    double l2_max = distance_map_->maxDistance();
+
+    delete occupancy_map_;
+    delete distance_map_;
+
+    occupancy_map_ = map;
+    distance_map_ = new DynamicDistanceMap(map->resolution, map->patch_length);
+    distance_map_->setMaxDistance(l2_max);
+
+    map->visit_all_cells([this, &map](auto& coords){
+        if (map->isOccupied(coords))
+            this->distance_map_->addObstacle(coords);
+    });
+
+    distance_map_->update();
+    return true;
+}
+
+
 lama::Slam2D::StrategyPtr lama::Slam2D::makeStrategy(const std::string& name, const VectorXd& parameters)
 {
     if (name == "lm"){