CAStarAlgorithm.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
   |                                                                           |
   |                       http://www.mrpt.org/                                |
   |                                                                           |
   |   Copyright (C) 2005-2011  University of Malaga                           |
   |                                                                           |
   |    This software was written by the Machine Perception and Intelligent    |
   |      Robotics Lab, University of Malaga (Spain).                          |
   |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
   |                                                                           |
   |  This file is part of the MRPT project.                                   |
   |                                                                           |
   |     MRPT is free software: you can redistribute it and/or modify          |
   |     it under the terms of the GNU General Public License as published by  |
   |     the Free Software Foundation, either version 3 of the License, or     |
   |     (at your option) any later version.                                   |
   |                                                                           |
   |   MRPT is distributed in the hope that it will be useful,                 |
   |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
   |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
   |     GNU General Public License for more details.                          |
   |                                                                           |
   |     You should have received a copy of the GNU General Public License     |
   |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
   |                                                                           |
   +---------------------------------------------------------------------------+ */
#ifndef CASTARALGORITHM_H
#define CASTARALGORITHM_H
#include <map>
#include <vector>
#include <cmath>
#include <mrpt/utils/CTicTac.h>

namespace mrpt
{
        namespace 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::utils::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;
                        }
                };
        }
}       //End of namespaces
#endif