单目相机标定实现--张正友标定法

---

---

原文链接：地址

个人笔记：
本次介绍针对于单目相机标定，实现方法：张正友标定法。

一：相机坐标系，像素平面坐标系，世界坐标系，归一化坐标系介绍

1：概述

如图，现实世界中有一个P点和一个相机（光心），描述这个P点的空间坐标首先得有一个坐标系，那么以光心为原点O建一个坐标系，叫相机坐标系。
那么就可以在相机坐标系下，设$P坐标(X,Y,Z)$和P的投影点$P^{\prime }(x^{\prime },y^{\prime },z^{\prime })$。值得一提的是，$P^{\prime }(x^{\prime },y^{\prime },z^{\prime })$坐落在物理成像平面和像素平面。
物理成像平面，像素平面是二维的，他们的坐标系并不一样：
物理成像平面在$O^{\prime }(x^{\prime },y^{\prime })$平面上；
像素平面的原点在那个黑灰色图的左上角(图片的左上角)，横轴向右称为$u$轴，纵轴向下称为$v$轴。
这样就得到了$P^{\prime }$的像素坐标$P(u,v)$，称为$Puv$。

所谓的归一化的成像平面，就是将三维空间点的坐标都除以Z，在相机坐标系下，P有X, Y, Z 三个量，如果把它们投影到归一化平面 Z = 1 上，就会得到P的归一化坐标P(X/Z, Y/Z, 1)。

公式

[ X Y Z 1 ] − > \left[\begin{array}{c} X \\ Y \\ Z \\ 1 \end{array}\right]-> ​XYZ1​ ​−>物体坐标.

$\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]->$外参

[ α γ u 0 0 0 β v 0 0 0 0 1 0 ] − > \left[\begin{array}{cccc} \alpha & \gamma & u_{0} & 0 \\ 0 & \beta & v_{0} & 0 \\ 0 & 0 & 1 & 0 \end{array}\right]-> ​α00​γβ0​u0​v0​1​000​ ​−>内参

[ u v 1 ] − > \left[\begin{array}{l} u \\ v \\ 1 \end{array}\right]-> ​uv1​ ​−>像素坐标

其中外参$T$是平移向量$(t1,t2,t3{)}^T$.
$R$旋转矩阵如下图：

二:实现

1：整体流程

第1步，第2步，第3步 暂不介绍了（可以用halcon算子块或者OpenCV获取特征点坐标），主要介绍获取到特征点以后，优化获取参数部分。

4：求出每张图像的单应性矩阵并用LMA优化

如何计算单应性矩阵：
[ x b y b w b ] = [ h 1   h 2   h 3   h 4   h 5   h 6   h 7   h 8   h 9 ] [ x a y a w a ] x b w b = h 1 x a + h 2 y a + h 3 w a h 7 x a + h 8 y a + h 9 w a = h 1 x a / w a + h 2 y a / w a + h 3 h 7 x a / w a + h 8 y a / w a + h 9 y b w b = h 4 x a + h 5 y a + h 6 w a h 7 x a + h 8 y a + h 9 w a = h 4 x a / w a + h 5 y a / w a + h 6 h 7 x a / w a + h 8 y a / w a + h 9 \begin{array}{c} \left[\begin{array}{l} x_{b} \\ y_{b} \\ w_{b} \end{array}\right]=\left[\begin{array}{ccc} \mathrm{h}_{1} & \mathrm{~h}_{2} & \mathrm{~h}_{3} \\ \mathrm{~h}_{4} & \mathrm{~h}_{5} & \mathrm{~h}_{6} \\ \mathrm{~h}_{7} & \mathrm{~h}_{8} & \mathrm{~h}_{9} \end{array}\right]\left[\begin{array}{l} x_{a} \\ y_{a} \\ w_{a} \end{array}\right] \\\\ \frac{x_{b}}{w_{b}}=\frac{h_{1} x_{a}+\mathrm{h}_{2} y_{a}+\mathrm{h}_{3} w_{a}}{h_{7} x_{a}+\mathrm{h}_{8} y_{a}+\mathrm{h}_{9} w_{a}}=\frac{h_{1} x_{a} / w_{a}+\mathrm{h}_{2} y_{a} / w_{a}+\mathrm{h}_{3}}{h_{7} x_{a} / w_{a}+\mathrm{h}_{8} y_{a} / w_{a}+\mathrm{h}_{9}} \\ \frac{y_{b}}{w_{b}}=\frac{h_{4} x_{a}+\mathrm{h}_{5} y_{a}+\mathrm{h}_{6} w_{a}}{h_{7} x_{a}+\mathrm{h}_{8} y_{a}+\mathrm{h}_{9} w_{a}}=\frac{h_{4} x_{a} / w_{a}+\mathrm{h}_{5} y_{a} / w_{a}+\mathrm{h}_{6}}{h_{7} x_{a} / w_{a}+\mathrm{h}_{8} y_{a} / w_{a}+\mathrm{h}_{9}} \end{array} ​xb​yb​wb​​ ​= ​h1​ h4​ h7​​ h2​ h5​ h8​​ h3​ h6​ h9​​ ​ ​xa​ya​wa​​ ​wb​xb​​=h7​xa​+h8​ya​+h9​wa​h1​xa​+h2​ya​+h3​wa​​=h7​xa​/wa​+h8​ya​/wa​+h9​h1​xa​/wa​+h2​ya​/wa​+h3​​wb​yb​​=h7​xa​+h8​ya​+h9​wa​h4​xa​+h5​ya​+h6​wa​​=h7​xa​/wa​+h8​ya​/wa​+h9​h4​xa​/wa​+h5​ya​/wa​+h6​​​

$\text{ 令 }{u}_a=\frac{x_a}{w_a},v_a=\frac{y_a}{w_a},u_b=\frac{x_b}{w_b},v_b=\frac{y_b}{w_b}\text{, 上式化简为 }$
$\begin{array}{l}{u}_b=\frac{h_1{u}_a+{\mathrm{h}}_2{v}_a+{\mathrm{h}}_3}{h_7{u}_a+{\mathrm{h}}_8{v}_a+{\mathrm{h}}_9}\\ v_b=\frac{h_4{u}_a+{\mathrm{h}}_5{v}_a+{\mathrm{h}}_6}{h_7{u}_a+{\mathrm{h}}_8{v}_a+{\mathrm{h}}_9}\end{array}$

$\begin{array}{c}{h}_1{u}_a+h_2{v}_a+h_3-h_7{u}_a{u}_b-h_8{v}_a{u}_b-h_9{u}_b=0\\ h_4{u}_a+h_5{v}_a+h_6-h_7{u}_a{v}_b-h_8{v}_a{v}_b-h_9{v}_b=0\end{array}$

$\text{ 可以直接设 }\Vert h{\Vert }_2^2=1\text{ ，此时仍然可以固定住尺度，且有: }$

此时系数矩阵秩为8，有线性空间解，解的自由度为1，满足齐次性，又由于限制单位长度，有唯一解，此时仍可以通过SVD分解求解出 h，从而得到单应矩阵。

代码实现：

//获取标准差
double CameraCalibration::StdDiffer(const Eigen::VectorXd& data)
{
    //获取平均值
    double mean = data.mean();
    //std::sqrt((Σ(x-_x)²) / n)
    return std::sqrt((data.array() - mean).pow(2).sum() / data.rows());
}

// 归一化
Eigen::Matrix3d CameraCalibration::Normalization (const Eigen::MatrixXd& P)
{
    Eigen::Matrix3d T;
    double cx = P.col ( 0 ).mean();
    double cy = P.col ( 1 ).mean();

    double stdx = StdDiffer(P.col(0));
    double stdy = StdDiffer(P.col(1));;


    double sqrt_2 = std::sqrt ( 2 );
    double scalex = sqrt_2 / stdx;
    double scaley = sqrt_2 / stdy;

    T << scalex, 0, -scalex*cx,
            0, scaley, -scaley*cy,
            0, 0, 1;
    return T;
}

//获取初始矩阵H
Eigen::VectorXd CameraCalibration::GetInitialH (const Eigen::MatrixXd& srcNormal,const Eigen::MatrixXd& dstNormal )
{
    Eigen::Matrix3d realNormMat = Normalization(srcNormal);
    Eigen::Matrix3d picNormMat = Normalization(dstNormal);

    int n = srcNormal.rows();
    // 2. DLT
    Eigen::MatrixXd input ( 2*n, 9 );

    for ( int i=0; i<n; ++i )
    {
        //转换齐次坐标
        Eigen::MatrixXd singleSrcCoor(3,1),singleDstCoor(3,1);
        singleSrcCoor(0,0) = srcNormal(i,0);
        singleSrcCoor(1,0) = srcNormal(i,1);
        singleSrcCoor(2,0) = 1;
        singleDstCoor(0,0) = dstNormal(i,0);
        singleDstCoor(1,0) = dstNormal(i,1);
        singleDstCoor(2,0) = 1;


        //坐标归一化
        Eigen::MatrixXd realNorm(3,1),picNorm(3,1);
        realNorm = realNormMat * singleSrcCoor;
        picNorm = picNormMat * singleDstCoor;

        input ( 2*i, 0 ) = realNorm ( 0, 0 );
        input ( 2*i, 1 ) = realNorm ( 1, 0 );
        input ( 2*i, 2 ) = 1.;
        input ( 2*i, 3 ) = 0.;
        input ( 2*i, 4 ) = 0.;
        input ( 2*i, 5 ) = 0.;
        input ( 2*i, 6 ) = -picNorm ( 0, 0 ) * realNorm ( 0, 0 );
        input ( 2*i, 7 ) = -picNorm ( 0, 0 ) * realNorm ( 1, 0 );
        input ( 2*i, 8 ) = -picNorm ( 0, 0 );

        input ( 2*i+1, 0 ) = 0.;
        input ( 2*i+1, 1 ) = 0.;
        input ( 2*i+1, 2 ) = 0.;
        input ( 2*i+1, 3 ) = realNorm ( 0, 0 );
        input ( 2*i+1, 4 ) = realNorm ( 1, 0 );
        input ( 2*i+1, 5 ) = 1.;
        input ( 2*i+1, 6 ) = -picNorm ( 1, 0 ) * realNorm ( 0, 0 );
        input ( 2*i+1, 7 ) = -picNorm ( 1, 0 ) * realNorm ( 1, 0 );
        input ( 2*i+1, 8 ) = -picNorm ( 1, 0 );
    }

    // 3. SVD分解
    JacobiSVD<Eigen::MatrixXd> svdSolver ( input, Eigen::ComputeFullU | Eigen::ComputeFullV );
    Eigen::MatrixXd V = svdSolver.matrixV();
    Eigen::Matrix3d H = V.rightCols(1).reshaped<RowMajor>(3,3);
    //去归一化
    H = (picNormMat.inverse() * H) * realNormMat;
    H /= H(2,2);
    return H.reshaped<RowMajor>(9,1);
}

求出初始单应性矩阵$h$，然后用$LMA$优化，得到具有实际意义的单应性矩阵。

优化代码如下：
求残差值向量：

//单应性残差值向量
class HomographyResidualsVector
{
public:
    Eigen::VectorXd  operator()(const Eigen::VectorXd& parameter,const QList<Eigen::MatrixXd> &otherArgs)
    {
        Eigen::MatrixXd inValue = otherArgs.at(0);
        Eigen::MatrixXd outValue = otherArgs.at(1);
        int dataCount = inValue.rows();
        //保存残差值
        Eigen::VectorXd residual(dataCount*2) ,residualNew(dataCount*2);
        Eigen::Matrix3d HH =  parameter.reshaped<RowMajor>(3,3);
        //获取预测偏差值 r= ^y(预测值) - y(实际值)
        for(int i=0;i<dataCount;++i)
        {
            //转换齐次坐标
            Eigen::VectorXd singleRealCoor(3),U(3);
            singleRealCoor(0,0) = inValue(i,0);
            singleRealCoor(1,0) = inValue(i,1);
            singleRealCoor(2,0) = 1;
            U = HH * singleRealCoor;
            U /= U(2);

            residual(i*2) = U(0);
            residual(i*2+1) = U(1);

            residualNew(i*2) = outValue(i,0);
            residualNew(i*2+1) = outValue(i,1);
        }

        residual -= residualNew;
        return residual;
    }

};

求雅克比矩阵构建原函数：
$\begin{array}{l}{u}_b=\frac{h_1{u}_a+{\mathrm{h}}_2{v}_a+{\mathrm{h}}_3}{h_7{u}_a+{\mathrm{h}}_8{v}_a+{\mathrm{h}}_9}\\ v_b=\frac{h_4{u}_a+{\mathrm{h}}_5{v}_a+{\mathrm{h}}_6}{h_7{u}_a+{\mathrm{h}}_8{v}_a+{\mathrm{h}}_9}\end{array}$

#define DERIV_STEP 1e-5

//求单应性雅克比矩阵
class HomographyJacobi
{
    //求偏导1
    double PartialDeriv_1(const Eigen::VectorXd& parameter,int paraIndex,const Eigen::MatrixXd &inValue,int objIndex)
    {
        Eigen::VectorXd para1 = parameter;
        Eigen::VectorXd para2 = parameter;
        para1(paraIndex) -= DERIV_STEP;
        para2(paraIndex) += DERIV_STEP;

        //逻辑
        double obj1 = ((para1(0))*inValue(objIndex,0) + para1(1)*inValue(objIndex,1) + para1(2)) / (para1(6)*inValue(objIndex,0) + para1(7)*inValue(objIndex,1) + para1(8));
        double obj2 = ((para2(0))*inValue(objIndex,0) + para2(1)*inValue(objIndex,1) + para2(2)) / (para2(6)*inValue(objIndex,0) + para2(7)*inValue(objIndex,1) + para2(8));;

        return (obj2 - obj1) / (2 * DERIV_STEP);
    }

    //求偏导2
    double PartialDeriv_2(const Eigen::VectorXd& parameter,int paraIndex,const Eigen::MatrixXd &inValue,int objIndex)
    {
        Eigen::VectorXd para1 = parameter;
        Eigen::VectorXd para2 = parameter;
        para1(paraIndex) -= DERIV_STEP;
        para2(paraIndex) += DERIV_STEP;

        //逻辑
        double obj1 = ((para1(3))*inValue(objIndex,0) + para1(4)*inValue(objIndex,1) + para1(5)) / (para1(6)*inValue(objIndex,0) + para1(7)*inValue(objIndex,1) + para1(8));
        double obj2 = ((para2(3))*inValue(objIndex,0) + para2(4)*inValue(objIndex,1) + para2(5)) / (para2(6)*inValue(objIndex,0) + para2(7)*inValue(objIndex,1) + para2(8));;

        return (obj2 - obj1) / (2 * DERIV_STEP);
    }
public:

    Eigen::MatrixXd operator()(const Eigen::VectorXd& parameter,const QList<Eigen::MatrixXd> &otherArgs)
    {
        Eigen::MatrixXd inValue = otherArgs.at(0);
        int rowNum = inValue.rows();

        Eigen::MatrixXd Jac(rowNum*2, parameter.rows());

        for (int i = 0; i < rowNum; i++)
        {
            //            //第一种方法：直接求偏导
            //            double sx = parameter(0)*inValue(i,0) + parameter(1)*inValue(i,1) + parameter(2);
            //            double sy = parameter(3)*inValue(i,0) + parameter(4)*inValue(i,1) + parameter(5);
            //            double w = parameter(6)*inValue(i,0) + parameter(7)*inValue(i,1) + parameter(8);
            //            double w2 = w*w;

            //            Jac(i*2,0) = inValue(i,0)/w;
            //            Jac(i*2,1) = inValue(i,1)/w;
            //            Jac(i*2,2) = 1/w;
            //            Jac(i*2,3) = 0;
            //            Jac(i*2,4) = 0;
            //            Jac(i*2,5) = 0;
            //            Jac(i*2,6) = -sx*inValue(i,0)/w2;
            //            Jac(i*2,7) = -sx*inValue(i,1)/w2;
            //            Jac(i*2,8) = -sx/w2;

            //            Jac(i*2+1,0) = 0;
            //            Jac(i*2+1,1) = 0;
            //            Jac(i*2+1,2) = 0;
            //            Jac(i*2+1,3) = inValue(i,0)/w;
            //            Jac(i*2+1,4) = inValue(i,1)/w;
            //            Jac(i*2+1,5) = 1/w;
            //            Jac(i*2+1,6) = -sy*inValue(i,0)/w2;
            //            Jac(i*2+1,7) = -sy*inValue(i,1)/w2;
            //            Jac(i*2+1,8) = -sy/w2;

            //第二种方法: 计算求偏导

            Jac(i*2,0) = PartialDeriv_1(parameter,0,inValue,i);
            Jac(i*2,1) = PartialDeriv_1(parameter,1,inValue,i);
            Jac(i*2,2) = PartialDeriv_1(parameter,2,inValue,i);
            Jac(i*2,3) = 0;
            Jac(i*2,4) = 0;
            Jac(i*2,5) = 0;
            Jac(i*2,6) = PartialDeriv_1(parameter,6,inValue,i);
            Jac(i*2,7) = PartialDeriv_1(parameter,7,inValue,i);
            Jac(i*2,8) = PartialDeriv_1(parameter,8,inValue,i);

            Jac(i*2+1,0) = 0;
            Jac(i*2+1,1) = 0;
            Jac(i*2+1,2) = 0;
            Jac(i*2+1,3) = PartialDeriv_2(parameter,3,inValue,i);
            Jac(i*2+1,4) = PartialDeriv_2(parameter,4,inValue,i);
            Jac(i*2+1,5) = PartialDeriv_2(parameter,5,inValue,i);
            Jac(i*2+1,6) = PartialDeriv_2(parameter,6,inValue,i);
            Jac(i*2+1,7) = PartialDeriv_2(parameter,7,inValue,i);
            Jac(i*2+1,8) = PartialDeriv_2(parameter,8,inValue,i);

        }
        return Jac;
    }
};
//求具有实际意义单应性矩阵H
Eigen::Matrix3d  CameraCalibration::GetHomography(const Eigen::MatrixXd& src,const Eigen::MatrixXd& dst)
{

    //获取初始单应性矩阵 -- svd
    Eigen::VectorXd H = GetInitialH(src,dst);
    QList<Eigen::MatrixXd> otherValue{src,dst};
    //非线性优化 H 参数 -- LM算法

    H =GlobleAlgorithm::getInstance()->LevenbergMarquardtAlgorithm(H,otherValue,HomographyResidualsVector(),HomographyJacobi());
    H /= H(8);
    // std::cout<<"H:"<

    return  H.reshaped<RowMajor>(3,3);
}

LevenbergMarquardtAlgorithm 函数实现地址在这边文章有介绍：LM算法实现

5：求解理想无畸变情况下的摄像机的内参数和外参数

在求取了单应性矩阵后, 还要进一步求出摄像机的内参数。首先令 $h_i$
表示 $H$ 的 每一列向量, 于是有:

$\left[\begin{array}{lll}{h}_1& h_2& h_3\end{array}\right]=\lambda K\left[\begin{array}{lll}{r}_1& r_2& t\end{array}\right]$

又因为$r_1$ 和 $r_2$ 是单位正交向量, 所以有 :

$\begin{array}{rl}{h}_1^T{K}^{-T}{K}^{-1}{h}_2& =0\\ h_1^T{K}^{-T}{K}^{-1}{h}_1& =h_2^T{K}^{-T}{K}^{-1}{h}_2\end{array}$

则：

这样就为内参数的求解提供了两个约束方程，令：
由于 $B$ 为对称矩阵, 所以它可以由一个 6 维向量来定义, 即:
$b={\left[\begin{array}{llllll}{B}_{11}& B_{12}& B_{22}& B_{13}& B_{23}& B_{33}\end{array}\right]}^T$

$令H的第i列向量为h_i={\left[\begin{array}{lll}{h}_{i1}& h_{i2}& h_{i3}\end{array}\right]}^T,则:$
$h_i^{T}B{h}_j=V_{ij}^{T}b$

其中：

所以组成一个方程组为：
$V为2n\ast 6矩阵$。如果$n＞=3$,则可以列出6个方程，从而解出6个内参数。这6个解出的内部参数不是唯一的，而是通过了一个比例因子缩放。求出内参：

即可求出相机内参矩阵：

内参求出后，求外参：
再根据$\left[\begin{array}{lll}{\mathbf{h}}_1& {\mathbf{h}}_2& {\mathbf{h}}_3\end{array}\right]=\lambda \mathbf{A}\left[\begin{array}{lll}{\mathbf{r}}_1& {\mathbf{r}}_2& \mathbf{t}\end{array}\right]$化简可得外部参数，即:

代码实现：

//根据单应性矩阵H返回pq位置对应的v向量
Eigen::VectorXd CameraCalibration::CreateV(int p, int q,const Eigen::Matrix3d& H)
{
    Eigen::VectorXd v(6,1);

    v << H(0, p) * H(0, q),
            H(0, p) * H(1, q) + H(1, p) * H(0, q),
            H(1, p) * H(1, q),
            H(2, p) * H(0, q) + H(0, p) * H(2, q),
            H(2, p) * H(1, q) + H(1, p) * H(2, q),
            H(2, p) * H(2, q);
    return v;

}
//求相机内参
Eigen::Matrix3d  CameraCalibration::GetIntrinsicParameter(const QList<Eigen::Matrix3d>& HList)
{
    int HCount = HList.count();
    //构建V矩阵
    Eigen::MatrixXd V(HCount*2,6);
    for(int i=0;i<HCount;++i)
    {
        V.row(i*2) = CreateV(0, 1, HList.at(i)).transpose();
        V.row(i*2+1) = (CreateV(0, 0, HList.at(i)) - CreateV(1, 1, HList.at(i))).transpose();
    }

    //Vb = 0
    //svd分解求x
    JacobiSVD<Eigen::MatrixXd> svd(V, Eigen::ComputeFullU | Eigen::ComputeFullV);
    //获取V矩阵最后一列就是b的值
    Eigen::VectorXd b = svd.matrixV().rightCols(1);
    double B11 = b(0);
    double B12 = b(1);
    double B22 = b(2);
    double B13 = b(3);
    double B23 = b(4);
    double B33 = b(5);

    double v0 = (B12*B13 - B11*B23) /  (B11*B22 - B12*B12);
    double lambda = B33 - (B13*B13 + v0*(B12*B13 - B11*B23))/B11;
    //double lambda = 1.0;
    double alpha = qSqrt(lambda / B11);
    double beta = qSqrt(lambda*B11 / (B11*B22 - B12 *B12));
    double gamma = (- B12*alpha*alpha*beta) / lambda;
    double u0 = (gamma*v0 / beta) - (B13 * alpha * alpha / lambda);

    Eigen::Matrix3d K;
    K<<alpha,gamma,u0,
           0,beta,v0,
           0,0,1;
    return  K;
}


//求相机外参
QList<Eigen::MatrixXd>  CameraCalibration::GetExternalParameter(const QList<Eigen::Matrix3d>& HList,const Eigen::Matrix3d& intrinsicParam)
{
    QList<Eigen::MatrixXd> exterParame;
    //内参逆矩阵
    Eigen::Matrix3d intrinsicParamInv = intrinsicParam.inverse();
    int HCount = HList.count();
    for(int i=0;i<HCount;++i)
    {
        Eigen::Matrix3d H = HList.at(i);
        Eigen::Vector3d h0,h1,h2;
        h0 = H.col(0);
        h1 = H.col(1);
        h2 = H.col(2);

        Eigen::Vector3d  r0,r1,r2,t;
        //比例因子λ
        double scaleFactor = 1 / (intrinsicParamInv * h0).lpNorm<2>();
        r0 = scaleFactor * (intrinsicParamInv * h0);
        r1 = scaleFactor * (intrinsicParamInv * h1);
        r2 = r0.cross(r1);
        t = scaleFactor * (intrinsicParamInv * h2);
        Eigen::MatrixXd Rt(3,4);
        Rt.col(0) = r0;
        Rt.col(1) = r1;
        Rt.col(2) = r2;
        Rt.col(3) = t;
        exterParame.append(Rt);
        // std::cout<<"Rt"<
    }

    return exterParame;
}

//无畸变优化
    Eigen::VectorXd disCoeff1(0);
    //GetDistortionCoeff(srcL,dstL,A,W,disCoeff);
    //OptimizeParameter 优化函数后面会介绍
    OptimizeParameter(srcL,dstL,A,disCoeff1,W);

6：应用最小二乘求出实际的畸变系数

相机主要包括径向畸变和切向畸变

代码实现：

//获取畸变系数 k1,k2,[p1,p2,[k3]]
void CameraCalibration::GetDistortionCoeff(const QList<Eigen::MatrixXd>&  srcL,const  QList<Eigen::MatrixXd>&  dstL,const Eigen::Matrix3d& intrinsicParam ,const QList<Eigen::MatrixXd>& externalParams,Eigen::VectorXd & disCoeff)
{
    //按照畸变个数获取参数
    int disCount = disCoeff.rows();

    if(!(disCount == 2 || disCount == 4 || disCount == 5))
    {
        qDebug()<<QString("畸变参数个数按照数组大小为2或者4或者5,请重新设置数组大小！");
        return;
    }
    int count = srcL.count();
    double u0 = intrinsicParam(0,2);
    double v0 = intrinsicParam(1,2);
    int rowS = 0;
    int k =  0;
    //获取数据个数
    for(int i=0;i<count;++i)
    {
        rowS += srcL.at(i).rows();
    }
    //初始化数据大小
    Eigen::MatrixXd D(rowS*2,disCount),d(rowS*2,1);
    for(int i=0;i<count;++i)
    {
        Eigen::MatrixXd src = srcL.at(i);
        int dataCount = src.rows();
        Eigen::MatrixXd dst = dstL.at(i);
        Eigen::MatrixXd externalParam = externalParams.at(i);

        for(int j=0;j<dataCount;++j)
        {
            //转换齐次坐标
            Eigen::VectorXd singleCoor(4);
            singleCoor(0) = src(j,0);
            singleCoor(1) = src(j,1);
            singleCoor(2) = 0.0;
            singleCoor(3) = 1.0;

            //用现有的内参及外参求估计图像坐标
            Eigen::VectorXd imageCoorEstimate = intrinsicParam * externalParam * singleCoor;
            //归一化图像坐标
            double u_estimate = imageCoorEstimate(0)/imageCoorEstimate(2);
            double v_estimate = imageCoorEstimate(1)/imageCoorEstimate(2);

            //相机坐标系下的坐标
            Eigen::VectorXd normCoor = externalParam * singleCoor;
            //归一化坐标
            normCoor /= normCoor(2);

            double r = std::sqrt(std::pow(normCoor(0),2) + std::pow(normCoor(1),2));

            //k1,k2
            if(disCount >= 2)
            {
                D(k,0) = (u_estimate - u0)*std::pow(r,2);
                D(k,1) = (u_estimate - u0)*std::pow(r,4);

                D(k+1,0) = (v_estimate - v0)*std::pow(r,2);
                D(k+1,1) = (v_estimate - v0)*std::pow(r,4);
            }
            //k1,k2,p1,p2
            if(disCount >= 4)
            {
                D(k,2) = (u_estimate - u0)*(v_estimate - v0)*2;
                D(k,3) = 2 * std::pow((u_estimate - u0),2) + std::pow(r,2);

                D(k+1,2) = 2 * std::pow((v_estimate - v0),2) + std::pow(r,2);
                D(k+1,3) = (u_estimate - u0)*(v_estimate - v0)*2;
            }
            //k1,k2,p1,p2,k3
            if(disCount >= 5)
            {
                D(k,4) = (u_estimate - u0)*std::pow(r,6);

                D(k+1,4) = (v_estimate - v0)*std::pow(r,6);
            }
            d(k,0) = dst(j,0) - u_estimate;
            d(k+1,0) = dst(j,1) - v_estimate;
            k += 2;
        }
    }
    // 最小二乘求解畸变系数  disCoeff 
    disCoeff = GlobleAlgorithm::getInstance()->LeastSquares(D,d);

}

LeastSquares函数：最小二乘实现

7：综合内参，外参，畸变系数，使用极大似然法(LMA)，优化估计，提升估计精度

构建原函数模型：
$\min {\sum }_{i=1}^n{\sum }_{j=1}^m{\Vert m_{ij}-\hat{m}\Vert }^2$

其中$m_{ij}$为实际像素坐标（算法提取到的），$\hat{m}$为重投影点（利用内参外参畸变计算得到的）

1:如何计算$\hat{m}$
设内参矩阵K = [ f x γ u 0 0 f y v 0 0 0 1 ] \left[\begin{array}{ccc} fx & \gamma & u_{0} \\ 0 & fy & v_{0} \\ 0 & 0 & 1 \end{array}\right] ​fx00​γfy0​u0​v0​1​ ​
旋转向量$r=[r1,r2,r3]$ （旋转向量有旋转矩阵转换得到实现链接），平移向量$[t1,t2,t3]$，令$\theta =|r|=\sqrt{r_1^2+r_2^2+r_3^2}$，记$r^{\prime }=\frac{r}{|r|}(单位化)=\frac{1}{\sqrt{r_1^2+r_2^2+r_3^2}}(r)$
记某角点世界坐标为 [ X Y Z ] = P → \left[\begin{array}{c} X \\ Y \\ Z \end{array}\right]=\overrightarrow{P} ​XYZ​ ​=P
则（旋转后）：
( X r Y r Z r ) = cos ⁡ θ ⋅ ( X Y Z ) + ( 1 − cos ⁡ θ ) ⋅ ( p ⃗ ⋅ r ′ → ) r ′ → + sin ⁡ θ ⋅ r ′ → × p ⃗ \left(\begin{array}{l} X_{r} \\ Y_{r} \\ Z_{r} \end{array}\right)=\cos \theta \cdot\left(\begin{array}{l} X \\ Y \\ Z \end{array}\right)+(1-\cos \theta) \cdot\left(\vec{p} \cdot \overrightarrow{r^{\prime}}\right) \overrightarrow{r^{\prime}}+\sin \theta \cdot \overrightarrow{r^{\prime}} \times \vec{p} ​Xr​Yr​Zr​​ ​=cosθ⋅ ​XYZ​ ​+(1−cosθ)⋅(p ​⋅r′ )r′ +sinθ⋅r′ ×p ​
= ( cos ⁡ θ ⋅ X cos ⁡ θ ⋅ Y cos ⁡ θ ⋅ Z ) + ( 1 − cos ⁡ θ ) ⋅ r 1 X + r 2 Y + r 3 Z r 1 2 + r 2 2 + r 3 2 ⋅ 1 r 1 2 + r 2 2 + r 3 2 ( r 1 r 2 r 3 ) + sin ⁡ θ ⋅ ( r 1 r 2 r 3 ) ⋅ 1 r 1 2 + r 2 2 + r 3 2 × ( X Y Z ) =\left(\begin{array}{l} \cos \theta \cdot X\\ \cos \theta \cdot Y\\ \cos \theta \cdot Z \end{array}\right)+(1-\cos \theta) \cdot\frac{r_1X+r_2Y+r_3Z}{\sqrt{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}}\cdot \frac{1}{\sqrt{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}}\left(\begin{array}{l} r1 \\ r2 \\ r3 \end{array}\right)+\sin \theta \cdot\left(\begin{array}{l} r1 \\ r2 \\ r3 \end{array}\right)\cdot \frac{1}{\sqrt{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}}×\left(\begin{array}{l} X \\ Y \\ Z \end{array}\right) = ​cosθ⋅Xcosθ⋅Ycosθ⋅Z​ ​+(1−cosθ)⋅r12​+r22​+r32​ ​r1​X+r2​Y+r3​Z​⋅r12​+r22​+r32​ ​1​ ​r1r2r3​ ​+sinθ⋅ ​r1r2r3​ ​⋅r12​+r22​+r32​ ​1​× ​XYZ​ ​

= ( cos ⁡ θ ⋅ X cos ⁡ θ ⋅ Y cos ⁡ θ ⋅ Z ) + ( 1 − cos ⁡ θ ) r 1 X + r 2 Y + r 3 Z r 1 2 + r 2 2 + r 3 2 ( r 1 r 2 r 3 ) + sin ⁡ θ r 1 2 + r 2 2 + r 3 2 ( r 2 Z − r 3 Y r 3 X − r 1 Z r 1 Y − r 2 X ) =\left(\begin{array}{c} \cos \theta \cdot X \\ \cos \theta \cdot Y \\ \cos \theta \cdot Z \end{array}\right)+(1-\cos \theta)\frac{r_{1} X+r_{2} Y+r_{3} Z}{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}\left(\begin{array}{l} r_{1} \\ r_{2} \\ r_{3} \end{array}\right)+\frac{\sin \theta}{\sqrt{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}}\left(\begin{array}{l} r_{2} Z-r_{3} Y \\ r_{3} X-r_{1} Z \\ r_{1} Y-r_{2} X \end{array}\right) = ​cosθ⋅Xcosθ⋅Ycosθ⋅Z​ ​+(1−cosθ)r12​+r22​+r32​r1​X+r2​Y+r3​Z​ ​r1​r2​r3​​ ​+r12​+r22​+r32​ ​sinθ​ ​r2​Z−r3​Yr3​X−r1​Zr1​Y−r2​X​ ​
平移后：
( X r t Y r t Z r t ) = ( cos ⁡ θ ⋅ X cos ⁡ θ ⋅ Y cos ⁡ θ ⋅ Z ) + ( 1 − cos ⁡ θ ) r 1 X + r 2 Y + r 3 Z r 1 2 + r 2 2 + r 3 2 ( r 1 r 2 r 3 ) + sin ⁡ θ r 1 2 + r 2 2 + r 3 2 ( r 2 Z − r 3 Y r 3 X − r 1 Z r 1 Y − r 2 X ) + ( t 1 t 2 t 3 ) \left(\begin{array}{l} X_{rt} \\ Y_{rt} \\ Z_{rt} \end{array}\right)=\left(\begin{array}{c} \cos \theta \cdot X \\ \cos \theta \cdot Y \\ \cos \theta \cdot Z \end{array}\right)+(1-\cos \theta)\frac{r_{1} X+r_{2} Y+r_{3} Z}{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}\left(\begin{array}{l} r_{1} \\ r_{2} \\ r_{3} \end{array}\right)+\frac{\sin \theta}{\sqrt{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}}\left(\begin{array}{l} r_{2} Z-r_{3} Y \\ r_{3} X-r_{1} Z \\ r_{1} Y-r_{2} X \end{array}\right)+\left(\begin{array}{l} t_{1} \\ t_{2} \\ t_{3} \end{array}\right) ​Xrt​Yrt​Zrt​​ ​= ​cosθ⋅Xcosθ⋅Ycosθ⋅Z​ ​+(1−cosθ)r12​+r22​+r32​r1​X+r2​Y+r3​Z​ ​r1​r2​r3​​ ​+r12​+r22​+r32​ ​sinθ​ ​r2​Z−r3​Yr3​X−r1​Zr1​Y−r2​X​ ​+ ​t1​t2​t3​​ ​

(这个公式没有考虑到畸变参数）重投影 m ^ → = [ u v c ] = [ f x γ u 0 0 f y v 0 0 0 1 ] ⋅ ( X r t Y r t Z r t ) = [ f x ⋅ X r t + Y r t ⋅ γ + u 0 ⋅ Z r t f y ⋅ Y r t + v 0 ⋅ Z r t Z r t ] \overrightarrow {\hat m} = \left[\begin{array}{l} u \\ v \\ c \end{array}\right]=\left[\begin{array}{ccc} fx& \gamma & u_{0} \\ 0 & fy & v_{0} \\ 0 & 0 & 1 \end{array}\right] \cdot\left(\begin{array}{l} X_{rt} \\ Y_{rt}\\ Z_{rt} \end{array}\right)=\left[\begin{array}{c} f_{x} \cdot X_{rt}+Y_{rt} \cdot \gamma +u_{0} \cdot Z_{rt} \\ f_y \cdot Y_{rt}+v_{0}\cdot Z_{rt} \\ Z_{rt} \end{array}\right] m^ = ​uvc​ ​= ​fx00​γfy0​u0​v0​1​ ​⋅ ​Xrt​Yrt​Zrt​​ ​= ​fx​⋅Xrt​+Yrt​⋅γ+u0​⋅Zrt​fy​⋅Yrt​+v0​⋅Zrt​Zrt​​ ​
即：$\begin{array}{l}{u}^{\prime }=\frac{f_x\cdot X_{rt}+Y_{rt}\cdot \gamma +u_0\cdot Z_{rt}}{Z_{rt}}=\frac{X_{rt}}{Z_{rt}}\cdot f_x+\frac{Y_{rt}}{Z_{rt}}\cdot \gamma +u_0\\ v^{\prime }=\frac{f_y\cdot Y_{rt}+v_0\cdot Z_{rt}}{Z_{rt}}=\frac{Y_{rt}}{Z_{rt}}\cdot f_y+v_0\end{array}$
完整原函数模型（含畸变）：

旋转矩阵和旋转向量互相转换，后面代码会用到

//旋转矩阵 --> 旋转向量    :罗德里格斯公式逆变换
    Vector3d Rodrigues(const Matrix3d& R)
    {
        Eigen::AngleAxisd rotAA2(R);
        Vector3d r{rotAA2.angle() * rotAA2.axis()};


        //        double theta = acos((R.trace() - 1) * 0.5);
        //        Matrix3d r_hat = (R - R.transpose()) * 0.5 / sin(theta);
        //        Vector3d r1;
        //        r1(0) = theta*(r_hat(2,1) - r_hat(1,2))*0.5;
        //        r1(1) = theta*(r_hat(0,2) - r_hat(2,0))*0.5;
        //        r1(2) = theta*(r_hat(1,0) - r_hat(0,1))*0.5;
        //        std::cout<<"R.trace():"<
        return r;
    }
    //旋转向量 --> 旋转矩阵  :罗德里格斯公式
    Matrix3d Rodrigues(const Vector3d& _r)
    {
        // 第1种方法
        Eigen::AngleAxisd  rotAA{_r.norm(), _r.normalized()};
        Matrix3d R {rotAA.toRotationMatrix()};

        //    // 第2种方法
        //    double theta = _r.lpNorm<2>();
        //    Vector3d r = _r / theta;
        //    Matrix3d r_hat;
        //    r_hat << 0, -r[2], r[1],
        //            r[2], 0, -r[0],
        //            -r[1], r[0], 0;

        //    Matrix3d R = cos(theta) * Matrix3d::Identity() + (1 - cos(theta)) * r * r.transpose() + sin(theta) * r_hat;
        //    std::cout << "R :" << R << std::endl;
        return R;
    }

代码实现：

#define DERIV_STEP 1e-5
#define INTRINSICP_COUNT 5 //内参个数
typedef struct _CameraOtherParameter
{
    QList<Eigen::MatrixXd>  srcL;              //物体点
    QList<Eigen::MatrixXd>  dstL;              //图像点
    int intrinsicCount;                //内参个数
    int disCount;                       //畸变个数 //畸变系数 2:k1,k2,(4:)p1,p2,[(5:)k3]
    int imageCount;                     // 图像个数

}S_CameraOtherParameter;

//相机标定残差值向量 -- 返回所有点的真实世界坐标映射到图像坐标 与 真实图像坐标的残差
class CalibrationResidualsVector
{
    //返回从真实世界坐标映射的图像坐标
    Eigen::Vector3d getMapCoor(const Eigen::Matrix3d& intrinsicParam ,const Eigen::VectorXd& distortionCoeff,const  Eigen::MatrixXd& externalParam,const Eigen::Vector3d& XYZ)
    {
        //畸变个数
        int disCount = distortionCoeff.rows();
        //转换齐次坐标
        Eigen::VectorXd singleCoor(4);
        singleCoor(0) = XYZ(0);
        singleCoor(1) = XYZ(1);
        singleCoor(2) = XYZ(2);
        singleCoor(3) = 1;

        //归一化坐标
        Eigen::Vector3d normCoor = externalParam * singleCoor;
        normCoor /= normCoor(2);

        double r = std::sqrt(std::pow(normCoor(0),2) + std::pow(normCoor(1),2));


        Eigen::Vector3d uv;
        uv(0)=0;
        uv(1)=0;
        uv(2)=1;

        //无畸变参数
        if(disCount == 0)
        {
            uv(0) = normCoor(0);
            uv(1) = normCoor(1);
        }
        double u_2=0,v_2=0,u_4=0,v_4=0,u_5=0,v_5=0,u_8=0,v_8=0,u_12=0,v_12=0;
        //k1,k2
        if(disCount >= 2)
        {
            u_2 = normCoor(0)*(1+std::pow(r,2)*distortionCoeff(0) + std::pow(r,4) * distortionCoeff(1));
            v_2 = normCoor(1)*(1+std::pow(r,2)*distortionCoeff(0) + std::pow(r,4) * distortionCoeff(1));
            uv(0) += u_2;
            uv(1) += v_2;
        }
        //k1,k2,p1,p2
        if(disCount >= 4)
        {
            u_4 = (2*normCoor(0)*normCoor(1)*distortionCoeff(2) + (2*std::pow(normCoor(0),2) + std::pow(r,2))*distortionCoeff(3));
            v_4 = ((2*std::pow(normCoor(1),2) + std::pow(r,2))*distortionCoeff(2) + normCoor(0)*normCoor(1)*2*distortionCoeff(3));
            uv(0) += u_4;
            uv(1) += v_4;
        }
        //k1,k2,p1,p2,k3
        if(disCount >= 5)
        {
            u_5 = normCoor(0)*std::pow(r,6)*distortionCoeff(4);
            v_5 = normCoor(1)*std::pow(r,6)*distortionCoeff(4);
            uv(0) += u_5;
            uv(1) += v_5;
        }
        //k1,k2,p1,p2,k3,k4,k5,k6
        if(disCount >= 8)
        {
            u_8 = (u_2 + u_5) / (1+std::pow(r,2)*distortionCoeff(5) + std::pow(r,4) * distortionCoeff(6) + std::pow(r,6)*distortionCoeff(7)) + u_4;
            v_8 = (v_2 + v_5) / (1+std::pow(r,2)*distortionCoeff(5) + std::pow(r,4) * distortionCoeff(6) + std::pow(r,6)*distortionCoeff(7)) + v_4;
            uv(0) = u_8;
            uv(1) = v_8;
        }
        //k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4
        if(disCount >= 12)
        {
            u_12 = std::pow(r,2)*distortionCoeff(8) + std::pow(r,4)*distortionCoeff(9);
            v_12 = std::pow(r,2)*distortionCoeff(10) + std::pow(r,4)*distortionCoeff(11);
            uv(0) += u_12;
            uv(1) += v_12;
        }
        uv = intrinsicParam * uv;

        return uv;
    }

public:
    Eigen::VectorXd  operator()(const Eigen::VectorXd& parameter,const S_CameraOtherParameter &otherArgs)
    {
        //获取数据总个数
        int allCount=0;
        for(int i=0;i<otherArgs.imageCount;++i)
        {
            allCount += otherArgs.srcL.at(i).rows();
        }
        Eigen::VectorXd real_uv(allCount*2),map_uv(allCount*2);

        //内参
        Eigen::Matrix3d intrinsicParam;

        intrinsicParam<<parameter(0),parameter(1),parameter(3),
                0,parameter(2),parameter(4),
                0,0,1;



        //畸变系数
        Eigen::VectorXd distortionCoeff(otherArgs.disCount);
        for(int i=0;i<otherArgs.disCount;++i)
        {
            distortionCoeff(i) = parameter(otherArgs.intrinsicCount+i);
        }
        //索引k存放数据
        int k=0;
        for(int i=0;i<otherArgs.imageCount;++i)
        {
            Eigen::MatrixXd src = otherArgs.srcL.at(i);
            Eigen::MatrixXd dst = otherArgs.dstL.at(i);
            int srcCount = src.rows();

            //外参
            Eigen::MatrixXd W(3,4);
            Eigen::Vector3d r ;
            r(0) = parameter(otherArgs.intrinsicCount+otherArgs.disCount+i*6);
            r(1) = parameter(otherArgs.intrinsicCount+otherArgs.disCount+i*6+1);
            r(2) = parameter(otherArgs.intrinsicCount+otherArgs.disCount+i*6+2);
            W.block(0,0,3,3) = GlobleAlgorithm::getInstance()->Rodrigues(r);
            W(0,3) = parameter(otherArgs.intrinsicCount+otherArgs.disCount+i*6+3);
            W(1,3) = parameter(otherArgs.intrinsicCount+otherArgs.disCount+i*6+4);
            W(2,3) = parameter(otherArgs.intrinsicCount+otherArgs.disCount+i*6+5);

            //遍历当前图片数据点
            for(int j=0;j<srcCount;++j)
            {
                //物体坐标
                Eigen::Vector3d XYZ;
                XYZ<<src(j,0),
                        src(j,1),
                        0;

                Eigen::Vector3d uv = getMapCoor(intrinsicParam,distortionCoeff,W,XYZ);
                map_uv(k) = uv(0);
                map_uv(k+1) = uv(1);

                real_uv(k) = dst(j,0);
                real_uv(k+1) = dst(j,1);

                k += 2;
            }
        }
        //获取预测偏差值 r=   ^y(预测值) - y(实际值)
        return map_uv - real_uv;
    }
};
//求相机标定雅克比矩阵
class CalibrationJacobi
{
    //求偏导1
    double PartialDeriv_1(const Eigen::VectorXd& parameter,int paraIndex, const S_CameraOtherParameter &otherArgs,int i,int j)
    {
        Eigen::VectorXd para1 = parameter;
        Eigen::VectorXd para2 = parameter;
        para1(paraIndex) -= DERIV_STEP;
        para2(paraIndex) += DERIV_STEP;


        double obj1 =0,obj2 =0;
        //坐标
        double x = otherArgs.srcL.at(i)(j,0);
        double y = otherArgs.srcL.at(i)(j,1);
        double z = 0;
        //旋转向量
        double r_1 = para1(otherArgs.intrinsicCount+otherArgs.disCount+i*6);
        double r_2 = para1(otherArgs.intrinsicCount+otherArgs.disCount+i*6+1);
        double r_3 = para1(otherArgs.intrinsicCount+otherArgs.disCount+i*6+2);
        //平移向量
        double t_1 = para1(otherArgs.intrinsicCount+otherArgs.disCount+i*6+3);
        double t_2 = para1(otherArgs.intrinsicCount+otherArgs.disCount+i*6+4);
        double t_3 = para1(otherArgs.intrinsicCount+otherArgs.disCount+i*6+5);


        double x1 = (((1-cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))))*r_1*(r_1*x+r_3*z+r_2*y))/(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*x+(sin(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*(r_2*z-r_3*y))/std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+t_1)/((sin(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*(r_1*y-r_2*x))/std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+((1-cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))))*r_3*(r_1*x+r_3*z+r_2*y))/(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*z+t_3);
        double y1 = ((sin(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*(r_3*x-r_1*z))/std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+((1-cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))))*r_2*(r_1*x+r_3*z+r_2*y))/(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*y+t_2)/((sin(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*(r_1*y-r_2*x))/std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+((1-cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))))*r_3*(r_1*x+r_3*z+r_2*y))/(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*z+t_3);
        double r = std::sqrt(std::pow(x1,2)+std::pow(y1,2));

        double x11=0,y11=0;
        //无畸变参数
        if(otherArgs.disCount == 0)
        {
            x11 = x1;
            y11 = y1;
        }

        double u_2=0,v_2=0,u_4=0,v_4=0,u_5=0,v_5=0,u_8=0,v_8=0,u_12=0,v_12=0;
        //k1,k2
        if(otherArgs.disCount >= 2)
        {
            u_2 = x1*(1+std::pow(r,2)*para1(otherArgs.intrinsicCount) + std::pow(r,4) * para1(otherArgs.intrinsicCount+1));
            v_2 = y1*(1+std::pow(r,2)*para1(otherArgs.intrinsicCount) + std::pow(r,4) * para1(otherArgs.intrinsicCount+1));
            x11 += u_2;
            y11 += v_2;
        }
        //k1,k2,p1,p2
        if(otherArgs.disCount >= 4)
        {
            u_4 = (2*x1*y1*para1(otherArgs.intrinsicCount+2) + (2*std::pow(x1,2) + std::pow(r,2))*para1(otherArgs.intrinsicCount+3));
            v_4 = ((2*std::pow(y1,2) + std::pow(r,2))*para1(otherArgs.intrinsicCount+2) + x1*y1*2*para1(otherArgs.intrinsicCount+3));
            x11 += u_4;
            y11 += v_4;
        }
        //k1,k2,p1,p2,k3
        if(otherArgs.disCount >= 5)
        {
            u_5 = x1*std::pow(r,6)*para1(otherArgs.intrinsicCount+4);
            v_5 = y1*std::pow(r,6)*para1(otherArgs.intrinsicCount+4);
            x11 += u_5;
            y11 += v_5;
        }
        //k1,k2,p1,p2,k3,k4,k5,k6
        if(otherArgs.disCount >= 8)
        {
            u_8 = (u_2 + u_5) / (1+std::pow(r,2)*para1(otherArgs.intrinsicCount+5) + std::pow(r,4) * para1(otherArgs.intrinsicCount+6) + std::pow(r,6)*para1(otherArgs.intrinsicCount+7)) + u_4;
            v_8 = (v_2 + v_5) / (1+std::pow(r,2)*para1(otherArgs.intrinsicCount+5) + std::pow(r,4) * para1(otherArgs.intrinsicCount+6) + std::pow(r,6)*para1(otherArgs.intrinsicCount+7)) + v_4;
            x11 = u_8;
            y11 = v_8;
        }
        //k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4
        if(otherArgs.disCount >= 12)
        {
            u_12 = std::pow(r,2)*para1(otherArgs.intrinsicCount+8) + std::pow(r,4)*para1(otherArgs.intrinsicCount+9);
            v_12 = std::pow(r,2)*para1(otherArgs.intrinsicCount+10) + std::pow(r,4)*para1(otherArgs.intrinsicCount+11);
            x11 += u_12;
            y11 += v_12;
        }

        double f_x = para1(0);
        double gam = para1(1);
        //double f_y = para1(2);
        double u_0 = para1(3);
        //double v_0 = para1(4);

        obj1 = f_x*x11+gam*y11+u_0;




        {
            //旋转向量
            double r_1 = para2(otherArgs.intrinsicCount+otherArgs.disCount+i*6);
            double r_2 = para2(otherArgs.intrinsicCount+otherArgs.disCount+i*6+1);
            double r_3 = para2(otherArgs.intrinsicCount+otherArgs.disCount+i*6+2);
            //平移向量
            double t_1 = para2(otherArgs.intrinsicCount+otherArgs.disCount+i*6+3);
            double t_2 = para2(otherArgs.intrinsicCount+otherArgs.disCount+i*6+4);
            double t_3 = para2(otherArgs.intrinsicCount+otherArgs.disCount+i*6+5);


            double x1 = (((1-cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))))*r_1*(r_1*x+r_3*z+r_2*y))/(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*x+(sin(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*(r_2*z-r_3*y))/std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+t_1)/((sin(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*(r_1*y-r_2*x))/std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+((1-cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))))*r_3*(r_1*x+r_3*z+r_2*y))/(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*z+t_3);
            double y1 = ((sin(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*(r_3*x-r_1*z))/std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+((1-cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))))*r_2*(r_1*x+r_3*z+r_2*y))/(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*y+t_2)/((sin(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*(r_1*y-r_2*x))/std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+((1-cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))))*r_3*(r_1*x+r_3*z+r_2*y))/(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*z+t_3);
            double r = std::sqrt(std::pow(x1,2)+std::pow(y1,2));

            double x11=0,y11=0;
            //无畸变参数
            if(otherArgs.disCount == 0)
            {
                x11 = x1;
                y11 = y1;
            }

            double u_2=0,v_2=0,u_4=0,v_4=0,u_5=0,v_5=0,u_8=0,v_8=0,u_12=0,v_12=0;
            //k1,k2
            if(otherArgs.disCount >= 2)
            {
                u_2 = x1*(1+std::pow(r,2)*para2(otherArgs.intrinsicCount) + std::pow(r,4) * para2(otherArgs.intrinsicCount+1));
                v_2 = y1*(1+std::pow(r,2)*para2(otherArgs.intrinsicCount) + std::pow(r,4) * para2(otherArgs.intrinsicCount+1));
                x11 += u_2;
                y11 += v_2;
            }
            //k1,k2,p1,p2
            if(otherArgs.disCount >= 4)
            {
                u_4 = (2*x1*y1*para2(otherArgs.intrinsicCount+2) + (2*std::pow(x1,2) + std::pow(r,2))*para2(otherArgs.intrinsicCount+3));
                v_4 = ((2*std::pow(y1,2) + std::pow(r,2))*para2(otherArgs.intrinsicCount+2) + x1*y1*2*para2(otherArgs.intrinsicCount+3));
                x11 += u_4;
                y11 += v_4;
            }
            //k1,k2,p1,p2,k3
            if(otherArgs.disCount >= 5)
            {
                u_5 = x1*std::pow(r,6)*para2(otherArgs.intrinsicCount+4);
                v_5 = y1*std::pow(r,6)*para2(otherArgs.intrinsicCount+4);
                x11 += u_5;
                y11 += v_5;
            }
            //k1,k2,p1,p2,k3,k4,k5,k6
            if(otherArgs.disCount >= 8)
            {
                u_8 = (u_2 + u_5) / (1+std::pow(r,2)*para2(otherArgs.intrinsicCount+5) + std::pow(r,4) * para2(otherArgs.intrinsicCount+6) + std::pow(r,6)*para2(otherArgs.intrinsicCount+7)) + u_4;
                v_8 = (v_2 + v_5) / (1+std::pow(r,2)*para2(otherArgs.intrinsicCount+5) + std::pow(r,4) * para2(otherArgs.intrinsicCount+6) + std::pow(r,6)*para2(otherArgs.intrinsicCount+7)) + v_4;
                x11 = u_8;
                y11 = v_8;
            }
            //k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4
            if(otherArgs.disCount >= 12)
            {
                u_12 = std::pow(r,2)*para2(otherArgs.intrinsicCount+8) + std::pow(r,4)*para2(otherArgs.intrinsicCount+9);
                v_12 = std::pow(r,2)*para2(otherArgs.intrinsicCount+10) + std::pow(r,4)*para2(otherArgs.intrinsicCount+11);
                x11 += u_12;
                y11 += v_12;
            }

            double f_x = para2(0);
            double gam = para2(1);
            //double f_y = para2(2);
            double u_0 = para2(3);
           // double v_0 = para2(4);

            obj2 = f_x*x11+gam*y11+u_0;



        }

        return (obj2 - obj1) / (2 * DERIV_STEP);
    }

    //求偏导2
    double PartialDeriv_2(const Eigen::VectorXd& parameter,int paraIndex, const S_CameraOtherParameter &otherArgs,int i,int j)
    {
        Eigen::VectorXd para1 = parameter;
        Eigen::VectorXd para2 = parameter;
        para1(paraIndex) -= DERIV_STEP;
        para2(paraIndex) += DERIV_STEP;


        double obj1 =0,obj2 =0;
        //坐标
        double x = otherArgs.srcL.at(i)(j,0);
        double y = otherArgs.srcL.at(i)(j,1);
        double z = 0;
        //旋转向量
        double r_1 = para1(otherArgs.intrinsicCount+otherArgs.disCount+i*6);
        double r_2 = para1(otherArgs.intrinsicCount+otherArgs.disCount+i*6+1);
        double r_3 = para1(otherArgs.intrinsicCount+otherArgs.disCount+i*6+2);
        //平移向量
        double t_1 = para1(otherArgs.intrinsicCount+otherArgs.disCount+i*6+3);
        double t_2 = para1(otherArgs.intrinsicCount+otherArgs.disCount+i*6+4);
        double t_3 = para1(otherArgs.intrinsicCount+otherArgs.disCount+i*6+5);


        double x1 = (((1-cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))))*r_1*(r_1*x+r_3*z+r_2*y))/(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*x+(sin(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*(r_2*z-r_3*y))/std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+t_1)/((sin(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*(r_1*y-r_2*x))/std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+((1-cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))))*r_3*(r_1*x+r_3*z+r_2*y))/(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*z+t_3);
        double y1 = ((sin(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*(r_3*x-r_1*z))/std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+((1-cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))))*r_2*(r_1*x+r_3*z+r_2*y))/(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*y+t_2)/((sin(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*(r_1*y-r_2*x))/std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+((1-cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))))*r_3*(r_1*x+r_3*z+r_2*y))/(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*z+t_3);
        double r = std::sqrt(std::pow(x1,2)+std::pow(y1,2));

        double x11=0,y11=0;
        //无畸变参数
        if(otherArgs.disCount == 0)
        {
            x11 = x1;
            y11 = y1;
        }
        double u_2=0,v_2=0,u_4=0,v_4=0,u_5=0,v_5=0,u_8=0,v_8=0,u_12=0,v_12=0;
        //k1,k2
        if(otherArgs.disCount >= 2)
        {
            u_2 = x1*(1+std::pow(r,2)*para1(otherArgs.intrinsicCount) + std::pow(r,4) * para1(otherArgs.intrinsicCount+1));
            v_2 = y1*(1+std::pow(r,2)*para1(otherArgs.intrinsicCount) + std::pow(r,4) * para1(otherArgs.intrinsicCount+1));
            x11 += u_2;
            y11 += v_2;
        }
        //k1,k2,p1,p2
        if(otherArgs.disCount >= 4)
        {
            u_4 = (2*x1*y1*para1(otherArgs.intrinsicCount+2) + (2*std::pow(x1,2) + std::pow(r,2))*para1(otherArgs.intrinsicCount+3));
            v_4 = ((2*std::pow(y1,2) + std::pow(r,2))*para1(otherArgs.intrinsicCount+2) + x1*y1*2*para1(otherArgs.intrinsicCount+3));
            x11 += u_4;
            y11 += v_4;
        }
        //k1,k2,p1,p2,k3
        if(otherArgs.disCount >= 5)
        {
            u_5 = x1*std::pow(r,6)*para1(otherArgs.intrinsicCount+4);
            v_5 = y1*std::pow(r,6)*para1(otherArgs.intrinsicCount+4);
            x11 += u_5;
            y11 += v_5;
        }
        //k1,k2,p1,p2,k3,k4,k5,k6
        if(otherArgs.disCount >= 8)
        {
            u_8 = (u_2 + u_5) / (1+std::pow(r,2)*para1(otherArgs.intrinsicCount+5) + std::pow(r,4) * para1(otherArgs.intrinsicCount+6) + std::pow(r,6)*para1(otherArgs.intrinsicCount+7)) + u_4;
            v_8 = (v_2 + v_5) / (1+std::pow(r,2)*para1(otherArgs.intrinsicCount+5) + std::pow(r,4) * para1(otherArgs.intrinsicCount+6) + std::pow(r,6)*para1(otherArgs.intrinsicCount+7)) + v_4;
            x11 = u_8;
            y11 = v_8;
        }
        //k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4
        if(otherArgs.disCount >= 12)
        {
            u_12 = std::pow(r,2)*para1(otherArgs.intrinsicCount+8) + std::pow(r,4)*para1(otherArgs.intrinsicCount+9);
            v_12 = std::pow(r,2)*para1(otherArgs.intrinsicCount+10) + std::pow(r,4)*para1(otherArgs.intrinsicCount+11);
            x11 += u_12;
            y11 += v_12;
        }

        //double f_x = para1(0);
       // double gam = para1(1);
        double f_y = para1(2);
        //double u_0 = para1(3);
        double v_0 = para1(4);

        obj1 = f_y*y11+v_0;




        {
            //旋转向量
            double r_1 = para2(otherArgs.intrinsicCount+otherArgs.disCount+i*6);
            double r_2 = para2(otherArgs.intrinsicCount+otherArgs.disCount+i*6+1);
            double r_3 = para2(otherArgs.intrinsicCount+otherArgs.disCount+i*6+2);
            //平移向量
            double t_1 = para2(otherArgs.intrinsicCount+otherArgs.disCount+i*6+3);
            double t_2 = para2(otherArgs.intrinsicCount+otherArgs.disCount+i*6+4);
            double t_3 = para2(otherArgs.intrinsicCount+otherArgs.disCount+i*6+5);


            double x1 = (((1-cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))))*r_1*(r_1*x+r_3*z+r_2*y))/(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*x+(sin(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*(r_2*z-r_3*y))/std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+t_1)/((sin(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*(r_1*y-r_2*x))/std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+((1-cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))))*r_3*(r_1*x+r_3*z+r_2*y))/(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*z+t_3);
            double y1 = ((sin(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*(r_3*x-r_1*z))/std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+((1-cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))))*r_2*(r_1*x+r_3*z+r_2*y))/(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*y+t_2)/((sin(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*(r_1*y-r_2*x))/std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+((1-cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))))*r_3*(r_1*x+r_3*z+r_2*y))/(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2))+cos(std::sqrt(std::pow(r_1,2)+std::pow(r_2,2)+std::pow(r_3,2)))*z+t_3);
            double r = std::sqrt(std::pow(x1,2)+std::pow(y1,2));

            double x11=0,y11=0;
            //无畸变参数
            if(otherArgs.disCount == 0)
            {
                x11 = x1;
                y11 = y1;
            }
            double u_2=0,v_2=0,u_4=0,v_4=0,u_5=0,v_5=0,u_8=0,v_8=0,u_12=0,v_12=0;
            //k1,k2
            if(otherArgs.disCount >= 2)
            {
                u_2 = x1*(1+std::pow(r,2)*para2(otherArgs.intrinsicCount) + std::pow(r,4) * para2(otherArgs.intrinsicCount+1));
                v_2 = y1*(1+std::pow(r,2)*para2(otherArgs.intrinsicCount) + std::pow(r,4) * para2(otherArgs.intrinsicCount+1));
                x11 += u_2;
                y11 += v_2;
            }
            //k1,k2,p1,p2
            if(otherArgs.disCount >= 4)
            {
                u_4 = (2*x1*y1*para2(otherArgs.intrinsicCount+2) + (2*std::pow(x1,2) + std::pow(r,2))*para2(otherArgs.intrinsicCount+3));
                v_4 = ((2*std::pow(y1,2) + std::pow(r,2))*para2(otherArgs.intrinsicCount+2) + x1*y1*2*para2(otherArgs.intrinsicCount+3));
                x11 += u_4;
                y11 += v_4;
            }
            //k1,k2,p1,p2,k3
            if(otherArgs.disCount >= 5)
            {
                u_5 = x1*std::pow(r,6)*para2(otherArgs.intrinsicCount+4);
                v_5 = y1*std::pow(r,6)*para2(otherArgs.intrinsicCount+4);
                x11 += u_5;
                y11 += v_5;
            }
            //k1,k2,p1,p2,k3,k4,k5,k6
            if(otherArgs.disCount >= 8)
            {
                u_8 = (u_2 + u_5) / (1+std::pow(r,2)*para2(otherArgs.intrinsicCount+5) + std::pow(r,4) * para2(otherArgs.intrinsicCount+6) + std::pow(r,6)*para2(otherArgs.intrinsicCount+7)) + u_4;
                v_8 = (v_2 + v_5) / (1+std::pow(r,2)*para2(otherArgs.intrinsicCount+5) + std::pow(r,4) * para2(otherArgs.intrinsicCount+6) + std::pow(r,6)*para2(otherArgs.intrinsicCount+7)) + v_4;
                x11 = u_8;
                y11 = v_8;
            }
            //k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4
            if(otherArgs.disCount >= 12)
            {
                u_12 = std::pow(r,2)*para2(otherArgs.intrinsicCount+8) + std::pow(r,4)*para2(otherArgs.intrinsicCount+9);
                v_12 = std::pow(r,2)*para2(otherArgs.intrinsicCount+10) + std::pow(r,4)*para2(otherArgs.intrinsicCount+11);
                x11 += u_12;
                y11 += v_12;
            }

            //double f_x = para2(0);
            //double gam = para2(1);
            double f_y = para2(2);
            //double u_0 = para2(3);
            double v_0 = para2(4);

            obj2 = f_y*y11+v_0;



        }

        return (obj2 - obj1) / (2 * DERIV_STEP);
    }
public:

    Eigen::MatrixXd  operator()(const Eigen::VectorXd& parameter,const S_CameraOtherParameter &otherArgs)
    {
        //获取数据总个数
        int allCount=0;
        for(int i=0;i<otherArgs.imageCount;++i)
        {
            allCount += otherArgs.srcL.at(i).rows();
        }

        //初始化雅可比矩阵都为0
        Eigen::MatrixXd Jac = Eigen::MatrixXd::Zero(allCount*2,parameter.rows());

        int k=0;
        for(int i=0;i<otherArgs.imageCount;++i)
        {
            Eigen::MatrixXd src = otherArgs.srcL.at(i);
            int srcCount = src.rows();


            //遍历当前图片数据点
            for(int j=0;j<srcCount;++j)
            {
                //内参偏导

                Jac(k,0) = PartialDeriv_1(parameter,0,otherArgs,i,j);
                Jac(k,1) = PartialDeriv_1(parameter,1,otherArgs,i,j);
                Jac(k,2) = 0;
                Jac(k,3) = 1;
                Jac(k,4) = 0;

                Jac(k+1,0) = 0;
                Jac(k+1,1) = 0;
                Jac(k+1,2) = PartialDeriv_2(parameter,2,otherArgs,i,j);
                Jac(k+1,3) = 0;
                Jac(k+1,4) = 1;


                //畸变偏导
                //k1,k2
                if(otherArgs.disCount >= 2)
                {
                    Jac(k,otherArgs.intrinsicCount) = PartialDeriv_1(parameter,otherArgs.intrinsicCount,otherArgs,i,j);
                    Jac(k,otherArgs.intrinsicCount+1) = PartialDeriv_1(parameter,otherArgs.intrinsicCount+1,otherArgs,i,j);

                    Jac(k+1,otherArgs.intrinsicCount) = PartialDeriv_2(parameter,otherArgs.intrinsicCount,otherArgs,i,j);
                    Jac(k+1,otherArgs.intrinsicCount+1) = PartialDeriv_2(parameter,otherArgs.intrinsicCount+1,otherArgs,i,j);
                }
                //k1,k2,p1,p2
                if(otherArgs.disCount >= 4)
                {
                    Jac(k,otherArgs.intrinsicCount+2) = PartialDeriv_1(parameter,otherArgs.intrinsicCount+2,otherArgs,i,j);
                    Jac(k,otherArgs.intrinsicCount+3) = PartialDeriv_1(parameter,otherArgs.intrinsicCount+3,otherArgs,i,j);

                    Jac(k+1,otherArgs.intrinsicCount+2) = PartialDeriv_2(parameter,otherArgs.intrinsicCount+2,otherArgs,i,j);
                    Jac(k+1,otherArgs.intrinsicCount+3) = PartialDeriv_2(parameter,otherArgs.intrinsicCount+3,otherArgs,i,j);
                }
                //k1,k2,p1,p2,k3
                if(otherArgs.disCount >= 5)
                {
                    Jac(k,otherArgs.intrinsicCount+4) = PartialDeriv_1(parameter,otherArgs.intrinsicCount+4,otherArgs,i,j);

                    Jac(k+1,otherArgs.intrinsicCount+4) = PartialDeriv_2(parameter,otherArgs.intrinsicCount+4,otherArgs,i,j);
                }
                //k1,k2,p1,p2,k3,k4,k5,k6
                if(otherArgs.disCount >= 8)
                {
                    Jac(k,otherArgs.intrinsicCount+5) = PartialDeriv_1(parameter,otherArgs.intrinsicCount+5,otherArgs,i,j);
                    Jac(k,otherArgs.intrinsicCount+6) = PartialDeriv_1(parameter,otherArgs.intrinsicCount+6,otherArgs,i,j);
                    Jac(k,otherArgs.intrinsicCount+7) = PartialDeriv_1(parameter,otherArgs.intrinsicCount+7,otherArgs,i,j);

                    Jac(k+1,otherArgs.intrinsicCount+5) = PartialDeriv_2(parameter,otherArgs.intrinsicCount+5,otherArgs,i,j);
                    Jac(k+1,otherArgs.intrinsicCount+6) = PartialDeriv_2(parameter,otherArgs.intrinsicCount+6,otherArgs,i,j);
                    Jac(k+1,otherArgs.intrinsicCount+7) = PartialDeriv_2(parameter,otherArgs.intrinsicCount+7,otherArgs,i,j);
                }
                //k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4
                if(otherArgs.disCount >= 12)
                {
                    Jac(k,otherArgs.intrinsicCount+8) = PartialDeriv_1(parameter,otherArgs.intrinsicCount+8,otherArgs,i,j);
                    Jac(k,otherArgs.intrinsicCount+9) = PartialDeriv_1(parameter,otherArgs.intrinsicCount+9,otherArgs,i,j);

                    Jac(k+1,otherArgs.intrinsicCount+10) = PartialDeriv_2(parameter,otherArgs.intrinsicCount+10,otherArgs,i,j);
                    Jac(k+1,otherArgs.intrinsicCount+11) = PartialDeriv_2(parameter,otherArgs.intrinsicCount+11,otherArgs,i,j);
                }

                //外参偏导 r1,r2,r3,t1,t2,t3

                Jac(k,otherArgs.intrinsicCount+otherArgs.disCount + i*6) = PartialDeriv_1(parameter,otherArgs.intrinsicCount+otherArgs.disCount+i*6,otherArgs,i,j);
                Jac(k,otherArgs.intrinsicCount+otherArgs.disCount + i*6 + 1) = PartialDeriv_1(parameter,otherArgs.intrinsicCount+otherArgs.disCount+i*6+1,otherArgs,i,j);
                Jac(k,otherArgs.intrinsicCount+otherArgs.disCount + i*6 + 2) = PartialDeriv_1(parameter,otherArgs.intrinsicCount+otherArgs.disCount+i*6+2,otherArgs,i,j);
                Jac(k,otherArgs.intrinsicCount+otherArgs.disCount + i*6 + 3) = PartialDeriv_1(parameter,otherArgs.intrinsicCount+otherArgs.disCount+i*6+3,otherArgs,i,j);
                Jac(k,otherArgs.intrinsicCount+otherArgs.disCount + i*6 + 4) = 0;
                Jac(k,otherArgs.intrinsicCount+otherArgs.disCount + i*6 + 5) = PartialDeriv_1(parameter,otherArgs.intrinsicCount+otherArgs.disCount+i*6+5,otherArgs,i,j);

                Jac(k+1,otherArgs.intrinsicCount+otherArgs.disCount + i*6) = PartialDeriv_2(parameter,otherArgs.intrinsicCount+otherArgs.disCount+i*6,otherArgs,i,j);
                Jac(k+1,otherArgs.intrinsicCount+otherArgs.disCount + i*6 + 1) = PartialDeriv_2(parameter,otherArgs.intrinsicCount+otherArgs.disCount+i*6+1,otherArgs,i,j);;
                Jac(k+1,otherArgs.intrinsicCount+otherArgs.disCount + i*6 + 2) = PartialDeriv_2(parameter,otherArgs.intrinsicCount+otherArgs.disCount+i*6+2,otherArgs,i,j);;
                Jac(k+1,otherArgs.intrinsicCount+otherArgs.disCount + i*6 + 3) = 0;
                Jac(k+1,otherArgs.intrinsicCount+otherArgs.disCount + i*6 + 4) = PartialDeriv_2(parameter,otherArgs.intrinsicCount+otherArgs.disCount+i*6+4,otherArgs,i,j);;
                Jac(k+1,otherArgs.intrinsicCount+otherArgs.disCount + i*6 + 5) = PartialDeriv_2(parameter,otherArgs.intrinsicCount+otherArgs.disCount+i*6+5,otherArgs,i,j);;



                k += 2;
            }
        }
        return Jac;
    }
};





//整合所有参数(内参,畸变系数,外参)到一个向量中
Eigen::VectorXd CameraCalibration::ComposeParameter(const Eigen::Matrix3d& intrinsicParam ,const Eigen::VectorXd& distortionCoeff,const QList<Eigen::MatrixXd>& externalParams)
{
    //畸变参数个数
    int disCount = distortionCoeff.rows();

    //外参个数
    int exterCount=0;
    for(int i=0;i<externalParams.count();++i)
    {
        //一张图片的外参个数 R->r(9->3) + t 3 = 6
        exterCount += 6;
    }

    Eigen::VectorXd P(INTRINSICP_COUNT+disCount+exterCount);

    //整合内参
    P(0) = intrinsicParam(0,0);
    P(1) = intrinsicParam(0,1);
    P(2) = intrinsicParam(1,1);
    P(3) = intrinsicParam(0,2);
    P(4) = intrinsicParam(1,2);


    //整合畸变
    for(int i=0;i<disCount;++i)
    {
        P(INTRINSICP_COUNT+i) = distortionCoeff(i);
    }

    //整合外参
    for(int i=0;i<externalParams.count();++i)
    {
        Eigen::Matrix3d R = externalParams.at(i).block(0,0,3,3);
        //旋转矩阵转旋转向量
        Eigen::Vector3d r =  GlobleAlgorithm::getInstance()->Rodrigues(R);
        Eigen::Vector3d t = externalParams.at(i).col(3);

        P(INTRINSICP_COUNT+disCount+i*6) = r(0);
        P(INTRINSICP_COUNT+disCount+i*6+1) = r(1);
        P(INTRINSICP_COUNT+disCount+i*6+2) = r(2);

        P(INTRINSICP_COUNT+disCount+i*6+3) = t(0);
        P(INTRINSICP_COUNT+disCount+i*6+4) = t(1);
        P(INTRINSICP_COUNT+disCount+i*6+5) = t(2);
    }

    return P;
}
//分解所有参数  得到对应的内参,畸变矫正系数,外参
void CameraCalibration::DecomposeParamter(const Eigen::VectorXd &P, Eigen::Matrix3d& intrinsicParam , Eigen::VectorXd& distortionCoeff, QList<Eigen::MatrixXd>& externalParams)
{
    //内参
    intrinsicParam << P(0),P(1),P(3),
            0,P(2),P(4),
            0,0,1;


    //畸变
    for(int i =0;i<distortionCoeff.rows();++i)
    {
        distortionCoeff(i) = P(INTRINSICP_COUNT+i);
    }
    //外参
    for(int i=0;i<externalParams.count();++i)
    {
        Eigen::Vector3d r,t;
        r(0) = P(INTRINSICP_COUNT+distortionCoeff.rows()+i*6);
        r(1) =  P(INTRINSICP_COUNT+distortionCoeff.rows()+i*6+1) ;
        r(2) =  P(INTRINSICP_COUNT+distortionCoeff.rows()+i*6+2);

        t(0) =  P(INTRINSICP_COUNT+distortionCoeff.rows()+i*6+3) ;
        t(1) =  P(INTRINSICP_COUNT+distortionCoeff.rows()+i*6+4);
        t(2) =  P(INTRINSICP_COUNT+distortionCoeff.rows()+i*6+5) ;

        Eigen::Matrix3d R = GlobleAlgorithm::getInstance()->Rodrigues(r);
        externalParams[i].block(0,0,3,3) = R;
        externalParams[i].col(3) = t;
    }
}


//优化所有参数 (内参,畸变系数,外参) 返回重投影误差值
double CameraCalibration::OptimizeParameter(const QList<Eigen::MatrixXd>&  srcL,const QList<Eigen::MatrixXd>&  dstL, Eigen::Matrix3d& intrinsicParam , Eigen::VectorXd& distortionCoeff, QList<Eigen::MatrixXd>& externalParams)
{
    //整合参数
    Eigen::VectorXd P = ComposeParameter(intrinsicParam,distortionCoeff,externalParams);
    S_CameraOtherParameter cameraParam;
    cameraParam.dstL = dstL;
    cameraParam.srcL = srcL;
    cameraParam.imageCount = dstL.count();
    cameraParam.intrinsicCount = INTRINSICP_COUNT;
    cameraParam.disCount = distortionCoeff.rows();

    Eigen::VectorXd P1 = GlobleAlgorithm::getInstance()->LevenbergMarquardtAlgorithm(P,cameraParam,CalibrationResidualsVector(),CalibrationJacobi(),m_epsilon,m_maxIteCount);


    //分解参数
    DecomposeParamter(P1,intrinsicParam,distortionCoeff,externalParams);


    //计算重投影误差
    CalibrationResidualsVector reV;

    //每张图片重投影误差
    m_reprojErrL.clear()