Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
16namespace mrpt
17{
18namespace 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,
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 {
79 return m_xMin + x * m_resolutionXY;
80 }
81
82 /** Returns coordinates from "indexes":
83 */
84 double idx2y(size_t y) const
85 {
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
122
123
124 /** Changes the limits and size of the grid, erasing previous contents:
125 */
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 {
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 ];
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
This is a template class for storing a 3D (2D+heading) grid containing any kind of data.
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:
size_t m_sizeX
The size of "m_data" is m_sizeX * m_sizeY * m_sizePhi.
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:
size_t y2idx(double y) const
Returns "indexes" from coordinates:
double getXMin() const
Get info about the grid:
size_t x2idx(double x) const
Returns "indexes" from coordinates:
T * getByIndex(size_t x, size_t y, size_t phi)
Reads the contents of a cell.
size_t phi2idx(double phi) const
Returns "indexes" from coordinates:
double idx2y(size_t y) const
Returns coordinates from "indexes":
std::vector< T > m_data
The data:
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...
T * getByPos(double x, double y, double phi)
Reads the contents of a cell.
double idx2phi(size_t phi) const
Returns coordinates from "indexes":
int m_idxLeftX
The indexes of the "left" borders:
double m_xMin
The limits and resolution of the grid:
double idx2x(size_t x) const
Returns coordinates from "indexes":
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
#define MRPT_START
Definition: mrpt_macros.h:349
#define ASSERT_(f)
Definition: mrpt_macros.h:261
#define MRPT_END
Definition: mrpt_macros.h:353
#define M_PIf
Definition: mrpt_macros.h:366
double DEG2RAD(const double x)
Degrees to radians.
Definition: bits.h:69
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.



Page generated by Doxygen 1.9.2 for MRPT 1.4.0 SVN: at Mon Sep 20 00:21:41 UTC 2021