Draft: Feature/update costmap

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