cv::StereoCalibrate 源码解析 （一） —— CvLevMarq求解器

解析之后：对比了下opencv，matalb、kalibr的双目校正程序；opencv的优势在于畸变参数支持比较多，应用性比较好，对于单目而言结果比较准，劣势在于双目的R,T存在问题。matlab在于其是鼻祖，opencv的函数都是迁移的matlab中的，可视化效果非常好，缺点在于代码封闭，我是没找到相关的.m文件，不知道其是如何实现的。kalibr中双目R t是先标定单目，然后初始化基线，再优化所有变量，但是python与c++混写，代码阅读起来过于困难，同时畸变模型参数比较少。同时发现一个问题 matlab和kalibr对图像要求更高一点，在检测标定板方格点就会拒绝图片，而opencv则会基本选择全部图片，从绘图中可以发现到，opencv方格点有一些选取是有问题的。

目前解决方案：opencv 计算双目每个相机的参数，将opencv采集的图像rosbag打包，利用kalibr获取R，t  。后续有时间，会扒一下kalibr的源代码，看一下怎么实现的。

=============================

工作原因，要大规模标定相机，cv::omni::StereoCalibrate出来的结果有明显问题，网上也找不到相关的解析，只有说这个函数怎么使用的，所以只能自己阅读一下StereoCalibrate的源码。

opencv 版本 4.1.1

StereoCalibrate 的步骤不是很难，可以总结为：1、检查输入值

2、确定优化变量，给所有待优化变量求解一个初值

3、利用LMsolver求解器进行求解

首先阅读CvLevMarq求解器文件

其头文件calib3d/calib3d_c.h 

源文件 calib3d/src/compat_ptsetreg.cpp

求解器运行：

1、初始定义 CvLevMarq() 给一些默认值，限制求解方法是SVD

2、清空变量与初始化 prevParam，param，JtJ，JtErr动态指针绑定内存 state = STARTED;

3、第一遍调用 CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm )

由于状态位STARTED，输入进来的指针将和CvLevMarq类的对应参量指针绑定，同时由于cvZero的原因所有的变量被赋值初值0，且状态位更改

4、第二遍调用updateAlt，状态位为CALC_J。 因为上一步进行了指针绑定，所以类内变量cv::Ptr<CvMat> JtJ 和 输入形参CvMat*& _JtJ指的是同一片内存（其他亦同）

首先对类内变量param进行了深拷贝，param=prevParam，执行step函数

5、step单步求解 首先提取了待优化变量，然后求了一个SVD获得了ΔX，然后更新了X

6、这以后比较迷惑，直接在源码上注释

//迭代
bool CvLevMarq::updateAlt( const CvMat*& _param, CvMat*& _JtJ, CvMat*& _JtErr, double*& _errNorm )
{
    CV_Assert( !err );
    if( state == DONE )
    {
        _param = param;
        return false;
    }

    if( state == STARTED )
    {
        _param = param;
        cvZero( JtJ );
        cvZero( JtErr );
        errNorm = 0;
        _JtJ = JtJ;
        _JtErr = JtErr;
        //绑定内存 初始为0
        _errNorm = &errNorm;
        state = CALC_J;
        return true;
    }

    if( state == CALC_J )
    {
        cvCopy( param, prevParam );
        step();
        _param = param;
        //errNorm与_errNorm共享内存，为当步未优化前的输入的误差值 prevErrNorm进行拷贝
        prevErrNorm = errNorm;
        //然后这里指空
        errNorm = 0;
        //重新绑定指针
        _errNorm = &errNorm;
        //更改状态
        state = CHECK_ERR;
        return true;
    }

    assert( state == CHECK_ERR );
    //这几步没理解，errNorm应该是当前重投影误差，prevErrNorm是上一步重投影误差，
    //如果当前误差，比上一步优化前的误差还糟糕 ？？？？？ 是指优化没起到作用？？？？  不应该是实际函数下降值和近似模型下降值之比么？？
    if( errNorm > prevErrNorm )
    {
        //增加lambdaLg10的值（与正则项相关），同时控制了lambdaLg10这个值最大为16，更倾向于梯度下降  ？？
        if( ++lambdaLg10 <= 16 )
        {
            //执行单步优化
            step();
            _param = param;
            errNorm = 0;
            _errNorm = &errNorm;
            state = CHECK_ERR;
            return true;
        }
    }
    //降低lambdaLg10的值，控制它最小是-16，更倾向于高斯牛顿 ？？
    lambdaLg10 = MAX(lambdaLg10-1, -16);
    //检查一下迭代步数和优化长度是否满足迭代终止条件，满足就返回false
    if( ++iters >= criteria.max_iter ||
        cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon )
    {
        _param = param;
        _JtJ = JtJ;
        _JtErr = JtErr;
        state = DONE;
        return false;
    }
    //清空JtJ和JtErr 标志位变为CALC_J    不知道是不是我理解有问题，是不是如果优化在下降，第一个数据进来step下降，第二个数据进来仅比较，第三个数据进来再step下降，这样子不会丢掉一部分数据么？？？
    prevErrNorm = errNorm;
    cvZero( JtJ );
    cvZero( JtErr );
    _param = param;
    _JtJ = JtJ;
    _JtErr = JtErr;
    state = CALC_J;
    return true;
}

void CvLevMarq::step()
{
    using namespace cv;
    const double LOG10 = log(10.);
    //
    double lambda = exp(lambdaLg10*LOG10);
    // 待优化参数个数
    int nparams = param->rows;
    //类型变换 mask 是忽视变量，这个向量在函数外面已经被赋值了
    Mat _JtJ = cvarrToMat(JtJ);
    Mat _mask = cvarrToMat(mask);
    // 是有多少个要优化的变量（部分内参不优化）
    int nparams_nz = countNonZero(_mask);
    // 第一次进来的时候 JtJN为空，所以满足条件（正常运行 只有第一次进来的时候会满足条件）
    // JtJN JtJV JtJW 被初始化
    if(!JtJN || JtJN->rows != nparams_nz) {
        // prevent re-allocation in every step
        JtJN.reset(cvCreateMat( nparams_nz, nparams_nz, CV_64F ));
        JtJV.reset(cvCreateMat( nparams_nz, 1, CV_64F ));
        JtJW.reset(cvCreateMat( nparams_nz, 1, CV_64F ));
    }
    //类型变换，共享内存
    Mat _JtJN = cvarrToMat(JtJN);
    Mat _JtErr = cvarrToMat(JtJV);
    Mat_<double> nonzero_param = cvarrToMat(JtJW);
    //JtErr是类内变量，其指向内存对应于updateAlt的_JtErr, subMatrix的意思是 取了矩阵一部分  这个很好理解，相当于划掉了部分变量
    subMatrix(cvarrToMat(JtErr), _JtErr, std::vector<uchar>(1, 1), _mask);
    //_JtJN同上       这时候 JtJN即是删减过的JtJ，JtJV即是删减过的JtErr
    subMatrix(_JtJ, _JtJN, _mask, _mask);
    //这一步保证_JtJN是对角阵，这是因为updateAlt进来的形参是右上角阵，这一步给赋值了一下     我猜err应该是判断LM有没有问题吧，没找到哪里变化的
    if( !err )
        completeSymm( _JtJN, completeSymmFlag );
    //这没啥好说的
    _JtJN.diag() *= 1. + lambda;
    //SVD求解，求一个(H+λI)ΔX = g 的ΔX     求出来赋值给 nonzero_param /* 要注意 nonzero_param，JtJW共享内存*/
    solve(_JtJN, _JtErr, nonzero_param, solveMethod);
    int j = 0;
    //对param中每个待优化变量进行赋值 X-ΔX 
    for( int i = 0; i < nparams; i++ )
        param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? nonzero_param(j++) : 0);
}

其单步迭代就是LM的传统方法，没什么疑问

但是什么时候单步迭代，以及正则项的值变化条件仍有一些疑问还未解决。

考虑到一般相机标定，都是会要求内参有优化的，都是内参+外参优化，考虑到外参的稀疏性，可以采用舒尔补来进行加速。

或者采用自适应信赖域来进行加速