CPose2DGridTemplate.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
#ifndef CPose2DGridTemplate_H
#define CPose2DGridTemplate_H

#include <mrpt/utils/CSerializable.h>
#include <mrpt/utils/round.h>  // for round()
#include <mrpt/utils/bits.h>  // for DEG2RAD()

namespace mrpt
{
namespace poses
{
/** This is a template class for storing a 3D (2D+heading) grid containing any
 * kind of data.
 * \ingroup poses_pdf_grp
 */
template <class T>
class CPose2DGridTemplate
{
   protected:
        /** The limits and resolution of the grid:
          */
        double m_xMin, m_xMax, m_yMin, m_yMax, m_phiMin, m_phiMax, m_resolutionXY,
                m_resolutionPhi;

        /** The size of "m_data" is m_sizeX * m_sizeY * m_sizePhi
          */
        size_t m_sizeX, m_sizeY, m_sizePhi, m_sizeXY;

        /** The indexes of the "left" borders:
          */
        int m_idxLeftX, m_idxLeftY, m_idxLeftPhi;

        /** The data:
          */
        std::vector<T> m_data;

   public:
        /** Returns "indexes" from coordinates:
          */
        size_t x2idx(double x) const
        {
                int idx = mrpt::utils::round((x - m_xMin) / m_resolutionXY);
                ASSERT_(idx >= 0 && idx < static_cast<int>(m_sizeX));
                return idx;
        }

        /** Returns "indexes" from coordinates:
          */
        size_t y2idx(double y) const
        {
                int idx = mrpt::utils::round((y - m_yMin) / m_resolutionXY);
                ASSERT_(idx >= 0 && idx < static_cast<int>(m_sizeY));
                return idx;
        }

        /** Returns "indexes" from coordinates:
          */
        size_t phi2idx(double phi) const
        {
                int idx = mrpt::utils::round((phi - m_phiMin) / m_resolutionPhi);
                ASSERT_(idx >= 0 && idx < static_cast<int>(m_sizePhi));
                return idx;
        }

        /** Returns coordinates from "indexes":
          */
        double idx2x(size_t x) const
        {
                ASSERT_(x < m_sizeX);
                return m_xMin + x * m_resolutionXY;
        }

        /** Returns coordinates from "indexes":
          */
        double idx2y(size_t y) const
        {
                ASSERT_(y < m_sizeY);
                return m_yMin + y * m_resolutionXY;
        }

        /** Returns coordinates from "indexes":
          */
        double idx2phi(size_t phi) const
        {
                ASSERT_(phi < m_sizePhi);
                return m_phiMin + phi * m_resolutionPhi;
        }

        /** Default constructor:
          */
        CPose2DGridTemplate(
                double xMin = -1.0f, double xMax = 1.0f, double yMin = -1.0f,
                double yMax = 1.0f, double resolutionXY = 0.5f,
                double resolutionPhi = mrpt::utils::DEG2RAD(180),
                double phiMin = -M_PIf, double phiMax = M_PIf)
                : m_xMin(),
                  m_xMax(),
                  m_yMin(),
                  m_yMax(),
                  m_phiMin(),
                  m_phiMax(),
                  m_resolutionXY(),
                  m_resolutionPhi(),
                  m_sizeX(),
                  m_sizeY(),
                  m_sizePhi(),
                  m_sizeXY(),
                  m_idxLeftX(),
                  m_idxLeftY(),
                  m_idxLeftPhi(),
                  m_data()
        {
                setSize(
                        xMin, xMax, yMin, yMax, resolutionXY, resolutionPhi, phiMin,
                        phiMax);
        }

        virtual ~CPose2DGridTemplate() {}
        /** Changes the limits and size of the grid, erasing previous contents:
          */
        void setSize(
                double xMin, double xMax, double yMin, double yMax, double resolutionXY,
                double resolutionPhi, double phiMin = -M_PIf, double phiMax = M_PIf)
        {
                // Checks
                ASSERT_(xMax > xMin);
                ASSERT_(yMax > yMin);
                ASSERT_(phiMax >= phiMin);
                ASSERT_(resolutionXY > 0);
                ASSERT_(resolutionPhi > 0);

                // Copy data:
                m_xMin = xMin;
                m_xMax = xMax;
                m_yMin = yMin;
                m_yMax = yMax;
                m_phiMin = phiMin;
                m_phiMax = phiMax;
                m_resolutionXY = resolutionXY;
                m_resolutionPhi = resolutionPhi;

                // Compute the indexes of the starting borders:
                m_idxLeftX = mrpt::utils::round(xMin / resolutionXY);
                m_idxLeftY = mrpt::utils::round(yMin / resolutionXY);
                m_idxLeftPhi = mrpt::utils::round(phiMin / resolutionPhi);

                // Compute new required space:
                m_sizeX = mrpt::utils::round(xMax / resolutionXY) - m_idxLeftX + 1;
                m_sizeY = mrpt::utils::round(yMax / resolutionXY) - m_idxLeftY + 1;
                m_sizePhi =
                        mrpt::utils::round(phiMax / resolutionPhi) - m_idxLeftPhi + 1;
                m_sizeXY = m_sizeX * m_sizeY;

                // Resize "m_data":
                m_data.clear();
                m_data.resize(m_sizeX * m_sizeY * m_sizePhi);
        }

        /** Reads the contents of a cell
          */
        const T* getByPos(double x, double y, double phi) const
        {
                return getByIndex(x2idx(x), y2idx(y), phi2idx(phi));
        }

        /** Reads the contents of a cell
          */
        T* getByPos(double x, double y, double phi)
        {
                return getByIndex(x2idx(x), y2idx(y), phi2idx(phi));
        }

        /** Reads the contents of a cell
          */
        const T* getByIndex(size_t x, size_t y, size_t phi) const
        {
                ASSERT_(x < m_sizeX && y < m_sizeY && phi < m_sizePhi)
                return &m_data[phi * m_sizeXY + y * m_sizeX + x];
        }

        /** Reads the contents of a cell
          */
        T* getByIndex(size_t x, size_t y, size_t phi)
        {
                ASSERT_(x < m_sizeX && y < m_sizeY && phi < m_sizePhi)
                return &m_data[phi * m_sizeXY + y * m_sizeX + x];
        }

        /** Returns the whole grid as a matrix, for a given constant "phi" and where
         * each row contains values for a fixed "y".
          */
        template <class MATRIXLIKE>
        void getAsMatrix(const double& phi, MATRIXLIKE& outMat)
        {
                MRPT_START
                outMat.setSize(m_sizeY, m_sizeX);
                size_t phiIdx = phi2idx(phi);
                ASSERT_(phi < m_sizePhi);
                for (size_t y = 0; y < m_sizeY; y++)
                        for (size_t x = 0; x < m_sizeX; x++)
                                outMat(y, x) = m_data[phiIdx * m_sizeXY + y * m_sizeX + x];
                MRPT_END
        }

        /** Get info about the grid:
          */
        double getXMin() const { return m_xMin; }
        double getXMax() const { return m_xMax; }
        double getYMin() const { return m_yMin; }
        double getYMax() const { return m_yMax; }
        double getPhiMin() const { return m_phiMin; }
        double getPhiMax() const { return m_phiMax; }
        double getResolutionXY() const { return m_resolutionXY; }
        double getResolutionPhi() const { return m_resolutionPhi; }
        size_t getSizeX() const { return m_sizeX; }
        size_t getSizeY() const { return m_sizeY; }
        size_t getSizePhi() const { return m_sizePhi; }
};  // End of class def.

}  // End of namespace
}  // End of namespace

#endif