OccupancyGridHelper.cpp

Go to the documentation of this file.

#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