optimizer.hpp

/* Copyright (c) 2022, Gonzalo Ferrer
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *
 * optimizer.hpp
 *
 *  Created on: Jul 4, 2020
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech 
 */

#ifndef OPTIMIZER_HPP_
#define OPTIMIZER_HPP_

#include "mrob/matrix_base.hpp"

namespace mrob{

class Optimizer
{
public:
    enum optimMethod{NEWTON_RAPHSON=0, LEVENBERG_MARQUARDT_SPHER, LEVENBERG_MARQUARDT_ELLIP};
    Optimizer(matData_t solutionTolerance = 1e-4, matData_t lambda = 1e-5);
    virtual ~Optimizer();

    uint_t solve(optimMethod method, uint_t max_iters = 1e2, double lambda = 1e-5);

    virtual matData_t calculate_error() = 0;
    virtual void calculate_gradient_hessian() = 0;
    virtual void update_state() = 0;
    virtual void bookkeep_state() = 0;
    virtual void update_state_from_bookkeep() = 0;

protected:

    uint_t optimize_newton_raphson();

    virtual uint_t optimize_newton_raphson_one_iteration(bool useLambda = false) = 0;

    uint_t optimize_levenberg_marquardt();

    virtual matData_t calculate_model_fidelity(matData_t diff_error) = 0;


    optimMethod optimization_method_ {};
    matData_t solutionTolerance_;
    uint_t max_iters_;
    MatX1 gradient_, dx_;

    // Necessary for LM, Other LM parameters are set to default (see .cpp)
    matData_t lambda_;

};

class OptimizerDense : public Optimizer
{
  public:
    OptimizerDense(matData_t solutionTolerance = 1e-4, matData_t lambda = 1e-5);
    virtual ~OptimizerDense();
  protected:
    uint_t optimize_newton_raphson_one_iteration(bool useLambda = false);
    matData_t calculate_model_fidelity(matData_t diff_error);
    MatX hessian_;
};


class OptimizerSparse: public Optimizer
{
  protected:
    MatX hessian_;
};


}
#endif /* SRC_COMMON_MROB_OPTIMIZER_HPP_ */