Realización de la calibración de la cámara monocular: método de calibración de Zhang Zhengyou

Directorio de artículos

Enlace original: Dirección

Notas personales:
esta introducción está dirigida a la calibración de cámaras monoculares, y el método de implementación es el método de calibración Zhang Zhengyou.

1: Introducción al sistema de coordenadas de la cámara, el sistema de coordenadas del plano de píxeles, el sistema de coordenadas mundial y el sistema de coordenadas normalizado

1. Información general

inserte la descripción de la imagen aquí

Como se muestra en la figura, hay un punto P y una cámara (centro óptico) en el mundo real. Para describir las coordenadas espaciales de este punto P, primero debe haber un sistema de coordenadas. Luego, se establece un sistema de coordenadas con el centro óptico como el origen O, que se denomina sistema de coordenadas de la cámara.
Luego, en el sistema de coordenadas de la cámara, establezca $Coordenadas P (X, Y, Z)$ y el punto de proyección de P $PAG^{'} (X^{'}, y^{'}, z^{'})$ . Cabe mencionar que $PAG^{'} (X^{'}, y^{'}, z^{'})$ se encuentran en el plano de imagen física y el plano de píxeles.
El plano de imagen física, el plano de píxeles es bidimensional y sus sistemas de coordenadas son diferentes: el
plano de imagen física está en $O^{'} (X^{'}, y^{'})$ ;
el origen del plano de píxel está en la esquina superior izquierda de la imagen gris-negra (la esquina superior izquierda de la imagen), y el eje horizontal a la derecha se llama $El eje u$ hacia abajo $eje v$ .
Esto da $PAG^{'}$ coordenadas de píxeles $pag (tú, v)$ , conocido como $P uv$ .

inserte la descripción de la imagen aquí El llamado plano de imagen normalizado consiste en dividir las coordenadas de los puntos del espacio tridimensional por Z. En el sistema de coordenadas de la cámara, P tiene tres cantidades de X, Y y Z. Si se proyectan en el plano normalizado Z = 1, se obtendrán las coordenadas normalizadas P(X/Z, Y/Z, 1) de P.

oficial

inserte la descripción de la imagen aquí
$\left[\begin{matriz}{c} X \\ Y \\ Z \\ 1 \end{matriz}\right]->$ Coordenadas del objeto.

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

$\left[\begin{array}{cccc} \alpha & \gamma & u_{0} & 0 \\ 0 & \beta & v_{ ;0} & 0 \\ 0 & 0 & 1 & 0 \ end {matriz} \ derecha] ->$ Referencia interna

$\left[\begin{arreglo}{l} u \\ v \\ 1 \end{arreglo}\right]->$ coordenadas de píxeles

Entre ellos, la referencia externa $T$ es el vector de traslación $t1,t2,t3)^T$ _
$La matriz de rotación R$ es la siguiente:
inserte la descripción de la imagen aquí

dos: darse cuenta

1: Proceso general

inserte la descripción de la imagen aquí
El primer paso, el segundo paso y el tercer paso no se presentarán por el momento (puede usar el bloque de operadores Halcon u OpenCV para obtener las coordenadas de los puntos característicos), y principalmente introducirá la optimización de la parte de adquisición de parámetros. después de obtener los puntos característicos.

4: Encuentra la matriz de homografía de cada imagen y optimízala con LMA

Cómo calcular la matriz de homografía:
$\begin{array}{c} \left[\begin{array}{l} x_{b} \\ y_{b} \\ w_{b} \end{matriz}\right]=\left[\begin{matriz}{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{matriz}\right]\left[\begin{matriz {l} x_{a} \\ y_{a} \\ w_{a} \end{matriz}\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{matriz}/ w_{a}+\mathrm{h}_{8} y_{a} / w_{a}+\mathrm{h}_{9}} \end{matriz}/ w_{a}+\mathrm{h}_{8} y_{a} / w_{a}+\mathrm{h}_{9}} \end{matriz}$

$\text { orden } 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 {, fórmula simple arriba}$
$\begin{matriz}{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{matriz}$

$\begin {matriz}{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{matriz}$

$\text { Puede establecer directamente}\|h\|_{2}^{2}=1 \text { , en este momento todavía es posible arreglar la escala y tener: }$
inserte la descripción de la imagen aquí

En este momento, el rango de la matriz de coeficientes es 8, hay una solución de espacio lineal, el grado de libertad de la solución es 1, y se satisface la homogeneidad, y debido a la limitación de la longitud unitaria, hay una solución única En este momento, todavía se puede resolver h por descomposición SVD, para obtener la matriz homográfica.

Código:

//获取标准差
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);
}

Encuentre la matriz homográfica inicial $h$ , luego use $L M A$ está optimizado para obtener una matriz de homografía con significado práctico.

El código de optimización es el siguiente:
encuentre el vector de valor residual:


//单应性残差值向量
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;
    }

};

va
$\begin{matriz}{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{matriz}$

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

La dirección de implementación de la función LevenbergMarquardtAlgorithm se presenta en este artículo: Implementación del algoritmo LM

5: resuelve los parámetros internos y externos de la cámara en la situación ideal sin distorsiones

Después de obtener la matriz de homografía, es necesario seguir obteniendo los parámetros internos de la cámara. Primero déjame $h_{i}$
significa Cada vector columna de $H , por lo que hay:$

$\left[\begin{array}{lll} h_{1} & h_{2} & h_{3} \end{array}\right ]=\lambda K\left[\begin{matriz}{lll} r_{1} & r_{2} & t \end{matriz}\right]$

Y porque $r_{1}$ y $r_{2}$ es un vector unitario ortogonal, entonces tenemos:

$\begin{alineado} 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{alineado}$
inserte la descripción de la imagen aquí

pero:
inserte la descripción de la imagen aquí

Esto proporciona dos ecuaciones de restricción para la solución de parámetros internos, sea:
inserte la descripción de la imagen aquí Desde $B$ es una matriz simétrica, por lo que puede definirse mediante un vector de 6 dimensiones, a saber:
$b=\left[\begin{array}{llllll} B_{ 11} & B_{12} & B_{22} & B_{13} & B_{23} & B_{33} \end{matriz}\right]^{T}$

$h_{i}=\left[\begin{array}{lll} h_{i 1 } & h_{i 2} & h_{i 3}\end{matriz}\right]^{T} , entonces:$
$h_{i}^{T} B h_{j}=V_{ij}^{T} b$

Entre ellos:
inserte la descripción de la imagen aquí
Entonces se forma un sistema de ecuaciones:
$V es 2n_* 6 matriz$ . Si $n ＞ = 3$ , entonces se pueden listar 6 ecuaciones para resolver 6 parámetros internos. Estos 6 parámetros internos resueltos no son únicos, sino escalados por un factor de escala. Encuentre los parámetros internos:
inserte la descripción de la imagen aquí
Puede encontrar la matriz de parámetros internos de la cámara:

Por último, invariablemente:
variable $\left[\begin{array}{lll} \mathbf{h}_{1} &; \mathbf{h} _ {2} & \mathbf{h}_{3} \end{matriz}\right]=\lambda \mathbf{A}\left[\begin{matriz}{lll}\mathbf{r }_{1} &\mathbf{r}_{2}&\mathbf{t}\end{matriz}\right]$ puede simplificarse para obtener parámetros externos, a saber:

inserte la descripción de la imagen aquí
Código:

//根据单应性矩阵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;
}

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

6: Aplicar mínimos cuadrados para encontrar el coeficiente de distorsión real

La cámara incluye principalmente distorsión radial y distorsión tangencial
inserte la descripción de la imagen aquí

Código:

//获取畸变系数 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);

}

Función LeastSquares: implementación de mínimos cuadrados

7: referencia interna integral, referencia externa, coeficiente de distorsión, utilizando el método de máxima verosimilitud (LMA), optimizar la estimación, mejorar la precisión de la estimación

Construya el modelo de función original:
$\min \sum_{i=1}^{n} \sum_{j=1}^{m}\ izquierda \|m_{ij}-\hat{m}\derecha\|^2$

donde $m_{ij}$ son las coordenadas de píxeles reales (extraídas por el algoritmo), $\hat{m}$ es el punto de reproyección (calculado usando distorsión de parámetros internos y externos)

1: Cómo calcular $\hat{m}$
clave de interfaz K = $\left[\begin{array}{ccc} fx&\gamma&u_{0}\\0&fy&v_{0}\\ 0&0&1\end{array}\right ]$
Vector de rotación $\overrightarrow{r}=[r1,r2,r3]$ (los vectores de rotación se transforman con matrices de rotación para lograrel enlace), vectores de traslación $[t 1, t 2, t 3]$ ，令 $\theta=|\overrightarrow{r}|=\sqrt{r_{1}^{2}+r_{2} ^{2}+r_{3}^{2}}$ , recuerda $\overrightarrow{r^{\prime}}=\frac{\vec{ r }}{|\vec{r}|}(unificación)=\frac{1}{\sqrt{r_{1}^{2}+r_{2}^{2}+r_{3}^{ 2} }}(\overrightarrow{r})$
registrar las coordenadas mundiales de un punto de esquina como $\left[\begin{array}{c} X \\ Y \\ Z \end{array}\right]=\overrightarrow{P}$
则(definición):
$\left(\begin{matriz}{l}X_{r}\\Y_{r}\\Z_{r}\end{matriz}\right)=\cos \theta\cdot\left(\begin { matriz}{l}X\\Y\\Z\end{matriz}\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{matriz}{l}\cos \theta\cdot X\\\cos \theta\cdot Y\\\cos \theta\cdot Z\end{matriz}\right)+(1-\ cos \theta) \cdot\frac{r_1X+r_2Y+r_3Z}{\sqrt{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}}\cdot\ fracción{1}{\sqrt{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}}\left(\begin{array}{l}r1\\ r2\\r3\end{matriz}\right)+\sin\theta\cdot\left(\begin{matriz}{l}r1\\r2\\r3 \end{matriz}\right)\cdot \frac{ 1}{\sqrt{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}}×\left(\begin{matriz}{l}X\\Y \\Z\end{matriz}\right)$

$=\left(\begin {matriz}{c} \cos \theta \cdot X \\ \cos \theta \cdot Y \\ \cos \theta \cdot Z \end{matriz}\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{matriz}{l}r_{1}\\r_{2}\\r_{3}\end{matriz}\right)+\frac{\sin \theta}{\sqrt{r_{1}^ {2}+r_{2}^{2}+r_{3}^{2}}}\left(\begin{matriz}{l}r_{2}Z-r_{3}Y\\r_{3 } X-r_{1}Z\\r_{1}Y-r_{2}X\end{matriz}\right)$
Definición:
$\left(\begin{matriz}{l}X_{rt}\\Y_{rt}\\Z_{rt}\end{matriz}\right)= \left (\begin{array}{c}\cos\theta\cdotX\\cos\theta\cdotY\\cos\theta\cdotZ\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 {matriz}{l}r_{1}\\r_{2}\\r_{3}\end{matriz}\right)+\frac{\sin\theta}{\sqrt{ r_{1}^{2 }+r_{2}^{2}+r_{3}^{2}}}\left(\begin{matriz}{l} r_{2} Z-r_{3} Y \\r_{3}X -r_{1}Z \\r_{1}Y-r_{2}X \end{matriz}\right)+\left(\begin{matriz}{l}t_{1} \\t_{2}\ \t_{3}\end{matriz}\right)$

(Especifique el equivalente del ciclo termodinámico) solvente $\overrightarrow {\hat m} = \left[\begin{array}{l}u\\v\\c\end{array}; \right]=\left[\begin{array}{ccc} por ejemplo,\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{matriz}\right)=\left[\begin{matriz}{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{matriz}\right ]$
即： $\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{matriz}$
Modelo de función primitiva completa (con distorsión):
inserte la descripción de la imagen aquí

La matriz de rotación y el vector de rotación se convierten entre sí, lo que se usará en el siguiente código

//旋转矩阵 --> 旋转向量    :罗德里格斯公式逆变换
    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;
    }

Código:

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

 //带畸变优化
 //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);

8: Obtenga los parámetros internos, los parámetros externos y los coeficientes de distorsión de la cámara

Referencia interna K, referencia externa W, parámetro de distorsión disCoeff

9: modelo OpenCV

Sin distorsión:
inserte la descripción de la imagen aquí Distorsión incluida:

El código anterior implementa la distorsión solo incluye: k1, k2, p1, p2, k3.

OpenCV: función calibrar cámara

Tres: reparación de distorsión (desdistorsión)

Usando el método de interpolación de doble línea para realizar
inserte la descripción de la imagen aquí
la realización del código:


//矫正图像 根据内参和畸变系数矫正
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;

}


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+" : 保存失败!");
        }
    }

Con Distorsión:
inserte la descripción de la imagen aquí Después de la Desdistorsión:

Interpolación bilineal
Cálculo de interpolación bilineal

Cuatro: Resumen

Todos los códigos anteriores todavía tienen espacio para la optimización y no se han optimizado.
1: Herramientas: la biblioteca principal Qt + Eigen
La biblioteca Eigen es una biblioteca para el cálculo matricial y el cálculo algebraico
2: El código completo anterior se cargó en GitHub
3: Consulte
el documento de Zhang Zhengyou
para construir
el código de Python del modelo de matriz jacobiana para darse cuenta
de lo que es un plano normalizado
Error de reproyección de coordenadas comprensión del cálculo del sistema de coordenadas de la cámara de
matriz de homografía , sistema de coordenadas del plano de píxeles, sistema de coordenadas mundial, resumen del sistema de coordenadas normalizado

Conversión mutua entre el vector de rotación y la matriz de rotación
Derivación del modelo de función original
Implementación del algoritmo LM Introducción a
la distorsión de la cámara
Explicación detallada de la corrección de la distorsión
inserte la descripción de la imagen aquí