Go to the documentation of this file.
28 double max_y,
double cell_res,
bool integrate_over_path)
const
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;
45 for (
int iX = 0; iX < nX; iX++)
47 const double x = min_x + dx * (0.5 + iX);
48 for (
int iY = 0; iY < nY; iY++)
50 const double y = min_y + dy * (0.5 + iY);
52 double clear_val = .0;
55 const double alpha = ::atan2(y, x);
56 const uint16_t actual_k =
59 const double dist = std::hypot(x, y);
63 Z(iX, iY) = clear_val;
85 this->
resize(m_actual_num_paths, decim_num);
86 in >> m_raw_clearances;
96 const uint8_t version = 0;
99 out << uint32_t(m_actual_num_paths) << uint32_t(m_raw_clearances.size());
100 out << m_raw_clearances;
110 size_t actual_k)
const
117 ASSERT_(m_actual_num_paths > 0 && !m_raw_clearances.empty());
119 ASSERT_(ret < m_raw_clearances.size());
125 ASSERT_(m_actual_num_paths > 0 && !m_raw_clearances.empty());
127 ASSERT_(ret < m_actual_num_paths);
132 uint16_t actual_k,
double dist,
bool integrate_over_path)
const
147 for (
const auto& e : rc_k)
149 if (integrate_over_path)
159 if (e.first > dist)
break;
164 res = rc_k.begin()->second;
168 res = res / avr_count;
181 size_t actual_num_paths,
size_t decimated_num_paths)
183 if (decimated_num_paths == 0)
190 m_actual_num_paths = actual_num_paths;
191 m_raw_clearances.resize(decimated_num_paths);
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);
void writeToStream(mrpt::serialization::CArchive &out) const
void clear()
Clear the contents of this container.
void enableWireFrame(bool v)
size_t m_actual_num_paths
void enableTransparency(bool v)
virtual CRenderizable & setColorA_u8(const uint8_t a)
Color components in the range [0,255].
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:
mrpt::vision::TStereoCalibResults out
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
#define ASSERT_(f)
Defines an assertion mechanism.
void setYBounds(const float min, const float max)
A planar (XY) grid where each cell has an associated height and, optionally, a texture map.
Virtual base class for "archives": classes abstracting I/O streams.
uint16_t alpha2index(double alpha) const
Discrete index value for the corresponding alpha value.
#define ASSERT_BELOW_(__A, __B)
int round(const T value)
Returns the closer integer (int) to x.
void readFromStream(mrpt::serialization::CArchive &in)
std::map< double, double > dist2clearance_t
[TPS_distance] => normalized_clearance_for_exactly_that_robot_pose
dist2clearance_t & get_path_clearance(size_t actual_k)
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...
#define ASSERT_ABOVEEQ_(__A, __B)
void setZ(const mrpt::math::CMatrixDynamic< float > &in_Z)
This method sets the matrix of heights for each position (cell) in the mesh grid.
void clear()
Reset to default, empty state.
void setXBounds(const float min, const float max)
ClearanceDiagram()
default ctor
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...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
std::vector< dist2clearance_t > m_raw_clearances
Container: [decimated_path_k][TPS_distance] => normalized_clearance_for_exactly_that_robot_pose
This template class provides the basic functionality for a general 2D any-size, resizable container o...
void enableColorFromZ(bool v, mrpt::img::TColormap colorMap=mrpt::img::cmHOT)
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020 | |