CAStarAlgorithm.h

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          |
   +------------------------------------------------------------------------+ */
#pragma once
#include <mrpt/system/CTicTac.h>
#include <cmath>
#include <map>
#include <vector>

namespace mrpt::graphs
{
/** This class is intended to efficiently solve graph-search problems using
 * heuristics to determine the best path. To use it, a solution class must be
 * defined
 * so that it contains all the information about any partial or complete
 * solution. Then, a class inheriting from CAStarAlgorithm<Solution class> must
 * also be
 * implemented, overriding five virtual methods which define the behaviour of
 * the solutions. These methods are isSolutionEnded, isSolutionValid,
 * generateChildren, getHeuristic and getCost.
 * Once both classes are generated, each object of the class inheriting from
 * CAStarAlgorithm represents a problem who can be solved by calling
 * getOptimalSolution. See http://en.wikipedia.org/wiki/A*_search_algorithm for
 * details about how this algorithm works.
 * \sa CAStarAlgorithm::isSolutionEnded
 * \sa CAStarAlgorithm::isSolutionValid
 * \sa CAStarAlgorithm::generateChildren
 * \sa CAStarAlgorithm::getHeuristic
 * \sa CAStarAlgorithm::getCost
 * \ingroup mrpt_graphs_grp
 */
template <typename T>
class CAStarAlgorithm
{
   public:
    /**
     * Client code must implement this method.
     * Returns true if the given solution is complete.
     */
    virtual bool isSolutionEnded(const T& sol) = 0;
    /**
     * Client code must implement this method.
     * Returns true if the given solution is acceptable, that is, doesn't
     * violate the problem logic.
     */
    virtual bool isSolutionValid(const T& sol) = 0;
    /**
     * Client code must implement this method.
     * Given a partial solution, returns all its children solution, regardless
     * of their validity or completeness.
     */
    virtual void generateChildren(const T& sol, std::vector<T>& sols) = 0;
    /**
     * Client code must implement this method.
     * Given a partial solution, estimates the cost of the remaining (unknown)
     * part.
     * This cost must always be greater or equal to zero, and not greater than
     * the actual cost. Thus, must be 0 if the solution is complete.
     */
    virtual double getHeuristic(const T& sol) = 0;
    /**
     * Client code must implement this method.
     * Given a (possibly partial) solution, calculates its cost so far.
     * This cost must not decrease with each step. That is, a solution cannot
     * have a smaller cost than the previous one from which it was generated.
     */
    virtual double getCost(const T& sol) = 0;

   private:
    /**
     * Calculates the total cost (known+estimated) of a solution.
     */
    inline double getTotalCost(const T& sol)
    {
        return getHeuristic(sol) + getCost(sol);
    }

   public:
    /**
     * Finds the optimal solution for a problem, using the A* algorithm.
     * Returns whether an optimal solution was actually found.
     * Returns 0 if no solution was found, 1 if an optimal solution was found
     * and 2 if a (possibly suboptimal) solution was found but the time lapse
     * ended.
     */
    int getOptimalSolution(
        const T& initialSol, T& finalSol, double upperLevel = HUGE_VAL,
        double maxComputationTime = HUGE_VAL)
    {
        // Time measuring object is defined.
        mrpt::system::CTicTac time;
        time.Tic();
        // The partial solution set is initialized with a single element (the
        // starting solution).
        std::multimap<double, T> partialSols;
        partialSols.insert(
            std::pair<double, T>(getTotalCost(initialSol), initialSol));
        // The best known solution is set to the upper bound (positive infinite,
        // if there is no given parameter).
        double currentOptimal = upperLevel;
        bool found = false;
        std::vector<T> children;
        // Main loop. Each iteration checks an element of the set, with minimum
        // estimated cost.
        while (!partialSols.empty())
        {
            // Return if elapsed time has been reached.
            if (time.Tac() >= maxComputationTime) return found ? 2 : 0;
            typename std::multimap<double, T>::iterator it =
                partialSols.begin();
            double tempCost = it->first;
            // If the minimum estimated cost is higher than the upper bound,
            // then also is every solution in the set. So the algorithm returns
            // immediately.
            if (tempCost >= currentOptimal) return found ? 1 : 0;
            T tempSol = it->second;
            partialSols.erase(it);
            // At this point, the solution cost is lesser than the upper bound.
            // So, if the solution is complete, the optimal solution and the
            // upper bound are updated.
            if (isSolutionEnded(tempSol))
            {
                currentOptimal = tempCost;
                finalSol = tempSol;
                found = true;
                continue;
            }
            // If the solution is not complete, check for its children. Each one
            // is included in the set only if it's valid and it's not yet
            // present in the set.
            generateChildren(tempSol, children);
            for (typename std::vector<T>::const_iterator it2 = children.begin();
                 it2 != children.end(); ++it2)
                if (isSolutionValid(*it2))
                {
                    bool alreadyPresent = false;
                    double cost = getTotalCost(*it2);
                    typename std::pair<
                        typename std::multimap<double, T>::const_iterator,
                        typename std::multimap<double, T>::const_iterator>
                        range = partialSols.equal_range(cost);
                    for (typename std::multimap<double, T>::const_iterator it3 =
                             range.first;
                         it3 != range.second; ++it3)
                        if (it3->second == *it2)
                        {
                            alreadyPresent = true;
                            break;
                        }
                    if (!alreadyPresent)
                        partialSols.insert(
                            std::pair<double, T>(getTotalCost(*it2), *it2));
                }
        }
        // No more solutions to explore...
        return found ? 1 : 0;
    }
};
}  // namespace mrpt::graphs