Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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 #ifndef CPose2DGridTemplate_H
10 #define CPose2DGridTemplate_H
11 
13 #include <mrpt/utils/round.h> // for round()
14 #include <mrpt/utils/bits.h> // for DEG2RAD()
15 
16 namespace mrpt
17 {
18 namespace poses
19 {
20  /** This is a template class for storing a 3D (2D+heading) grid containing any 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  */
29  double m_xMin, m_xMax,
30  m_yMin, m_yMax,
33 
34  /** The size of "m_data" is m_sizeX * m_sizeY * m_sizePhi
35  */
37 
38  /** The indexes of the "left" borders:
39  */
41 
42  /** The data:
43  */
44  std::vector<T> m_data;
45 
46  public:
47  /** Returns "indexes" from coordinates:
48  */
49  size_t x2idx(double x) const
50  {
51  int idx = mrpt::utils::round( (x-m_xMin) / m_resolutionXY );
52  ASSERT_(idx>=0 && idx< static_cast<int>(m_sizeX));
53  return idx;
54  }
55 
56  /** Returns "indexes" from coordinates:
57  */
58  size_t y2idx(double y) const
59  {
60  int idx = mrpt::utils::round( (y-m_yMin) / m_resolutionXY );
61  ASSERT_(idx>=0 && idx< static_cast<int>(m_sizeY));
62  return idx;
63  }
64 
65  /** Returns "indexes" from coordinates:
66  */
67  size_t phi2idx(double phi) const
68  {
69  int idx = mrpt::utils::round( (phi-m_phiMin) / m_resolutionPhi );
70  ASSERT_(idx>=0 && idx< static_cast<int>(m_sizePhi) );
71  return idx;
72  }
73 
74  /** Returns coordinates from "indexes":
75  */
76  double idx2x(size_t x) const
77  {
78  ASSERT_(x<m_sizeX);
79  return m_xMin + x * m_resolutionXY;
80  }
81 
82  /** Returns coordinates from "indexes":
83  */
84  double idx2y(size_t y) const
85  {
86  ASSERT_(y<m_sizeY);
87  return m_yMin + y * m_resolutionXY;
88  }
89 
90  /** Returns coordinates from "indexes":
91  */
92  double idx2phi(size_t phi) const
93  {
94  ASSERT_(phi<m_sizePhi);
95  return m_phiMin + phi * m_resolutionPhi;
96  }
97 
98  /** Default constructor:
99  */
101  double xMin = -1.0f,
102  double xMax = 1.0f,
103  double yMin = -1.0f,
104  double yMax = 1.0f,
105  double resolutionXY = 0.5f,
106  double resolutionPhi = mrpt::utils::DEG2RAD(180),
107  double phiMin = -M_PIf,
108  double phiMax = M_PIf
109  ) :
110  m_xMin(), m_xMax(),
111  m_yMin(), m_yMax(),
112  m_phiMin(), m_phiMax(),
116  m_data()
117  {
118  setSize(xMin,xMax,yMin,yMax,resolutionXY,resolutionPhi,phiMin,phiMax);
119  }
120 
121  virtual ~CPose2DGridTemplate() { }
122 
123 
124  /** Changes the limits and size of the grid, erasing previous contents:
125  */
126  void setSize(
127  double xMin,
128  double xMax,
129  double yMin,
130  double yMax,
131  double resolutionXY,
132  double resolutionPhi,
133  double phiMin = -M_PIf,
134  double phiMax = M_PIf
135  )
136  {
137  // Checks
138  ASSERT_( xMax > xMin );
139  ASSERT_( yMax > yMin );
140  ASSERT_( phiMax >= phiMin );
141  ASSERT_( resolutionXY>0 );
142  ASSERT_( resolutionPhi>0 );
143 
144  // Copy data:
145  m_xMin = xMin; m_xMax = xMax;
146  m_yMin = yMin; m_yMax = yMax;
147  m_phiMin = phiMin; m_phiMax = phiMax;
148  m_resolutionXY = resolutionXY;
149  m_resolutionPhi = resolutionPhi;
150 
151  // Compute the indexes of the starting borders:
152  m_idxLeftX = mrpt::utils::round( xMin/resolutionXY ) ;
153  m_idxLeftY = mrpt::utils::round( yMin/resolutionXY ) ;
154  m_idxLeftPhi = mrpt::utils::round( phiMin/resolutionPhi ) ;
155 
156  // Compute new required space:
157  m_sizeX = mrpt::utils::round( xMax/resolutionXY ) - m_idxLeftX + 1;
158  m_sizeY = mrpt::utils::round( yMax/resolutionXY ) - m_idxLeftY + 1;
159  m_sizePhi = mrpt::utils::round( phiMax/resolutionPhi ) - m_idxLeftPhi + 1;
161 
162  // Resize "m_data":
163  m_data.clear();
164  m_data.resize( m_sizeX * m_sizeY * m_sizePhi );
165  }
166 
167  /** Reads the contents of a cell
168  */
169  const T* getByPos( double x,double y, double phi ) const
170  {
171  return getByIndex( x2idx(x),y2idx(y),phi2idx(phi) );
172  }
173 
174  /** Reads the contents of a cell
175  */
176  T* getByPos( double x,double y, double phi )
177  {
178  return getByIndex( x2idx(x),y2idx(y),phi2idx(phi) );
179  }
180 
181  /** Reads the contents of a cell
182  */
183  const T* getByIndex( size_t x,size_t y, size_t phi ) const
184  {
185  ASSERT_(x<m_sizeX && y<m_sizeY && phi<m_sizePhi)
186  return &m_data[ phi*m_sizeXY + y*m_sizeX + x ];
187  }
188 
189  /** Reads the contents of a cell
190  */
191  T* getByIndex( size_t x,size_t y, size_t phi )
192  {
193  ASSERT_(x<m_sizeX && y<m_sizeY && phi<m_sizePhi)
194  return &m_data[ phi*m_sizeXY + y*m_sizeX + x ];
195  }
196 
197  /** Returns the whole grid as a matrix, for a given constant "phi" and where each row contains values for a fixed "y".
198  */
199  template <class MATRIXLIKE>
200  void getAsMatrix( const double &phi, MATRIXLIKE &outMat )
201  {
202  MRPT_START
203  outMat.setSize( m_sizeY, m_sizeX );
204  size_t phiIdx = phi2idx(phi);
205  ASSERT_(phi<m_sizePhi);
206  for (size_t y=0;y<m_sizeY;y++)
207  for (size_t x=0;x<m_sizeX;x++)
208  outMat(y,x)=m_data[ phiIdx*m_sizeXY + y*m_sizeX + x ];
209  MRPT_END
210  }
211 
212  /** Get info about the grid:
213  */
214  double getXMin() const { return m_xMin; }
215  double getXMax() const { return m_xMax; }
216  double getYMin() const { return m_yMin; }
217  double getYMax() const { return m_yMax; }
218  double getPhiMin() const { return m_phiMin; }
219  double getPhiMax() const { return m_phiMax; }
220  double getResolutionXY() const { return m_resolutionXY; }
221  double getResolutionPhi() const { return m_resolutionPhi; }
222  size_t getSizeX() const { return m_sizeX; }
223  size_t getSizeY() const { return m_sizeY; }
224  size_t getSizePhi() const { return m_sizePhi; }
225 
226 
227  }; // End of class def.
228 
229  } // End of namespace
230 } // End of namespace
231 
232 #endif
double getXMin() const
Get info about the grid:
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.
void setSize(double xMin, double xMax, double yMin, double yMax, double resolutionXY, double resolutionPhi, double phiMin=-M_PIf, double phiMax=M_PIf)
Changes the limits and size of the grid, erasing previous contents:
#define M_PIf
#define MRPT_END
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":
#define MRPT_START
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 ASSERT_(f)
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
GLenum GLint GLint y
Definition: glext.h:3516
double idx2x(size_t x) const
Returns coordinates from "indexes":
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)
Default constructor:
GLenum GLint x
Definition: glext.h:3516
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.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019