张正友相机标定

1.关于资料说明

张正友标定2000<>：https://www.microsoft.com/en-us/research/wp-content/uploads/2016/02/tr98-71.pdf

 

LM算法1978《The levenberg-marquardt algorithm, implementation and theory》：https://www.osti.gov/servlets/purl/7256021/

opencv代码参考：https://blog.csdn.net/hongbin_xu/article/details/78988450

其中关于calibrateCamera函数可见：https://raw.githubusercontent.com/opencv/opencv/master/modules/calib3d/src/calibration.cpp

此函数会输出一个3X3内参矩阵，一个5维畸变系数，n幅图像即对应n个3维平移向量和3X3旋转矩阵。输出都是cv::Mat数据格式。输入是n幅图像的二维像素点坐标及其对应的物理坐标系下的三维坐标（注意：采用棋盘格时，Z轴的坐标是0），数据格式：

vector> corner_points_of_all_imgs;

vector> objectPoints;

工程文件和图片见mail

matlab代码参考：http://www.vision.caltech.edu/bouguetj/calib_doc/#examples

 

2.关于calibrateCamera函数的进一步探究，注： 下载OpenCV, 用cmake 和 vs编译，可以debug到源码

定位到1372行的

static double cvCalibrateCamera2Internal( const CvMat* objectPoints,
                    const CvMat* imagePoints, const CvMat* npoints,
                    CvSize imageSize, int iFixedPoint, CvMat* cameraMatrix, CvMat* distCoeffs,
                    CvMat* rvecs, CvMat* tvecs, CvMat* newObjPoints, CvMat* stdDevs,
                    CvMat* perViewErrors, int flags, CvTermCriteria termCrit )

CvMat* cameraMatrix是地址传递。

代码分为步骤:

0.检查参数和分配内存

1.初始化内参和LM解决器

2.初始化外参

3.开始优化

4.保存结果

 

3.关于LM解决器

CvLevMarq solver( nparams, 0, termCrit );//即初始化类得到实例

关于nparams

const int NINTRINSIC = CV_CALIB_NINTRINSIC;

nparams = NINTRINSIC + nimages*6;

-----------

#define     CV_CALIB_NINTRINSIC  18//#include "opencv2/core/types_c.h"

以下来自：https://blog.csdn.net/xuelangwin/article/details/81095651

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 mask;                            // 标记优化过程中不优化的量。0代表不优化，1代表优化。大小跟param相同
    cv::Ptr prevParam;                       // 前一步的优化参数，用途有两个：1、判断变量是否在变化，不变化停止迭代。2、在此参数上减去一个高斯方向得到下一个参数。
    cv::Ptr param;                           // 所有要优化的变脸集合
    cv::Ptr J;                               // 目标函数的偏导数
    cv::Ptr err;                             // 测量误差向量
    cv::Ptr JtJ;                             // 正规方程左侧的部分
    cv::Ptr JtJN;                            // 去掉非优化变量后的正规方程左侧部分
    cv::Ptr JtErr;                           // 正规方程右侧的部分 
    cv::Ptr JtJV;                            // 去掉非优化变量的JtErr，
    cv::Ptr 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;                                // 正规方程的解法
};

张正友标定相机opencv源码简化版【单独抠出opencv源码用于个人相机标定】：https://github.com/Dhtu/question8

python版标定：https://github.com/Dhtu/-question6

4.内参的初始化

在初始化内参的过程中，会先计算单应矩阵，再根据r1和r2的正交性得出一个非齐次线性方程组（见公式张论文中3和4公式，由于这里先把有平移向量的单应矩阵转换为无平移向量的单应矩阵，故这里会只有二个参数，二个式子，对一幅图像而言），接着求解出n幅图像的单应矩阵，列出n个非齐次线性方程组，但是参数只有两个1/fx^2和1/fy^2，接着利用

cvSolve(matA, _b, &_focal_length, CV_NORMAL + CV_SVD);//Least-squares solution

最小二乘算法求解出这两个参数。主点就是图像的中心点。

这一共四个参数作为LM的初始值。

5.外参的初始化

在内参的基础上，利用张论文中9公式后的公式计算得出r1,r2,r3和t，其中主要涉及矩阵求逆、矩阵相乘和向量叉乘运算。

为了进一步优化外参，这里也采用LM算法对外参进行了优化。

CvLevMarq solver(6, count * 2, cvTermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, max_iter, FLT_EPSILON), true);

6.关于LM算法的计算过程

解决非线性最小化问题

x是一个n维向量，F是一个映射，该映射通过参数与向量做运算，最终输出一个m维向量，向量的范数最小，即求得最小时参数的值是多少。其中fi是一个多元函数，即输入一个多元变量，输出一个值，这里有m个多元函数映射，即对x做m次映射，将映射后的值的平方相加再除以2，记为代价函数的值。

其中张正友标定算法中，第一个包括4个内参+5个畸变系数；第二个包括n幅图像对应的3维旋转向量和3维平移向量。