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

---

---

原文链接：地址

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

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

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​⎦⎥⎥⎤​−>物体坐标.

[ R T 0 1 ] − > \left[

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

[ α γ 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{, 上式化简为 }$
u b = h 1 u a + h 2 v a + h 3 h 7 u a + h 8 v a + h 9 v b = h 4 u a + h 5 v a + h 6 h 7 u a + h 8 v a + h 9

$$\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}$$ ub​=h7​ua​+h8​va​+h9​h1​ua​+h2​va​+h3​​vb​=h7​ua​+h8​va​+h9​h4​ua​+h5​va​+h6​​​

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

$$\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}$$ h1​ua​+h2​va​+h3​−h7​ua​ub​−h8​va​ub​−h9​ub​=0h4​ua​+h5​va​+h6​−h7​ua​vb​−h8​va​vb​−h9​vb​=0​

$\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);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87

求出初始单应性矩阵$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;
    }

};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

求雅克比矩阵构建原函数：
u b = h 1 u a + h 2 v a + h 3 h 7 u a + h 8 v a + h 9 v b = h 4 u a + h 5 v a + h 6 h 7 u a + h 8 v a + h 9

$$\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}$$ ub​=h7​ua​+h8​va​+h9​h1​ua​+h2​va​+h3​​vb​=h7​ua​+h8​va​+h9​h4​ua​+h5​va​+h6​​​

#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:"<<std::endl<<H<<std::endl;

    return  H.reshaped<RowMajor>(3,3);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112

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

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

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

[ h 1 h 2 h 3 ] = λ K [ r 1 r 2 t ] \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] [h1​​h2​​h3​​]=λK[r1​​r2​​t​]

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

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

$$\begin{aligned} 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{aligned}$$ h1T​K−TK−1h2​h1T​K−TK−1h1​​=0=h2T​K−TK−1h2​​

则：

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

$$\begin{array}{llllll} B_{11} & B_{12} & B_{22} & B_{13} & B_{23} & B_{33} \end{array}$$ \right]^{T} b=[B11​​B12​​B22​​B13​​B23​​B33​​]T

令 H 的 第 i 列 向 量 为 h i = [ h i 1 h i 2 h i 3 ] T , 则 : 令 H 的第 i 列向量为 h_{i}=\left[

$$\begin{array}{lll}h_{i 1} & h_{i 2} & h_{i 3}\end{array}$$ \right]^{T} , 则: 令H的第i列向量为hi​=[hi1​​hi2​​hi3​​]T,则:
$h_i^{T}B{h}_j=V_{ij}^{T}b$

其中：

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

即可求出相机内参矩阵：

内参求出后，求外参：
再根据 [ h 1 h 2 h 3 ] = λ A [ r 1 r 2 t ] \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] [h1​​h2​​h3​​]=λA[r1​​r2​​t​]化简可得外部参数，即:

代码实现：

//根据单应性矩阵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"<<i<<":"<<std::endl<<Rt<<std::endl;
    }

    return exterParame;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88

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

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);

}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86

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​⎦⎤​
旋转向量$\overrightarrow{r}=[r1,r2,r3]$ （旋转向量有旋转矩阵转换得到实现链接），平移向量$[t1,t2,t3]$，令$\theta =|\overrightarrow{r}|=\sqrt{r_1^2+r_2^2+r_3^2}$，记$\overrightarrow{r^{\prime }}=\frac{\vec{r}}{|\vec{r}|}(单位化)=\frac{1}{\sqrt{r_1^2+r_2^2+r_3^2}}(\overrightarrow{r})$
记某角点世界坐标为$\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]=\overrightarrow{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}$
$=\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_{1}X+r_{2}Y+r_{3}Z}{\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}}\times \left(\begin{array}{l}X\\ Y\\ Z\end{array}\right)$

= ( 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​⎠⎞​
平移后：
$\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)$

(这个公式没有考虑到畸变参数）重投影 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():"<<R.trace()<<"  theta: "<<theta<<std::endl<<"r:"<< r <<std::endl<<std::endl<<"r1:"<< r1 <<std::endl;
        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;
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35

代码实现：

#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();
    Eigen::VectorXd PP(INTRINSICP_COUNT+distortionCoeff.rows()+6);
    PP.block(0,0,INTRINSICP_COUNT+distortionCoeff.rows(),1) = P1.block(0,0,INTRINSICP_COUNT+distortionCoeff.rows(),1);
    for(int i=0;i<externalParams.count();++i)
    {
        PP(INTRINSICP_COUNT+distortionCoeff.rows())= P1(INTRINSICP_COUNT+distortionCoeff.rows()+i*6);
        PP(INTRINSICP_COUNT+distortionCoeff.rows()+1)= P1(INTRINSICP_COUNT+distortionCoeff.rows()+i*6+1);
        PP(INTRINSICP_COUNT+distortionCoeff.rows()+2)= P1(INTRINSICP_COUNT+distortionCoeff.rows()+i*6+2);
        PP(INTRINSICP_COUNT+distortionCoeff.rows()+3)= P1(INTRINSICP_COUNT+distortionCoeff.rows()+i*6+3);
        PP(INTRINSICP_COUNT+distortionCoeff.rows()+4)= P1(INTRINSICP_COUNT+distortionCoeff.rows()+i*6+4);
        PP(INTRINSICP_COUNT+distortionCoeff.rows()+5)= P1(INTRINSICP_COUNT+distortionCoeff.rows()+i*6+5);

        S_CameraOtherParameter cameraParam1;
        cameraParam1.dstL.append(dstL.at(i));
        cameraParam1.srcL.append(srcL.at(i));
        cameraParam1.imageCount = 1;
        cameraParam1.intrinsicCount = INTRINSICP_COUNT;
        cameraParam1.disCount = distortionCoeff.rows();

        Eigen::VectorXd reV1 = reV(PP,cameraParam1);

        int pointCount = reV1.rows()/2;
        Eigen::VectorXd errorV(pointCount);
        for(int i=0,k=0;i<pointCount;++i,k+=2)
        {
            errorV(i) = std::sqrt(std::pow(reV1(k),2) + std::pow(reV1(k+1),2));
        }

        m_reprojErrL.append(std::sqrt(errorV.sum()/pointCount));
        //qDebug()<<"errorV: "<<errorV.lpNorm<2>()<<"  :   "<<std::sqrt(errorV.sum()/pointCount)<<"  :   "<<errorV.maxCoeff()<<" :"<<i;
    }

    //总重投影误差
    Eigen::VectorXd reV1 = reV(P1,cameraParam);
    int pointCount = reV1.rows()/2;
    Eigen::VectorXd errorV(pointCount);
    for(int i=0,k=0;i<pointCount;++i,k+=2)
    {
        errorV(i) = std::sqrt(std::pow(reV1(k),2) + std::pow(reV1(k+1),2));
    }

    //qDebug()<<"errorV: "<<errorV.lpNorm<2>()<<"  :   "<<std::sqrt(errorV.sum()/pointCount)<<"  :   "<<errorV.maxCoeff();

    return std::sqrt(errorV.sum()/pointCount);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762

 //带畸变优化
 //m_disCount 畸变个数 ，可选 [2,[4,[5]]]
    Eigen::VectorXd disCoeff = Eigen::VectorXd::Zero(m_disCount);
    //获取初始畸变参数
    GetDistortionCoeff(srcL,dstL,K,W,disCoeff);
    //优化内参，外参，畸变参数
    double reprojErr = OptimizeParameter(srcL,dstL,K,disCoeff,W);
1
2
3
4
5
6
7

8：得出相机的内参，外参和畸变系数

内参 K，外参 W，畸变参数 disCoeff

9：OpenCV模型

无畸变：
含畸变：

上面代码实现畸变只包含了：k1，k2，p1，p2，k3。

OpenCV：calibrateCamera函数

三：畸变修复（去畸变）

采用双线插值法实现

代码实现：

//矫正图像 根据内参和畸变系数矫正
Eigen::MatrixXi GlobleAlgorithm::RectifiedImage(Eigen::MatrixXi src,Eigen::Matrix3d intrinsicParam , Eigen::VectorXd distortionCoeff )
{
    int rowCount = src.rows();
    int colCount = src.cols();

    int disCount = distortionCoeff.rows();
    //无畸变参数
    if(disCount == 0)
    {
        return src;
    }
    Eigen::MatrixXi dst = Eigen::MatrixXi::Zero(rowCount,colCount);

    double f_x = intrinsicParam(0,0);
    double gam = intrinsicParam(0,1);
    double f_y = intrinsicParam(1,1);
    double u_0 = intrinsicParam(0,2);
    double v_0 = intrinsicParam(1,2);

    for(int i=0;i<rowCount;++i)
    {
        for(int j=0;j<colCount;++j)
        {
            double y1 = (j-v_0)/f_y;
            double x1 = (i-u_0-y1*gam)/f_x;
            double r = std::sqrt(std::pow(x1,2)+std::pow(y1,2));

            double x11=0,y11=0;
            //k1,k2
            if(disCount >= 2)
            {
                x11 += x1*(1+std::pow(r,2)*distortionCoeff(0) + std::pow(r,4) * distortionCoeff(1));
                y11 += y1*(1+std::pow(r,2)*distortionCoeff(0) + std::pow(r,4) * distortionCoeff(1));
            }
            //k1,k2,p1,p2
            if(disCount >= 4)
            {
                x11 += (2*x1*y1*distortionCoeff(2) + (2*std::pow(x1,2) + std::pow(r,2))*distortionCoeff(3));
                y11 += ((2*std::pow(y1,2) + std::pow(r,2))*distortionCoeff(2) + x1*y1*2*distortionCoeff(3));
            }
            //k1,k2,p1,p2,k3
            if(disCount >= 5)
            {
                x11 += x1*std::pow(r,6)*distortionCoeff(4);
                y11 += y1*std::pow(r,6)*distortionCoeff(4);
            }

            double ud = f_x*x11 + y11*gam + u_0;
            double vd = y11*f_y + v_0;

            // 赋值 (双线性插值)
            if (ud >= 0 && vd >= 0 && ud < rowCount && vd < colCount)
            {

                //取整数
                quint32 au = (quint32)std::floor(ud);
                quint32 av = (quint32)std::floor(vd);
                //取小数
                double du = ud - au;
                double dv = vd - av;
                //找出临近的四个数据(像素值)
                int a=0,b=0,c=0,d=0;
                a = src(au,av);
                if(vd+1<colCount)
                {
                    b =  src(au,av+1);
                }
                if(ud+1<rowCount)
                {
                    c =  src(au+1,av);
                }

                if(vd+1<colCount && ud+1<rowCount)
                {
                    d =  src(au+1,av+1);
                }
                dst(i, j) = (1-du)*(1-dv)*a + (1-du)*dv*b + (1-dv)*du*c + du*dv*d;

            }
            else
            {
                dst(i, j) = 0;
            }
        }
    }
    return dst;
}


//修复图片
QImage ImageCorrectionWidget::RectifiedImage(const QImage& srcImage, const Eigen::Matrix3d& intrinsicParameter,const Eigen::VectorXd& distortionCoeff)
{

    int width = srcImage.width ();                               // 图像宽度
    int height = srcImage.height ();                             // 图像高度
    //qDebug()<< srcImage.depth() <<srcImage.allGray()<<"  width:"<<width<<"  height:"<<height;
    //获取rgb数值
    Eigen::MatrixXi srcR(height,width),srcG(height,width),srcB(height,width);
    for (int i = 0; i < height; i++)                        // 遍历每一行
    {
        for ( int j = 0; j < width; j++ )                   // 遍历每一列
        {
            QColor color = srcImage.pixelColor(j,i);
            srcR(i,j) = color.red();
            srcG(i,j) = color.green();
            srcB(i,j) = color.blue();

        }
    }
    //rgb数值转换去畸变
    Eigen::MatrixXi dstR = GlobleAlgorithm::getInstance()->RectifiedImage(srcR,intrinsicParameter,distortionCoeff);
    Eigen::MatrixXi dstG = GlobleAlgorithm::getInstance()->RectifiedImage(srcG,intrinsicParameter,distortionCoeff);
    Eigen::MatrixXi dstB = GlobleAlgorithm::getInstance()->RectifiedImage(srcB,intrinsicParameter,distortionCoeff);
    QImage dstImage(width,height,srcImage.format());
    //填充去畸变图像
    for (int i = 0; i < height; i++)                        // 遍历每一行
    {
        for ( int j = 0; j < width; j++ )                   // 遍历每一列
        {
            dstImage.setPixelColor(j,i,QColor(dstR(i,j),dstG(i,j),dstB(i,j)));
        }
    }

    return dstImage;

}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129

QStringList m_files;//文件路径列表
for(int i=0;i<m_files.count();++i)
    {

        QImage srcImage(m_files.at(i));
        QImage dstImage = RectifiedImage(srcImage,intrinsicParameter,distortionCoeff);
        QString fileName = m_files.at(i).split(QDir::separator()).last();
        QString path = m_savePath+QDir::separator()+QDateTime::currentDateTime().toString("yyyy.MM.dd-hh.mm.ss.zzz")+"_"+fileName;
        if(dstImage.save(path))
        {
            m_outTextEdit->append(path+" : 保存成功!");
        }
        else
        {
            m_outTextEdit->append(path+" : 保存失败!");
        }
    }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

有畸变：
去畸变后：

双线性插值
双线性插值法计算

四：总结

以上所有代码还有优化空间，并未优化。
1：工具：主要Qt + Eigen库
Eigen库是一个用于矩阵计算，代数计算库
2：上面完整代码已上传GitHub
3：参考文献
张正友论文
构建雅可比矩阵模型
Python代码实现
什么是归一化的平面坐标
重投影误差理解
单应矩阵的计算
相机坐标系，像素平面坐标系，世界坐标系，归一化坐标系总结

旋转向量与旋转矩阵的相互转化
原函数模型推导
LM算法实现介绍
相机畸变详解
畸变校正详解