MRPT  2.0.4
CPose2DGridTemplate.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/core/bits_math.h> // for .0_deg
12 #include <mrpt/core/round.h> // for round()
14 
15 namespace mrpt::poses
16 {
17 /** This is a template class for storing a 3D (2D+heading) grid containing any
18  * kind of data.
19  * \ingroup poses_pdf_grp
20  */
21 template <class T>
23 {
24  protected:
25  /** The limits and resolution of the grid:
26  */
29 
30  /** The size of "m_data" is m_sizeX * m_sizeY * m_sizePhi
31  */
33 
34  /** The indexes of the "left" borders:
35  */
37 
38  /** The data:
39  */
40  std::vector<T> m_data;
41 
42  public:
43  /** Returns "indexes" from coordinates:
44  */
45  size_t x2idx(double x) const
46  {
47  int idx = mrpt::round((x - m_xMin) / m_resolutionXY);
48  ASSERT_(idx >= 0 && idx < static_cast<int>(m_sizeX));
49  return idx;
50  }
51 
52  /** Returns "indexes" from coordinates:
53  */
54  size_t y2idx(double y) const
55  {
56  int idx = mrpt::round((y - m_yMin) / m_resolutionXY);
57  ASSERT_(idx >= 0 && idx < static_cast<int>(m_sizeY));
58  return idx;
59  }
60 
61  /** Returns "indexes" from coordinates:
62  */
63  size_t phi2idx(double phi) const
64  {
65  int idx = mrpt::round((phi - m_phiMin) / m_resolutionPhi);
66  ASSERT_(idx >= 0 && idx < static_cast<int>(m_sizePhi));
67  return idx;
68  }
69 
70  /** Returns coordinates from "indexes":
71  */
72  double idx2x(size_t x) const
73  {
74  ASSERT_(x < m_sizeX);
75  return m_xMin + x * m_resolutionXY;
76  }
77 
78  /** Returns coordinates from "indexes":
79  */
80  double idx2y(size_t y) const
81  {
82  ASSERT_(y < m_sizeY);
83  return m_yMin + y * m_resolutionXY;
84  }
85 
86  /** Returns coordinates from "indexes":
87  */
88  double idx2phi(size_t phi) const
89  {
90  ASSERT_(phi < m_sizePhi);
91  return m_phiMin + phi * m_resolutionPhi;
92  }
93 
94  /** Default constructor:
95  */
97  double xMin = -1.0f, double xMax = 1.0f, double yMin = -1.0f,
98  double yMax = 1.0f, double resolutionXY = 0.5f,
99  double resolutionPhi = mrpt::DEG2RAD(180.0), double phiMin = -M_PI,
100  double phiMax = M_PI)
101  : m_data()
102  {
103  setSize(
104  xMin, xMax, yMin, yMax, resolutionXY, resolutionPhi, phiMin,
105  phiMax);
106  }
107 
108  virtual ~CPose2DGridTemplate() = default;
109  /** Changes the limits and size of the grid, erasing previous contents:
110  */
111  void setSize(
112  double xMin, double xMax, double yMin, double yMax, double resolutionXY,
113  double resolutionPhi, double phiMin = -M_PI, double phiMax = M_PI)
114  {
115  // Checks
116  ASSERT_(xMax > xMin);
117  ASSERT_(yMax > yMin);
118  ASSERT_(phiMax >= phiMin);
119  ASSERT_(resolutionXY > 0);
120  ASSERT_(resolutionPhi > 0);
121 
122  // Copy data:
123  m_xMin = xMin;
124  m_xMax = xMax;
125  m_yMin = yMin;
126  m_yMax = yMax;
127  m_phiMin = phiMin;
128  m_phiMax = phiMax;
129  m_resolutionXY = resolutionXY;
130  m_resolutionPhi = resolutionPhi;
131 
132  // Compute the indexes of the starting borders:
133  m_idxLeftX = mrpt::round(xMin / resolutionXY);
134  m_idxLeftY = mrpt::round(yMin / resolutionXY);
135  m_idxLeftPhi = mrpt::round(phiMin / resolutionPhi);
136 
137  // Compute new required space:
138  m_sizeX = mrpt::round(xMax / resolutionXY) - m_idxLeftX + 1;
139  m_sizeY = mrpt::round(yMax / resolutionXY) - m_idxLeftY + 1;
140  m_sizePhi = mrpt::round(phiMax / resolutionPhi) - m_idxLeftPhi + 1;
142 
143  // Resize "m_data":
144  m_data.clear();
145  m_data.resize(m_sizeX * m_sizeY * m_sizePhi);
146  }
147 
148  /** Reads the contents of a cell
149  */
150  const T* getByPos(double x, double y, double phi) const
151  {
152  return getByIndex(x2idx(x), y2idx(y), phi2idx(phi));
153  }
154 
155  /** Reads the contents of a cell
156  */
157  T* getByPos(double x, double y, double phi)
158  {
159  return getByIndex(x2idx(x), y2idx(y), phi2idx(phi));
160  }
161 
162  /** Reads the contents of a cell
163  */
164  const T* getByIndex(size_t x, size_t y, size_t phi) const
165  {
166  ASSERT_(x < m_sizeX && y < m_sizeY && phi < m_sizePhi);
167  return &m_data[phi * m_sizeXY + y * m_sizeX + x];
168  }
169 
170  /** Reads the contents of a cell
171  */
172  T* getByIndex(size_t x, size_t y, size_t phi)
173  {
174  ASSERT_(x < m_sizeX && y < m_sizeY && phi < m_sizePhi);
175  return &m_data[phi * m_sizeXY + y * m_sizeX + x];
176  }
177 
178  /** Returns the whole grid as a matrix, for a given constant "phi" and where
179  * each row contains values for a fixed "y".
180  */
181  template <class MATRIXLIKE>
182  void getAsMatrix(double phi, MATRIXLIKE& outMat)
183  {
184  MRPT_START
185  outMat.setSize(m_sizeY, m_sizeX);
186  size_t phiIdx = phi2idx(phi);
187  ASSERT_(phi < m_sizePhi);
188  for (size_t y = 0; y < m_sizeY; y++)
189  for (size_t x = 0; x < m_sizeX; x++)
190  outMat(y, x) = m_data[phiIdx * m_sizeXY + y * m_sizeX + x];
191  MRPT_END
192  }
193 
194  /** Get info about the grid:
195  */
196  double getXMin() const { return m_xMin; }
197  double getXMax() const { return m_xMax; }
198  double getYMin() const { return m_yMin; }
199  double getYMax() const { return m_yMax; }
200  double getPhiMin() const { return m_phiMin; }
201  double getPhiMax() const { return m_phiMax; }
202  double getResolutionXY() const { return m_resolutionXY; }
203  double getResolutionPhi() const { return m_resolutionPhi; }
204  size_t getSizeX() const { return m_sizeX; }
205  size_t getSizeY() const { return m_sizeY; }
206  size_t getSizePhi() const { return m_sizePhi; }
207 }; // End of class def.
208 
209 } // namespace mrpt::poses
CPose2DGridTemplate(double xMin=-1.0f, double xMax=1.0f, double yMin=-1.0f, double yMax=1.0f, double resolutionXY=0.5f, double resolutionPhi=mrpt::DEG2RAD(180.0), double phiMin=-M_PI, double phiMax=M_PI)
Default constructor:
double getXMin() const
Get info about the grid:
#define MRPT_START
Definition: exceptions.h:241
void setSize(double xMin, double xMax, double yMin, double yMax, double resolutionXY, double resolutionPhi, double phiMin=-M_PI, double phiMax=M_PI)
Changes the limits and size of the grid, erasing previous contents:
virtual ~CPose2DGridTemplate()=default
size_t m_sizeX
The size of "m_data" is m_sizeX * m_sizeY * m_sizePhi.
double idx2y(size_t y) const
Returns coordinates from "indexes":
const T * getByPos(double x, double y, double phi) const
Reads the contents of a cell.
std::vector< T > m_data
The data:
const T * getByIndex(size_t x, size_t y, size_t phi) const
Reads the contents of a cell.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
constexpr double DEG2RAD(const double x)
Degrees to radians.
void getAsMatrix(double phi, MATRIXLIKE &outMat)
Returns the whole grid as a matrix, for a given constant "phi" and where each row contains values for...
size_t phi2idx(double phi) const
Returns "indexes" from coordinates:
size_t x2idx(double x) const
Returns "indexes" from coordinates:
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
T * getByPos(double x, double y, double phi)
Reads the contents of a cell.
double idx2phi(size_t phi) const
Returns coordinates from "indexes":
size_t y2idx(double y) const
Returns "indexes" from coordinates:
double m_xMin
The limits and resolution of the grid:
int m_idxLeftX
The indexes of the "left" borders:
#define MRPT_END
Definition: exceptions.h:245
double idx2x(size_t x) const
Returns coordinates from "indexes":
This is a template class for storing a 3D (2D+heading) grid containing any kind of data...
T * getByIndex(size_t x, size_t y, size_t phi)
Reads the contents of a cell.
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020