From 68c5fae10b3a7e31bb82bb5b2779967c9ef00551 Mon Sep 17 00:00:00 2001
From: "Eurico F. Pedrosa" <efp@ua.pt>
Date: Tue, 26 Jan 2021 17:53:04 +0000
Subject: [PATCH] [slam2d] Add an interface to set a new occupancy map.

---
 include/lama/slam2d.h |  5 +++++
 src/slam2d.cpp        | 24 ++++++++++++++++++++++++
 2 files changed, 29 insertions(+)

diff --git a/include/lama/slam2d.h b/include/lama/slam2d.h
index ebf8c3c..0f12877 100644
--- a/include/lama/slam2d.h
+++ b/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);
diff --git a/src/slam2d.cpp b/src/slam2d.cpp
index 4a881f7..6f1ffd2 100644
--- a/src/slam2d.cpp
+++ b/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"){