C++ 用Eigen的非线性求解LevenbergMarquardt，亲测ok

Eigen的非线性求解LevenbergMarquardt_eigen求非线性方程-CSDN博客

#ifndef Optimization_HEADER
#define Optimization_HEADER
#endif
#pragma once

#include <Eigen/Eigen>
#include <unsupported/Eigen/NonLinearOptimization>
#include "../math/vector.h"

template <class T>
class CostFunction {

public:
    using Scalar = typename T::Scalar ;
    
    static constexpr auto InputsAtCompileTime = Eigen::Dynamic ;
	static constexpr auto ValuesAtCompileTime = Eigen::Dynamic ;
    using InputType = Eigen::Matrix<Scalar ,InputsAtCompileTime ,1> ;
	using ValueType = Eigen::Matrix<Scalar ,ValuesAtCompileTime ,1> ;
	using JacobianType = Eigen::Matrix<Scalar ,ValuesAtCompileTime ,InputsAtCompileTime> ;

private:
    T &mSolver ;

public:
    CostFunction () = delete ; 

    explicit CostFunction (T &solver) :
         mSolver (solver) {}

    int inputs () const {
        return mSolver.inputs () ;
    }
    
    int values () const {
        return mSolver.values () ;
    }

    inline int operator() (const Eigen::Matrix<Scalar ,Eigen::Dynamic ,1> &param ,Eigen::Matrix<Scalar ,Eigen::Dynamic ,1> &residual) const {
		mSolver.cost_function (param ,residual) ;
		return 0 ;
	}
};


class OptimizerSolver  {
public:
    using Scalar = double ;
private:
    
    friend CostFunction<OptimizerSolver> ;
    const std::vector<math::Vec3d>& mPoints ;
    Eigen::VectorXd& mParameters ;

    Eigen::Matrix<Scalar ,Eigen::Dynamic ,1> mOptimizingParam ;
	Eigen::DenseIndex mDenseIndex ;

public:
    OptimizerSolver () = delete ;

    explicit OptimizerSolver (const std::vector<math::Vec3d>&points ,
                              Eigen::VectorXd& parameter )
                              : mPoints (points) ,
                                mParameters (parameter) {}
    /*需要优化的参数个数*/
    int inputs () const {
        return 6 ;
    }

    /*cost function 的项数*/
    int values () const {
        return 100 ;
    }

    /** 优化方程 costfunction
     *  param1 param 需要优化的参数
     *  param2 residual cost function的每一项
    */
    void cost_function (const Eigen::VectorXd& param , Eigen::VectorXd& residual ) const {
   
        const auto r1x = decode_param (param) ;
        for (int i = 0 ; i < 100 ; i++) {
            // residual[i] = cost function的每一项 ;
        }
        
    }

    /*执行优化*/
    inline void operator() () {
		mOptimizingParam = encode_param (mParameters) ;
		mDenseIndex = Eigen::DenseIndex () ;
		const auto r1x = CostFunction<OptimizerSolver> (*this) ;
		const auto r2x = Eigen::LevenbergMarquardt<const CostFunction<OptimizerSolver> ,Scalar>::lmdif1 (r1x ,mOptimizingParam ,&mDenseIndex) ;
        assert(r2x > 0 ) ;
        const auto r3x = decode_param (mOptimizingParam) ;
        mParameters = r3x.row(0) ; // 更新参数
	}

private:
    /** 把需要优化的参数变成向量
    */
    static Eigen::VectorXd encode_param (const Eigen::VectorXd& parameter) {
        Eigen::VectorXd ret (6) ;
        
        return ret ;
    }

    /** 把之前编码成向量的参数变回原来的参数模式
    */
    static Eigen::MatrixXd decode_param (const Eigen::VectorXd& param) {
        Eigen::MatrixXd ret (2 ,4) ;

        return ret ;
    }
};

简直省了一大步，直接调用就行了。

用法：

		OptimizerSolver Lmsolver(points, initRigidParams);
		Lmsolver();

就是这么方便!