occupancy grid helper

source/RobotAPI/libraries/armem_vision/CMakeLists.txt

@@ -4,9 +4,9 @@ armarx_component_set_name("${LIB_NAME}")
 armarx_set_target("Library: ${LIB_NAME}")
 
 armarx_add_library(
-    LIBS 
+    LIBS
         # ArmarX
-        ArmarXCore 
+        ArmarXCore
         # This package
         RobotAPI::Core
         RobotAPI::armem
@@ -17,17 +17,17 @@ armarx_add_library(
         ./aron_conversions.h
         ./client/laser_scans/Reader.h
         ./client/laser_scans/Writer.h
-        # ./client/occupancy_grid/Reader.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/Reader.cpp
         ./client/occupancy_grid/Writer.cpp
+        ./OccupancyGridHelper.cpp
 )
 
-
 armarx_enable_aron_file_generation_for_target(
     TARGET_NAME
         "${LIB_NAME}"
@@ -36,5 +36,8 @@ armarx_enable_aron_file_generation_for_target(
         aron/OccupancyGrid.xml
 )
 
-
-add_library(RobotAPI::armem_vision ALIAS armem_vision)
+add_library(
+    RobotAPI::armem_vision
+    ALIAS
+    armem_vision
+)

source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp

+#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

source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h

+#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