CHolonomicFullEval.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/math/geometry.h>
#include <mrpt/math/ops_containers.h>
#include <mrpt/nav/holonomic/CHolonomicFullEval.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/serialization/stl_serialization.h>

#include <Eigen/Dense>  // col(),...
#include <cmath>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::nav;
using namespace std;

IMPLEMENTS_SERIALIZABLE(
    CLogFileRecord_FullEval, CHolonomicLogFileRecord, mrpt::nav)
IMPLEMENTS_SERIALIZABLE(
    CHolonomicFullEval, CAbstractHolonomicReactiveMethod, mrpt::nav)

const unsigned NUM_FACTORS = 7U;

CHolonomicFullEval::CHolonomicFullEval(
    const mrpt::config::CConfigFileBase* INI_FILE)
    : CAbstractHolonomicReactiveMethod("CHolonomicFullEval"),
      m_last_selected_sector(std::numeric_limits<unsigned int>::max())
{
    if (INI_FILE != nullptr) initialize(*INI_FILE);
}

void CHolonomicFullEval::saveConfigFile(mrpt::config::CConfigFileBase& c) const
{
    options.saveToConfigFile(c, getConfigFileSectionName());
}

void CHolonomicFullEval::initialize(const mrpt::config::CConfigFileBase& c)
{
    options.loadFromConfigFile(c, getConfigFileSectionName());
}

struct TGap
{
    int k_from{-1}, k_to{-1};
    double min_eval, max_eval;
    bool contains_target_k{false};
    /** Direction with the best evaluation inside the gap. */
    int k_best_eval{-1};

    TGap()
        : min_eval(std::numeric_limits<double>::max()),
          max_eval(-std::numeric_limits<double>::max())

    {
    }
};

void CHolonomicFullEval::evalSingleTarget(
    unsigned int target_idx, const NavInput& ni, EvalOutput& eo)
{
    ASSERT_(target_idx < ni.targets.size());
    const auto target = ni.targets[target_idx];

    using mrpt::square;

    eo = EvalOutput();

    const auto ptg = getAssociatedPTG();
    const size_t nDirs = ni.obstacles.size();

    const double target_dir = ::atan2(target.y, target.x);
    const unsigned int target_k =
        CParameterizedTrajectoryGenerator::alpha2index(target_dir, nDirs);
    const double target_dist = target.norm();

    m_dirs_scores.resize(nDirs, options.factorWeights.size() + 2);

    // TP-Obstacles in 2D:
    std::vector<mrpt::math::TPoint2D> obstacles_2d(nDirs);

    mrpt::obs::T2DScanProperties sp;
    sp.aperture = 2.0 * M_PI;
    sp.nRays = nDirs;
    sp.rightToLeft = true;
    const auto& sc_lut = m_sincos_lut.getSinCosForScan(sp);

    for (unsigned int i = 0; i < nDirs; i++)
    {
        obstacles_2d[i].x = ni.obstacles[i] * sc_lut.ccos[i];
        obstacles_2d[i].y = ni.obstacles[i] * sc_lut.csin[i];
    }

    // Sanity checks:
    ASSERT_EQUAL_(options.factorWeights.size(), NUM_FACTORS);
    ASSERT_ABOVE_(nDirs, 3);

    for (unsigned int i = 0; i < nDirs; i++)
    {
        double scores[NUM_FACTORS];  // scores for each criterion

        // Too close to obstacles? (unless target is in between obstacles and
        // the robot)
        if (ni.obstacles[i] < options.TOO_CLOSE_OBSTACLE &&
            !(i == target_k && ni.obstacles[i] > 1.02 * target_dist))
        {
            for (size_t l = 0; l < NUM_FACTORS; l++) m_dirs_scores(i, l) = .0;
            continue;
        }

        const double d = std::min(ni.obstacles[i], 0.95 * target_dist);

        // The TP-Space representative coordinates for this direction:
        const double x = d * sc_lut.ccos[i];
        const double y = d * sc_lut.csin[i];

        // Factor [0]: collision-free distance
        // -----------------------------------------------------
        if (mrpt::abs_diff(i, target_k) <= 1 &&
            target_dist < 1.0 - options.TOO_CLOSE_OBSTACLE &&
            ni.obstacles[i] > 1.05 * target_dist)
        {
            // Don't count obstacles ahead of the target.
            scores[0] =
                std::max(target_dist, ni.obstacles[i]) / (target_dist * 1.05);
        }
        else
        {
            scores[0] =
                std::max(0.0, ni.obstacles[i] - options.TOO_CLOSE_OBSTACLE);
        }

        // Discount "circular loop aparent free distance" here, but don't count
        // it for clearance, since those are not real obstacle points.
        if (ptg != nullptr)
        {
            const double max_real_freespace =
                ptg->getActualUnloopedPathLength(i);
            const double max_real_freespace_norm =
                max_real_freespace / ptg->getRefDistance();

            mrpt::keep_min(scores[0], max_real_freespace_norm);
        }

        // Factor [1]: Closest approach to target along straight line
        // (Euclidean)
        // -------------------------------------------
        mrpt::math::TSegment2D sg;
        sg.point1.x = 0;
        sg.point1.y = 0;
        sg.point2.x = x;
        sg.point2.y = y;

        // Range of attainable values: 0=passes thru target. 2=opposite
        // direction
        double min_dist_target_along_path = sg.distance(target);

        // Idea: if this segment is taking us *away* from target, don't make
        // the segment to start at (0,0), since all paths "running away"
        // will then have identical minimum distances to target. Use the
        // middle of the segment instead:
        const double endpt_dist_to_target = (target - TPoint2D(x, y)).norm();
        const double endpt_dist_to_target_norm =
            std::min(1.0, endpt_dist_to_target);

        if ((endpt_dist_to_target_norm > target_dist &&
             endpt_dist_to_target_norm >= 0.95 * target_dist) &&
            /* the path does not get any closer to trg */
            min_dist_target_along_path >
                1.05 * std::min(target_dist, endpt_dist_to_target_norm))
        {
            // path takes us away or way blocked:
            sg.point1.x = x * 0.5;
            sg.point1.y = y * 0.5;
            min_dist_target_along_path = sg.distance(target);
        }

        scores[1] = 1.0 / (1.0 + square(min_dist_target_along_path));

        // Factor [2]: Distance of end collision-free point to target
        // (Euclidean)
        // Factor [5]: idem (except: no decimation afterwards)
        // -----------------------------------------------------
        scores[2] = std::sqrt(1.01 - endpt_dist_to_target_norm);
        scores[5] = scores[2];
        // the 1.01 instead of 1.0 is to be 100% sure we don't get a domain
        // error in sqrt()

        // Factor [3]: Stabilizing factor (hysteresis) to avoid quick switch
        // among very similar paths:
        // ------------------------------------------------------------------------------------------
        if (m_last_selected_sector != std::numeric_limits<unsigned int>::max())
        {
            // It's fine here to consider that -PI is far from +PI.
            const unsigned int hist_dist =
                mrpt::abs_diff(m_last_selected_sector, i);

            if (hist_dist >= options.HYSTERESIS_SECTOR_COUNT)
                scores[3] = square(
                    1.0 - (hist_dist - options.HYSTERESIS_SECTOR_COUNT) /
                              double(nDirs));
            else
                scores[3] = 1.0;
        }
        else
        {
            scores[3] = 1.0;
        }

        // Factor [4]: clearance to nearest obstacle along path
        // Use TP-obstacles instead of real obstacles in Workspace since
        // it's way faster, despite being an approximation:
        // -------------------------------------------------------------------
        {
            sg.point1.x = 0;
            sg.point1.y = 0;
            sg.point2.x = x;
            sg.point2.y = y;

            double& closest_obs = scores[4];
            closest_obs = 1.0;

            // eval obstacles within a certain region of this "i" direction only
            const int W = std::max(1, round(nDirs * 0.1));
            const int i_min = std::max(0, static_cast<int>(i) - W);
            const int i_max =
                std::min(static_cast<int>(nDirs) - 1, static_cast<int>(i) + W);
            for (int oi = i_min; oi <= i_max; oi++)
            {
                // "no obstacle" (norm_dist=1.0) doesn't count as a real obs:
                if (ni.obstacles[oi] >= 0.99) continue;
                mrpt::keep_min(closest_obs, sg.distance(obstacles_2d[oi]));
            }
        }

        // Factor [6]: Direct distance in "sectors":
        // -------------------------------------------------------------------
        scores[6] =
            1.0 /
            (1.0 + mrpt::square((4.0 / nDirs) * mrpt::abs_diff(i, target_k)));

        // If target is not directly reachable for this i-th direction, decimate
        // its scorings:
        if (target_dist < 1.0 - options.TOO_CLOSE_OBSTACLE &&
            ni.obstacles[i] < 1.01 * target_dist)
        {
            // this direction cannot reach target, so assign a low score:
            scores[1] *= 0.1;
            scores[2] *= 0.1;
            scores[6] *= 0.1;
        }

        // Save stats for debugging:
        for (size_t l = 0; l < NUM_FACTORS; l++)
            m_dirs_scores(i, l) = scores[l];

    }  // end for each direction "i"

    // Normalize factors?
    ASSERT_(options.factorNormalizeOrNot.size() == NUM_FACTORS);
    for (size_t l = 0; l < NUM_FACTORS; l++)
    {
        if (!options.factorNormalizeOrNot[l]) continue;

        const double mmax = m_dirs_scores.col(l).maxCoeff();
        const double mmin = m_dirs_scores.col(l).minCoeff();
        const double span = mmax - mmin;
        if (span <= .0) continue;

        m_dirs_scores.col(l).array() -= mmin;
        m_dirs_scores.col(l).array() /= span;
    }

    // Phase 1: average of PHASE1_FACTORS and thresholding:
    // ----------------------------------------------------------------------
    const unsigned int NUM_PHASES = options.PHASE_FACTORS.size();
    ASSERT_(NUM_PHASES >= 1);

    std::vector<double> weights_sum_phase(NUM_PHASES, .0),
        weights_sum_phase_inv(NUM_PHASES);
    for (unsigned int i = 0; i < NUM_PHASES; i++)
    {
        for (unsigned int l : options.PHASE_FACTORS[i])
            weights_sum_phase[i] += options.factorWeights.at(l);
        ASSERT_(weights_sum_phase[i] > .0);
        weights_sum_phase_inv[i] = 1.0 / weights_sum_phase[i];
    }

    eo.phase_scores = std::vector<std::vector<double>>(
        NUM_PHASES, std::vector<double>(nDirs, .0));
    auto& phase_scores = eo.phase_scores;  // shortcut
    double last_phase_threshold = -1.0;  // don't threshold for the first phase

    for (unsigned int phase_idx = 0; phase_idx < NUM_PHASES; phase_idx++)
    {
        double phase_min = std::numeric_limits<double>::max(), phase_max = .0;

        for (unsigned int i = 0; i < nDirs; i++)
        {
            double this_dir_eval = 0;

            if (ni.obstacles[i] <
                    options.TOO_CLOSE_OBSTACLE ||  // Too close to obstacles ?
                (phase_idx > 0 &&
                 phase_scores[phase_idx - 1][i] <
                     last_phase_threshold)  // thresholding of the previous
                // phase
            )
            {
                this_dir_eval = .0;
            }
            else
            {
                // Weighted avrg of factors:
                for (unsigned int l : options.PHASE_FACTORS[phase_idx])
                    this_dir_eval +=
                        options.factorWeights.at(l) *
                        std::log(std::max(1e-6, m_dirs_scores(i, l)));

                this_dir_eval *= weights_sum_phase_inv[phase_idx];
                this_dir_eval = std::exp(this_dir_eval);
            }
            phase_scores[phase_idx][i] = this_dir_eval;

            mrpt::keep_max(phase_max, phase_scores[phase_idx][i]);
            mrpt::keep_min(phase_min, phase_scores[phase_idx][i]);

        }  // for each direction

        ASSERT_(options.PHASE_THRESHOLDS.size() == NUM_PHASES);
        ASSERT_(
            options.PHASE_THRESHOLDS[phase_idx] > .0 &&
            options.PHASE_THRESHOLDS[phase_idx] < 1.0);

        last_phase_threshold =
            options.PHASE_THRESHOLDS[phase_idx] * phase_max +
            (1.0 - options.PHASE_THRESHOLDS[phase_idx]) * phase_min;
    }  // end for each phase

    // Give a chance for a derived class to manipulate the final evaluations:
    auto& dirs_eval = phase_scores.back();

    postProcessDirectionEvaluations(dirs_eval, ni, target_idx);

    // Recalculate the threshold just in case the postProcess function above
    // changed things:
    {
        double phase_min = std::numeric_limits<double>::max(), phase_max = .0;
        for (unsigned int i = 0; i < nDirs; i++)
        {
            mrpt::keep_max(phase_max, dirs_eval[i]);
            mrpt::keep_min(phase_min, dirs_eval[i]);
        }
        last_phase_threshold =
            options.PHASE_THRESHOLDS.back() * phase_max +
            (1.0 - options.PHASE_THRESHOLDS.back()) * phase_min;
    }

    // Thresholding:
    for (unsigned int i = 0; i < nDirs; i++)
    {
        double& val = dirs_eval[i];
        if (val < last_phase_threshold) val = .0;
    }
}

void CHolonomicFullEval::navigate(const NavInput& ni, NavOutput& no)
{
    using mrpt::square;

    ASSERT_(ni.clearance != nullptr);
    ASSERT_(!ni.targets.empty());

    // Create a log record for returning data.
    auto log = std::make_shared<CLogFileRecord_FullEval>();
    no.logRecord = log;

    // Evaluate for each target:
    const size_t numTrgs = ni.targets.size();
    std::vector<EvalOutput> evals(numTrgs);
    for (unsigned int trg_idx = 0; trg_idx < numTrgs; trg_idx++)
    {
        evalSingleTarget(trg_idx, ni, evals[trg_idx]);
    }

    ASSERT_(!evals.empty());
    const auto nDirs = evals.front().phase_scores.back().size();
    ASSERT_EQUAL_(nDirs, ni.obstacles.size());

    // Now, sum all weights for the last stage for each target into an "overall"
    // score vector, one score per direction of motion:
    std::vector<double> overall_scores;
    overall_scores.assign(nDirs, .0);
    for (const auto& e : evals)
    {
        for (unsigned int i = 0; i < nDirs; i++)
            overall_scores[i] += e.phase_scores.back()[i];
    }
    // Normalize:
    for (unsigned int i = 0; i < nDirs; i++)
        overall_scores[i] *= (1.0 / numTrgs);

    // Search for best direction in the "overall score" vector:

    // Keep the GAP with the largest maximum value within;
    // then pick the MIDDLE point as the final selection.
    std::vector<TGap> gaps;
    std::size_t best_gap_idx = std::string::npos;
    {
        bool inside_gap = false;
        for (unsigned int i = 0; i < nDirs; i++)
        {
            const double val = overall_scores[i];
            if (val < 0.01)
            {
                // This direction didn't pass the cut threshold for the "last
                // phase":
                if (inside_gap)
                {
                    // We just ended a gap:
                    auto& active_gap = *gaps.rbegin();
                    active_gap.k_to = i - 1;
                    inside_gap = false;
                }
            }
            else
            {
                // higher or EQUAL to the treshold (equal is important just in
                // case we have a "flat" diagram...)
                if (!inside_gap)
                {
                    // We just started a gap:
                    TGap new_gap;
                    new_gap.k_from = i;
                    gaps.emplace_back(new_gap);
                    inside_gap = true;
                }
            }

            if (inside_gap)
            {
                auto& active_gap = *gaps.rbegin();
                if (val >= active_gap.max_eval)
                {
                    active_gap.k_best_eval = i;
                }
                mrpt::keep_max(active_gap.max_eval, val);
                mrpt::keep_min(active_gap.min_eval, val);

                if (best_gap_idx == std::string::npos ||
                    val > gaps[best_gap_idx].max_eval)
                {
                    best_gap_idx = gaps.size() - 1;
                }
            }
        }  // end for i

        // Handle the case where we end with an open, active gap:
        if (inside_gap)
        {
            auto& active_gap = *gaps.rbegin();
            active_gap.k_to = nDirs - 1;
        }
    }

    // Not heading to target: go thru the "middle" of the gap to maximize
    // clearance
    int best_dir_k = -1;
    double best_dir_eval = 0;

    // We may have no gaps if all paths are blocked by obstacles, for example:
    if (!gaps.empty())
    {
        ASSERT_(best_gap_idx < gaps.size());
        const TGap& best_gap = gaps[best_gap_idx];
        best_dir_k = best_gap.k_best_eval;
        best_dir_eval = overall_scores.at(best_dir_k);
    }

    // Prepare NavigationOutput data:
    if (best_dir_eval == .0)
    {
        // No way found!
        no.desiredDirection = 0;
        no.desiredSpeed = 0;
    }
    else
    {
        // A valid movement:
        const auto ptg = getAssociatedPTG();
        const double ptg_ref_dist = ptg ? ptg->getRefDistance() : 1.0;

        no.desiredDirection =
            CParameterizedTrajectoryGenerator::index2alpha(best_dir_k, nDirs);

        // Speed control: Reduction factors
        // ---------------------------------------------
        const double targetNearnessFactor =
            m_enableApproachTargetSlowDown
                ? std::min(
                      1.0, ni.targets.front().norm() /
                               (options.TARGET_SLOW_APPROACHING_DISTANCE /
                                ptg_ref_dist))
                : 1.0;

        const double obs_dist = ni.obstacles[best_dir_k];
        // Was: min with obs_clearance too.
        const double obs_dist_th = std::max(
            options.TOO_CLOSE_OBSTACLE,
            options.OBSTACLE_SLOW_DOWN_DISTANCE * ni.maxObstacleDist);
        double riskFactor = 1.0;
        if (obs_dist <= options.TOO_CLOSE_OBSTACLE)
        {
            riskFactor = 0.0;
        }
        else if (
            obs_dist < obs_dist_th && obs_dist_th > options.TOO_CLOSE_OBSTACLE)
        {
            riskFactor = (obs_dist - options.TOO_CLOSE_OBSTACLE) /
                         (obs_dist_th - options.TOO_CLOSE_OBSTACLE);
        }
        no.desiredSpeed =
            ni.maxRobotSpeed * std::min(riskFactor, targetNearnessFactor);
    }

    m_last_selected_sector = best_dir_k;

    // LOG --------------------------
    if (log)
    {
        log->selectedTarget = 0;  // was: best_trg_idx
        log->selectedSector = best_dir_k;
        log->evaluation = best_dir_eval;
        // Copy the evaluation of first phases for (arbitrarily) the first
        // target, then overwrite the scores of its last phase with the OVERALL
        // phase scores:
        log->dirs_eval = evals.front().phase_scores;
        log->dirs_eval.back() = overall_scores;

        if (options.LOG_SCORE_MATRIX)
        {
            log->dirs_scores = m_dirs_scores;
        }
    }
}

unsigned int CHolonomicFullEval::direction2sector(
    const double a, const unsigned int N)
{
    const int idx = round(0.5 * (N * (1 + mrpt::math::wrapToPi(a) / M_PI) - 1));
    if (idx < 0)
        return 0;
    else
        return static_cast<unsigned int>(idx);
}

CLogFileRecord_FullEval::CLogFileRecord_FullEval() : dirs_scores() {}
uint8_t CLogFileRecord_FullEval::serializeGetVersion() const { return 3; }
void CLogFileRecord_FullEval::serializeTo(
    mrpt::serialization::CArchive& out) const
{
    out << CHolonomicLogFileRecord::dirs_eval << dirs_scores << selectedSector
        << evaluation << selectedTarget /*v3*/;
}

/*---------------------------------------------------------------
                    readFromStream
  ---------------------------------------------------------------*/
void CLogFileRecord_FullEval::serializeFrom(
    mrpt::serialization::CArchive& in, uint8_t version)
{
    switch (version)
    {
        case 0:
        case 1:
        case 2:
        case 3:
        {
            if (version >= 2)
            {
                in >> CHolonomicLogFileRecord::dirs_eval;
            }
            else
            {
                CHolonomicLogFileRecord::dirs_eval.resize(2);
                in >> CHolonomicLogFileRecord::dirs_eval[0];
                if (version >= 1)
                {
                    in >> CHolonomicLogFileRecord::dirs_eval[1];
                }
            }
            in >> dirs_scores >> selectedSector >> evaluation;
            if (version >= 3)
            {
                in >> selectedTarget;
            }
            else
            {
                selectedTarget = 0;
            }
        }
        break;
        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
    };
}

/*---------------------------------------------------------------
                        TOptions
  ---------------------------------------------------------------*/
CHolonomicFullEval::TOptions::TOptions()
    : factorWeights{0.1, 0.5, 0.5, 0.01, 1, 1, 1},
      factorNormalizeOrNot{0, 0, 0, 0, 1, 0, 0},
      PHASE_FACTORS{{1, 2}, {4}, {0, 2}},
      PHASE_THRESHOLDS{0.5, 0.6, 0.7}

{
}

void CHolonomicFullEval::TOptions::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& c, const std::string& s)
{
    MRPT_START

    // Load from config text:
    MRPT_LOAD_CONFIG_VAR(TOO_CLOSE_OBSTACLE, double, c, s);
    MRPT_LOAD_CONFIG_VAR(TARGET_SLOW_APPROACHING_DISTANCE, double, c, s);
    MRPT_LOAD_CONFIG_VAR(OBSTACLE_SLOW_DOWN_DISTANCE, double, c, s);
    MRPT_LOAD_CONFIG_VAR(HYSTERESIS_SECTOR_COUNT, double, c, s);
    MRPT_LOAD_CONFIG_VAR(LOG_SCORE_MATRIX, bool, c, s);
    MRPT_LOAD_CONFIG_VAR(clearance_threshold_ratio, double, c, s);
    MRPT_LOAD_CONFIG_VAR(gap_width_ratio_threshold, double, c, s);

    c.read_vector(
        s, "factorWeights", std::vector<double>(), factorWeights, true);
    ASSERT_EQUAL_(factorWeights.size(), NUM_FACTORS);

    c.read_vector(
        s, "factorNormalizeOrNot", factorNormalizeOrNot, factorNormalizeOrNot);
    ASSERT_EQUAL_(factorNormalizeOrNot.size(), factorWeights.size());

    // Phases:
    int PHASE_COUNT = 0;
    MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(PHASE_COUNT, int, c, s);

    PHASE_FACTORS.resize(PHASE_COUNT);
    PHASE_THRESHOLDS.resize(PHASE_COUNT);
    for (int i = 0; i < PHASE_COUNT; i++)
    {
        c.read_vector(
            s, mrpt::format("PHASE%i_FACTORS", i + 1), PHASE_FACTORS[i],
            PHASE_FACTORS[i], true);
        ASSERT_(!PHASE_FACTORS[i].empty());

        PHASE_THRESHOLDS[i] = c.read_double(
            s, mrpt::format("PHASE%i_THRESHOLD", i + 1), .0, true);
        ASSERT_(PHASE_THRESHOLDS[i] >= .0 && PHASE_THRESHOLDS[i] <= 1.0);
    }

    MRPT_END
}

void CHolonomicFullEval::TOptions::saveToConfigFile(
    mrpt::config::CConfigFileBase& c, const std::string& s) const
{
    MRPT_START

    const int WN = mrpt::config::MRPT_SAVE_NAME_PADDING(),
              WV = mrpt::config::MRPT_SAVE_VALUE_PADDING();

    MRPT_SAVE_CONFIG_VAR_COMMENT(
        TOO_CLOSE_OBSTACLE,
        "Directions with collision-free distances below this threshold are not "
        "elegible.");
    MRPT_SAVE_CONFIG_VAR_COMMENT(
        TARGET_SLOW_APPROACHING_DISTANCE,
        "Start to reduce speed when closer than this to target.");
    MRPT_SAVE_CONFIG_VAR_COMMENT(
        OBSTACLE_SLOW_DOWN_DISTANCE,
        "Start to reduce speed when clearance is below this value ([0,1] ratio "
        "wrt obstacle reference/max distance)");
    MRPT_SAVE_CONFIG_VAR_COMMENT(
        HYSTERESIS_SECTOR_COUNT,
        "Range of `sectors` (directions) for hysteresis over successive "
        "timesteps");
    MRPT_SAVE_CONFIG_VAR_COMMENT(
        LOG_SCORE_MATRIX, "Save the entire score matrix in log files");
    MRPT_SAVE_CONFIG_VAR_COMMENT(
        clearance_threshold_ratio,
        "Ratio [0,1], times path_count, gives the minimum number of paths at "
        "each side of a target direction to be accepted as desired direction");
    MRPT_SAVE_CONFIG_VAR_COMMENT(
        gap_width_ratio_threshold,
        "Ratio [0,1], times path_count, gives the minimum gap width to accept "
        "a direct motion towards target.");

    ASSERT_EQUAL_(factorWeights.size(), NUM_FACTORS);
    c.write(
        s, "factorWeights",
        mrpt::system::sprintf_container("%.2f ", factorWeights), WN, WV,
        "[0]=Free space, [1]=Dist. in sectors, [2]=Closer to target "
        "(Euclidean), [3]=Hysteresis, [4]=clearance along path, [5]=Like [2] "
        "without decimation if path obstructed");
    c.write(
        s, "factorNormalizeOrNot",
        mrpt::system::sprintf_container("%u ", factorNormalizeOrNot), WN, WV,
        "Normalize factors or not (1/0)");

    c.write(
        s, "PHASE_COUNT", PHASE_FACTORS.size(), WN, WV,
        "Number of evaluation phases to run (params for each phase below)");

    for (unsigned int i = 0; i < PHASE_FACTORS.size(); i++)
    {
        c.write(
            s, mrpt::format("PHASE%u_THRESHOLD", i + 1), PHASE_THRESHOLDS[i],
            WN, WV,
            "Phase scores must be above this relative range threshold [0,1] to "
            "be considered in next phase (Default:`0.75`)");
        c.write(
            s, mrpt::format("PHASE%u_FACTORS", i + 1),
            mrpt::system::sprintf_container("%d ", PHASE_FACTORS[i]), WN, WV,
            "Indices of the factors above to be considered in this phase");
    }

    MRPT_END
}

uint8_t CHolonomicFullEval::serializeGetVersion() const { return 4; }
void CHolonomicFullEval::serializeTo(mrpt::serialization::CArchive& out) const
{
    // Params:
    out << options.factorWeights << options.HYSTERESIS_SECTOR_COUNT
        << options.PHASE_FACTORS <<  // v3
        options.TARGET_SLOW_APPROACHING_DISTANCE << options.TOO_CLOSE_OBSTACLE
        << options.PHASE_THRESHOLDS  // v3
        << options.OBSTACLE_SLOW_DOWN_DISTANCE  // v1
        << options.factorNormalizeOrNot  // v2
        << options.clearance_threshold_ratio
        << options.gap_width_ratio_threshold  // v4:
        ;
    // State:
    out << m_last_selected_sector;
}
void CHolonomicFullEval::serializeFrom(
    mrpt::serialization::CArchive& in, uint8_t version)
{
    switch (version)
    {
        case 0:
        case 1:
        case 2:
        case 3:
        case 4:
        {
            // Params:
            in >> options.factorWeights >> options.HYSTERESIS_SECTOR_COUNT;

            if (version >= 3)
            {
                in >> options.PHASE_FACTORS;
            }
            else
            {
                options.PHASE_THRESHOLDS.resize(2);
                in >> options.PHASE_FACTORS[0] >> options.PHASE_FACTORS[1];
            }
            in >> options.TARGET_SLOW_APPROACHING_DISTANCE >>
                options.TOO_CLOSE_OBSTACLE;

            if (version >= 3)
            {
                in >> options.PHASE_THRESHOLDS;
            }
            else
            {
                options.PHASE_THRESHOLDS.resize(1);
                in >> options.PHASE_THRESHOLDS[0];
            }

            if (version >= 1) in >> options.OBSTACLE_SLOW_DOWN_DISTANCE;
            if (version >= 2) in >> options.factorNormalizeOrNot;

            if (version >= 4)
            {
                in >> options.clearance_threshold_ratio >>
                    options.gap_width_ratio_threshold;
            }

            // State:
            in >> m_last_selected_sector;
        }
        break;
        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
    };
}

void CHolonomicFullEval::postProcessDirectionEvaluations(
    std::vector<double>& dir_evals, const NavInput& ni, unsigned int trg_idx)
{
    // Default: do nothing
}