optimization_algorithm_levenberg.h

#pragma once
#include "optimization_algorithm_with_hessian.h"
 
namespace NDEVR 
{
    constexpr g_type gs_epsilon = g_type(1e-6); 
    template<class t_type>
    class OptimizationAlgorithmLevenberg : public OptimizationAlgorithmWithHessian<t_type>
    {
    public:
        explicit OptimizationAlgorithmLevenberg()
        {
            _currentLambda = g_type(-1.);
            _tau = g_type(1e-5); // Carlos: originally 1e-5
            _goodStepUpperScale = g_type(2. / 3.);
            _goodStepLowerScale = g_type(1. / 3.);
            _ni = 2.;
            _levenbergIterations = 0;
            _nBad = 0;
        }

        void clear()
        {
            _currentLambda = g_type(-1.);
            _tau = g_type(1e-5); // Carlos: originally 1e-5
            _goodStepUpperScale = g_type(2. / 3.);
            _goodStepLowerScale = g_type(1. / 3.);
            _ni = 2.;
            _levenbergIterations = 0;
            _nBad = 0;
            OptimizationAlgorithmWithHessian<t_type>::clear();
        }
        ~OptimizationAlgorithmLevenberg() {};

        OptimizationAlgorithm::SolverResult solve(int iteration, bool online, IndexScratch& scratch)
        {
            
            if (iteration == 0 && !online) 
            { // built up the CCS structure, here due to easy time measure
                bool ok = OptimizationAlgorithmWithHessian<t_type>::solver.buildStructure(false, scratch);
                if (!ok)
                    return OptimizationAlgorithm::Fail;
            }
 
            OptimizationAlgorithmWithHessian<t_type>::optimizer().computeActiveErrors();
 
 
            g_type currentChi = OptimizationAlgorithmWithHessian<t_type>::optimizer().activeRobustChi2();
            g_type tempChi = currentChi;
 
            g_type iniChi = currentChi;
 
            OptimizationAlgorithmWithHessian<t_type>::solver.buildSystem();
 
            // core part of the Levenbarg algorithm
            if (iteration == 0)
            {
                _currentLambda = computeLambdaInit();
                _ni = 2;
                _nBad = 0;
            }
 
            g_type rho = 0;
            int& qmax = _levenbergIterations;
            qmax = 0;
            do 
            {
                OptimizationAlgorithmWithHessian<t_type>::optimizer().push();
                // update the diagonal of the system matrix
                OptimizationAlgorithmWithHessian<t_type>::solver.setLambda(_currentLambda, true);
                bool ok2 = OptimizationAlgorithmWithHessian<t_type>::solver.solve();
                //lib_assert(ok2, "bad solve?");
                if (ok2)
                {
                    OptimizationAlgorithmWithHessian<t_type>::optimizer().update(OptimizationAlgorithmWithHessian<t_type>::solver.x());
                    // restore the diagonal
                    OptimizationAlgorithmWithHessian<t_type>::solver.restoreDiagonal();
 
                    OptimizationAlgorithmWithHessian<t_type>::optimizer().computeActiveErrors();
                    tempChi = OptimizationAlgorithmWithHessian<t_type>::optimizer().activeRobustChi2();
                }
                else
                {
                    tempChi = std::numeric_limits<g_type>::max();
                }
                rho = (currentChi - tempChi);
                g_type scale = computeScale();
                scale += g_type(1e-3); // make sure it's non-zero :)
                rho /= scale;
 
                if (rho > gs_epsilon && std::isfinite(tempChi))
                { // last step was good
                    g_type alpha = g_type(1. - pow((2 * rho - 1), 3));
                    // crop lambda between minimum and maximum factors
                    alpha = getMin(alpha, _goodStepUpperScale);
                    g_type scaleFactor = getMax(_goodStepLowerScale, alpha);
                    _currentLambda *= scaleFactor;
                    _ni = 2;
                    currentChi = tempChi;
                    OptimizationAlgorithmWithHessian<t_type>::optimizer().discardTop();
                }
                else {
                    _currentLambda *= _ni;
                    _ni *= 2;
                    OptimizationAlgorithmWithHessian<t_type>::optimizer().pop(); // restore the last state before trying to optimize
                }
                qmax++;
            } while (rho < gs_epsilon && qmax < 10 && !OptimizationAlgorithmWithHessian<t_type>::optimizer().terminate());
 
            if (qmax == 10 || rho <= gs_epsilon)
                return OptimizationAlgorithm::Terminate;
 
            //Stop criterium (Raul)
            if ((iniChi - currentChi) * 1e3 < iniChi)
                _nBad++;
            else
                _nBad = 0;
 
            if (_nBad >= 3)
                return OptimizationAlgorithm::Terminate;
            return OptimizationAlgorithm::OK;
        }

        void optimize(uint04 iterations, bool online, IndexScratch& scratch)
        {
            //WLock lock(blockPoseIndices);
            if (!OptimizationAlgorithmWithHessian<t_type>::optimizer().isReadyToUpdate())
                return;
            if (!OptimizationAlgorithmWithHessian<t_type>::init(online))
                return;
            for (uint04 i = 0; i < iterations; i++)
            {
                if (OptimizationAlgorithmWithHessian<t_type>::optimizer().terminate())
                    return;
                OptimizationAlgorithm::SolverResult result = solve(i, online, scratch);
                if (result != OptimizationAlgorithm::OK)
                    return;
            }
        }

        g_type currentLambda() const { return _currentLambda; }

        void setUserLambdaInit(g_type lambda)
        {
            _lambdaInit = lambda;
        }

        int levenbergIteration() { return _levenbergIterations; }
 
    protected:
        // Levenberg parameters
        g_type _lambdaInit = 0.0;       
        g_type _currentLambda;          
        g_type _tau;                    
        g_type _goodStepLowerScale;     
        g_type _goodStepUpperScale;     
        g_type _ni;                     
        int _levenbergIterations;       
        int _nBad;                      

        g_type computeLambdaInit() const
        {
            if (_lambdaInit > 0.0)
                return _lambdaInit;
            g_type maxDiagonal = 0.;
            for (uint04 k = 0; k < OptimizationAlgorithmWithHessian<t_type>::optimizer().indexMapping().size(); k++) {
                OptimizableGraph::OGVertex* v = OptimizationAlgorithmWithHessian<t_type>::optimizer().indexMapping()[k];
                assert(v);
                int dim = v->dimension();
                for (int j = 0; j < dim; ++j) {
                    maxDiagonal = std::max(fabs(v->hessian(j, j)), maxDiagonal);
                }
            }
            return _tau * maxDiagonal;
        }

        g_type computeScale() const
        {
            g_type scale = 0.;
            for (size_t j = 0; j < OptimizationAlgorithmWithHessian<t_type>::solver.vectorSize(); j++) {
                scale += OptimizationAlgorithmWithHessian<t_type>::solver.x()[j] * (_currentLambda * OptimizationAlgorithmWithHessian<t_type>::solver.x()[j] + OptimizationAlgorithmWithHessian<t_type>::solver.b()[j]);
            }
            return scale;
        }
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
}