Main MRPT website > C++ reference for MRPT 1.5.5
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  // dont integrate: forget past part:
143  res = 0;
144  avr_count = 0;
145  }
146  // Keep min clearance along straight path:
147  res += e.second;
148  avr_count ++;
149 
150  if (e.first>dist)
151  break; // target dist reached.
152  }
153 
154  if (!avr_count) {
155  res = rc_k.begin()->second;
156  } else {
157  res = res / avr_count;
158  }
159  return res;
160 }
161 
163 {
164  m_actual_num_paths = 0;
165  m_raw_clearances.clear();
166  m_k_a2d = m_k_d2a = .0;
167 }
168 
169 void mrpt::nav::ClearanceDiagram::resize(size_t actual_num_paths, size_t decimated_num_paths)
170 {
171  if (decimated_num_paths == 0)
172  {
173  this->clear();
174  return;
175  }
176  ASSERT_ABOVEEQ_(actual_num_paths, decimated_num_paths);
177 
178  m_actual_num_paths = actual_num_paths;
179  m_raw_clearances.resize(decimated_num_paths);
180 
181  m_k_d2a = double(m_actual_num_paths-1) / (m_raw_clearances.size()-1);
182  m_k_a2d = double(m_raw_clearances.size()-1) / (m_actual_num_paths-1);
183 }
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.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019