Skip to content
Snippets Groups Projects

Draft: Feature/update costmap

Open Fabian Reister requested to merge feature/update-costmap into master
3 files
+ 140
34
Compare changes
  • Side-by-side
  • Inline
Files
3
@@ -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
Loading