Optimizer.h

Go to the documentation of this file.

#pragma once

#include <Eigen/Dense>

#include "Properties.h"
#include "OptimizationInterface.h"
#include "SparseSystem.h"
#include "Cholesky.h"
#include "Node.h"

namespace isam {

class Optimizer {

private:

  OptimizationInterface& function_system;

  Cholesky* _cholesky;

  Eigen::VectorXd gradient;

  double Delta;

  Eigen::VectorXd last_accepted_hdl;

  double current_SSE_at_linpoint;


  void update_trust_radius(double rho, double hdl_norm);

  Eigen::VectorXd compute_dog_leg(double alpha, const Eigen::VectorXd& h_sd,
      const Eigen::VectorXd& h_gn, double delta,
      double& gain_ratio_denominator);

  bool powells_dog_leg_update(double epsilon1, double epsilon3,
      SparseSystem& jacobian, Eigen::VectorXd& f_x, Eigen::VectorXd& gradient);

  void permute_vector(const Eigen::VectorXd& v, Eigen::VectorXd& p,
      const int* permutation);

  Eigen::VectorXd compute_gauss_newton_step(const SparseSystem& jacobian,
      SparseSystem* R = NULL, double lambda = 0.);

  void gauss_newton(const Properties& prop, int* num_iterations = NULL);

  void levenberg_marquardt(const Properties& prop, int* num_iterations = NULL);

  void powells_dog_leg(int* num_iterations = NULL, double delta0 = 1.0,
      int max_iterations = 0, double epsilon1 = 1e-4, double epsilon2 = 1e-4,
      double epsilon3 = 1e-4);

public:

  Optimizer(OptimizationInterface& fs)
      : function_system(fs), Delta(1.0) {
    //Initialize the Cholesky object
    _cholesky = Cholesky::Create();
  }

  void batch_optimize(const Properties& prop, int* num_iterations);

  void augment_sparse_linear_system(SparseSystem& W, const Properties& prop);

  void relinearize(const Properties& prop);

  void update_estimate(const Properties& prop);

  ~Optimizer() {
    delete _cholesky;
  }

};

}