From 9eeba20f8c1fcb58164b3d605e01a3b5c6293ec9 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 10 Jun 2021 08:52:25 +0200
Subject: [PATCH] occupancy grid helper
---
.../libraries/armem_vision/CMakeLists.txt | 17 ++++----
.../armem_vision/OccupancyGridHelper.cpp | 39 ++++++++++++++++++
.../armem_vision/OccupancyGridHelper.h | 40 +++++++++++++++++++
3 files changed, 89 insertions(+), 7 deletions(-)
create mode 100644 source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp
create mode 100644 source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.h
diff --git a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt b/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
index 3aa2d0d4f..1767c9ceb 100644
--- a/source/RobotAPI/libraries/armem_vision/CMakeLists.txt
+++ b/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
+)
diff --git a/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp b/source/RobotAPI/libraries/armem_vision/OccupancyGridHelper.cpp
new file mode 100644
index 000000000..f88cc7545
--- /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 000000000..17628ef34
--- /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
--
GitLab