Main MRPT website > C++ reference for MRPT 1.5.7
ClearanceDiagram.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 "nav-precomp.h" // Precomp header
11 
14 #include <mrpt/opengl/CMesh.h>
15 #include <mrpt/utils/CStream.h>
16 #include <mrpt/utils/round.h>
18 #include <limits>
19 
20 using namespace mrpt::nav;
21 
23  m_actual_num_paths(0),
24  m_k_a2d(.0), m_k_d2a(.0)
25 {
26 }
27 
29  mrpt::opengl::CMesh &mesh, double min_x, double max_x, double min_y, double max_y, double cell_res, bool integrate_over_path) const
30 {
31  ASSERT_(cell_res>0.0);
32  ASSERT_(max_x>min_x);
33  ASSERT_(max_y>min_y);
34 
35  mesh.setXBounds(min_x, max_x);
36  mesh.setYBounds(min_y, max_y);
37  const int nX = (int)::ceil((max_x - min_x) / cell_res);
38  const int nY = (int)::ceil((max_y - min_y) / cell_res);
39  const double dx = (max_x - min_x) / nX;
40  const double dy = (max_y - min_y) / nY;
41 
42  mrpt::math::CMatrixFloat Z(nX, nY);
43 
44  if (m_raw_clearances.empty())
45  return; // Nothing to do: empty structure!
46 
47  for (int iX = 0; iX < nX; iX++)
48  {
49  const double x = min_x + dx*(0.5+ iX);
50  for (int iY = 0; iY < nY; iY++)
51  {
52  const double y = min_y + dy*(0.5 + iY);
53 
54  double clear_val = .0;
55  if (x != 0 || y != 0)
56  {
57  const double alpha = ::atan2(y, x);
59  const double dist = std::hypot(x, y);
60  clear_val = this->getClearance(actual_k, dist, integrate_over_path);
61  }
62  Z(iX, iY) = clear_val;
63  }
64  }
65 
66  mesh.setZ(Z);
67  mesh.enableColorFromZ(true);
68  mesh.enableTransparency(true);
69  mesh.setColorA_u8(0x50);
70  mesh.enableWireFrame(false);
71 }
72 
74 {
76  in >> version;
77  switch (version)
78  {
79  case 0:
80  uint32_t decim_num;
81  in.ReadAsAndCastTo<uint32_t, size_t>(m_actual_num_paths);
82  in >> decim_num;
83  this->resize(m_actual_num_paths, decim_num);
84  in >> m_raw_clearances;
85  break;
86  default:
88  };
89 }
90 
92 {
93  const uint8_t version = 0;
94  out << version;
95 
96  out << uint32_t(m_actual_num_paths) << uint32_t(m_raw_clearances.size());
97  out << m_raw_clearances;
98 }
99 
101 {
102  return m_raw_clearances[real_k_to_decimated_k(actual_k)];
103 }
104 
106 {
107  return m_raw_clearances[real_k_to_decimated_k(actual_k)];
108 }
109 
111 {
112  ASSERT_(m_actual_num_paths>0 && !m_raw_clearances.empty());
113  const size_t ret = mrpt::utils::round(k*m_k_a2d);
114  ASSERT_(ret<m_raw_clearances.size());
115  return ret;
116 }
117 
119 {
120  ASSERT_(m_actual_num_paths>0 && !m_raw_clearances.empty());
121  const size_t ret = mrpt::utils::round(k*m_k_d2a);
122  ASSERT_(ret<m_actual_num_paths);
123  return ret;
124 }
125 
126 double ClearanceDiagram::getClearance(uint16_t actual_k, double dist, bool integrate_over_path) const
127 {
128  if (this->empty()) // If we are not using clearance values, just return a fixed value:
129  return 0.0;
130 
132 
133  const size_t k = real_k_to_decimated_k(actual_k);
134 
135  const auto & rc_k = m_raw_clearances[k];
136 
137  double res = 0;
138  int avr_count = 0; //weighted avrg: closer to query points weight more than at path start.
139  for (const auto &e : rc_k)
140  {
141  if (integrate_over_path)
142  {
143  res = e.second;
144  avr_count=1;
145  }
146  else
147  {
148  res += e.second;
149  avr_count++;
150  }
151  if (e.first>dist)
152  break; // target dist reached.
153  }
154 
155  if (!avr_count) {
156  res = rc_k.begin()->second;
157  } else {
158  res = res / avr_count;
159  }
160  return res;
161 }
162 
164 {
165  m_actual_num_paths = 0;
166  m_raw_clearances.clear();
167  m_k_a2d = m_k_d2a = .0;
168 }
169 
170 void mrpt::nav::ClearanceDiagram::resize(size_t actual_num_paths, size_t decimated_num_paths)
171 {
172  if (decimated_num_paths == 0)
173  {
174  this->clear();
175  return;
176  }
177  ASSERT_ABOVEEQ_(actual_num_paths, decimated_num_paths);
178 
179  m_actual_num_paths = actual_num_paths;
180  m_raw_clearances.resize(decimated_num_paths);
181 
182  m_k_d2a = double(m_actual_num_paths-1) / (m_raw_clearances.size()-1);
183  m_k_a2d = double(m_raw_clearances.size()-1) / (m_actual_num_paths-1);
184 }
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3510
unsigned __int16 uint16_t
Definition: rptypes.h:46
void setZ(const mrpt::math::CMatrixTemplateNumeric< float > &in_Z)
This method sets the matrix of heights for each position (cell) in the mesh grid. ...
Definition: CMesh.cpp:490
void enableTransparency(bool v)
Definition: CMesh.h:91
#define ASSERT_BELOW_(__A, __B)
void clear()
Reset to default, empty state.
std::vector< dist2clearance_t > m_raw_clearances
Container: [decimated_path_k][TPS_distance] => normalized_clearance_for_exactly_that_robot_pose.
void enableWireFrame(bool v)
Definition: CMesh.h:92
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
void writeToStream(mrpt::utils::CStream &out) const
dist2clearance_t & get_path_clearance(size_t actual_k)
unsigned char uint8_t
Definition: rptypes.h:43
double getClearance(uint16_t k, double TPS_query_distance, bool integrate_over_path) const
Gets the clearance for path k and distance TPS_query_distance in one of two modes: ...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
size_t decimated_k_to_real_k(size_t k) const
std::map< double, double > dist2clearance_t
[TPS_distance] => normalized_clearance_for_exactly_that_robot_pose */
size_t real_k_to_decimated_k(size_t k) const
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void setYBounds(const float min, const float max)
Definition: CMesh.h:152
int version
Definition: mrpt_jpeglib.h:898
void resize(size_t actual_num_paths, size_t decimated_num_paths)
Initializes the container to allocate decimated_num_paths entries, as a decimated subset of a total o...
void readFromStream(mrpt::utils::CStream &in)
#define ASSERT_ABOVEEQ_(__A, __B)
void renderAs3DObject(mrpt::opengl::CMesh &mesh, double min_x, double max_x, double min_y, double max_y, double cell_res, bool integrate_over_path) const
void enableColorFromZ(bool v, mrpt::utils::TColormap colorMap=mrpt::utils::cmHOT)
Definition: CMesh.h:93
GLuint in
Definition: glext.h:6301
#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
void setXBounds(const float min, const float max)
Definition: CMesh.h:147
A planar (XY) grid where each cell has an associated height and, optionally, a texture map...
Definition: CMesh.h:40
GLuint res
Definition: glext.h:6298
GLenum GLint x
Definition: glext.h:3516
unsigned __int32 uint32_t
Definition: rptypes.h:49
CRenderizable & setColorA_u8(const uint8_t a) MRPT_OVERRIDE
Color components in the range [0,255].
uint16_t alpha2index(double alpha) const
Discrete index value for the corresponding alpha value.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019