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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019