Skip to content

Instantly share code, notes, and snippets.

@mitmul
Last active December 16, 2015 05:08
Show Gist options
  • Save mitmul/5381919 to your computer and use it in GitHub Desktop.
Save mitmul/5381919 to your computer and use it in GitHub Desktop.
#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;
}
#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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment