Eigen中Levenberg-Marquardt算法的应用

Eigen中Levenberg-Marquardt算法的应用

#include <Eigen/Dense>
#include <iostream>
#include <fstream>
#include <istream>
#include <string>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>
template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct Functor
{
typedef _Scalar Scalar;
enum {
InputsAtCompileTime = NX,
ValuesAtCompileTime = NY
};
typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

int m_inputs, m_values;

Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

int inputs() const { return m_inputs; }
int values() const { return m_values; }

};
struct my_functor : Functor<float>
{
private:
Eigen::MatrixXf     mVec;
int                 mLen;
public:
void setValues(Eigen::MatrixXf vec)
{
mVec = vec;
mLen = vec.rows();
}
my_functor(void): Functor<float>(5, 500) {}
my_functor(int src, int dst): Functor<float>(src,dst) {}
int operator()(const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
{
//此处建立目标函数
return 0;
}
int df(const Eigen::VectorXf &x, Eigen::MatrixXf &fjac) const
{
//此处建立雅可比矩阵[一阶导数]
return 0;
}
float getError(const Eigen::VectorXf &x, float &maxError)
{
//此处可自行编写误差函数与最大误差检测
return result;
}
};
std::vector<float> split(std::string str,std::string pattern)
{
std::string::size_type pos;
std::vector<float> result;
str+=pattern;
int size=str.size();

for(int i=0; i<size; i++)
{
pos=str.find(pattern,i);
if(pos<size)
{
std::string s=str.substr(i,pos-i);
float num = std::atof(s.c_str());
result.push_back(num);
i=pos+pattern.size()-1;
}
}
return result;
}
void computerCalibParameters(Eigen::MatrixXf &values, std::string filename)
{
//读取测试数据
const int MAX_POINTS = 500;
values = Eigen::MatrixXf(MAX_POINTS, 4);

std::string szLine, szSub;
std::ifstream infile;
infile.open(filename.c_str());

int totalPoints = 0;
while(getline(infile, szLine))
{
//读取文本文档内容
totalPoints++;
}
infile.close();
}
int _tmain(int argc, _TCHAR* argv[])
{
for (int i = 130; i < 145; ++i)
{
Eigen::VectorXf x = Eigen::VectorXf(5, 1);
x << -4.4, 821.04, i, 0, 0; //设置初始值[很重要]
my_functor fucnctor(x.rows(), 500);
Eigen::MatrixXf values;
computerCalibParameters(values, "motorpos1.txt");
fucnctor.setValues(values);
Eigen::NumericalDiff<my_functor> numDiff(fucnctor);
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<my_functor>,float> lm(numDiff);
lm.parameters.maxfev = 100000;
lm.parameters.xtol = 1.0e-10;
int ret = lm.minimize(x);
std::cout << i << "\t";
float maxError;
std::cout << fucnctor.getError(x, maxError) << "\t";
std::cout << maxError << "\t";
std::cout << x.transpose() << "\n";
}
getchar();
return 0;
}