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;
}