downhill_simplex.h

//Copyright (C) Edward Rosten 2009

//All rights reserved.
//
//Redistribution and use in source and binary forms, with or without
//modification, are permitted provided that the following conditions
//are met:
//1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//2. Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
//THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND OTHER CONTRIBUTORS ``AS IS''
//AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
//IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
//ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR OTHER CONTRIBUTORS BE
//LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
//CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
//SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
//INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
//CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
//ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
//POSSIBILITY OF SUCH DAMAGE.

#ifndef TOON_DOWNHILL_SIMPLEX_H
#define TOON_DOWNHILL_SIMPLEX_H
#include <TooN/TooN.h>
#include <TooN/helpers.h>
#include <algorithm>
#include <cstdlib>

namespace TooN
{

template<int N=-1, typename Precision=double> class DownhillSimplex
{
    static const int Vertices = (N==-1?-1:N+1);
    typedef Matrix<Vertices, N, Precision> Simplex;
    typedef Vector<Vertices, Precision> Values;

    public:
        template<class Function> DownhillSimplex(const Function& func, const Vector<N>& c, Precision spread=1)
        :simplex(c.size()+1, c.size()),values(c.size()+1)
        {
            alpha = 1.0;
            rho = 2.0;
            gamma = 0.5;
            sigma = 0.5;

            using std::sqrt;
            epsilon = sqrt(numeric_limits<Precision>::epsilon());
            zero_epsilon = 1e-20;

            restart(func, c, spread);
        }
        
        template<class Function> void restart(const Function& func, const Vector<N>& c, Precision spread)
        {
            for(int i=0; i < simplex.num_rows(); i++)
                simplex[i] = c;

            for(int i=0; i < simplex.num_cols(); i++)
                simplex[i][i] += spread;

            for(int i=0; i < values.size(); i++)
                values[i] = func(simplex[i]);
        }
        
        bool finished()
        {
            Precision span =  norm(simplex[get_best()] - simplex[get_worst()]);
            Precision scale = norm(simplex[get_best()]);

            if(span/scale < epsilon || span < zero_epsilon)
                return 1;
            else 
                return 0;
        }
        
        template<class Function> void restart(const Function& func, Precision spread)
        {
            restart(func, simplex[get_best()], spread);
        }

        const Simplex& get_simplex() const
        {
            return simplex;
        }
        
        const Values& get_values() const
        {
            return values;
        }
        
        int get_best() const 
        {
            return std::min_element(&values[0], &values[0] + values.size()) - &values[0];
        }
        
        int get_worst() const 
        {
            return std::max_element(&values[0], &values[0] + values.size()) - &values[0];
        }

        template<class Function> void find_next_point(const Function& func)
        {
            //Find various things:
            // - The worst point
            // - The second worst point
            // - The best point
            // - The centroid of all the points but the worst
            int worst = get_worst();
            Precision second_worst_val=-HUGE_VAL, bestval = HUGE_VAL, worst_val = values[worst];
            int best=0;
            Vector<N> x0 = Zeros(simplex.num_cols());


            for(int i=0; i < simplex.num_rows(); i++)
            {
                if(values[i] < bestval)
                {
                    bestval = values[i];
                    best = i;
                }

                if(i != worst)
                {
                    if(values[i] > second_worst_val)
                        second_worst_val = values[i];

                    //Compute the centroid of the non-worst points;
                    x0 += simplex[i];
                }
            }
            x0 *= 1.0 / simplex.num_cols();


            //Reflect the worst point about the centroid.
            Vector<N> xr = (1 + alpha) * x0 - alpha * simplex[worst];
            Precision fr = func(xr);

            if(fr < bestval)
            {
                //If the new point is better than the smallest, then try expanding the simplex.
                Vector<N> xe = rho * xr + (1-rho) * x0;
                Precision fe = func(xe);

                //Keep whichever is best
                if(fe < fr)
                {
                    simplex[worst] = xe;
                    values[worst] = fe;
                }
                else
                {
                    simplex[worst] = xr;
                    values[worst] = fr;
                }

                return;
            }

            //Otherwise, if the new point lies between the other points
            //then keep it and move on to the next iteration.
            if(fr < second_worst_val)
            {
                simplex[worst] = xr;
                values[worst] = fr;
                return;
            }


            //Otherwise, if the new point is a bit better than the worst point,
            //(ie, it's got just a little bit better) then contract the simplex
            //a bit.
            if(fr < worst_val)
            {
                Vector<N> xc = (1 + gamma) * x0 - gamma * simplex[worst];
                Precision fc = func(xc);

                //If this helped, use it
                if(fc <= fr)
                {
                    simplex[worst] = xc;
                    values[worst] = fc;
                    return;
                }
            }
            
            //Otherwise, fr is worse than the worst point, or the fc was worse
            //than fr. So shrink the whole simplex around the best point.
            for(int i=0; i < simplex.num_rows(); i++)
                if(i != best)
                {
                    simplex[i] = simplex[best] + sigma * (simplex[i] - simplex[best]);
                    values[i] = func(simplex[i]);
                }
        }

        template<class Function> bool iterate(const Function& func)
        {
            find_next_point(func);
            return !finished();
        }

        Precision alpha; 
        Precision rho;   
        Precision gamma; 
        Precision sigma; 
        Precision epsilon;  
        Precision zero_epsilon; 

    private:

        //Each row is a simplex vertex
        Simplex simplex;

        //Function values for each vertex
        Values values;


};
}
#endif