MRPT  2.0.3
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
mrpt::poses::CPose2DGridTemplate::getResolutionPhi
double getResolutionPhi() const
Definition: CPose2DGridTemplate.h:203
mrpt::poses::CPose2DGridTemplate::getYMax
double getYMax() const
Definition: CPose2DGridTemplate.h:199
mrpt::poses::CPose2DGridTemplate::m_phiMin
double m_phiMin
Definition: CPose2DGridTemplate.h:27
mrpt::poses::CPose2DGridTemplate::getByPos
T * getByPos(double x, double y, double phi)
Reads the contents of a cell.
Definition: CPose2DGridTemplate.h:157
mrpt::poses::CPose2DGridTemplate::m_resolutionPhi
double m_resolutionPhi
Definition: CPose2DGridTemplate.h:28
mrpt::poses::CPose2DGridTemplate::setSize
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:
Definition: CPose2DGridTemplate.h:111
mrpt::poses::CPose2DGridTemplate::m_data
std::vector< T > m_data
The data:
Definition: CPose2DGridTemplate.h:40
mrpt::poses::CPose2DGridTemplate::idx2x
double idx2x(size_t x) const
Returns coordinates from "indexes":
Definition: CPose2DGridTemplate.h:72
mrpt::poses::CPose2DGridTemplate::m_idxLeftPhi
int m_idxLeftPhi
Definition: CPose2DGridTemplate.h:36
mrpt::poses::CPose2DGridTemplate::m_idxLeftY
int m_idxLeftY
Definition: CPose2DGridTemplate.h:36
mrpt::poses::CPose2DGridTemplate::getXMin
double getXMin() const
Get info about the grid:
Definition: CPose2DGridTemplate.h:196
mrpt::poses::CPose2DGridTemplate::getYMin
double getYMin() const
Definition: CPose2DGridTemplate.h:198
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::poses::CPose2DGridTemplate::getByIndex
const T * getByIndex(size_t x, size_t y, size_t phi) const
Reads the contents of a cell.
Definition: CPose2DGridTemplate.h:164
mrpt::poses::CPose2DGridTemplate::~CPose2DGridTemplate
virtual ~CPose2DGridTemplate()=default
mrpt::poses::CPose2DGridTemplate::m_sizeXY
size_t m_sizeXY
Definition: CPose2DGridTemplate.h:32
mrpt::poses::CPose2DGridTemplate::getPhiMax
double getPhiMax() const
Definition: CPose2DGridTemplate.h:201
mrpt::poses::CPose2DGridTemplate::m_idxLeftX
int m_idxLeftX
The indexes of the "left" borders:
Definition: CPose2DGridTemplate.h:36
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::poses::CPose2DGridTemplate::getByPos
const T * getByPos(double x, double y, double phi) const
Reads the contents of a cell.
Definition: CPose2DGridTemplate.h:150
mrpt::poses::CPose2DGridTemplate::m_sizeX
size_t m_sizeX
The size of "m_data" is m_sizeX * m_sizeY * m_sizePhi.
Definition: CPose2DGridTemplate.h:32
mrpt::poses::CPose2DGridTemplate::getResolutionXY
double getResolutionXY() const
Definition: CPose2DGridTemplate.h:202
round.h
mrpt::poses::CPose2DGridTemplate::getByIndex
T * getByIndex(size_t x, size_t y, size_t phi)
Reads the contents of a cell.
Definition: CPose2DGridTemplate.h:172
mrpt::poses::CPose2DGridTemplate
This is a template class for storing a 3D (2D+heading) grid containing any kind of data.
Definition: CPose2DGridTemplate.h:22
mrpt::poses::CPose2DGridTemplate::getAsMatrix
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...
Definition: CPose2DGridTemplate.h:182
mrpt::poses::CPose2DGridTemplate::m_xMax
double m_xMax
Definition: CPose2DGridTemplate.h:27
mrpt::poses::CPose2DGridTemplate::m_xMin
double m_xMin
The limits and resolution of the grid:
Definition: CPose2DGridTemplate.h:27
mrpt::poses::CPose2DGridTemplate::CPose2DGridTemplate
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:
Definition: CPose2DGridTemplate.h:96
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
mrpt::poses::CPose2DGridTemplate::getSizePhi
size_t getSizePhi() const
Definition: CPose2DGridTemplate.h:206
mrpt::poses::CPose2DGridTemplate::getPhiMin
double getPhiMin() const
Definition: CPose2DGridTemplate.h:200
mrpt::poses::CPose2DGridTemplate::getSizeX
size_t getSizeX() const
Definition: CPose2DGridTemplate.h:204
bits_math.h
mrpt::poses::CPose2DGridTemplate::m_yMax
double m_yMax
Definition: CPose2DGridTemplate.h:27
mrpt::poses::CPose2DGridTemplate::getSizeY
size_t getSizeY() const
Definition: CPose2DGridTemplate.h:205
mrpt::poses::CPose2DGridTemplate::idx2y
double idx2y(size_t y) const
Returns coordinates from "indexes":
Definition: CPose2DGridTemplate.h:80
M_PI
#define M_PI
Definition: core/include/mrpt/core/bits_math.h:43
mrpt::poses::CPose2DGridTemplate::y2idx
size_t y2idx(double y) const
Returns "indexes" from coordinates:
Definition: CPose2DGridTemplate.h:54
mrpt::poses::CPose2DGridTemplate::m_yMin
double m_yMin
Definition: CPose2DGridTemplate.h:27
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::poses::CPose2DGridTemplate::m_phiMax
double m_phiMax
Definition: CPose2DGridTemplate.h:27
mrpt::poses::CPose2DGridTemplate::m_resolutionXY
double m_resolutionXY
Definition: CPose2DGridTemplate.h:27
mrpt::poses::CPose2DGridTemplate::getXMax
double getXMax() const
Definition: CPose2DGridTemplate.h:197
mrpt::poses::CPose2DGridTemplate::m_sizeY
size_t m_sizeY
Definition: CPose2DGridTemplate.h:32
CSerializable.h
mrpt::poses::CPose2DGridTemplate::idx2phi
double idx2phi(size_t phi) const
Returns coordinates from "indexes":
Definition: CPose2DGridTemplate.h:88
mrpt::poses::CPose2DGridTemplate::m_sizePhi
size_t m_sizePhi
Definition: CPose2DGridTemplate.h:32
mrpt::poses::CPose2DGridTemplate::x2idx
size_t x2idx(double x) const
Returns "indexes" from coordinates:
Definition: CPose2DGridTemplate.h:45
mrpt::poses::CPose2DGridTemplate::phi2idx
size_t phi2idx(double phi) const
Returns "indexes" from coordinates:
Definition: CPose2DGridTemplate.h:63



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020