MRPT  2.0.1
CPose3DGridTemplate.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()
13 #include <mrpt/math/TPose3D.h>
15 
16 namespace mrpt::poses
17 {
18 /** This is a template class for storing a 6-dimensional grid, with components
19  * corresponding to Euler angle parameterization of SE(3) poses. \ingroup
20  * poses_pdf_grp
21  */
22 template <class T>
24 {
25  protected:
27 
28  mrpt::math::TPose3D m_bb_min{-1., -1., -1., -M_PI, -.5 * M_PI, -.5 * M_PI};
29  mrpt::math::TPose3D m_bb_max{+1., +1., +1., +M_PI, +.5 * M_PI, +.5 * M_PI};
30 
31  /** Resolution of the grid */
33 
34  uint32_t m_sizeX{0}, m_sizeY{0}, m_sizeZ{0};
35  uint32_t m_sizeYaw{0}, m_sizePitch{0}, m_sizeRoll{0};
36 
37  // Cached products of the ones above:
39 
41  {
47  }
48 
49  /** Minimum "cell indexes" for each coordinate */
52 
53  /** The data. Stored in this order:
54  *
55  * m_data[
56  * ci_x
57  * + size_x * ci_y
58  * + size_xy * ci_z
59  * + size_xyz * ci_yaw
60  * + size_xyzy * ci_pitch
61  * + size_xyzyp * ci_roll
62  * ]
63  */
64  std::vector<T> m_data;
65 
66  public:
67  /** Default constructor:
68  */
71  mrpt::math::TPose3D(-1., -1., -1., -M_PI, -.5 * M_PI, -.5 * M_PI),
73  mrpt::math::TPose3D(1., 1., 1., M_PI, .5 * M_PI, .5 * M_PI),
74  double resolution_XYZ = 0.10,
75  double resolution_YPR = mrpt::DEG2RAD(10.0))
76  {
77  setSize(bb_min, bb_max, resolution_XYZ, resolution_YPR);
78  }
79 
80  virtual ~CPose3DGridTemplate() = default;
81 
82  /** @name Return "indexes" from coordinates
83  * @{
84  */
85  int x2idx(double x) const
86  {
87  const int idx = mrpt::round((x - m_bb_min.x) / m_resolutionXYZ);
88  ASSERT_(idx >= 0 && idx < static_cast<int>(m_sizeX));
89  return idx;
90  }
91  int y2idx(double y) const
92  {
93  const int idx = mrpt::round((y - m_bb_min.y) / m_resolutionXYZ);
94  ASSERT_(idx >= 0 && idx < static_cast<int>(m_sizeY));
95  return idx;
96  }
97  int z2idx(double z) const
98  {
99  const int idx = mrpt::round((z - m_bb_min.z) / m_resolutionXYZ);
100  ASSERT_(idx >= 0 && idx < static_cast<int>(m_sizeZ));
101  return idx;
102  }
103  int yaw2idx(double yaw) const
104  {
105  const int idx = mrpt::round((yaw - m_bb_min.yaw) / m_resolutionYPR);
106  ASSERT_(idx >= 0 && idx < static_cast<int>(m_sizeYaw));
107  return idx;
108  }
109  int pitch2idx(double pitch) const
110  {
111  const int idx = mrpt::round((pitch - m_bb_min.pitch) / m_resolutionYPR);
112  ASSERT_(idx >= 0 && idx < static_cast<int>(m_sizePitch));
113  return idx;
114  }
115  int roll2idx(double roll) const
116  {
117  const int idx = mrpt::round((roll - m_bb_min.roll) / m_resolutionYPR);
118  ASSERT_(idx >= 0 && idx < static_cast<int>(m_sizeRoll));
119  return idx;
120  }
121  /** @} */
122 
123  /** @name Return coordinates from "indexes"
124  * @{
125  */
126  double idx2x(uint32_t cx) const
127  {
128  ASSERT_(cx < m_sizeX);
129  return m_bb_min.x + cx * m_resolutionXYZ;
130  }
131  double idx2y(uint32_t cy) const
132  {
133  ASSERT_(cy < m_sizeY);
134  return m_bb_min.y + cy * m_resolutionXYZ;
135  }
136  double idx2z(uint32_t cz) const
137  {
138  ASSERT_(cz < m_sizeZ);
139  return m_bb_min.z + cz * m_resolutionXYZ;
140  }
141 
142  double idx2yaw(uint32_t cY) const
143  {
144  ASSERT_(cY < m_sizeYaw);
145  return m_bb_min.yaw + cY * m_resolutionYPR;
146  }
147  double idx2pitch(uint32_t cP) const
148  {
149  ASSERT_(cP < m_sizePitch);
150  return m_bb_min.pitch + cP * m_resolutionYPR;
151  }
152  double idx2roll(uint32_t cR) const
153  {
154  ASSERT_(cR < m_sizeRoll);
155  return m_bb_min.roll + cR * m_resolutionYPR;
156  }
157 
158  /** @} */
159 
160  /** Changes the limits and size of the grid, erasing previous contents:
161  */
162  void setSize(
164  double resolution_XYZ, double resolution_YPR)
165  {
166  // Checks
167  for (int i = 0; i < 6; i++) ASSERT_ABOVE_(bb_max[i], bb_min[i]);
168  ASSERT_ABOVE_(resolution_XYZ, .0);
169  ASSERT_ABOVE_(resolution_YPR, .0);
170 
171  // Copy data:
172  m_bb_min = bb_min;
173  m_bb_max = bb_max;
174  m_resolutionXYZ = resolution_XYZ;
175  m_resolutionYPR = resolution_YPR;
176 
177  // Compute the indexes of the starting borders:
178  m_min_cidX = mrpt::round(bb_min.x / resolution_XYZ);
179  m_min_cidY = mrpt::round(bb_min.y / resolution_XYZ);
180  m_min_cidZ = mrpt::round(bb_min.z / resolution_XYZ);
181 
182  m_min_cidYaw = mrpt::round(bb_min.yaw / resolution_YPR);
183  m_min_cidPitch = mrpt::round(bb_min.pitch / resolution_YPR);
184  m_min_cidRoll = mrpt::round(bb_min.roll / resolution_YPR);
185 
186  // Compute new required space:
187  m_sizeX = mrpt::round(bb_max.x / resolution_XYZ) - m_min_cidX + 1;
188  m_sizeY = mrpt::round(bb_max.y / resolution_XYZ) - m_min_cidY + 1;
189  m_sizeZ = mrpt::round(bb_max.z / resolution_XYZ) - m_min_cidZ + 1;
190 
191  m_sizeYaw = mrpt::round(bb_max.yaw / resolution_YPR) - m_min_cidYaw + 1;
192  m_sizePitch =
193  mrpt::round(bb_max.pitch / resolution_YPR) - m_min_cidPitch + 1;
194  m_sizeRoll =
195  mrpt::round(bb_max.roll / resolution_YPR) - m_min_cidRoll + 1;
196 
197  // Products:
199 
200  // Resize "m_data":
201  m_data.clear();
202  m_data.resize(m_size_xyzYPR);
203  }
204 
205  /** Reads the contents of a cell */
206  const T* getByPos(
207  double x, double y, double z, double yaw, double pitch,
208  double roll) const
209  {
210  return getByIndex(
211  x2idx(x), y2idx(y), z2idx(z), yaw2idx(yaw), pitch2idx(pitch),
212  roll2idx(roll));
213  }
214 
216  double x, double y, double z, double yaw, double pitch, double roll)
217  {
218  return const_cast<T*>(const_cast<const self_t&>(*this).getByPos(
219  x, y, z, yaw, pitch, roll));
220  }
221 
222  const T* getByPos(const mrpt::math::TPose3D& p) const
223  {
224  return getByPos(p.x, p.y, p.z, p.yaw, p.pitch, p.roll);
225  }
226 
228  {
229  return getByPos(p.x, p.y, p.z, p.yaw, p.pitch, p.roll);
230  }
231 
232  /** Reads the contents of a cell */
233  const T* getByIndex(int cx, int cy, int cz, int cY, int cP, int cR) const
234  {
235  ASSERT_(
236  cx < static_cast<int>(m_sizeX) && cy < static_cast<int>(m_sizeY) &&
237  cz < static_cast<int>(m_sizeZ) &&
238  cY < static_cast<int>(m_sizeYaw) &&
239  cP < static_cast<int>(m_sizePitch) &&
240  cR < static_cast<int>(m_sizeRoll));
241  return &m_data
242  [cx + m_sizeX * cy + m_size_xy * cz + m_size_xyz * cY +
243  m_size_xyzY * cP + m_size_xyzYP * cR];
244  }
245 
246  T* getByIndex(int cx, int cy, int cz, int cY, int cP, int cR)
247  {
248  return const_cast<T*>(const_cast<const self_t&>(*this).getByIndex(
249  cx, cy, cz, cY, cP, cR));
250  }
251 
252  /** Returns a XY slice of the grid, for given constant z,yaw, pitch and
253  * roll.
254  */
255  template <class MATRIXLIKE>
257  MATRIXLIKE& outMat, const double z, const double yaw,
258  const double pitch, const double roll) const
259  {
260  MRPT_START
261  outMat.setSize(m_sizeY, m_sizeX);
262  const auto cz = z2idx(z), cY = yaw2idx(yaw), cP = pitch2idx(pitch),
263  cR = roll2idx(roll);
264  ASSERT_BELOW_(cz, m_sizeZ);
268  for (uint32_t cy = 0; cy < m_sizeY; cy++)
269  for (uint32_t cx = 0; cx < m_sizeX; cx++)
270  outMat(cy, cx) = *getByIndex(cx, cy, cz, cY, cP, cR);
271  MRPT_END
272  }
273 
274  /** Get info about the 6D grid */
277 
278  double getResolutionXYZ() const { return m_resolutionXYZ; }
279  double getResolutionAngles() const { return m_resolutionYPR; }
280 
281  inline void fill(const T& val)
282  {
283  for (auto& v : m_data) v = val;
284  }
285 
286  auto getSizeX() const { return m_sizeX; }
287  auto getSizeY() const { return m_sizeY; }
288  auto getSizeZ() const { return m_sizeZ; }
289  auto getSizeYaw() const { return m_sizeYaw; }
290  auto getSizePitch() const { return m_sizePitch; }
291  auto getSizeRoll() const { return m_sizeRoll; }
292 
293  auto getTotalVoxelCount() const { return m_size_xyzYPR; }
294 
295  const std::vector<T>& getData() const { return m_data; }
296  std::vector<T>& getData() { return m_data; }
297 
298 }; // End of class def.
299 
300 } // namespace mrpt::poses
const T * getByIndex(int cx, int cy, int cz, int cY, int cP, int cR) const
Reads the contents of a cell.
const std::vector< T > & getData() const
T * getByIndex(int cx, int cy, int cz, int cY, int cP, int cR)
#define MRPT_START
Definition: exceptions.h:241
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
double roll
Roll coordinate (rotation angle over X coordinate).
Definition: TPose3D.h:38
double idx2z(uint32_t cz) const
double x
X,Y,Z, coords.
Definition: TPose3D.h:32
double idx2y(uint32_t cy) const
This is a template class for storing a 6-dimensional grid, with components corresponding to Euler ang...
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: TPose3D.h:34
mrpt::math::TPose3D getMaxBoundingBox() const
double idx2x(uint32_t cx) const
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
double idx2yaw(uint32_t cY) const
void getAsMatrix(MATRIXLIKE &outMat, const double z, const double yaw, const double pitch, const double roll) const
Returns a XY slice of the grid, for given constant z,yaw, pitch and roll.
double m_resolutionXYZ
Resolution of the grid.
void setSize(const mrpt::math::TPose3D &bb_min, const mrpt::math::TPose3D &bb_max, double resolution_XYZ, double resolution_YPR)
Changes the limits and size of the grid, erasing previous contents:
constexpr double DEG2RAD(const double x)
Degrees to radians.
T * getByPos(double x, double y, double z, double yaw, double pitch, double roll)
std::vector< T > m_data
The data.
int val
Definition: mrpt_jpeglib.h:957
double idx2pitch(uint32_t cP) const
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double pitch
Pitch coordinate (rotation angle over Y axis).
Definition: TPose3D.h:36
int m_min_cidX
Minimum "cell indexes" for each coordinate.
virtual ~CPose3DGridTemplate()=default
CPose3DGridTemplate(const mrpt::math::TPose3D &bb_min=mrpt::math::TPose3D(-1., -1., -1., -M_PI, -.5 *M_PI, -.5 *M_PI), const mrpt::math::TPose3D &bb_max=mrpt::math::TPose3D(1., 1., 1., M_PI,.5 *M_PI,.5 *M_PI), double resolution_XYZ=0.10, double resolution_YPR=mrpt::DEG2RAD(10.0))
Default constructor:
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
#define MRPT_END
Definition: exceptions.h:245
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
double idx2roll(uint32_t cR) const
const auto bb_max
T * getByPos(const mrpt::math::TPose3D &p)
const T * getByPos(const mrpt::math::TPose3D &p) const
mrpt::math::TPose3D getMinBoundingBox() const
Get info about the 6D grid.
const auto bb_min
const T * getByPos(double x, double y, double z, double yaw, double pitch, double roll) const
Reads the contents of a cell.
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020