MRPT  1.9.9
ClearanceDiagram.h
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 #pragma once
10 
13 #include <cstdint>
14 #include <map>
15 #include <vector>
16 
17 namespace mrpt::nav
18 {
19 /** Clearance information for one particular PTG and one set of obstacles.
20  * Usage:
21  * - Declare an object of this type (it will be initialized to "empty"),
22  * - Call CParameterizedTrajectoryGenerator::initClearanceDiagram()
23  * - Repeatedly call CParameterizedTrajectoryGenerator::updateClearance() for
24  * each 2D obstacle point.
25  *
26  * \ingroup nav_tpspace
27  */
29 {
30  public:
31  /** default ctor */
33  /** Reset to default, empty state */
34  void clear();
35  /** Initializes the container to allocate `decimated_num_paths` entries, as
36  * a decimated
37  * subset of a total of `actual_num_paths` paths */
38  void resize(size_t actual_num_paths, size_t decimated_num_paths);
39  inline bool empty() const { return m_raw_clearances.empty(); }
40  inline size_t get_actual_num_paths() const { return m_actual_num_paths; }
41  inline size_t get_decimated_num_paths() const
42  {
43  return m_raw_clearances.size();
44  }
45 
46  /** Gets the clearance for path `k` and distance `TPS_query_distance` in one
47  * of two modes:
48  * - [integrate_over_path=false] clearance from that specific spot, or
49  * - [integrate_over_path=true] average clearance over the path from the
50  * origin to that specific spot.
51  */
52  double getClearance(
53  uint16_t k, double TPS_query_distance, bool integrate_over_path) const;
54  void renderAs3DObject(
55  mrpt::opengl::CMesh& mesh, double min_x, double max_x, double min_y,
56  double max_y, double cell_res, bool integrate_over_path) const;
57 
60 
61  /** [TPS_distance] => normalized_clearance_for_exactly_that_robot_pose */
62  using dist2clearance_t = std::map<double, double>;
63  dist2clearance_t& get_path_clearance(size_t actual_k);
64  const dist2clearance_t& get_path_clearance(size_t actual_k) const;
65 
67  {
68  return m_raw_clearances[decim_k];
69  }
71  size_t decim_k) const
72  {
73  return m_raw_clearances[decim_k];
74  }
75 
76  size_t real_k_to_decimated_k(size_t k) const;
77  size_t decimated_k_to_real_k(size_t k) const;
78 
79  protected:
80  /** Container: [decimated_path_k][TPS_distance] =>
81  * normalized_clearance_for_exactly_that_robot_pose */
82  std::vector<dist2clearance_t> m_raw_clearances;
83 
85  0}; // The decimated number of paths is implicit in
86  // raw_clearances.size()
87  double m_k_a2d{.0}, m_k_d2a{.0};
88 };
89 
90 } // namespace mrpt::nav
std::map< double, double > dist2clearance_t
[TPS_distance] => normalized_clearance_for_exactly_that_robot_pose
size_t get_decimated_num_paths() const
dist2clearance_t & get_path_clearance_decimated(size_t decim_k)
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.
ClearanceDiagram()
default ctor
Clearance information for one particular PTG and one set of obstacles.
dist2clearance_t & get_path_clearance(size_t actual_k)
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: ...
size_t decimated_k_to_real_k(size_t k) const
size_t get_actual_num_paths() const
size_t real_k_to_decimated_k(size_t k) const
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...
const dist2clearance_t & get_path_clearance_decimated(size_t decim_k) const
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
A planar (XY) grid where each cell has an associated height and, optionally, a texture map...
Definition: CMesh.h:37



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 3a26b90fd Wed Mar 25 20:17:03 2020 +0100 at miƩ mar 25 23:05:41 CET 2020