ClearanceDiagram.cpp

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 |
   +------------------------------------------------------------------------+ */

#include "nav-precomp.h"  // Precomp header

#include <mrpt/nav/holonomic/ClearanceDiagram.h>
#include <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>
#include <mrpt/opengl/CMesh.h>
#include <mrpt/utils/CStream.h>
#include <mrpt/utils/round.h>
#include <mrpt/utils/stl_serialization.h>
#include <limits>

using namespace mrpt::nav;

ClearanceDiagram::ClearanceDiagram()
        : m_actual_num_paths(0), m_k_a2d(.0), m_k_d2a(.0)
{
}

void ClearanceDiagram::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
{
        ASSERT_(cell_res > 0.0);
        ASSERT_(max_x > min_x);
        ASSERT_(max_y > min_y);

        mesh.setXBounds(min_x, max_x);
        mesh.setYBounds(min_y, max_y);
        const int nX = (int)::ceil((max_x - min_x) / cell_res);
        const int nY = (int)::ceil((max_y - min_y) / cell_res);
        const double dx = (max_x - min_x) / nX;
        const double dy = (max_y - min_y) / nY;

        mrpt::math::CMatrixFloat Z(nX, nY);

        if (m_raw_clearances.empty()) return;  // Nothing to do: empty structure!

        for (int iX = 0; iX < nX; iX++)
        {
                const double x = min_x + dx * (0.5 + iX);
                for (int iY = 0; iY < nY; iY++)
                {
                        const double y = min_y + dy * (0.5 + iY);

                        double clear_val = .0;
                        if (x != 0 || y != 0)
                        {
                                const double alpha = ::atan2(y, x);
                                const uint16_t actual_k =
                                        CParameterizedTrajectoryGenerator::alpha2index(
                                                alpha, m_actual_num_paths);
                                const double dist = std::hypot(x, y);
                                clear_val =
                                        this->getClearance(actual_k, dist, integrate_over_path);
                        }
                        Z(iX, iY) = clear_val;
                }
        }

        mesh.setZ(Z);
        mesh.enableColorFromZ(true);
        mesh.enableTransparency(true);
        mesh.setColorA_u8(0x50);
        mesh.enableWireFrame(false);
}

void mrpt::nav::ClearanceDiagram::readFromStream(mrpt::utils::CStream& in)
{
        uint8_t version;
        in >> version;
        switch (version)
        {
                case 0:
                        uint32_t decim_num;
                        in.ReadAsAndCastTo<uint32_t, size_t>(m_actual_num_paths);
                        in >> decim_num;
                        this->resize(m_actual_num_paths, decim_num);
                        in >> m_raw_clearances;
                        break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
        };
}

void mrpt::nav::ClearanceDiagram::writeToStream(mrpt::utils::CStream& out) const
{
        const uint8_t version = 0;
        out << version;

        out << uint32_t(m_actual_num_paths) << uint32_t(m_raw_clearances.size());
        out << m_raw_clearances;
}

ClearanceDiagram::dist2clearance_t& ClearanceDiagram::get_path_clearance(
        size_t actual_k)
{
        return m_raw_clearances[real_k_to_decimated_k(actual_k)];
}

const ClearanceDiagram::dist2clearance_t& ClearanceDiagram::get_path_clearance(
        size_t actual_k) const
{
        return m_raw_clearances[real_k_to_decimated_k(actual_k)];
}

size_t mrpt::nav::ClearanceDiagram::real_k_to_decimated_k(size_t k) const
{
        ASSERT_(m_actual_num_paths > 0 && !m_raw_clearances.empty());
        const size_t ret = mrpt::utils::round(k * m_k_a2d);
        ASSERT_(ret < m_raw_clearances.size());
        return ret;
}

size_t mrpt::nav::ClearanceDiagram::decimated_k_to_real_k(size_t k) const
{
        ASSERT_(m_actual_num_paths > 0 && !m_raw_clearances.empty());
        const size_t ret = mrpt::utils::round(k * m_k_d2a);
        ASSERT_(ret < m_actual_num_paths);
        return ret;
}

double ClearanceDiagram::getClearance(
        uint16_t actual_k, double dist, bool integrate_over_path) const
{
        if (this->empty())  // If we are not using clearance values, just return a
                // fixed value:
                return 0.0;

        ASSERT_BELOW_(actual_k, m_actual_num_paths);

        const size_t k = real_k_to_decimated_k(actual_k);

        const auto& rc_k = m_raw_clearances[k];

        double res = 0;
        int avr_count = 0;  // weighted avrg: closer to query points weight more
        // than at path start.
        for (const auto& e : rc_k)
        {
                if (!integrate_over_path)
                {
                        // dont integrate: forget past part:
                        res = 0;
                        avr_count = 0;
                }
                // Keep min clearance along straight path:
                res += e.second;
                avr_count++;

                if (e.first > dist) break;  // target dist reached.
        }

        if (!avr_count)
        {
                res = rc_k.begin()->second;
        }
        else
        {
                res = res / avr_count;
        }
        return res;
}

void ClearanceDiagram::clear()
{
        m_actual_num_paths = 0;
        m_raw_clearances.clear();
        m_k_a2d = m_k_d2a = .0;
}

void mrpt::nav::ClearanceDiagram::resize(
        size_t actual_num_paths, size_t decimated_num_paths)
{
        if (decimated_num_paths == 0)
        {
                this->clear();
                return;
        }
        ASSERT_ABOVEEQ_(actual_num_paths, decimated_num_paths);

        m_actual_num_paths = actual_num_paths;
        m_raw_clearances.resize(decimated_num_paths);

        m_k_d2a = double(m_actual_num_paths - 1) / (m_raw_clearances.size() - 1);
        m_k_a2d = double(m_raw_clearances.size() - 1) / (m_actual_num_paths - 1);
}