OccupancyGrid.h

Go to the documentation of this file.

#pragma once
 
#include <boost/dynamic_bitset/dynamic_bitset.hpp>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
namespace armarx
{
 
    // https://stackoverflow.com/questions/9255887/stl-function-to-test-whether-a-value-is-within-some-range
    template<typename T>
    class Interval
    {
    public:
        Interval(T lo, T hi) : low(lo), high(hi) {}
        bool contains(T value) const
        {
            return low <= value && value < high;
        }
    private:
        T low;
        T high;
    };
    template<typename T>
    Interval<T> interval(T lo, T hi)
    {
        return Interval<T>(lo, hi);
    }
 
 
    struct OccupancyGrid
    {
        void init(std::size_t gridSizeX,
                  std::size_t gridSizeY,
                  float originX,
                  float originY,
                  float gridStepSize)
        {
            sizeX                = gridSizeX;
            sizeY                = gridSizeY;
            this->originX        = originX;
            this->originY        = originY;
            this->stepSize       = gridStepSize;
            std::size_t gridSize = sizeX * sizeY;
            occupied.clear();
            occupied.resize(gridSize);
        }
 
        bool isOccupied(std::size_t indexX, std::size_t indexY) const
        {
            std::size_t index = indexY * sizeX + indexX;
            if (index < occupied.size())
            {
                return occupied[index];
            }
            return false;
        }
 
        void setOccupied(std::size_t indexX, std::size_t indexY, bool posOccupied)
        {
            std::size_t index = indexY * sizeX + indexX;
            if (index < occupied.size())
            {
                occupied[index] = posOccupied;
            }
        }
 
        Eigen::Vector2f getCentralPosition(std::size_t indexX, std::size_t indexY) const
        {
            float posX = originX + indexX * stepSize;
            float posY = originY + indexY * stepSize;
            return Eigen::Vector2f(posX, posY);
        }
 
        bool isOccupied(float posX, float posY) const
        {
            std::size_t indexX = (std::size_t)std::roundf((posX - originX) / stepSize);
            std::size_t indexY = (std::size_t)std::roundf((posY - originY) / stepSize);
            return isOccupied(indexX, indexY);
        }
 
        bool isInBounds(float posX, float posY) const
        {
            const int indexX = static_cast<int>(std::roundf((posX - originX) / stepSize));
            const int indexY = static_cast<int>(std::roundf((posY - originY) / stepSize));
 
            return Interval<int>(0, sizeX).contains(indexX) && Interval<int>(0, sizeY).contains(indexY);
        }
 
        float computeDistance(Eigen::Vector2f pos, Eigen::Vector2f dir)
        {
            // std::size_t indexX = (std::size_t)std::roundf((pos.x() - originX) / stepSize);
            // std::size_t indexY = (std::size_t)std::roundf((pos.y() - originY) / stepSize);
 
 
            // if (indexX >= sizeX || indexY >= sizeY)
            // {
            //     return 0.0f;
            // }
 
            // We are inside the grid
            // float gridMaxX  = originX + stepSize * sizeX;
            // float gridMaxY  = originY + stepSize * sizeY;
            // float distanceX = 0.0f;
            // if (dir.x() > 0.0f)
            // {
            //     distanceX = std::abs((gridMaxX - pos.x()) / dir.x());
            // }
            // else if (dir.x() < 0.0f)
            // {
            //     distanceX = std::abs((pos.x() - originX) / dir.x());
            // }
            // float distanceY = 0.0f;
            // if (dir.y() > 0.0f)
            // {
            //     distanceY = std::abs((gridMaxY - pos.y()) / dir.y());
            // }
            // else if (dir.y() < 0.0f)
            // {
            //     distanceY = std::abs((pos.y() - originY) / dir.y());
            // }
 
            // What is the max distance we can travel along dir until we leave the grid?
            float maxDistance        = 15'000; // TODO based on sensor specs
            float testStepSize       = 0.5f * stepSize;
            Eigen::Vector2f testStep = testStepSize * dir;
            std::size_t lineSteps    = std::ceil(maxDistance / testStepSize);
            for (std::size_t i = 0; i < lineSteps; ++i)
            {
                Eigen::Vector2f testPos = pos + i * testStep;
                if (isInBounds(testPos.x(), testPos.y()) && isOccupied(testPos.x(), testPos.y()))
                {
                    return i * testStepSize;
                }
            }
 
            return -1.F;
        }
 
        std::size_t sizeX = 0;
        std::size_t sizeY = 0;
        float originX     = 0.0f;
        float originY     = 0.0f;
        float stepSize    = 0.0f;
        boost::dynamic_bitset<std::size_t> occupied;
    };
 
} // namespace armarx