ClearanceDiagram.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */
#pragma once

#include <vector>
#include <map>
#include <mrpt/utils/mrpt_stdint.h>    // compiler-independent version of "stdint.h"
#include <mrpt/opengl/opengl_frwds.h>
#include <mrpt/nav/link_pragmas.h>
namespace mrpt { namespace utils { class CStream; } }

namespace mrpt {
namespace nav {
        /** Clearance information for one particular PTG and one set of obstacles. 
                * Usage: 
                * - Declare an object of this type (it will be initialized to "empty"), 
                * - Call CParameterizedTrajectoryGenerator::initClearanceDiagram()
                * - Repeatedly call CParameterizedTrajectoryGenerator::updateClearance() for each 2D obstacle point. 
                *
                *  \ingroup nav_tpspace
                */
        class NAV_IMPEXP ClearanceDiagram
        {
        public:
                ClearanceDiagram(); //!< default ctor
                void clear(); //!< Reset to default, empty state
                /** Initializes the container to allocate `decimated_num_paths` entries, as a decimated 
                  * subset of a total of `actual_num_paths` paths */
                void resize(size_t actual_num_paths, size_t decimated_num_paths);
                inline bool empty() const { return m_raw_clearances.empty(); }
                inline size_t get_actual_num_paths() const { return m_actual_num_paths; }
                inline size_t get_decimated_num_paths() const { return m_raw_clearances.size(); }

                /** Gets the clearance for path `k` and distance `TPS_query_distance` in one of two modes: 
                  * - [integrate_over_path=false] clearance from that specific spot, or 
                  * - [integrate_over_path=true] average clearance over the path from the origin to that specific spot.
                  */
                double getClearance(uint16_t k, double TPS_query_distance, bool integrate_over_path) 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;

                void readFromStream(mrpt::utils::CStream &in);
                void writeToStream(mrpt::utils::CStream &out) const;

                typedef std::map<double, double> dist2clearance_t; //!< [TPS_distance] => normalized_clearance_for_exactly_that_robot_pose  */
                dist2clearance_t & get_path_clearance(size_t actual_k);
                const dist2clearance_t & get_path_clearance(size_t actual_k) const;
                
                inline dist2clearance_t & get_path_clearance_decimated(size_t decim_k) { return m_raw_clearances[decim_k]; }
                inline const dist2clearance_t & get_path_clearance_decimated(size_t decim_k) const { return m_raw_clearances[decim_k]; }

                size_t real_k_to_decimated_k(size_t k) const;
                size_t decimated_k_to_real_k(size_t k) const;

        protected:
                /** Container: [decimated_path_k][TPS_distance] => normalized_clearance_for_exactly_that_robot_pose  */
                std::vector<dist2clearance_t> m_raw_clearances;

                size_t m_actual_num_paths; // The decimated number of paths is implicit in raw_clearances.size()
                double m_k_a2d, m_k_d2a;
        };

} // ns
} // ns