ClearanceDiagram.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

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

#include <mrpt/core/round.h>
#include <mrpt/nav/holonomic/ClearanceDiagram.h>
#include <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>
#include <mrpt/opengl/CMesh.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/stl_serialization.h>
#include <limits>

using namespace mrpt::nav;

ClearanceDiagram::ClearanceDiagram()

    = default;

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::serialization::CArchive& 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::serialization::CArchive& 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::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::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)
        {
            res = e.second;
            avr_count = 1;
        }
        else
        {
            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);
}