Main MRPT website > C++ reference for MRPT 1.5.6
CAtan2LookUpTable.cpp
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 
10 #include "base-precomp.h" // Precompiled headers
11 
13 #include <vector>
14 #include <cmath>
15 
16 using namespace mrpt::math;
17 
19 {
20  this->resize(-1.0,1.0,-1.0,1.0,0.5);
21 }
22 CAtan2LookUpTable::CAtan2LookUpTable(double xmin,double xmax,double ymin,double ymax, double resolution) MRPT_NO_THROWS
23 {
24  this->resize(xmin,xmax,ymin,ymax,resolution);
25 }
26 
27 void CAtan2LookUpTable::resize(double xmin,double xmax,double ymin,double ymax, double resolution) MRPT_NO_THROWS
28 {
29  const double def = .0;
30  if (m_grid.getResolution()==resolution)
31  m_grid.resize(xmin,xmax,ymin,ymax, def, .0);
32  else m_grid.setSize(xmin, xmax, ymin, ymax, resolution, &def);
33 
34  const size_t nx = m_grid.getSizeX(), ny = m_grid.getSizeY();
35 
36  std::vector<double> idx2x(nx), idx2y(ny);
37 
38  for (size_t ix = 0;ix<nx; ix++) idx2x[ix] = m_grid.idx2x(ix);
39  for (size_t iy = 0;iy<ny; iy++) idx2y[iy] = m_grid.idx2y(iy);
40 
41  for (size_t ix = 0;ix<nx; ix++)
42  {
43  const double x = idx2x[ix];
44  for (size_t iy = 0;iy<ny; iy++)
45  {
46  const double y = idx2y[iy];
47  double *cp = m_grid.cellByIndex(ix,iy);
48  if(cp) *cp =::atan2(y,x);
49  }
50  }
51 }
52 
53 bool CAtan2LookUpTable::atan2(double y,double x, double &out_atan2) const MRPT_NO_THROWS
54 {
55  const double *cp = m_grid.cellByPos(x,y);
56  if (!cp) return false;
57  out_atan2 = *cp;
58  return true;
59 }
60 
61 
62 
64 {
65 }
66 CAtan2LookUpTableMultiRes::CAtan2LookUpTableMultiRes(const std::map<double,double> & lst_resolutions2extensions ) MRPT_NO_THROWS
67 {
68  resize(lst_resolutions2extensions);
69 }
70 void CAtan2LookUpTableMultiRes::resize(const std::map<double,double> & lst_resolutions2extensions) MRPT_NO_THROWS
71 {
72  m_grids.clear();
73 
74  for (const auto &it : lst_resolutions2extensions)
75  {
76  const double res = it.first;
77  const double exten = it.second;
78 
79  mrpt::utils::CDynamicGrid<double> & dg = m_grids[exten] = mrpt::utils::CDynamicGrid<double>(-exten,exten,-exten,exten, res);
80 
81  const size_t nx = dg.getSizeX(), ny = dg.getSizeY();
82  std::vector<double> idx2x(nx), idx2y(ny);
83 
84  for (size_t ix = 0;ix<nx; ix++) idx2x[ix] = dg.idx2x(ix);
85  for (size_t iy = 0;iy<ny; iy++) idx2y[iy] = dg.idx2y(iy);
86 
87  for (size_t ix = 0;ix<nx; ix++)
88  {
89  const double x = idx2x[ix];
90  for (size_t iy = 0;iy<ny; iy++)
91  {
92  const double y = idx2y[iy];
93  double *cp = dg.cellByIndex(ix,iy);
94  if(cp) *cp =::atan2(y,x);
95  }
96  }
97  }
98 }
99 
100 bool CAtan2LookUpTableMultiRes::atan2(double y,double x, double &out_atan2) const MRPT_NO_THROWS
101 {
102  for (const auto & it : m_grids)
103  {
104  const double exten = it.first;
105  const mrpt::utils::CDynamicGrid<double> & dg = it.second;
106 
107  if (std::abs(x)>exten || std::abs(y)>exten)
108  continue;
109 
110  const double *cp = dg.cellByPos(x,y);
111  if (!cp) continue;
112  out_atan2 = *cp;
113  return true;
114  }
115  return false;
116 }
117 
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: CDynamicGrid.h:223
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
bool atan2(double y, double x, double &out_atan2) const MRPT_NO_THROWS
Returns the precomputed value for atan2(y,x).
#define MRPT_NO_THROWS
C++11 noexcept: Used after member declarations.
void resize(double xmin, double xmax, double ymin, double ymax, double resolution) MRPT_NO_THROWS
GLhalf ny
Definition: glew.h:8038
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:220
T * cellByPos(double x, double y)
Returns a pointer to the contents of a cell given by its coordinates, or NULL if it is out of the map...
Definition: CDynamicGrid.h:183
GLfloat GLfloat GLfloat GLfloat nx
Definition: glew.h:10079
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
double idx2y(int cy) const
Definition: CDynamicGrid.h:254
void resize(const std::map< double, double > &lst_resolutions2extensions) MRPT_NO_THROWS
See CAtan2LookUpTableMultiRes for a discussion of the parameters.
bool atan2(double y, double x, double &out_atan2) const MRPT_NO_THROWS
Returns the precomputed value for atan2(y,x).
T * cellByIndex(unsigned int cx, unsigned int cy)
Returns a pointer to the contents of a cell given by its cell indexes, or NULL if it is out of the ma...
Definition: CDynamicGrid.h:203
GLuint res
Definition: glew.h:7143
double idx2x(int cx) const
Transform a cell index into a coordinate value of the cell central point.
Definition: CDynamicGrid.h:253



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018