Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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>
16 #include <mrpt/core/round.h>
18 #include <limits>
19 
20 using namespace mrpt::nav;
21 
23  : m_actual_num_paths(0), m_k_a2d(.0), m_k_d2a(.0)
24 {
25 }
26 
28  mrpt::opengl::CMesh& mesh, double min_x, double max_x, double min_y,
29  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()) return; // Nothing to do: empty structure!
45 
46  for (int iX = 0; iX < nX; iX++)
47  {
48  const double x = min_x + dx * (0.5 + iX);
49  for (int iY = 0; iY < nY; iY++)
50  {
51  const double y = min_y + dy * (0.5 + iY);
52 
53  double clear_val = .0;
54  if (x != 0 || y != 0)
55  {
56  const double alpha = ::atan2(y, x);
57  const uint16_t actual_k =
60  const double dist = std::hypot(x, y);
61  clear_val =
62  this->getClearance(actual_k, dist, integrate_over_path);
63  }
64  Z(iX, iY) = clear_val;
65  }
66  }
67 
68  mesh.setZ(Z);
69  mesh.enableColorFromZ(true);
70  mesh.enableTransparency(true);
71  mesh.setColorA_u8(0x50);
72  mesh.enableWireFrame(false);
73 }
74 
77 {
78  uint8_t version;
79  in >> version;
80  switch (version)
81  {
82  case 0:
83  uint32_t decim_num;
84  in.ReadAsAndCastTo<uint32_t, size_t>(m_actual_num_paths);
85  in >> decim_num;
86  this->resize(m_actual_num_paths, decim_num);
87  in >> m_raw_clearances;
88  break;
89  default:
91  };
92 }
93 
96 {
97  const uint8_t version = 0;
98  out << version;
99 
100  out << uint32_t(m_actual_num_paths) << uint32_t(m_raw_clearances.size());
101  out << m_raw_clearances;
102 }
103 
105  size_t actual_k)
106 {
107  return m_raw_clearances[real_k_to_decimated_k(actual_k)];
108 }
109 
111  size_t actual_k) const
112 {
113  return m_raw_clearances[real_k_to_decimated_k(actual_k)];
114 }
115 
117 {
118  ASSERT_(m_actual_num_paths > 0 && !m_raw_clearances.empty());
119  const size_t ret = mrpt::round(k * m_k_a2d);
120  ASSERT_(ret < m_raw_clearances.size());
121  return ret;
122 }
123 
125 {
126  ASSERT_(m_actual_num_paths > 0 && !m_raw_clearances.empty());
127  const size_t ret = mrpt::round(k * m_k_d2a);
128  ASSERT_(ret < m_actual_num_paths);
129  return ret;
130 }
131 
133  uint16_t actual_k, double dist, bool integrate_over_path) const
134 {
135  if (this->empty()) // If we are not using clearance values, just return a
136  // fixed value:
137  return 0.0;
138 
140 
141  const size_t k = real_k_to_decimated_k(actual_k);
142 
143  const auto& rc_k = m_raw_clearances[k];
144 
145  double res = 0;
146  int avr_count = 0; // weighted avrg: closer to query points weight more
147  // than at path start.
148  for (const auto& e : rc_k)
149  {
150  if (!integrate_over_path)
151  {
152  // dont integrate: forget past part:
153  res = 0;
154  avr_count = 0;
155  }
156  // Keep min clearance along straight path:
157  res += e.second;
158  avr_count++;
159 
160  if (e.first > dist) break; // target dist reached.
161  }
162 
163  if (!avr_count)
164  {
165  res = rc_k.begin()->second;
166  }
167  else
168  {
169  res = res / avr_count;
170  }
171  return res;
172 }
173 
175 {
176  m_actual_num_paths = 0;
177  m_raw_clearances.clear();
178  m_k_a2d = m_k_d2a = .0;
179 }
180 
182  size_t actual_num_paths, size_t decimated_num_paths)
183 {
184  if (decimated_num_paths == 0)
185  {
186  this->clear();
187  return;
188  }
189  ASSERT_ABOVEEQ_(actual_num_paths, decimated_num_paths);
190 
191  m_actual_num_paths = actual_num_paths;
192  m_raw_clearances.resize(decimated_num_paths);
193 
194  m_k_d2a = double(m_actual_num_paths - 1) / (m_raw_clearances.size() - 1);
195  m_k_a2d = double(m_raw_clearances.size() - 1) / (m_actual_num_paths - 1);
196 }
GLclampf GLclampf GLclampf alpha
Definition: glext.h:3525
std::map< double, double > dist2clearance_t
[TPS_distance] => normalized_clearance_for_exactly_that_robot_pose
unsigned __int16 uint16_t
Definition: rptypes.h:44
CRenderizable & setColorA_u8(const uint8_t a) override
Color components in the range [0,255].
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:165
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:525
void enableTransparency(bool v)
Definition: CMesh.h:122
void clear()
Reset to default, empty state.
void writeToStream(mrpt::serialization::CArchive &out) const
void readFromStream(mrpt::serialization::CArchive &in)
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:127
dist2clearance_t & get_path_clearance(size_t actual_k)
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
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: ...
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
size_t decimated_k_to_real_k(size_t k) const
size_t real_k_to_decimated_k(size_t k) const
void setYBounds(const float min, const float max)
Definition: CMesh.h:204
void enableColorFromZ(bool v, mrpt::img::TColormap colorMap=mrpt::img::cmHOT)
Definition: CMesh.h:132
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...
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:183
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
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
GLuint in
Definition: glext.h:7274
GLenum GLint GLint y
Definition: glext.h:3538
void setXBounds(const float min, const float max)
Definition: CMesh.h:197
A planar (XY) grid where each cell has an associated height and, optionally, a texture map...
Definition: CMesh.h:39
GLuint res
Definition: glext.h:7268
GLenum GLint x
Definition: glext.h:3538
unsigned __int32 uint32_t
Definition: rptypes.h:47
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:188
uint16_t alpha2index(double alpha) const
Discrete index value for the corresponding alpha value.
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019