levmar.cpp

#include "levmar.h"

Levmar::Levmar()
  : param_num(0),
    func_num(0),
    iter_limit(0),
    covar(NULL)
{
  // 最適化パラメータ初期化
  optimize_param[0] = LM_INIT_MU;
  optimize_param[1] = LM_STOP_THRESH;
  optimize_param[2] = LM_STOP_THRESH;
  optimize_param[3] = LM_STOP_THRESH;
  optimize_param[4] = LM_DIFF_DELTA;
}

/*!
  * \brief 最適化するパラメータ数をセット
  * \param _param_num 最適化するパラメータの数
  */
void Levmar::setParamNum(const int _param_num)
{
  param_num = _param_num;
  params.resize(param_num);
}

/*!
  * \brief 最適化するためのコスト関数の数をセット
  * \param _func_num コスト関数の数
  */
void Levmar::setFuncNum(const int _func_num)
{
  func_num = _func_num;
}

/*!
  * \brief パラメータ初期値をセット
  * \param _init パラメータ初期値
  */
void Levmar::setParamInitVal(const vector<double> _init)
{
  init_params.resize(_init.size());
  for(int i = 0; i < (int)_init.size(); ++i)
  {
    init_params(i) = _init.at(i);
  }
}

/*!
  * \brief 各パラメータに対する探索の際の重みをセット
  * \param _param_weight 重みベクトル
  */
void Levmar::setParamWeight(const vector<double> _param_weight)
{
  param_weight = _param_weight;
}

/*!
  * \brief 各コスト関数の重要度（重み）をセット
  * \param _func_weight 重みベクトル
  */
void Levmar::setFuncWeight(const vector<double> _func_weight)
{
  func_weight = _func_weight;
}

/*!
  * \brief levmarの設定パラメータをセット
  * \param mu the scale factor for initial mu
  * \param epsilon1 stopping thresholds for ||J^T e||_inf
  * \param epsilon2 stopping thresholds for ||Dp||_2
  * \param epsilon3 stopping thresholds for ||e||_2
  * \param delta the step used in difference approximation to the Jacobian
  */
void Levmar::setOptimizeParam(double mu, double epsilon1, double epsilon2, double epsilon3, double delta)
{
  optimize_param[0] = mu; // tau: the scale factor for initial \mu
  optimize_param[1] = epsilon1; // epsilon1: stopping thresholds for ||J^T e||_inf
  optimize_param[2] = epsilon2; // epsilon2: stopping thresholds for ||Dp||_2
  optimize_param[3] = epsilon3; // epsilon3: stopping thresholds for ||e||_2
  optimize_param[4] = delta; // delta: the step used in difference approximation to the Jacobian
}

/*!
  * \brief 最適化処理の反復限界数をセット
  * \param limit 反復限界数
  */
void Levmar::setIterationLimit(const int limit)
{
  iter_limit = limit;
}

/*!
 * \brief 最適化を開始
 */
void Levmar::startOptimization()
{
  try
  {
    if(param_num != 0 && func_num != 0)
    {
      // 設定値
      cout << "Optimize Parameters:" << endl;
      cout << "tau - the scale factor for initial mu: " << optimize_param[0] << endl;
      cout << "epsilon1 - stopping thresholds for ||J^T e||_inf: " << optimize_param[1] << endl;
      cout << "epsilon2 - stopping thresholds for ||Dp||_2: " << optimize_param[2] << endl;
      cout << "epsilon3 - stopping thresholds for ||e||_2: " << optimize_param[3] << endl;
      cout << "delta - the step used in difference approximation to the Jacobian: " << optimize_param[4] << endl;

      // パラメータ初期値
      double p[param_num];
      for(int i = 0; i < param_num; ++i)
      {
        p[i] = 0.0;
      }

      // 目標値の初期値
      double x[func_num];
      for(int i = 0; i < func_num; ++i)
      {
        x[i] = 0.0;
      }

      // ベスト結果保存用
      iterations = 0;
      best_param.resize(param_num);
      best_score = DBL_MAX;

      double *work;
      work = (double*)malloc((LM_DIF_WORKSZ(param_num, func_num) + param_num * param_num) * sizeof(double));
      covar = work + LM_DIF_WORKSZ(param_num, func_num);

      // 最適化開始
      int ret = dlevmar_dif(evaluate, p, x, param_num, func_num,
                            iter_limit, optimize_param, optimize_info,
                            work, covar, this);

      // 理由が4か5である限りtauを2倍にして繰り返す
      while((int)optimize_info[6] == 4 || (int)optimize_info[6] == 5)
      {
        for(int i = 0; i < param_num; ++i)
          p[i] = best_param(i);

        optimize_param[0] *= 2;

        ret = dlevmar_dif(evaluate, p, x, param_num, func_num,
                          iter_limit, optimize_param, optimize_info,
                          work, covar, this);

        // 結果を表示
        cout << "Levenberg-Marquardt returned " << ret << endl;

        // 各値
        cout << "||e||_2 at initial p:\t" << optimize_info[0] << endl;
        cout << "||e||_2:\t\t" << optimize_info[1] << endl;
        cout << "||J^T e||_inf:\t" << optimize_info[2] << endl;
        cout << "||Dp||_2:\t\t" << optimize_info[3] << endl;
        cout << "mu / max[J^T J]_ii:\t" << optimize_info[4] << endl << endl;
        cout << "iterations: " << optimize_info[5] << endl;
      }

      // 理由
      cout << "reason: " << optimize_info[6] << " ";
      switch((int)optimize_info[6])
      {
        case 1:
          cout << "stopped by small gradient J^T e" << endl;
          break;
        case 2:
          cout << "stopped by small Dp" << endl;
          break;
        case 3:
          cout << "stopped by itmax" << endl;
          break;
        case 4:
          cout << "singular matrix. Restart from current p with increased mu" << endl;
          break;
        case 5:
          cout << "no further error reduction is possible. Restart with increased mu" << endl;
          break;
        case 6:
          cout << "stopped by small ||e||_2" << endl;
          break;
        case 7:
          cout << "stopped by invalid (i.e. NaN or Inf) \"func\" values; a user error" << endl;
          break;
      }

      cout << "function evaluations: " << optimize_info[7] << endl;
      cout << "Jacobian evaluations: " << optimize_info[8] << endl;
      cout << "linear systems solved: " << optimize_info[9] << endl << endl;

      cout << "Covariance of the fit:" << endl;
      for(int i = 0; i < param_num; ++i)
      {
        for(int j = 0; j < func_num; ++j)
        {
          cout << covar[i * param_num + j];
          if(j != func_num - 1)
            cout << "\t";
        }
        cout << endl;
      }
      cout << endl;

      // 最終パラメータ
      cout << "parameter result: ";
      for(int i = 0; i < param_num; ++i)
      {
        params(i) = best_param(i);
        cout << best_param(i);
        if(i != param_num - 1)
          cout << ", ";
      }
      cout << endl;
      cout << "final score: " << best_score << endl << endl;

      vector<double> costs = getCost();
    }
    else
    {
      cerr << "param_num or func_num is 0" << endl;
    }
  }
  catch(std::exception &ex)
  {
    std::cerr << "Optimize::startOptimization" << endl
              << ex.what() << std::endl;
  }
}

/*!
  * \brief 評価関数
  * \param *p 最適化するパラメータの配列
  * \param *hx コスト関数の値の配列
  * \param m 最適化パラメータの数
  * \param n コスト関数の数
  */
void Levmar::evaluation(double *p, double *hx, int m, int n)
{
  cout << "---------- eval " << iterations << " ----------\n";

  // パラメータ
  cout << params.rows() << " param: ";
  for(int i = 0; i < m; ++i)
  {
    params(i) = init_params(i) + p[i] * param_weight.at(i);
    cout << params(i) << "(" << param_weight.at(i) << ")\t";
  }
  cout << "\n";

  // コスト計算
  double cost_sum = 0.0;
  vector<double> costs = getCost();
  cout << "func: ";
  for(int i = 0; i < n; ++i)
  {
    hx[i] = costs.at(i) * func_weight.at(i);
    cost_sum += hx[i];
    cout << hx[i] << "(" << func_weight.at(i) << ")\t";
  }
  cout << "\n";

  // 現在のコスト合計
  cout << "cost sum: " << cost_sum << "\n" << endl;

  // これまでの最小コストとなるパラメータを保存
  if(best_score > cost_sum)
  {
    best_score = cost_sum;
    best_param = params;
  }

  ++iterations;
}

levmar.h

#ifndef LEVMAR_H
#define LEVMAR_H

#include <levmar.h>
#include <float.h>
#include <iostream>
#include <vector>
#include <Eigen/Eigen>

using namespace std;
using namespace Eigen;

/*!
 * \brief Levenberg-Marquardt法を使った非線形最適化クラス
 *
 * setParamNum, setFuncNum, setParamInitVal, setParamWeight,
 * setFuncWeight, setOptimizeParam, setIterationLimitを呼んだ
 * のちに，getCost関数をオーバロードしてることを確認してから
 * startOptimizationを呼べばよい．
 */
class Levmar
{
  public:
    Levmar();

    void setParamNum(const int _param_num);
    void setFuncNum(const int _func_num);
    void setParamInitVal(const vector<double> _init);
    void setParamWeight(const vector<double> _param_weight);
    void setFuncWeight(const vector<double> _func_weight);
    void setOptimizeParam(double mu, double epsilon1, double epsilon2, double epsilon3, double delta);
    void setIterationLimit(const int limit);

    // 評価関数
    virtual vector<double> getCost() = 0;

    void startOptimization();

  protected:
    static void evaluate(double *p, double *hx, int m, int n, void *adata)
    {
      ((Levmar*)adata)->evaluation(p, hx, m, n);
    }
    virtual void evaluation(double *p, double *hx, int m, int n);

    // 最適化設定値
    int param_num;
    int func_num;
    int iter_limit;
    vector<double> param_weight;
    vector<double> func_weight;
    double optimize_param[LM_OPTS_SZ];

    // パラメータ
    VectorXd init_params;
    VectorXd params;

    // 結果データ
    int iterations;
    VectorXd best_param;
    double best_score;
    double *covar;
    double optimize_info[LM_INFO_SZ];
};

#endif // LEVMAR_H