Main MRPT website > C++ reference
MRPT logo
CPose2DGridTemplate.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CPose2DGridTemplate_H
29 #define CPose2DGridTemplate_H
30 
31 #include <mrpt/utils/CSerializable.h>
32 
33 namespace mrpt
34 {
35 namespace poses
36 {
37  using namespace mrpt::math;
38  using namespace mrpt::utils;
39 
40  /** This is a template class for storing a 3D (2D+heading) grid containing any kind of data.
41  * \ingroup poses_pdf_grp
42  */
43  template<class T>
44  class CPose2DGridTemplate
45  {
46  protected:
47  /** The limits and resolution of the grid:
48  */
49  double m_xMin, m_xMax,
50  m_yMin, m_yMax,
51  m_phiMin, m_phiMax,
52  m_resolutionXY,m_resolutionPhi;
53 
54  /** The size of "m_data" is m_sizeX ·m_sizeY ·m_sizePhi
55  */
56  size_t m_sizeX,m_sizeY,m_sizePhi, m_sizeXY;
57 
58  /** The indexes of the "left" borders:
59  */
60  int m_idxLeftX, m_idxLeftY, m_idxLeftPhi;
61 
62  /** The data:
63  */
64  std::vector<T> m_data;
65 
66  public:
67  /** Returns "indexes" from coordinates:
68  */
69  size_t x2idx(double x) const
70  {
71  int idx = round( (x-m_xMin) / m_resolutionXY );
72  ASSERT_(idx>=0 && idx< static_cast<int>(m_sizeX));
73  return idx;
74  }
75 
76  /** Returns "indexes" from coordinates:
77  */
78  size_t y2idx(double y) const
79  {
80  int idx = round( (y-m_yMin) / m_resolutionXY );
81  ASSERT_(idx>=0 && idx< static_cast<int>(m_sizeY));
82  return idx;
83  }
84 
85  /** Returns "indexes" from coordinates:
86  */
87  size_t phi2idx(double phi) const
88  {
89  int idx = round( (phi-m_phiMin) / m_resolutionPhi );
90  ASSERT_(idx>=0 && idx< static_cast<int>(m_sizePhi) );
91  return idx;
92  }
93 
94  /** Returns coordinates from "indexes":
95  */
96  double idx2x(size_t x) const
97  {
98  ASSERT_(x<m_sizeX);
99  return m_xMin + x * m_resolutionXY;
100  }
101 
102  /** Returns coordinates from "indexes":
103  */
104  double idx2y(size_t y) const
105  {
106  ASSERT_(y<m_sizeY);
107  return m_yMin + y * m_resolutionXY;
108  }
109 
110  /** Returns coordinates from "indexes":
111  */
112  double idx2phi(size_t phi) const
113  {
114  ASSERT_(phi<m_sizePhi);
115  return m_phiMin + phi * m_resolutionPhi;
116  }
117 
118  /** Default constructor:
119  */
120  CPose2DGridTemplate(
121  double xMin = -1.0f,
122  double xMax = 1.0f,
123  double yMin = -1.0f,
124  double yMax = 1.0f,
125  double resolutionXY = 0.5f,
126  double resolutionPhi = DEG2RAD(180),
127  double phiMin = -M_PIf,
128  double phiMax = M_PIf
129  ) :
130  m_xMin(), m_xMax(),
131  m_yMin(), m_yMax(),
132  m_phiMin(), m_phiMax(),
133  m_resolutionXY(),m_resolutionPhi(),
134  m_sizeX(),m_sizeY(),m_sizePhi(), m_sizeXY(),
135  m_idxLeftX(), m_idxLeftY(), m_idxLeftPhi(),
136  m_data()
137  {
138  setSize(xMin,xMax,yMin,yMax,resolutionXY,resolutionPhi,phiMin,phiMax);
139  }
140 
141  virtual ~CPose2DGridTemplate() { }
142 
143 
144  /** Changes the limits and size of the grid, erasing previous contents:
145  */
146  void setSize(
147  double xMin,
148  double xMax,
149  double yMin,
150  double yMax,
151  double resolutionXY,
152  double resolutionPhi,
153  double phiMin = -M_PIf,
154  double phiMax = M_PIf
155  )
156  {
157  // Checks
158  ASSERT_( xMax > xMin );
159  ASSERT_( yMax > yMin );
160  ASSERT_( phiMax >= phiMin );
161  ASSERT_( resolutionXY>0 );
162  ASSERT_( resolutionPhi>0 );
163 
164  // Copy data:
165  m_xMin = xMin; m_xMax = xMax;
166  m_yMin = yMin; m_yMax = yMax;
167  m_phiMin = phiMin; m_phiMax = phiMax;
168  m_resolutionXY = resolutionXY;
169  m_resolutionPhi = resolutionPhi;
170 
171  // Compute the indexes of the starting borders:
172  m_idxLeftX = round( xMin/resolutionXY ) ;
173  m_idxLeftY = round( yMin/resolutionXY ) ;
174  m_idxLeftPhi = round( phiMin/resolutionPhi ) ;
175 
176  // Compute new required space:
177  m_sizeX = round( xMax/resolutionXY ) - m_idxLeftX + 1;
178  m_sizeY = round( yMax/resolutionXY ) - m_idxLeftY + 1;
179  m_sizePhi = round( phiMax/resolutionPhi ) - m_idxLeftPhi + 1;
180  m_sizeXY = m_sizeX * m_sizeY;
181 
182  // Resize "m_data":
183  m_data.clear();
184  m_data.resize( m_sizeX * m_sizeY * m_sizePhi );
185  }
186 
187  /** Reads the contents of a cell
188  */
189  const T* getByPos( double x,double y, double phi ) const
190  {
191  return getByIndex( x2idx(x),y2idx(y),phi2idx(phi) );
192  }
193 
194  /** Reads the contents of a cell
195  */
196  T* getByPos( double x,double y, double phi )
197  {
198  return getByIndex( x2idx(x),y2idx(y),phi2idx(phi) );
199  }
200 
201  /** Reads the contents of a cell
202  */
203  const T* getByIndex( size_t x,size_t y, size_t phi ) const
204  {
205  ASSERT_(x>=0 && x<m_sizeX);
206  ASSERT_(y>=0 && y<m_sizeY);
207  ASSERT_(phi>=0 && phi<m_sizePhi);
208  return &m_data[ phi*m_sizeXY + y*m_sizeX + x ];
209  }
210 
211  /** Reads the contents of a cell
212  */
213  T* getByIndex( size_t x,size_t y, size_t phi )
214  {
215  ASSERT_(x>=0 && x<m_sizeX);
216  ASSERT_(y>=0 && y<m_sizeY);
217  ASSERT_(phi>=0 && phi<m_sizePhi);
218  return &m_data[ phi*m_sizeXY + y*m_sizeX + x ];
219  }
220 
221  /** Returns the whole grid as a matrix, for a given constant "phi" and where each row contains values for a fixed "y".
222  */
223  void getAsMatrix( const double &phi, CMatrixTemplate<T> &outMat )
224  {
225  MRPT_START
226  outMat.setSize( m_sizeY, m_sizeX );
227  size_t phiIdx = phi2idx(phi);
228  ASSERT_(phi<m_sizePhi);
229  for (size_t y=0;y<m_sizeY;y++)
230  for (size_t x=0;x<m_sizeX;x++)
231  outMat(y,x)=m_data[ phiIdx*m_sizeXY + y*m_sizeX + x ];
232  MRPT_END
233  }
234 
235  /** Get info about the grid:
236  */
237  double getXMin() const { return m_xMin; }
238  double getXMax() const { return m_xMax; }
239  double getYMin() const { return m_yMin; }
240  double getYMax() const { return m_yMax; }
241  double getPhiMin() const { return m_phiMin; }
242  double getPhiMax() const { return m_phiMax; }
243  double getResolutionXY() const { return m_resolutionXY; }
244  double getResolutionPhi() const { return m_resolutionPhi; }
245  size_t getSizeX() const { return m_sizeX; }
246  size_t getSizeY() const { return m_sizeY; }
247  size_t getSizePhi() const { return m_sizePhi; }
248 
249 
250  }; // End of class def.
251 
252  } // End of namespace
253 } // End of namespace
254 
255 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013