LM 优化算法 opencv中的实现

此类实现LM算法，注意优化函数的导数以及误差都是在此类外部实现的。

参考例程：opencv中的单目相机和双目相机标定都是用此方法，进行非线性优化；

注意：opencv中的LM算法没有针对稀疏结构进行优化。

class CV_EXPORTS CvLevMarq
{
public:
    CvLevMarq();

    CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria=
              cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON),
              bool completeSymmFlag=false );
    ~CvLevMarq();
	/*
	* 功能：
	* 参数：
	*   [in]      nparams                    优化变量的参数
	*   [in]      nerrs                      nerrs代表样本的个数（一个测量点代表两个样本x和y）。
	                                         nerrs = 0，优化时使用updateAlt函数，因为此函数不关心样本个数（JtJ外部计算，自然不需要知道J的大小）
											 nerrs > 0，优化时使用update函数，应为内部需要使用J的大小，所以J的维度需要提前声明，其维度大小为nerrs*nparams
	*   [in]      criteria                   迭代停止标准
	*   [in]      completeSymmFlag           当nerrs=0时有效。防止外部计算得到的JtJ不对称。此标记位不管是false和true都会使JtJ 变为对称。
	*
	*  返回值：
	*
	*  备注：
	*             此函数相对于updateAlt函数，计算相对简单，但自己自由发挥的空间较少。例如代权重的最小二乘此函数就无法实现。
	*
	*/
    void init( int nparams, int nerrs, CvTermCriteria criteria=
              cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON),
              bool completeSymmFlag=false );




	/*
	* 功能： 
	* 参数：
	*   [out]      param       需要优化的参数，根据内部参数的优化状况，对当前参数进行输出，方便外部误差的计算
	*   [in]       J           目标函数的偏导数，内部计算JtJ
	*   [in]       err         内部计算JtErr 和 errNorm
	*
	*  返回值：
	*
	*  备注：
	*             此函数相对于updateAlt函数，计算相对简单，但自己自由发挥的空间较少。例如代权重的最小二乘此函数就无法实现。
	*
	*/
    bool update( const CvMat*& param, CvMat*& J, CvMat*& err );
	/*
	* 功能：
	* 参数：
	*   [out]      param         需要优化的参数，根据内部参数的优化状况，对当前参数进行输出，方便外部误差的计算
	*   [in]       JtJ           正规方程左侧的部分，此变量与类内部成员变量关联，在此函数后面对其赋值。可以变成J'ΩJ
	*   [in]       JtErr         正规方程右侧的部分，此变量与类内部成员变量关联，在此函数后面对其赋值。
	*   [in]       errNorm       测量误差（测量值减去计算出的测量值的模），此变量与类内部成员变量关联，在此函数后面对其赋值。
	*
	*  返回值：
	*
	*  备注：
	*      目标函数的导数，正规方程的左侧和右侧计算都是在外部完成计算的，测量误差的计算也是在外部完成的。     
	*/
	bool updateAlt( const CvMat*& param, CvMat*& JtJ, CvMat*& JtErr, double*& errNorm );                 // 控制优化过程中的逻辑步骤（DONE, STARTED, CALC_J, CHECK_ERR）

    void clear();
    void step();                                                                                         // 计算正规方程的解，用SVD计算
    enum { DONE=0, STARTED=1, CALC_J=2, CHECK_ERR=3 };

    cv::Ptr<CvMat> mask;                            // 标记优化过程中不优化的量。0代表不优化，1代表优化。大小跟param相同
    cv::Ptr<CvMat> prevParam;                       // 前一步的优化参数，用途有两个：1、判断变量是否在变化，不变化停止迭代。2、在此参数上减去一个高斯方向得到下一个参数。
    cv::Ptr<CvMat> param;                           // 所有要优化的变脸集合
    cv::Ptr<CvMat> J;                               // 目标函数的偏导数
    cv::Ptr<CvMat> err;                             // 测量误差向量
    cv::Ptr<CvMat> JtJ;                             // 正规方程左侧的部分
    cv::Ptr<CvMat> JtJN;                            // 去掉非优化变量后的正规方程左侧部分
    cv::Ptr<CvMat> JtErr;                           // 正规方程右侧的部分 
    cv::Ptr<CvMat> JtJV;                            // 去掉非优化变量的JtErr，
    cv::Ptr<CvMat> JtJW;                            // 去掉非优化变量后待求向量
    double prevErrNorm, errNorm;                    // 测量误差，更新前的测量误差，更新后的测量误差
    int lambdaLg10;                                 // LM 算法中的lanbda，此处的lamdaLg10 加1代表lambda变大10倍，减1缩小10倍
    CvTermCriteria criteria;
    int state;                                      // 优化步骤中的状态
    int iters;                                      // 记录迭代的次数
    bool completeSymmFlag;                          // 使去掉非优化变量后的JtJN变成对称矩阵，只是在updateAlt函数是有用。
    int solveMethod;                                // 正规方程的解法
};