diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index da477a238532c316ed729e833ebcc0a07f86cc2e..d020665e8b417864b882fdcd9aee235570a5770f 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
@@ -71,9 +71,15 @@ namespace armarx::armem::server::robot_state
 
     armarx::PropertyDefinitionsPtr RobotStateMemory::createPropertyDefinitions()
     {
+        armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
+
+        const std::string prefix = "mem.";
+
+        workingMemory.name() = "RobotState";
+        defs->optional(workingMemory.name(), prefix + "MemoryName", "Name of this memory server.");
+
         const std::string robotUnitPrefix{sensorValuePrefix};
 
-        armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
         defs->optional(robotUnitSensorPrefix, robotUnitPrefix + "SensorValuePrefix", "Prefix of all sensor values");
         defs->optional(robotUnitMemoryBatchSize, robotUnitPrefix + "MemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
         defs->optional(robotUnitPollFrequency, robotUnitPrefix + "UpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY));
@@ -124,7 +130,7 @@ namespace armarx::armem::server::robot_state
 
         ArmarXDataPath::getAbsolutePath(robotUnitConfigPath, robotUnitConfigPath, includePaths);
 
-        workingMemory.name() = workingMemoryName;
+        // workingMemory.name() = workingMemoryName;
     }
 
 
diff --git a/source/RobotAPI/libraries/CMakeLists.txt b/source/RobotAPI/libraries/CMakeLists.txt
index 5df6b028f0272c51a0538392375c27421d19dc06..02e5c638bfdfc145c271a995e016ce0c5f15ba01 100644
--- a/source/RobotAPI/libraries/CMakeLists.txt
+++ b/source/RobotAPI/libraries/CMakeLists.txt
@@ -22,7 +22,7 @@ add_subdirectory(armem_gui)
 add_subdirectory(armem_objects)
 add_subdirectory(armem_robot)
 add_subdirectory(armem_robot_state)
-add_subdirectory(armem_robot_mapping)
+add_subdirectory(armem_vision)
 add_subdirectory(armem_skills)
 add_subdirectory(aron)
 
diff --git a/source/RobotAPI/libraries/armem/CMakeLists.txt b/source/RobotAPI/libraries/armem/CMakeLists.txt
index eb83670b3cf4633d3fe2ce6bf52e61256a8d7931..c4ad28ab1f6880f59adb4ca7eda5e4e8a8a65007 100644
--- a/source/RobotAPI/libraries/armem/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem/CMakeLists.txt
@@ -66,6 +66,9 @@ set(LIB_FILES
     client/Writer.cpp
     client/WriterComponentPlugin.cpp
 
+    client/util/SimpleReaderBase.cpp
+    client/util/SimpleWriterBase.cpp
+
     client/Query.cpp
     client/query/Builder.cpp
     client/query/selectors.cpp
@@ -163,6 +166,10 @@ set(LIB_HEADERS
     client/Writer.h
     client/WriterComponentPlugin.h
 
+    client/util/SimpleReaderBase.h
+    client/util/SimpleWriterBase.h
+
+
     client/Query.h
     client/query/Builder.h
     client/query/query_fns.h
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6c3afb9a2adcc3f08495bc8c979d33ebc11742f2
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.cpp
@@ -0,0 +1,55 @@
+#include "SimpleReaderBase.h"
+
+#include "RobotAPI/libraries/armem/client/ComponentPlugin.h"
+
+namespace armarx::armem::client::util
+{
+    SimpleReaderBase::SimpleReaderBase(ComponentPluginUser& memoryClient) :
+        component(memoryClient)
+    {
+    }
+
+    void SimpleReaderBase::registerPropertyDefinitions(
+        armarx::PropertyDefinitionsPtr& def)
+    {
+        ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
+
+        const std::string prefix = propertyPrefix();
+
+        def->optional(props.memoryName, prefix + "Memory");
+        def->optional(props.coreSegmentName, prefix + "CoreSegment");
+    }
+
+    void SimpleReaderBase::connect()
+    {
+        // Wait for the memory to become available and add it as dependency.
+        ARMARX_IMPORTANT << "Writer: Waiting for memory '" << props.memoryName
+                         << "' ...";
+        auto result = component.useMemory(props.memoryName);
+        if (not result.success)
+        {
+            ARMARX_ERROR << result.errorMessage;
+            return;
+        }
+
+        ARMARX_IMPORTANT << "SimpleReaderBase: Connected to memory '"
+                         << props.memoryName;
+
+        memoryReaderClient.setReadingMemory(result.proxy);
+    }
+
+    std::mutex& SimpleReaderBase::memoryReaderMutex()
+    {
+        return memoryMutex;
+    }
+
+    const armem::client::Reader& SimpleReaderBase::memoryReader() const
+    {
+        return memoryReaderClient;
+    }
+
+    const SimpleReaderBase::Properties& SimpleReaderBase::properties() const
+    {
+        return props;
+    }
+} // namespace armarx::armem::client::util
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..4e2c8edf24d8b99362e81225b439f1ea7ed56522
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
@@ -0,0 +1,73 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX 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 General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+#include "RobotAPI/libraries/armem/client/Reader.h"
+
+
+namespace armarx::armem::client
+{
+    class ComponentPluginUser;
+}
+
+namespace armarx::armem::client::util
+{
+
+    class SimpleReaderBase
+    {
+    public:
+        SimpleReaderBase(ComponentPluginUser& memoryClient);
+        virtual ~SimpleReaderBase() = default;
+
+        void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+        void connect();
+
+    protected:
+        // Properties
+        struct Properties
+        {
+            std::string memoryName      = "";
+            std::string coreSegmentName = "";
+        };
+
+        const Properties& properties() const;
+
+        virtual std::string propertyPrefix() const   = 0;
+        virtual Properties defaultProperties() const = 0;
+
+        std::mutex& memoryReaderMutex();
+        const armem::client::Reader& memoryReader() const;
+
+    private:
+        Properties props;
+
+        armem::client::Reader memoryReaderClient;
+        std::mutex memoryMutex;
+
+        ComponentPluginUser& component;
+    };
+
+} // namespace armarx::armem::client::util
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a7ac78e00496cf890b97f11523f200c5fc9c3ab3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.cpp
@@ -0,0 +1,61 @@
+#include "SimpleWriterBase.h"
+
+#include "RobotAPI/libraries/armem/client/ComponentPlugin.h"
+
+namespace armarx::armem::client::util
+{
+    SimpleWriterBase::SimpleWriterBase(ComponentPluginUser& component) :
+        component(component)
+    {
+    }
+
+    void
+    SimpleWriterBase::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
+    {
+        ARMARX_DEBUG << "Writer: registerPropertyDefinitions";
+
+        const std::string prefix = propertyPrefix();
+
+        props = defaultProperties();
+
+        def->optional(props.memoryName, prefix + "Memory");
+        def->optional(props.coreSegmentName, prefix + "CoreSegment");
+
+        def->required(props.providerName,
+                      prefix + "Provider",
+                      "Name of this provider");
+    }
+
+    void SimpleWriterBase::connect()
+    {
+        // Wait for the memory to become available and add it as dependency.
+        ARMARX_IMPORTANT << "SimpleWriterBase: Waiting for memory '"
+                         << props.memoryName << "' ...";
+        auto result = component.useMemory(props.memoryName);
+        if (not result.success)
+        {
+            ARMARX_ERROR << result.errorMessage;
+            return;
+        }
+
+        ARMARX_IMPORTANT << "SimpleWriterBase: Connected to memory '"
+                         << props.memoryName;
+
+        memoryWriterClient.setWritingMemory(result.proxy);
+        // memoryReader.setReadingMemory(result.proxy);
+    }
+
+    std::mutex& SimpleWriterBase::memoryWriterMutex()
+    {
+        return memoryMutex;
+    }
+
+    armem::client::Writer& SimpleWriterBase::memoryWriter()
+    {
+        return memoryWriterClient;
+    }
+    const SimpleWriterBase::Properties& SimpleWriterBase::properties() const
+    {
+        return props;
+    }
+} // namespace armarx::armem::client::util
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..55829b2c7bbc3dc5e9758156ad7c4f7e14956db1
--- /dev/null
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleWriterBase.h
@@ -0,0 +1,77 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX 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 General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include "ArmarXCore/core/application/properties/PropertyDefinitionContainer.h"
+
+// #include "RobotAPI/libraries/armem/client/Reader.h"
+#include "RobotAPI/libraries/armem/client/Writer.h"
+
+
+namespace armarx::armem::client
+{
+    class ComponentPluginUser;
+}
+
+namespace armarx::armem::client::util
+{
+
+    class SimpleWriterBase
+    {
+    public:
+        SimpleWriterBase(ComponentPluginUser& component);
+        virtual ~SimpleWriterBase() = default;
+
+        void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
+        void connect();
+
+    protected:
+        std::mutex& memoryWriterMutex();
+        armem::client::Writer& memoryWriter();
+
+        struct Properties
+        {
+            std::string memoryName      = "";
+            std::string coreSegmentName = "";
+            std::string providerName    = ""; // required property
+        };
+
+        const Properties& properties() const;
+
+        virtual std::string propertyPrefix() const   = 0;
+        virtual Properties defaultProperties() const = 0;
+
+    private:
+        Properties props;
+
+        armem::client::Writer memoryWriterClient;
+        std::mutex memoryMutex;
+
+        // armem::client::Reader memoryReader;
+        // std::mutex memoryReaderMutex;
+
+        ComponentPluginUser& component;
+    };
+
+} // namespace armarx::armem::client::util
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt b/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
deleted file mode 100644
index 145ced819b68db659a92e232747c645c24037ce5..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_mapping/CMakeLists.txt
+++ /dev/null
@@ -1,34 +0,0 @@
-set(LIB_NAME armem_robot_mapping)
-
-armarx_component_set_name("${LIB_NAME}")
-armarx_set_target("Library: ${LIB_NAME}")
-
-armarx_add_library(
-    LIBS 
-        # ArmarX
-        ArmarXCore 
-        # This package
-        RobotAPICore 
-        RobotAPI::armem
-        # System / External
-        Eigen3::Eigen
-    HEADERS
-        ./aron_conversions.h
-        ./MappingDataWriter.h
-        ./MappingDataReader.h
-    SOURCES
-        ./aron_conversions.cpp
-        ./MappingDataWriter.cpp
-        ./MappingDataReader.cpp
-)
-
-
-armarx_enable_aron_file_generation_for_target(
-    TARGET_NAME
-        "${LIB_NAME}"
-    ARON_FILES
-        aron/LaserScan.xml
-)
-
-
-add_library(RobotAPI::armem_robot_mapping ALIAS armem_robot_mapping)
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h b/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
deleted file mode 100644
index 7ecffc237d480fd33c30226ef7f1cf02c09308de..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.h
+++ /dev/null
@@ -1,70 +0,0 @@
-/*
- * This file is part of ArmarX.
- *
- * ArmarX is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * ArmarX 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 General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- *
- * @author     Fabian Reister ( fabian dot reister at kit dot edu )
- * @date       2021
- * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
- *             GNU General Public License
- */
-
-
-#pragma once
-
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
-#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
-#include <RobotAPI/libraries/armem/core/Time.h>
-
-namespace armarx
-{
-
-    namespace arondto
-    {
-        struct LaserScanStamped;
-    } // namespace aron
-
-    // struct LaserScan;
-    struct LaserScanStamped;
-
-    void fromAron(
-        const arondto::LaserScanStamped& aronLaserScan,
-        LaserScan& laserScan,
-        std::int64_t& timestamp,
-        std::string& frame,
-        std::string& agentName);
-
-
-    template<typename T>
-    auto fromAron(const aron::datanavigator::NDArrayNavigatorPtr& navigator)
-    {
-        return aron::converter::AronVectorConverter::ConvertToVector<T>(navigator);
-    }
-
-    void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScanStamped& laserScan);
-
-    void toAron(
-        const LaserScan& laserScan,
-        const armem::Time& timestamp,
-        const std::string& frame,
-        const std::string& agentName,
-        arondto::LaserScanStamped& aronLaserScan);
-
-    inline aron::datanavigator::NDArrayNavigatorPtr toAron(const LaserScan& laserScan)
-    {
-        return aron::converter::AronVectorConverter::ConvertFromVector(laserScan);
-    }
-
-
-} // namespace armarx
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index 2d1d98ddf82c5c137b934b3be91079bb87298503..20a8a10f2ebb6d8c501d0156f08d8fc4781d4918 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
@@ -66,7 +66,7 @@ namespace armarx::armem::client::robot_state::localization
         // Properties
         struct Properties
         {
-            std::string memoryName             = "RobotStateMemory";
+            std::string memoryName             = "RobotState";
             std::string localizationSegment    = "Localization";
         } properties;
 
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
index 0402d8188e52b9a967805a7ddb9ca5df9c14320c..ab924dce652da8d4dc6f45d3e65e1b613989ba26 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.h
@@ -69,7 +69,7 @@ namespace armarx::armem::client::robot_state::localization
         // Properties
         struct Properties
         {
-            std::string memoryName             = "RobotStateMemory";
+            std::string memoryName             = "RobotState";
             std::string localizationSegment    = "Localization";
         } properties;
 
diff --git a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..1767c9cebd878b1d15cad6f8043a2bd7cf932814
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
@@ -0,0 +1,43 @@
+set(LIB_NAME armem_vision)
+
+armarx_component_set_name("${LIB_NAME}")
+armarx_set_target("Library: ${LIB_NAME}")
+
+armarx_add_library(
+    LIBS
+        # ArmarX
+        ArmarXCore
+        # This package
+        RobotAPI::Core
+        RobotAPI::armem
+        aroncommon
+        # System / External
+        Eigen3::Eigen
+    HEADERS
+        ./aron_conversions.h
+        ./client/laser_scans/Reader.h
+        ./client/laser_scans/Writer.h
+        ./client/occupancy_grid/Reader.h
+        ./client/occupancy_grid/Writer.h
+    SOURCES
+        ./aron_conversions.cpp
+        ./client/laser_scans/Reader.cpp
+        ./client/laser_scans/Writer.cpp
+        ./client/occupancy_grid/Reader.cpp
+        ./client/occupancy_grid/Writer.cpp
+        ./OccupancyGridHelper.cpp
+)
+
+armarx_enable_aron_file_generation_for_target(
+    TARGET_NAME
+        "${LIB_NAME}"
+    ARON_FILES
+        aron/LaserScan.xml
+        aron/OccupancyGrid.xml
+)
+
+add_library(
+    RobotAPI::armem_vision
+    ALIAS
+    armem_vision
+)
diff --git a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f88cc75454fd6f83545c3207c1e58642bedcc225
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp
@@ -0,0 +1,39 @@
+#include "OccupancyGridHelper.h"
+
+#include "types.h"
+
+namespace armarx
+{
+    OccupancyGridHelper::OccupancyGridHelper(const OccupancyGrid& occupancyGrid,
+            const Params& params) :
+        occupancyGrid(occupancyGrid), params(params)
+    {
+    }
+
+    OccupancyGridHelper::BinaryArray OccupancyGridHelper::knownCells() const
+    {
+        return (occupancyGrid.grid > 0.F).cast<bool>();
+    }
+
+    OccupancyGridHelper::BinaryArray OccupancyGridHelper::freespace() const
+    {
+        // matrix1 = matrix1 .unaryExpr(std::ptr_fun(ReplaceNanWithValue<1>));
+        // return (occupancyGrid.grid ).cast<bool>();
+
+        const auto isFree = [&](OccupancyGrid::CellType p) -> float
+        { return static_cast<float>(p < params.freespaceThreshold and p > 0.F); };
+
+        // TODO(fabian.reister): which one to choose?
+        // return occupancyGrid.grid.unaryExpr(isFree).cast<bool>();
+        return occupancyGrid.grid.unaryViewExpr(isFree).cast<bool>();
+    }
+
+    OccupancyGridHelper::BinaryArray OccupancyGridHelper::obstacles() const
+    {
+        const auto isOccupied = [&](OccupancyGrid::CellType p) -> float
+        { return static_cast<float>(p > params.occupiedThreshold); };
+
+        return occupancyGrid.grid.unaryViewExpr(isOccupied).cast<bool>();
+    }
+
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
new file mode 100644
index 0000000000000000000000000000000000000000..17628ef34fc4ef30fc2b46dd8f031f2fc46dbda3
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include <Eigen/Core>
+
+namespace armarx::armem
+{
+    struct OccupancyGrid;
+}
+
+namespace armarx
+{
+    using armarx::armem::OccupancyGrid;
+
+    namespace detail
+    {
+        struct OccupancyGridHelperParams
+        {
+            float freespaceThreshold = 0.45F;
+            float occupiedThreshold = 0.55F;
+        };
+    }
+
+    class OccupancyGridHelper
+    {
+    public:
+        using Params = detail::OccupancyGridHelperParams;
+
+        OccupancyGridHelper(const OccupancyGrid& occupancyGrid, const Params& params);
+
+        using BinaryArray = Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic>;
+
+        BinaryArray knownCells() const;
+        BinaryArray freespace() const;
+        BinaryArray obstacles() const;
+
+    private:
+        const OccupancyGrid& occupancyGrid;
+        const Params params;
+    };
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml b/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
similarity index 83%
rename from source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml
rename to source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
index 659df4a1536b0f81b81ba64f77e4049ba42fdd5c..f2d11c2e4111dfd0c24896ba807aef6435b7014b 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.xml
+++ b/source/RobotAPI/libraries/armem_vision/aron/LaserScan.xml
@@ -8,7 +8,7 @@
 
     <GenerateTypes>
 
-        <Object name='armarx::arondto::LaserScannerInfo'>
+        <Object name='armarx::armem::arondto::LaserScannerInfo'>
             <ObjectChild key='device'>
                 <string />
             </ObjectChild>
@@ -26,7 +26,7 @@
             </ObjectChild>
         </Object>
 
-        <Object name="armarx::arondto::SensorHeader">
+        <Object name="armarx::armem::arondto::SensorHeader">
             <ObjectChild key="agent">
                 <string/>
             </ObjectChild>
@@ -39,9 +39,9 @@
         </Object>
 
 
-        <Object name='armarx::arondto::LaserScanStamped'>
+        <Object name='armarx::armem::arondto::LaserScanStamped'>
             <ObjectChild key="header">
-                <armarx::arondto::SensorHeader />
+                <armarx::armem::arondto::SensorHeader />
             </ObjectChild>
 
             <!-- 
diff --git a/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml b/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml
new file mode 100644
index 0000000000000000000000000000000000000000..0c508a4e2138b4b04c126287bf46d8826fb3da6f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/aron/OccupancyGrid.xml
@@ -0,0 +1,30 @@
+<!--Some fancy comment -->
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+    <CodeIncludes>
+    </CodeIncludes>
+    <AronIncludes>
+    </AronIncludes>
+
+    <GenerateTypes>
+
+        <Object name='armarx::armem::arondto::OccupancyGrid'>
+            <ObjectChild key='resolution'>
+                <float />
+            </ObjectChild>
+            <ObjectChild key='frame'>
+                <string />
+            </ObjectChild>
+            <ObjectChild key='pose'>
+                <Pose />
+            </ObjectChild>
+            
+            <!-- 
+            <ObjectChild key='grid'>
+                <NdArray />
+            </ObjectChild> -->
+        </Object>
+
+
+    </GenerateTypes>
+</AronTypeDefinition> 
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
similarity index 67%
rename from source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
rename to source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
index b37a88381383b41ea6c3ca9bd3075a50e8b733ef..24029fa4c9422bd6da0618a8717f827fb081246f 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/aron_conversions.cpp
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.cpp
@@ -5,14 +5,14 @@
 #include <iterator>
 
 #include <RobotAPI/interface/units/LaserScannerUnit.h>
-#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
 #include <RobotAPI/libraries/aron/converter/common/Converter.h>
 #include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
 
 #include "types.h"
 
-
-namespace armarx
+namespace armarx::armem
 {
 
     /************ fromAron ************/
@@ -25,8 +25,8 @@ namespace armarx
     SensorHeader fromAron(const arondto::SensorHeader& aronSensorHeader)
     {
 
-        return {.agent = aronSensorHeader.agent,
-                .frame = aronSensorHeader.frame,
+        return {.agent     = aronSensorHeader.agent,
+                .frame     = aronSensorHeader.frame,
                 .timestamp = timeFromAron(aronSensorHeader.timestamp)};
     }
 
@@ -37,8 +37,10 @@ namespace armarx
         // laserScan.data = fromAron(aronLaserScan.data);
     }
 
-    void fromAron(const arondto::LaserScanStamped& aronLaserScan, LaserScan& laserScan,
-                  std::int64_t& timestamp, std::string& frame,
+    void fromAron(const arondto::LaserScanStamped& aronLaserScan,
+                  LaserScan& laserScan,
+                  std::int64_t& timestamp,
+                  std::string& frame,
                   std::string& agentName)
     {
         const auto header = fromAron(aronLaserScan.header);
@@ -46,14 +48,12 @@ namespace armarx
         // laserScan = fromAron(aronLaserScan.data);
 
         timestamp = header.timestamp.toMicroSeconds();
-        frame = header.frame;
+        frame     = header.frame;
         agentName = header.agent;
     }
 
-
     /************ toAron ************/
 
-
     // auto toAron(const LaserScan& laserScan, aron::LaserScan& aronLaserScan)
     // {
     //     aronLaserScan.scan = toAron(laserScan);
@@ -68,8 +68,8 @@ namespace armarx
     {
         arondto::SensorHeader aronSensorHeader;
 
-        aronSensorHeader.agent = sensorHeader.agent;
-        aronSensorHeader.frame = sensorHeader.frame;
+        aronSensorHeader.agent     = sensorHeader.agent;
+        aronSensorHeader.frame     = sensorHeader.frame;
         aronSensorHeader.timestamp = toAron(sensorHeader.timestamp);
 
         return aronSensorHeader;
@@ -92,9 +92,26 @@ namespace armarx
         {
             .agent = agentName, .frame = frame, .timestamp = timestamp};
 
-        const LaserScanStamped laserScanStamped{.header = header, .data = laserScan};
+        const LaserScanStamped laserScanStamped{.header = header,
+                                                .data   = laserScan};
 
         toAron(laserScanStamped, aronLaserScanStamped);
     }
 
-} // namespace armarx
\ No newline at end of file
+    void toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo)
+    {
+        aron::toAron(dto.frame, bo.frame);
+        aron::toAron(dto.pose, bo.pose);
+        aron::toAron(dto.resolution, bo.resolution);
+        // bo.grid is NdArray -> need special handling.
+    }
+
+    void fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo)
+    {
+        aron::fromAron(dto.frame, bo.frame);
+        aron::fromAron(dto.pose, bo.pose);
+        aron::fromAron(dto.resolution, bo.resolution);
+        // bo.grid is NdArray -> need special handling.
+    }
+
+} // namespace armarx::armem
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_vision/aron_conversions.h b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
new file mode 100644
index 0000000000000000000000000000000000000000..db97ad5076457805d953100201821f912a046291
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/aron_conversions.h
@@ -0,0 +1,82 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX 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 General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include "RobotAPI/libraries/armem_vision/types.h"
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
+#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
+#include <RobotAPI/libraries/aron/converter/common/VectorConverter.h>
+#include <RobotAPI/libraries/aron/converter/eigen/EigenConverter.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+
+namespace armarx::armem
+{
+
+    namespace arondto
+    {
+        struct LaserScanStamped;
+    } // namespace arondto
+
+    // struct LaserScan;
+    struct LaserScanStamped;
+
+    void fromAron(const arondto::LaserScanStamped& aronLaserScan,
+                  LaserScan& laserScan,
+                  std::int64_t& timestamp,
+                  std::string& frame,
+                  std::string& agentName);
+
+    template <typename T>
+    auto fromAron(const aron::datanavigator::NDArrayNavigatorPtr& navigator)
+    {
+        return aron::converter::AronVectorConverter::ConvertToVector<T>(
+                   navigator);
+    }
+
+    void fromAron(const arondto::LaserScanStamped& aronLaserScan,
+                  LaserScanStamped& laserScan);
+
+    void toAron(const LaserScan& laserScan,
+                const armem::Time& timestamp,
+                const std::string& frame,
+                const std::string& agentName,
+                arondto::LaserScanStamped& aronLaserScan);
+
+    inline aron::datanavigator::NDArrayNavigatorPtr
+    toAron(const LaserScan& laserScan)
+    {
+        using aron::converter::AronVectorConverter;
+        return AronVectorConverter::ConvertFromVector(laserScan);
+    }
+
+    // OccupancyGrid
+    void toAron(arondto::OccupancyGrid& dto, const OccupancyGrid& bo);
+    void fromAron(const arondto::OccupancyGrid& dto, OccupancyGrid& bo);
+
+    inline aron::datanavigator::NDArrayNavigatorPtr
+    toAron(const OccupancyGrid::Grid& grid)
+    {
+        return aron::converter::AronEigenConverter::ConvertFromArray(grid);
+    }
+
+} // namespace armarx::armem
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
similarity index 69%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
index fd8250c5b2b086f3869729e2bf6836e5e3c5dc32..84efcbcca26c05913e2ad8b1b71420f7ad0ae70e 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.cpp
@@ -1,31 +1,32 @@
-#include "MappingDataReader.h"
+#include "Reader.h"
 
 // STD / STL
-#include <cstring>
-#include <vector>
 #include <algorithm>
+#include <cstring>
 #include <map>
 #include <optional>
 #include <ostream>
-#include <type_traits>
 #include <utility>
+#include <vector>
+
+#include <type_traits>
 
 // ICE
-#include <IceUtil/Time.h>
 #include <IceUtil/Handle.h>
+#include <IceUtil/Time.h>
 
 // Simox
 #include <SimoxUtility/algorithm/get_map_keys_values.h>
 
 // ArmarXCore
-#include <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/logging/LogSender.h>
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/LogSender.h>
+#include <ArmarXCore/core/logging/Logging.h>
 
 // RobotAPI Interfaces
-#include <RobotAPI/interface/units/LaserScannerUnit.h>
 #include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
 #include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
 
 // RobotAPI Aron
 #include <RobotAPI/libraries/aron/core/Exception.h>
@@ -36,42 +37,42 @@
 #include <RobotAPI/libraries/armem/client/Reader.h>
 #include <RobotAPI/libraries/armem/client/query/Builder.h>
 #include <RobotAPI/libraries/armem/client/query/selectors.h>
-
-#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
 #include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
-#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
 #include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
 #include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
-
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
 #include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
 
-#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
-#include <RobotAPI/libraries/armem_robot_mapping/aron_conversions.h>
-#include <RobotAPI/libraries/armem_robot_mapping/types.h>
-
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
 {
 
-    MappingDataReader::MappingDataReader(armem::ClientReaderComponentPluginUser& memoryClient)
-        : memoryClient(memoryClient) {}
+    Reader::Reader(armem::ClientReaderComponentPluginUser& memoryClient) :
+        memoryClient(memoryClient)
+    {
+    }
 
-    MappingDataReader::~MappingDataReader() = default;
+    Reader::~Reader() = default;
 
-    void MappingDataReader::registerPropertyDefinitions(
-        armarx::PropertyDefinitionsPtr& def)
+    void
+    Reader::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
     {
         ARMARX_DEBUG << "TransformReader: registerPropertyDefinitions";
         registerPropertyDefinitions(def);
 
         const std::string prefix = propertyPrefix;
 
-        def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
+        def->optional(properties.coreSegmentName,
+                      prefix + "CoreSegment",
                       "Name of the mapping memory core segment to use.");
 
         def->optional(properties.memoryName, prefix + "MemoryName");
     }
 
-    void MappingDataReader::connect()
+    void Reader::connect()
     {
         // Wait for the memory to become available and add it as dependency.
         ARMARX_IMPORTANT << "TransformReader: Waiting for memory '"
@@ -89,19 +90,19 @@ namespace armarx::armem
         memoryReader.setReadingMemory(result.proxy);
     }
 
-    armem::client::query::Builder
-    MappingDataReader::buildQuery(const Query& query) const
+    armarx::armem::client::query::Builder
+    Reader::buildQuery(const Query& query) const
     {
-        armem::client::query::Builder qb;
+        armarx::armem::client::query::Builder qb;
 
         ARMARX_INFO << "Query for agent: " << query.agent
-                    << " memory name: " << properties.mappingMemoryName;
+                    << " memory name: " << properties.memoryName;
 
         if (query.sensorList.empty()) // all sensors
         {
             // clang-format off
             qb
-            .coreSegments().withName(properties.mappingMemoryName)
+            .coreSegments().withName(properties.memoryName)
             .providerSegments().withName(query.agent)
             .entities().all()
             .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
@@ -111,7 +112,7 @@ namespace armarx::armem
         {
             // clang-format off
             qb
-            .coreSegments().withName(properties.mappingMemoryName)
+            .coreSegments().withName(properties.memoryName)
             .providerSegments().withName(query.agent)
             .entities().withNames(query.sensorList)
             .snapshots().timeRange(query.timeRange.min, query.timeRange.max);
@@ -119,10 +120,10 @@ namespace armarx::armem
         }
 
         return qb;
-
     }
 
-    std::vector<LaserScanStamped> asLaserScans(const std::map<std::string, wm::Entity>& entities)
+    std::vector<LaserScanStamped>
+    asLaserScans(const std::map<std::string, wm::Entity>& entities)
     {
         std::vector<LaserScanStamped> outV;
 
@@ -131,25 +132,27 @@ namespace armarx::armem
             ARMARX_WARNING << "No entities!";
         }
 
-        const auto convert = [](const arondto::LaserScanStamped & aronLaserScanStamped, const wm::EntityInstance & ei) -> LaserScanStamped
+        const auto convert =
+            [](const arondto::LaserScanStamped & aronLaserScanStamped,
+               const wm::EntityInstance & ei) -> LaserScanStamped
         {
             LaserScanStamped laserScanStamped;
             fromAron(aronLaserScanStamped, laserScanStamped);
 
-            const auto ndArrayNavigator = aron::datanavigator::NDArrayNavigator::DynamicCast(ei.data()->getElement("scan"));
+            const auto ndArrayNavigator =
+            aron::datanavigator::NDArrayNavigator::DynamicCast(
+                ei.data()->getElement("scan"));
 
             ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
 
             laserScanStamped.data = fromAron<LaserScanStep>(ndArrayNavigator);
             ARMARX_IMPORTANT << "4";
 
-
             return laserScanStamped;
-
         };
 
         // loop over all entities and their snapshots
-        for (const auto &[s, entity] : entities)
+        for (const auto& [s, entity] : entities)
         {
             if (entity.empty())
             {
@@ -158,11 +161,12 @@ namespace armarx::armem
 
             ARMARX_DEBUG << "History size: " << entity.size();
 
-            for (const auto &[ss, entitySnapshot] : entity)
+            for (const auto& [ss, entitySnapshot] : entity)
             {
                 for (const auto& entityInstance : entitySnapshot.instances())
                 {
-                    const auto o = tryCast<arondto::LaserScanStamped>(entityInstance);
+                    const auto o =
+                        tryCast<arondto::LaserScanStamped>(entityInstance);
 
                     if (o)
                     {
@@ -175,8 +179,7 @@ namespace armarx::armem
         return outV;
     }
 
-    MappingDataReader::Result
-    MappingDataReader::queryData(const Query& query) const
+    Reader::Result Reader::queryData(const Query& query) const
     {
         const auto qb = buildQuery(query);
 
@@ -191,25 +194,25 @@ namespace armarx::armem
         {
             ARMARX_WARNING << "Failed to query data from memory: "
                            << qResult.errorMessage;
-            return {.laserScans = {},
-                    .sensors = {},
-                    .status = Result::Status::Error,
+            return {.laserScans   = {},
+                    .sensors      = {},
+                    .status       = Result::Status::Error,
                     .errorMessage = qResult.errorMessage};
         }
 
         // now create result from memory
         const auto& entities =
-            qResult.memory.getCoreSegment(properties.mappingMemoryName)
+            qResult.memory.getCoreSegment(properties.memoryName)
             .getProviderSegment(query.agent)
             .entities();
 
         const auto laserScans = asLaserScans(entities);
-        const auto sensors = simox::alg::get_keys(entities);
+        const auto sensors    = simox::alg::get_keys(entities);
 
-        return {.laserScans = laserScans,
-                .sensors = sensors,
-                .status = Result::Status::Success,
+        return {.laserScans   = laserScans,
+                .sensors      = sensors,
+                .status       = Result::Status::Success,
                 .errorMessage = ""};
     }
 
-} // namespace armarx::armem
+} // namespace armarx::armem::vision::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
similarity index 81%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
index aee783764f4b5720e4ae0d8d69fbf6faed297e98..931fc1ef2fc5127c1cb6487ea01370dae5373e35 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataReader.h
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Reader.h
@@ -21,7 +21,7 @@
 
 #pragma once
 
-#include <stdint.h>
+#include <cstdint>
 #include <string>
 #include <vector>
 
@@ -30,16 +30,14 @@
 #include <RobotAPI/libraries/armem/client.h>
 #include <RobotAPI/libraries/armem/client/Reader.h>
 #include <RobotAPI/libraries/armem/client/query/Builder.h>
-#include <RobotAPI/libraries/armem_robot_mapping/types.h>
-
-
+#include <RobotAPI/libraries/armem_vision/types.h>
 
 namespace armarx
 {
     class ManagedIceObject;
 }
 
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
 {
 
     struct TimeRange
@@ -59,16 +57,15 @@ namespace armarx::armem
     *
     * Detailed description of class ExampleClient.
     */
-    class MappingDataReader
+    class Reader
     {
     public:
-        MappingDataReader(armem::ClientReaderComponentPluginUser& memoryClient);
+        Reader(armem::ClientReaderComponentPluginUser& memoryClient);
 
-        virtual ~MappingDataReader();
+        virtual ~Reader();
 
         void connect();
 
-
         struct Query
         {
             std::string agent;
@@ -77,7 +74,6 @@ namespace armarx::armem
 
             // if empty, all sensors will be queried
             std::vector<std::string> sensorList;
-
         };
 
         struct Result
@@ -94,32 +90,28 @@ namespace armarx::armem
             } status;
 
             std::string errorMessage;
-
         };
 
         Result queryData(const Query& query) const;
 
         void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def);
 
-
-        client::query::Builder buildQuery(const Query& query) const ;
-
+        armarx::armem::client::query::Builder
+        buildQuery(const Query& query) const;
 
     private:
-
         armem::client::Reader memoryReader;
 
         // Properties
         struct Properties
         {
-            std::string memoryName             = "RobotState";
-            std::string mappingMemoryName = "Mapping";
+            std::string memoryName      = "Vision";
+            std::string coreSegmentName = "LaserScans";
         } properties;
 
-
-        const std::string propertyPrefix = "mem.mapping.";
+        const std::string propertyPrefix = "mem.vision.";
 
         armem::ClientReaderComponentPluginUser& memoryClient;
     };
 
-}  // namespace armarx::armem
+} // namespace armarx::armem::vision::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
similarity index 56%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
index 351d71398a6959f346aadf45c6c604edcd865a53..f1e1e371b23631a054b13f5977b28a24503394bc 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.cpp
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.cpp
@@ -1,35 +1,37 @@
-#include "MappingDataWriter.h"
+#include "Writer.h"
 
-#include "RobotAPI/libraries/armem_robot_mapping/aron_conversions.h"
-#include <RobotAPI/libraries/armem_robot_mapping/aron/LaserScan.aron.generated.h>
+#include "RobotAPI/libraries/armem_vision/aron_conversions.h"
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
 
-
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
 {
 
-    MappingDataWriter::MappingDataWriter(armem::ClientWriterComponentPluginUser& component)
-        : component(component) {}
+    Writer::Writer(armem::ClientWriterComponentPluginUser& component) :
+        component(component)
+    {
+    }
 
-    MappingDataWriter::~MappingDataWriter() = default;
+    Writer::~Writer() = default;
 
-    void MappingDataWriter::registerPropertyDefinitions(
-        armarx::PropertyDefinitionsPtr& def)
+    void
+    Writer::registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def)
     {
-        ARMARX_DEBUG << "TransformWriter: registerPropertyDefinitions";
+        ARMARX_DEBUG << "LaserScansWriter: registerPropertyDefinitions";
 
         const std::string prefix = propertyPrefix;
 
-        def->optional(properties.mappingMemoryName, prefix + "MappingMemoryName",
+        def->optional(properties.coreSegmentName,
+                      prefix + "CoreSegment",
                       "Name of the mapping memory core segment to use.");
 
         def->optional(properties.memoryName, prefix + "MemoryName");
     }
 
-    void MappingDataWriter::connect()
+    void Writer::connect()
     {
         // Wait for the memory to become available and add it as dependency.
-        ARMARX_IMPORTANT << "MappingDataWriter: Waiting for memory '" << properties.memoryName
-                         << "' ...";
+        ARMARX_IMPORTANT << "LaserScansWriter: Waiting for memory '"
+                         << properties.memoryName << "' ...";
         auto result = component.useMemory(properties.memoryName);
         if (not result.success)
         {
@@ -37,21 +39,21 @@ namespace armarx::armem
             return;
         }
 
-        ARMARX_IMPORTANT << "TransformWriter: Connected to memory '" << properties.memoryName;
+        ARMARX_IMPORTANT << "LaserScansWriter: Connected to memory '"
+                         << properties.memoryName;
 
         memoryWriter.setWritingMemory(result.proxy);
     }
 
-
-    bool MappingDataWriter::storeSensorData(const LaserScan& laserScan,
-                                            const std::string& frame,
-                                            const std::string& agentName,
-                                            const std::int64_t& timestamp)
+    bool Writer::storeSensorData(const LaserScan& laserScan,
+                                 const std::string& frame,
+                                 const std::string& agentName,
+                                 const std::int64_t& timestamp)
     {
         std::lock_guard g{memoryWriterMutex};
 
         const auto result =
-            memoryWriter.addSegment(properties.mappingMemoryName, agentName);
+            memoryWriter.addSegment(properties.memoryName, agentName);
 
         if (not result.success)
         {
@@ -78,7 +80,7 @@ namespace armarx::armem
         dict->addElement("scan", toAron(laserScan));
 
         update.instancesData = {dict};
-        update.timeCreated = iceTimestamp;
+        update.timeCreated   = iceTimestamp;
 
         ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
         armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
@@ -93,4 +95,4 @@ namespace armarx::armem
         return updateResult.success;
     }
 
-} // namespace armarx::armem
\ No newline at end of file
+} // namespace armarx::armem::vision::laser_scans::client
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
similarity index 71%
rename from source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
rename to source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
index e5de996746b7d54b46faa891cf2cd26007fbaf02..6ae488dd50db037ab14ba5836ab7bbaaa4829b06 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/MappingDataWriter.h
+++ b/source/RobotAPI/libraries/armem_vision/client/laser_scans/Writer.h
@@ -25,12 +25,10 @@
 #include <mutex>
 
 #include <RobotAPI/interface/units/LaserScannerUnit.h>
-
-#include <RobotAPI/libraries/armem/client/Writer.h>
 #include <RobotAPI/libraries/armem/client.h>
+#include <RobotAPI/libraries/armem/client/Writer.h>
 
-
-namespace armarx::armem
+namespace armarx::armem::vision::laser_scans::client
 {
 
     /**
@@ -44,11 +42,11 @@ namespace armarx::armem
     *
     * Detailed description of class ExampleClient.
     */
-    class MappingDataWriter
+    class Writer
     {
     public:
-        MappingDataWriter(armem::ClientWriterComponentPluginUser& component);
-        virtual ~MappingDataWriter();
+        Writer(armem::ClientWriterComponentPluginUser& component);
+        virtual ~Writer();
 
         void connect();
 
@@ -57,9 +55,13 @@ namespace armarx::armem
         // void connect() override;
 
         /// to be called in Component::addPropertyDefinitions
-        void registerPropertyDefinitions(armarx::PropertyDefinitionsPtr& def) /*override*/;
+        void registerPropertyDefinitions(
+            armarx::PropertyDefinitionsPtr& def) /*override*/;
 
-        bool storeSensorData(const LaserScan& laserScan, const std::string& frame, const std::string& agentName, const std::int64_t& timestamp);
+        bool storeSensorData(const LaserScan& laserScan,
+                             const std::string& frame,
+                             const std::string& agentName,
+                             const std::int64_t& timestamp);
 
     private:
         armem::client::Writer memoryWriter;
@@ -67,17 +69,15 @@ namespace armarx::armem
         // Properties
         struct Properties
         {
-            std::string memoryName         = "RobotState";
-            std::string mappingMemoryName  = "Mapping";
+            std::string memoryName      = "Vision";
+            std::string coreSegmentName = "LaserScans";
         } properties;
 
         std::mutex memoryWriterMutex;
 
-
-        const std::string propertyPrefix = "mem.mapping.";
+        const std::string propertyPrefix = "mem.vision.";
 
         armem::ClientWriterComponentPluginUser& component;
     };
 
-
-} // namespace armarx::armem
+} // namespace armarx::armem::vision::laser_scans::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ba7edf547d1d45873609d723b92934b18d7f5c88
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.cpp
@@ -0,0 +1,146 @@
+#include "Reader.h"
+
+// STD / STL
+#include <algorithm>
+#include <cstring>
+#include <map>
+#include <optional>
+#include <ostream>
+#include <utility>
+#include <vector>
+
+#include <type_traits>
+
+// ICE
+#include <IceUtil/Handle.h>
+#include <IceUtil/Time.h>
+
+// Simox
+#include <SimoxUtility/algorithm/get_map_keys_values.h>
+
+// ArmarXCore
+#include "ArmarXCore/core/exceptions/LocalException.h"
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
+#include <ArmarXCore/core/logging/LogSender.h>
+#include <ArmarXCore/core/logging/Logging.h>
+
+// RobotAPI Interfaces
+#include "RobotAPI/libraries/aron/converter/eigen/EigenConverter.h"
+#include <RobotAPI/interface/armem/mns/MemoryNameSystemInterface.h>
+#include <RobotAPI/interface/armem/server/ReadingMemoryInterface.h>
+#include <RobotAPI/interface/units/LaserScannerUnit.h>
+
+// RobotAPI Aron
+#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
+#include <RobotAPI/libraries/aron/core/Exception.h>
+#include <RobotAPI/libraries/aron/core/navigator/data/complex/NDArray.h>
+
+// RobotAPI Armem
+#include <RobotAPI/libraries/armem/client/Query.h>
+#include <RobotAPI/libraries/armem/client/Reader.h>
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+#include <RobotAPI/libraries/armem/client/query/selectors.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/CoreSegment.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Entity.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/EntityInstance.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/Memory.h>
+#include <RobotAPI/libraries/armem/core/workingmemory/ProviderSegment.h>
+#include <RobotAPI/libraries/armem/util/util.h>
+#include <RobotAPI/libraries/armem_vision/aron/LaserScan.aron.generated.h>
+#include <RobotAPI/libraries/armem_vision/aron_conversions.h>
+#include <RobotAPI/libraries/armem_vision/types.h>
+
+namespace armarx::armem::vision::occupancy_grid::client
+{
+
+    armarx::armem::client::query::Builder Reader::buildQuery(const Query& query) const
+    {
+        armarx::armem::client::query::Builder qb;
+
+        // clang-format off
+        qb
+        .coreSegments().withName(properties().memoryName)
+        .providerSegments().withName(query.providerName)
+        .entities().all()
+        .snapshots().beforeOrAtTime(query.timestamp);
+        // clang-format on
+
+        return qb;
+    }
+
+    OccupancyGrid asOccupancyGrid(const std::map<std::string, wm::Entity>& entities)
+    {
+        ARMARX_CHECK(not entities.empty()) << "No entities";
+        ARMARX_CHECK(entities.size() == 1) << "There should be only one entity!";
+
+        const wm::Entity& entity = entities.begin()->second;
+        ARMARX_CHECK(not entity.empty()) << "No snapshots";
+
+        const auto& entitySnapshot = entity.getLatestSnapshot();
+        ARMARX_CHECK(not entitySnapshot.empty()) << "No entity snapshot instances";
+
+        const auto& entityInstance = entitySnapshot.instances().front();
+
+        const auto aronDto = tryCast<arondto::OccupancyGrid>(entityInstance);
+        ARMARX_CHECK(aronDto) << "Failed casting to OccupancyGrid";
+
+        OccupancyGrid occupancyGrid;
+        fromAron(*aronDto, occupancyGrid);
+
+        // direct access to grid data
+        const auto ndArrayNavigator = aron::datanavigator::NDArrayNavigator::DynamicCast(
+                                          entityInstance.data()->getElement("grid"));
+        ARMARX_CHECK_NOT_NULL(ndArrayNavigator);
+
+        occupancyGrid.grid = aron::converter::AronEigenConverter::ConvertToArray<float>(*ndArrayNavigator);
+
+        return occupancyGrid;
+    }
+
+    Reader::Result Reader::query(const Query& query) const
+    {
+        const auto qb = buildQuery(query);
+
+        ARMARX_IMPORTANT << "[MappingDataReader] query ... ";
+
+        const armem::client::QueryResult qResult =
+            memoryReader().query(qb.buildQueryInput());
+
+        ARMARX_DEBUG << "[MappingDataReader] result: " << qResult;
+
+        if (not qResult.success)
+        {
+            ARMARX_WARNING << "Failed to query data from memory: "
+                           << qResult.errorMessage;
+            return {.occupancyGrid = std::nullopt,
+                    .status        = Result::Status::Error,
+                    .errorMessage  = qResult.errorMessage};
+        }
+
+        // now create result from memory
+        const auto& entities = qResult.memory.getCoreSegment(properties().memoryName)
+                               .getProviderSegment(query.providerName)
+                               .entities();
+
+        if (entities.empty())
+        {
+            ARMARX_WARNING << "No entities.";
+            return {.occupancyGrid = std::nullopt,
+                    .status        = Result::Status::NoData,
+                    .errorMessage  = "No entities"};
+        }
+
+        try
+        {
+            const auto occupancyGrid = asOccupancyGrid(entities);
+            return Result{.occupancyGrid = occupancyGrid,
+                          .status        = Result::Status::Success};
+        }
+        catch (...)
+        {
+            return Result{.status       = Result::Status::Error,
+                          .errorMessage = GetHandledExceptionString()};
+        }
+    }
+
+} // namespace armarx::armem::vision::occupancy_grid::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..a43c2c7c1151223358d5b1ab608f824fd3cdf66f
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Reader.h
@@ -0,0 +1,74 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX 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 General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include "RobotAPI/libraries/armem/client/util/SimpleReaderBase.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+#include "RobotAPI/libraries/armem_vision/types.h"
+#include <RobotAPI/libraries/armem/client/query/Builder.h>
+
+namespace armarx::armem::vision::occupancy_grid::client
+{
+
+    class Reader : virtual public armarx::armem::client::util::SimpleReaderBase
+    {
+    public:
+        using armarx::armem::client::util::SimpleReaderBase::SimpleReaderBase;
+        ~Reader() override;
+
+        struct Query
+        {
+            std::string providerName;
+            armem::Time timestamp;
+        };
+
+        struct Result
+        {
+            std::optional<OccupancyGrid> occupancyGrid = std::nullopt;
+
+            enum Status
+            {
+                Success,
+                NoData,
+                Error
+            } status;
+
+            std::string errorMessage = "";
+
+            operator bool() const noexcept
+            {
+                return status == Status::Success;
+            }
+        };
+
+        Result query(const Query& query) const;
+
+        ::armarx::armem::client::query::Builder buildQuery(const Query& query) const;
+
+    protected:
+        std::string propertyPrefix() const override;
+        Properties defaultProperties() const override;
+    };
+
+} // namespace armarx::armem::vision::occupancy_grid::client
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f2930f14f479d9ae68ee222f2643edf327392a43
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.cpp
@@ -0,0 +1,70 @@
+#include "Writer.h"
+
+#include "RobotAPI/libraries/armem_vision/aron_conversions.h"
+#include <RobotAPI/libraries/armem_vision/aron/OccupancyGrid.aron.generated.h>
+
+namespace armarx::armem::vision::occupancy_grid::client
+{
+    Writer::~Writer() = default;
+
+    bool Writer::store(const OccupancyGrid& grid,
+                       const std::string& frame,
+                       const std::string& agentName,
+                       const std::int64_t& timestamp)
+    {
+        std::lock_guard g{memoryWriterMutex()};
+
+        const auto result = memoryWriter().addSegment(properties().coreSegmentName, agentName);
+
+        if (not result.success)
+        {
+            ARMARX_ERROR << result.errorMessage;
+
+            // TODO(fabian.reister): message
+            return false;
+        }
+
+        const auto iceTimestamp = Time::microSeconds(timestamp);
+
+        const auto providerId = armem::MemoryID(result.segmentID);
+        const auto entityID   = providerId.withEntityName(frame).withTimestamp(iceTimestamp);
+
+        armem::EntityUpdate update;
+        update.entityID = entityID;
+
+        arondto::OccupancyGrid aronGrid;
+        // currently only sets the header
+        toAron(aronGrid, grid);
+
+        auto dict = aronGrid.toAron();
+        dict->addElement("grid", toAron(grid.grid));
+
+        update.instancesData = {dict};
+        update.timeCreated   = iceTimestamp;
+
+        ARMARX_DEBUG << "Committing " << update << " at time " << iceTimestamp;
+        armem::EntityUpdateResult updateResult = memoryWriter().commit(update);
+
+        ARMARX_DEBUG << updateResult;
+
+        if (not updateResult.success)
+        {
+            ARMARX_ERROR << updateResult.errorMessage;
+        }
+
+        return updateResult.success;
+    }
+
+    std::string Writer::propertyPrefix() const
+    {
+        return "mem.vision.occupancy_grid.";
+    }
+
+    armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase::Properties
+    Writer::defaultProperties() const
+    {
+
+        return SimpleWriterBase::Properties{.memoryName      = "Vision",
+                                            .coreSegmentName = "OccupancyGrid"};
+    }
+} // namespace armarx::armem::vision::occupancy_grid::client
\ No newline at end of file
diff --git a/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
new file mode 100644
index 0000000000000000000000000000000000000000..bf1268444c333d10bb3c2f9efb68da11fe697cc8
--- /dev/null
+++ b/source/RobotAPI/libraries/armem_vision/client/occupancy_grid/Writer.h
@@ -0,0 +1,62 @@
+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX 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 General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @package    RobotAPI::ArmarXObjects::
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include "RobotAPI/libraries/armem/client/util/SimpleWriterBase.h"
+#include "RobotAPI/libraries/armem_vision/types.h"
+
+namespace armarx::armem::vision::occupancy_grid::client
+{
+
+    /**
+    * @defgroup Component-ExampleClient ExampleClient
+    * @ingroup RobotAPI-Components
+    * A description of the component ExampleClient.
+    *
+    * @class ExampleClient
+    * @ingroup Component-ExampleClient
+    * @brief Brief description of class ExampleClient.
+    *
+    * Detailed description of class ExampleClient.
+    */
+    class Writer : virtual public armarx::armem::client::util::SimpleWriterBase
+    {
+    public:
+        using armarx::armem::client::util::SimpleWriterBase::SimpleWriterBase;
+        ~Writer() override;
+
+        bool store(const OccupancyGrid& grid,
+                   const std::string& frame,
+                   const std::string& agentName,
+                   const std::int64_t& timestamp);
+
+    protected:
+        std::string propertyPrefix() const override;
+        Properties defaultProperties() const override;
+    };
+
+
+
+} // namespace armarx::armem::vision::occupancy_grid::client
diff --git a/source/RobotAPI/libraries/armem_robot_mapping/types.h b/source/RobotAPI/libraries/armem_vision/types.h
similarity index 73%
rename from source/RobotAPI/libraries/armem_robot_mapping/types.h
rename to source/RobotAPI/libraries/armem_vision/types.h
index d822597e103177b0833014d25f0530e25f2b2075..dd975e9b1e6e76484c3077bebf9dc99c9f3a8d85 100644
--- a/source/RobotAPI/libraries/armem_robot_mapping/types.h
+++ b/source/RobotAPI/libraries/armem_vision/types.h
@@ -21,10 +21,12 @@
 
 #pragma once
 
-#include <RobotAPI/libraries/armem/core/Time.h>
+#include <vector>
+
 #include <RobotAPI/interface/units/LaserScannerUnit.h>
+#include <RobotAPI/libraries/armem/core/Time.h>
 
-namespace armarx
+namespace armarx::armem
 {
 
     struct SensorHeader
@@ -40,4 +42,20 @@ namespace armarx
         LaserScan data;
     };
 
-} // namespace armarx
+    // template<typename _ValueT = float>
+    struct OccupancyGrid
+    {
+        float resolution;
+
+        std::string frame;
+        Eigen::Affine3f pose;
+
+        // using ValueType = _ValueT;
+        using CellType = float;
+        using Grid     = Eigen::Array<CellType, Eigen::Dynamic, Eigen::Dynamic>;
+
+        Grid grid;
+    };
+
+
+} // namespace armarx::armem
diff --git a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
index 787ae30a23f097821989fcd9658a3fa31d6b4761..4a5a02e56edc8fae399b2a216b4ee77a6f4536d8 100644
--- a/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
+++ b/source/RobotAPI/libraries/aron/converter/eigen/EigenConverter.h
@@ -28,6 +28,7 @@
 // Eigen
 #include <Eigen/Geometry>
 #include <Eigen/Core>
+#include <Eigen/src/Core/util/Constants.h>
 
 // ArmarX
 #include <ArmarXCore/core/exceptions/local/ExpressionException.h>
@@ -103,9 +104,26 @@ namespace armarx::aron::converter
             return ConvertToMatrix<T, Rows, Cols>(*nav);
         }
 
-        template<typename T, int Rows, int Cols>
+        template<typename T>
+        static Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicMatrix(const datanavigator::NDArrayNavigator& nav)
+        {
+            const auto dims = nav.getDimensions();
+
+            using MatrixT = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
+
+            MatrixT ret;
+            memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>()));
+            return ret;
+        }
+
+        template<typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
         static Eigen::Matrix<T, Rows, Cols> ConvertToMatrix(const datanavigator::NDArrayNavigator& nav)
         {
+            if constexpr(Rows == Eigen::Dynamic and Cols == Eigen::Dynamic)
+            {
+                return ConvertToDynamicMatrix<T>(nav);
+            }
+
             checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
             auto dims = nav.getDimensions();
 
@@ -114,6 +132,61 @@ namespace armarx::aron::converter
             return ret;
         }
 
+        template<typename T>
+        static datanavigator::NDArrayNavigatorPtr ConvertFromMatrix(const Eigen::Matrix < T, Eigen::Dynamic, Eigen::Dynamic >& mat)
+        {
+            datanavigator::NDArrayNavigatorPtr ndArr(new datanavigator::NDArrayNavigator);
+
+            ndArr->setDimensions({static_cast<int>(mat.rows()), static_cast<int>(mat.cols())});
+            ndArr->setData(sizeof(T) * mat.size(), reinterpret_cast <const unsigned char* >(mat.data()));
+
+            return ndArr;
+        }
+
+
+        // Eigen::Array
+
+        template<typename T>
+        static Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic> ConvertToDynamicArray(const datanavigator::NDArrayNavigator& nav)
+        {
+            const auto dims = nav.getDimensions();
+
+            using ArrayT = Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic>;
+
+            ArrayT ret;
+            memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<>()));
+            return ret;
+        }
+
+        template<typename T, int Rows = Eigen::Dynamic, int Cols = Eigen::Dynamic>
+        static Eigen::Matrix<T, Rows, Cols> ConvertToArray(const datanavigator::NDArrayNavigator& nav)
+        {
+            if constexpr(Rows == Eigen::Dynamic and Cols == Eigen::Dynamic)
+            {
+                return ConvertToDynamicArray<T>(nav);
+            }
+
+            checkDimensions(nav, {Rows, Cols, sizeof(T)}, "ConvertToMatrix");
+            auto dims = nav.getDimensions();
+
+            Eigen::Array<T, Rows, Cols> ret;
+            memcpy(reinterpret_cast<unsigned char*>(ret.data()), nav.getData(), std::accumulate(std::begin(dims), std::end(dims), 1, std::multiplies<int>()));
+            return ret;
+        }
+
+        template<typename T>
+        static datanavigator::NDArrayNavigatorPtr ConvertFromArray(const Eigen::Array < T, Eigen::Dynamic, Eigen::Dynamic >& mat)
+        {
+            datanavigator::NDArrayNavigatorPtr ndArr(new datanavigator::NDArrayNavigator);
+
+            ndArr->setDimensions({static_cast<int>(mat.rows()), static_cast<int>(mat.cols())});
+            ndArr->setData(sizeof(T) * mat.size(), reinterpret_cast <const unsigned char* >(mat.data()));
+
+            return ndArr;
+        }
+
+
+
     private:
 
         /**