conjugate_gradient.h

//Copyright (C) Edward Rosten 2009, 2010, 2012

//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.

#include <TooN/optimization/brent.h>
#include <utility>
#include <cmath>
#include <cassert>
#include <cstdlib>

namespace TooN{
    namespace Internal{


    template<int Size, typename Precision, typename Func> struct LineSearch
    {
        const Vector<Size, Precision>& start; 
        const Vector<Size, Precision>& direction;

        const Func& f;

        LineSearch(const Vector<Size, Precision>& s, const Vector<Size, Precision>& d, const Func& func)
        :start(s),direction(d),f(func)
        {}

        Precision operator()(Precision x) const
        {
            return f(start + x * direction);
        }
    };

    template<typename Precision, typename Func> Matrix<3,2,Precision> bracket_minimum_forward(Precision a_val, const Func& func, Precision initial_lambda, Precision zeps)
    {
        //Get a, b, c to  bracket a minimum along a line
        Precision a, b, c, b_val, c_val;

        a=0;

        //Search forward in steps of lambda
        Precision lambda=initial_lambda;
        b = lambda;
        b_val = func(b);

        while(std::isnan(b_val))
        {
            //We've probably gone in to an invalid region. This can happen even 
            //if following the gradient would never get us there.
            //try backing off lambda
            lambda*=.5;
            b = lambda;
            b_val = func(b);

        }


        if(b_val < a_val) //We've gone downhill, so keep searching until we go back up
        {
            double last_good_lambda = lambda;
            
            for(;;)
            {
                lambda *= 2;
                c = lambda;
                c_val = func(c);

                if(std::isnan(c_val))
                    break;
                last_good_lambda = lambda;
                if(c_val >  b_val) // we have a bracket
                    break;
                else
                {
                    a = b;
                    a_val = b_val;
                    b=c;
                    b_val=c_val;

                }
            }

            //We took a step too far.
            //Back up: this will not attempt to ensure a bracket
            if(std::isnan(c_val))
            {
                double bad_lambda=lambda;
                double l=1;

                for(;;)
                {
                    l*=.5;
                    c = last_good_lambda + (bad_lambda - last_good_lambda)*l;
                    c_val = func(c);

                    if(!std::isnan(c_val))
                        break;
                }


            }

        }
        else //We've overshot the minimum, so back up
        {
            c = b;
            c_val = b_val;
            //Here, c_val > a_val

            for(;;)
            {
                lambda *= .5;
                b = lambda;
                b_val = func(b);

                if(b_val < a_val)// we have a bracket
                    break;
                else if(lambda < zeps)
                    return Zeros;
                else //Contract the bracket
                {
                    c = b;
                    c_val = b_val;
                }
            }
        }

        Matrix<3,2> ret;
        ret[0] = makeVector(a, a_val);
        ret[1] = makeVector(b, b_val);
        ret[2] = makeVector(c, c_val);

        return ret;
    }

}


template<int Size=Dynamic, class Precision=double> struct ConjugateGradient
{
    const int size;      
    Vector<Size> g;      
    Vector<Size> h;      
    Vector<Size> minus_h;
    Vector<Size> old_g;  
    Vector<Size> old_h;  
    Vector<Size> x;      
    Vector<Size> old_x;  
    Precision y;         
    Precision old_y;     

    Precision tolerance; 
    Precision epsilon;   
    int       max_iterations; 

    Precision bracket_initial_lambda;
    Precision linesearch_tolerance; 
    Precision linesearch_epsilon; 
    int linesearch_max_iterations;  

    Precision bracket_epsilon; 

    int iterations; 

    template<class Func, class Deriv> ConjugateGradient(const Vector<Size>& start, const Func& func, const Deriv& deriv)
    : size(start.size()),
      g(size),h(size),minus_h(size),old_g(size),old_h(size),x(start),old_x(size)
    {
        init(start, func(start), deriv(start));
    }   

    template<class Func> ConjugateGradient(const Vector<Size>& start, const Func& func, const Vector<Size>& deriv)
    : size(start.size()),
      g(size),h(size),minus_h(size),old_g(size),old_h(size),x(start),old_x(size)
    {
        init(start, func(start), deriv);
    }   

    void init(const Vector<Size>& start, const Precision& func, const Vector<Size>& deriv)
    {

        using std::numeric_limits;
        using std::sqrt;
        x = start;

        //Start with the conjugate direction aligned with
        //the gradient
        g = deriv;
        h = g;
        minus_h=-h;

        y = func;
        old_y = y;

        tolerance = sqrt(numeric_limits<Precision>::epsilon());
        epsilon = 1e-20;
        max_iterations = size * 100;

        bracket_initial_lambda = 1;

        linesearch_tolerance =  sqrt(numeric_limits<Precision>::epsilon());
        linesearch_epsilon = 1e-20;
        linesearch_max_iterations=100;

        bracket_epsilon=1e-20;

        iterations=0;
    }


    template<class Func> void find_next_point(const Func& func)
    {
        Internal::LineSearch<Size, Precision, Func> line(x, minus_h, func);

        //Always search in the conjugate direction (h)
        //First bracket a minimum.
        Matrix<3,2,Precision> bracket = Internal::bracket_minimum_forward(y, line, bracket_initial_lambda, bracket_epsilon);
        
        double a = bracket[0][0];
        double b = bracket[1][0];
        double c = bracket[2][0];

        double a_val = bracket[0][1];
        double b_val = bracket[1][1];
        double c_val = bracket[2][1];

        old_y = y;
        old_x = x;
        iterations++;
        
        //Local maximum achieved!
        if(a==0 && b== 0 && c == 0)
            return;

        //We should have a bracket here

        if(c < b)
        {
            //Failed to bracket due to NaN, so c is the best known point.
            //Simply go there.
            x-=h * c;
            y=c_val;

        }
        else
        {
            assert(a < b && b < c);
            assert(a_val > b_val && b_val < c_val);

            //Find the real minimum
            Vector<2, Precision>  m = brent_line_search(a, b, c, b_val, line, linesearch_max_iterations, linesearch_tolerance, linesearch_epsilon);

            assert(m[0] >= a && m[0] <= c);
            assert(m[1] <= b_val);

            //Update the current position and value
            x -= m[0] * h;
            y = m[1];
        }
    }

    bool finished()
    {
        using std::abs;
        return iterations > max_iterations || 2*abs(y - old_y) <= tolerance * (abs(y) + abs(old_y) + epsilon);
    }

    void update_vectors_PR(const Vector<Size>& grad)
    {
        //Update the position, gradient and conjugate directions
        old_g = g;
        old_h = h;

        g = grad;
        //Precision gamma = (g * g - oldg*g)/(oldg * oldg);
        Precision gamma = (g * g - old_g*g)/(old_g * old_g);
        h = g + gamma * old_h;
        minus_h=-h;
    }

    template<class Func, class Deriv> bool iterate(const Func& func, const Deriv& deriv)
    {
        find_next_point(func);

        if(!finished())
        {
            update_vectors_PR(deriv(x));
            return 1;
        }
        else
            return 0;
    }
};

}