MRPT  2.0.4
ClearanceDiagram.cpp
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 
10 #include "nav-precomp.h" // Precomp header
11 
12 #include <mrpt/core/round.h>
15 #include <mrpt/opengl/CMesh.h>
18 #include <limits>
19 
20 using namespace mrpt::nav;
21 
23 
24  = default;
25 
27  mrpt::opengl::CMesh& mesh, double min_x, double max_x, double min_y,
28  double max_y, double cell_res, bool integrate_over_path) const
29 {
30  ASSERT_(cell_res > 0.0);
31  ASSERT_(max_x > min_x);
32  ASSERT_(max_y > min_y);
33 
34  mesh.setXBounds(min_x, max_x);
35  mesh.setYBounds(min_y, max_y);
36  const int nX = (int)::ceil((max_x - min_x) / cell_res);
37  const int nY = (int)::ceil((max_y - min_y) / cell_res);
38  const double dx = (max_x - min_x) / nX;
39  const double dy = (max_y - min_y) / nY;
40 
41  mrpt::math::CMatrixFloat Z(nX, nY);
42 
43  if (m_raw_clearances.empty()) return; // Nothing to do: empty structure!
44 
45  for (int iX = 0; iX < nX; iX++)
46  {
47  const double x = min_x + dx * (0.5 + iX);
48  for (int iY = 0; iY < nY; iY++)
49  {
50  const double y = min_y + dy * (0.5 + iY);
51 
52  double clear_val = .0;
53  if (x != 0 || y != 0)
54  {
55  const double alpha = ::atan2(y, x);
56  const uint16_t actual_k =
58  alpha, m_actual_num_paths);
59  const double dist = std::hypot(x, y);
60  clear_val =
61  this->getClearance(actual_k, dist, integrate_over_path);
62  }
63  Z(iX, iY) = clear_val;
64  }
65  }
66 
67  mesh.setZ(Z);
68  mesh.enableColorFromZ(true);
69  mesh.enableTransparency(true);
70  mesh.setColorA_u8(0x50);
71  mesh.enableWireFrame(false);
72 }
73 
76 {
77  uint8_t version;
78  in >> version;
79  switch (version)
80  {
81  case 0:
82  uint32_t decim_num;
83  in.ReadAsAndCastTo<uint32_t, size_t>(m_actual_num_paths);
84  in >> decim_num;
85  this->resize(m_actual_num_paths, decim_num);
86  in >> m_raw_clearances;
87  break;
88  default:
90  };
91 }
92 
95 {
96  const uint8_t version = 0;
97  out << version;
98 
99  out << uint32_t(m_actual_num_paths) << uint32_t(m_raw_clearances.size());
100  out << m_raw_clearances;
101 }
102 
104  size_t actual_k)
105 {
106  return m_raw_clearances[real_k_to_decimated_k(actual_k)];
107 }
108 
110  size_t actual_k) const
111 {
112  return m_raw_clearances[real_k_to_decimated_k(actual_k)];
113 }
114 
116 {
117  ASSERT_(m_actual_num_paths > 0 && !m_raw_clearances.empty());
118  const size_t ret = mrpt::round(k * m_k_a2d);
119  ASSERT_(ret < m_raw_clearances.size());
120  return ret;
121 }
122 
124 {
125  ASSERT_(m_actual_num_paths > 0 && !m_raw_clearances.empty());
126  const size_t ret = mrpt::round(k * m_k_d2a);
127  ASSERT_(ret < m_actual_num_paths);
128  return ret;
129 }
130 
132  uint16_t actual_k, double dist, bool integrate_over_path) const
133 {
134  if (this->empty()) // If we are not using clearance values, just return a
135  // fixed value:
136  return 0.0;
137 
139 
140  const size_t k = real_k_to_decimated_k(actual_k);
141 
142  const auto& rc_k = m_raw_clearances[k];
143 
144  double res = 0;
145  int avr_count = 0; // weighted avrg: closer to query points weight more
146  // than at path start.
147  for (const auto& e : rc_k)
148  {
149  if (integrate_over_path)
150  {
151  res = e.second;
152  avr_count = 1;
153  }
154  else
155  {
156  res += e.second;
157  avr_count++;
158  }
159  if (e.first > dist) break; // target dist reached.
160  }
161 
162  if (!avr_count)
163  {
164  res = rc_k.begin()->second;
165  }
166  else
167  {
168  res = res / avr_count;
169  }
170  return res;
171 }
172 
174 {
175  m_actual_num_paths = 0;
176  m_raw_clearances.clear();
177  m_k_a2d = m_k_d2a = .0;
178 }
179 
181  size_t actual_num_paths, size_t decimated_num_paths)
182 {
183  if (decimated_num_paths == 0)
184  {
185  this->clear();
186  return;
187  }
188  ASSERT_ABOVEEQ_(actual_num_paths, decimated_num_paths);
189 
190  m_actual_num_paths = actual_num_paths;
191  m_raw_clearances.resize(decimated_num_paths);
192 
193  m_k_d2a = double(m_actual_num_paths - 1) / (m_raw_clearances.size() - 1);
194  m_k_a2d = double(m_raw_clearances.size() - 1) / (m_actual_num_paths - 1);
195 }
std::map< double, double > dist2clearance_t
[TPS_distance] => normalized_clearance_for_exactly_that_robot_pose
virtual CRenderizable & setColorA_u8(const uint8_t a)
Color components in the range [0,255].
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
void enableTransparency(bool v)
Definition: CMesh.h:92
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:97
ClearanceDiagram()
default ctor
dist2clearance_t & get_path_clearance(size_t actual_k)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
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:120
size_t decimated_k_to_real_k(size_t k) const
void ReadAsAndCastTo(CAST_TO_TYPE &read_here)
Read a value from a stream stored in a type different of the target variable, making the conversion v...
Definition: CArchive.h:147
size_t real_k_to_decimated_k(size_t k) const
void setYBounds(const float min, const float max)
Definition: CMesh.h:169
void enableColorFromZ(bool v, mrpt::img::TColormap colorMap=mrpt::img::cmHOT)
Definition: CMesh.h:102
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:167
void setZ(const mrpt::math::CMatrixDynamic< float > &in_Z)
This method sets the matrix of heights for each position (cell) in the mesh grid. ...
Definition: CMesh.cpp:547
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:54
mrpt::vision::TStereoCalibResults out
void setXBounds(const float min, const float max)
Definition: CMesh.h:162
A planar (XY) grid where each cell has an associated height and, optionally, a texture map...
Definition: CMesh.h:37
This template class provides the basic functionality for a general 2D any-size, resizable container o...
images resize(NUM_IMGS)
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
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:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020