From e0181ab7052eeee9fbe6eba046c039fb561d2492 Mon Sep 17 00:00:00 2001
From: Meixner <andre.meixner@kit.edu>
Date: Thu, 13 Jul 2023 20:28:20 +0200
Subject: [PATCH 1/2] Added filter to costmap
---
.../armarx/navigation/algorithms/Costmap.cpp | 74 ++++++++++++++++++-
source/armarx/navigation/algorithms/Costmap.h | 15 ++++
2 files changed, 87 insertions(+), 2 deletions(-)
diff --git a/source/armarx/navigation/algorithms/Costmap.cpp b/source/armarx/navigation/algorithms/Costmap.cpp
index 862dc263..ace775b0 100644
--- a/source/armarx/navigation/algorithms/Costmap.cpp
+++ b/source/armarx/navigation/algorithms/Costmap.cpp
@@ -172,6 +172,61 @@ namespace armarx::navigation::algorithms
return {.value = minVal, .index = {row, col}, .position = toPositionGlobal({row, col})};
}
+ void
+ Costmap::filter(const Filter &filter)
+ {
+ ARMARX_TRACE;
+
+ ARMARX_CHECK(filter.matrix.rows() % 2 == 1 and filter.matrix.cols() % 2 == 1);
+
+ if (mask.has_value())
+ {
+ ARMARX_CHECK_GREATER(mask->array().sum(), 0)
+ << "At least one element has to be valid. Here, all elements are masked out!";
+ }
+
+ Grid newGrid(grid.rows(), grid.cols());
+ const double max = grid.maxCoeff();
+ newGrid.setConstant(max);
+ for (int r = 0; r < grid.rows(); r++)
+ {
+ for (int c = 0; c < grid.cols(); c++)
+ {
+ if (mask.has_value())
+ {
+ if (not mask.value()(r, c)) // skip invalid cells
+ {
+ continue;
+ }
+
+ Eigen::MatrixXf valueMatrix(filter.matrix.rows(), filter.matrix.cols());
+ const int radius_r = (filter.matrix.rows() - 1) / 2;
+ const int radius_c = (filter.matrix.cols() - 1) / 2;
+ valueMatrix.setConstant(max);
+ for (int i = -radius_r; i <= radius_r; i++)
+ {
+ for (int j = -radius_c; j <= radius_c; j++)
+ {
+ const int ri = r + i;
+ const int cj = c + j;
+ if (ri > 0 and ri < grid.rows() and cj > 0 and cj < grid.cols() and mask.value()(ri, cj))
+ {
+ valueMatrix(radius_r + i, radius_c + j) = grid(ri, cj);
+ }
+ }
+ }
+ const double sum = filter.matrix.sum();
+ if (sum > 0)
+ {
+ const float filteredValue = (valueMatrix.cwiseProduct(filter.matrix)).sum() / sum;
+ newGrid(r,c) = filter.useMinimum ? std::min(grid(r,c), filteredValue) : filteredValue;
+ }
+ }
+ }
+ }
+ grid = newGrid;
+ }
+
const Costmap::Parameters&
Costmap::params() const noexcept
{
@@ -423,5 +478,20 @@ namespace armarx::navigation::algorithms
return mergedCostmap;
}
-
-} // namespace armarx::navigation::algorithms
+ Costmap::Filter Costmap::Filter::gaussian(int radius, float sigma) {
+ ARMARX_CHECK(radius > 0 and sigma > 0.);
+ Filter gaussianFilter
+ {
+ .matrix = Eigen::MatrixXf::Zero(radius * 2 + 1, radius * 2 + 1)
+ };
+ for (int i = -radius; i <= radius; i++) {
+ for (int j = -radius; j <= radius; j++) {
+ gaussianFilter.matrix(radius + i, radius + j) =
+ std::exp(-0.5 * (std::pow(i / sigma, 2.0) +
+ std::pow(j / sigma, 2.0))) /
+ (2 * M_PI * sigma * sigma);
+ }
+ }
+ return gaussianFilter;
+ }
+ } // namespace armarx::navigation::algorithms
diff --git a/source/armarx/navigation/algorithms/Costmap.h b/source/armarx/navigation/algorithms/Costmap.h
index e7f4fc0c..35010af8 100644
--- a/source/armarx/navigation/algorithms/Costmap.h
+++ b/source/armarx/navigation/algorithms/Costmap.h
@@ -27,6 +27,19 @@ namespace armarx::navigation::algorithms
float cellSize = 100.f;
};
+ struct Filter
+ {
+ Eigen::MatrixXf matrix;
+ bool useMinimum = false;
+
+ inline Filter& setUseMinimum(bool value)
+ {
+ useMinimum = value;
+ return *this;
+ }
+
+ static Filter gaussian(int radius = 2, float sigma = 1.);
+ };
using Index = Eigen::Array2i;
using Position = Eigen::Vector2f;
@@ -86,6 +99,8 @@ namespace armarx::navigation::algorithms
Optimum optimum() const;
+ void filter(const Filter &filter);
+
bool add(const Costmap& other, float weight = 1.0);
enum class AddMode
--
GitLab
From 76368fc7127c16e9fe570435652dd4f6d78efef8 Mon Sep 17 00:00:00 2001
From: Meixner <andre.meixner@kit.edu>
Date: Thu, 13 Jul 2023 20:28:58 +0200
Subject: [PATCH 2/2] Partially fixed writing costmaps and improved debug
images for cost maps
---
.../navigation/algorithms/persistence.cpp | 85 ++++++++++++-------
1 file changed, 53 insertions(+), 32 deletions(-)
diff --git a/source/armarx/navigation/algorithms/persistence.cpp b/source/armarx/navigation/algorithms/persistence.cpp
index efcef663..ad21df29 100644
--- a/source/armarx/navigation/algorithms/persistence.cpp
+++ b/source/armarx/navigation/algorithms/persistence.cpp
@@ -31,6 +31,7 @@
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/mat.hpp>
#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
#include <opencv2/opencv.hpp>
#include <SimoxUtility/json/json.hpp>
@@ -109,22 +110,28 @@ namespace armarx::navigation::algorithms
std::filesystem::create_directories(directory);
}
- nlohmann::json j;
-
- // params
- {
- auto& jParam = j.at("params");
- jParam.at("binary_grid") = costmap.params().binaryGrid;
- jParam.at("cell_size") = costmap.params().cellSize;
- }
-
- // scene bounds
+ nlohmann::json j = {
+ { "params", {
+ {"binary_grid", costmap.params().binaryGrid },
+ {"cell_size", costmap.params().cellSize }
+ }},
+ {"scene_bounds", {
+ {"min", std::vector<float>{costmap.getLocalSceneBounds().min.x(),
+ costmap.getLocalSceneBounds().min.y()} },
+ {"max", std::vector<float>{costmap.getLocalSceneBounds().max.x(),
+ costmap.getLocalSceneBounds().max.y()}}
+ }}
+ };
+
+ std::optional<cv::Mat1b> mask;
+ if (costmap.getMask().has_value())
{
- auto& jSceneBounds = j.at("scene_bounds");
- jSceneBounds.at("min") = std::vector<float>{costmap.getLocalSceneBounds().min.x(),
- costmap.getLocalSceneBounds().min.y()};
- jSceneBounds.at("max") = std::vector<float>{costmap.getLocalSceneBounds().max.x(),
- costmap.getLocalSceneBounds().max.y()};
+ // mask, if any
+ Eigen::Matrix<std::uint8_t, Eigen::Dynamic, Eigen::Dynamic> maskI =
+ costmap.getMask()->cast<std::uint8_t>();
+ cv::Mat1b m;
+ cv::eigen2cv(maskI, m);
+ mask = m;
}
// grid
@@ -137,32 +144,46 @@ namespace armarx::navigation::algorithms
const std::string gridFilename = "grid.exr";
cv::imwrite((directory / gridFilename).string(), grid);
- j.at("grid_filename") = gridFilename;
+ j["grid_filename"] = gridFilename;
// for debugging purpose, also save a png image
- const std::string gridDebuggingFilename = "grid.png";
-
- cv::Mat gridDebug;
- grid.convertTo(gridDebug, CV_16UC1);
- cv::imwrite((directory / gridFilename).string(), gridDebug);
- }
+ const std::string gridDebuggingFilename = "grid";
- // mask, if any
- {
- ARMARX_TRACE;
+ double min;
+ double max;
+ if (mask)
+ {
+ cv::minMaxIdx(grid, &min, &max, 0, 0, mask.value());
+ }
+ else
+ {
+ cv::minMaxIdx(grid, &min, &max);
+ }
+ cv::Mat adjMap;
+ grid.convertTo(adjMap,CV_8UC1, 255 / (max-min), -255*min/(max-min));
- if (costmap.getMask().has_value())
+ cv::Mat colorsMap;
+ applyColorMap(adjMap, colorsMap, cv::COLORMAP_JET);
+ cv::Mat maskedColorsMap;
+ if (mask)
{
- Eigen::Matrix<std::uint8_t, Eigen::Dynamic, Eigen::Dynamic> maskI =
- costmap.getMask()->cast<std::uint8_t>();
+ colorsMap.copyTo(maskedColorsMap, mask.value());
+ }
+ else
+ {
+ maskedColorsMap = colorsMap;
+ }
+ cv::imwrite((directory / (gridDebuggingFilename + ".png")).string(), maskedColorsMap);
- cv::Mat1b mask;
- cv::eigen2cv(maskI, mask);
+ ARMARX_TRACE;
+ // mask if any
+ if (mask)
+ {
const std::string maskFilename = "mask.ppm";
- cv::imwrite((directory / maskFilename).string(), mask);
+ cv::imwrite((directory / maskFilename).string(), mask.value());
- j.at("mask_filename") = maskFilename;
+ j["mask_filename"] = maskFilename;
}
}
--
GitLab