Eigen C++非线性最小二乘法代码实现

最近在研究中需要使用LM等优化算法，对特定的东西进行优化。理论懂了一些些，论文也看了不少， 但是实际上手了两天却发现，自己根本写不出来，很多东西一知半解，现在还是要静下心，急不得啊

参考资料

• 非线性优化（一）：优化方法
• 非线性优化（二）：徒手实现LM算法
• 卡尔曼滤波家族
• 卡尔曼滤波：从入门到精通

#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <eigen3/Eigen/Cholesky>
#include <chrono>

/* 计时类 */
class Runtimer{
   
public:
    inline void start()
    {
   
        t_s_  = std::chrono::steady_clock::now();
    }
    
    inline void stop()
    {
   
        t_e_ = std::chrono::steady_clock::now();
    }
    
    inline double duration()
    {
   
        return std::chrono::duration_cast<std::chrono::duration<double>>(t_e_ - t_s_).count() * 1000.0;
    }
    
private:
    std::chrono::steady_clock::time_point t_s_; //start time ponit
    std::chrono::steady_clock::time_point t_e_; //stop time point
};

/*  优化方程 */
class LevenbergMarquardt{
   
public:
    LevenbergMarquardt(double* a, double* b, double* c):
    a_(a), b_(b), c_(c)
    {
   
        epsilon_1_ = 1e-6;
        epsilon_2_ = 1e-6;
        max_iter_ = 50; 	// 迭代次数
        is_out_ = true;
    }
    
    void setParameters(double epsilon_1, double epsilon_2, int max_iter, bool is_out)
    {
   
        epsilon_1_ = epsilon_1;
        epsilon_2_ = epsilon_2;
        max_iter_ = max_iter;
        is_out_ = is_out;
    }
    
    void addObservation(const double& x, const double& y)
    {
   
        obs_.push_back(Eigen::Vector2d(x, y));
    }
    
    void calcJ_fx() // 计算 Jacobi 矩阵
    {
   
        J_ .resize(obs_.size(), 3);
        fx_.resize(obs_.size()