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


文章目录

    • 一:相机坐标系,像素平面坐标系,世界坐标系,归一化坐标系介绍
      • 1:概述
      • 公式
    • 二:实现
      • 1:整体流程
      • 4:求出每张图像的单应性矩阵并用LMA优化
      • 5:求解理想无畸变情况下的摄像机的内参数和外参数
      • 6:应用最小二乘求出实际的畸变系数
      • 7:综合内参,外参,畸变系数,使用极大似然法(LMA),优化估计,提升估计精度
      • 8:得出相机的内参,外参和畸变系数
      • 9:OpenCV模型
    • 三:畸变修复(去畸变)
    • 四:总结


原文链接:地址

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

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

1:概述

单目相机标定实现--张正友标定法_第1张图片
单目相机标定实现--张正友标定法_第2张图片

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

单目相机标定实现--张正友标定法_第3张图片所谓的归一化的成像平面,就是将三维空间点的坐标都除以Z,在相机坐标系下,P有X, Y, Z 三个量,如果把它们投影到归一化平面 Z = 1 上,就会得到P的归一化坐标P(X/Z, Y/Z, 1)。

公式

单目相机标定实现--张正友标定法_第4张图片
[ 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]-> [R0T1]>外参

[ α γ 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γβ0u0v01000 >内参

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

其中外参 T T T是平移向量 ( t 1 , t 2 , t 3 ) T (t1,t2,t3)^T (t1,t2,t3)T.
R R R旋转矩阵如下图:
单目相机标定实现--张正友标定法_第5张图片

二:实现

1:整体流程

单目相机标定实现--张正友标定法_第6张图片
第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} xbybwb = h1 h4 h7 h2 h5 h8 h3 h6 h9 xayawa wbxb=h7xa+h8ya+h9wah1xa+h2ya+h3wa=h7xa/wa+h8ya/wa+h9h1xa/wa+h2ya/wa+h3wbyb=h7xa+h8ya+h9wah4xa+h5ya+h6wa=h7xa/wa+h8ya/wa+h9h4xa/wa+h5ya/wa+h6

 令  u a = x a w a , v a = y a w a , u b = x b w b , v b = y b w b , 上式化简为  \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 {, 上式化简为 }   ua=waxa,va=waya,ub=wbxb,vb=wbyb上式化简为 
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=h7ua+h8va+h9h1ua+h2va+h3vb=h7ua+h8va+h9h4ua+h5va+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} h1ua+h2va+h3h7uaubh8vaubh9ub=0h4ua+h5va+h6h7uavbh8vavbh9vb=0

 可以直接设  ∥ h ∥ 2 2 = 1  ,此时仍然可以固定住尺度,且有:  \text { 可以直接设 }\|h\|_{2}^{2}=1 \text { ,此时仍然可以固定住尺度,且有: }  可以直接设 h22=1 ,此时仍然可以固定住尺度,且有
单目相机标定实现--张正友标定法_第7张图片

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

代码实现:

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

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

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


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

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

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

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

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


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

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

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

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

求出初始单应性矩阵 h h h,然后用 L M A LMA 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;
    }

};

求雅克比矩阵构建原函数:
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=h7ua+h8va+h9h1ua+h2va+h3vb=h7ua+h8va+h9h4ua+h5va+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:"<

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

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

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

在求取了单应性矩阵后, 还要进一步求出摄像机的内参数。首先令 h i h_{i} hi
表示 H H 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] [h1h2h3]=λK[r1r2t]

又因为 r 1 r_{1} r1 r 2 r_{2} r2 是单位正交向量, 所以有 :

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} h1TKTK1h2h1TKTK1h1=0=h2TKTK1h2
单目相机标定实现--张正友标定法_第8张图片

则:
单目相机标定实现--张正友标定法_第9张图片

这样就为内参数的求解提供了两个约束方程,令:
单目相机标定实现--张正友标定法_第10张图片由于 B B 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=[B11B12B22B13B23B33]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=[hi1hi2hi3]T,:
h i T B h j = V i j T b h_{i}^{T} B h_{j}=V_{i j}^{T} b hiTBhj=VijTb

其中:
在这里插入图片描述
所以组成一个方程组为:
单目相机标定实现--张正友标定法_第11张图片 V 为 2 n ∗ 6 矩阵 V为2n*6矩阵 V2n6矩阵。如果 n > = 3 n>=3 n=3,则可以列出6个方程,从而解出6个内参数。这6个解出的内部参数不是唯一的,而是通过了一个比例因子缩放。求出内参:
单目相机标定实现--张正友标定法_第12张图片
即可求出相机内参矩阵:
单目相机标定实现--张正友标定法_第13张图片

内参求出后,求外参:
再根据 [ 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] [h1h2h3]=λA[r1r2t]化简可得外部参数,即:

单目相机标定实现--张正友标定法_第14张图片
代码实现:

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

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

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

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

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

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


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

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

    return exterParame;
}

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

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

相机主要包括径向畸变和切向畸变
单目相机标定实现--张正友标定法_第15张图片单目相机标定实现--张正友标定法_第16张图片单目相机标定实现--张正友标定法_第17张图片
单目相机标定实现--张正友标定法_第18张图片

代码实现:

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

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

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

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

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

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

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

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

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

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

}

LeastSquares函数:最小二乘实现

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

构建原函数模型:
min ⁡ ∑ i = 1 n ∑ j = 1 m ∥ m i j − m ^ ∥ 2 \min \sum_{i=1}^{n} \sum_{j=1}^{m}\left\|m_{i j}-\hat{m}\right\|^2 mini=1nj=1mmijm^2

其中 m i j m_{ij} mij为实际像素坐标(算法提取到的), m ^ \hat{m} m^为重投影点(利用内参外参畸变计算得到的)

1:如何计算 m ^ \hat{m} 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γfy0u0v01
旋转向量 r → = [ r 1 , r 2 , r 3 ] \overrightarrow{r}=[r1,r2,r3] r =[r1,r2,r3] (旋转向量有旋转矩阵转换得到实现链接),平移向量 [ t 1 , t 2 , t 3 ] [t1,t2,t3] [t1,t2,t3],令 θ = ∣ r → ∣ = r 1 2 + r 2 2 + r 3 2 \theta=|\overrightarrow{r}|=\sqrt{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}} θ=r =r12+r22+r32 ,记 r ′ → = r ⃗ ∣ r ⃗ ∣ ( 单位化 ) = 1 r 1 2 + r 2 2 + r 3 2 ( r → ) \overrightarrow{r^{\prime}}=\frac{\vec{r}}{|\vec{r}|}(单位化)=\frac{1}{\sqrt{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}}(\overrightarrow{r}) r =r r (单位化)=r12+r22+r32 1(r )
记某角点世界坐标为 [ X Y Z ] = P → \left[\begin{array}{c} X \\ Y \\ Z \end{array}\right]=\overrightarrow{P} XYZ =P
则(旋转后):
( X r Y r Z r ) = cos ⁡ θ ⋅ ( X Y Z ) + ( 1 − cos ⁡ θ ) ⋅ ( p ⃗ ⋅ r ′ → ) r ′ → + sin ⁡ θ ⋅ r ′ → × p ⃗ \left(\begin{array}{l} X_{r} \\ Y_{r} \\ Z_{r} \end{array}\right)=\cos \theta \cdot\left(\begin{array}{l} X \\ Y \\ Z \end{array}\right)+(1-\cos \theta) \cdot\left(\vec{p} \cdot \overrightarrow{r^{\prime}}\right) \overrightarrow{r^{\prime}}+\sin \theta \cdot \overrightarrow{r^{\prime}} \times \vec{p} XrYrZr =cosθ XYZ +(1cosθ)(p r )r +sinθr ×p
= ( cos ⁡ θ ⋅ X cos ⁡ θ ⋅ Y cos ⁡ θ ⋅ Z ) + ( 1 − cos ⁡ θ ) ⋅ r 1 X + r 2 Y + r 3 Z r 1 2 + r 2 2 + r 3 2 ⋅ 1 r 1 2 + r 2 2 + r 3 2 ( r 1 r 2 r 3 ) + sin ⁡ θ ⋅ ( r 1 r 2 r 3 ) ⋅ 1 r 1 2 + r 2 2 + r 3 2 × ( X Y Z ) =\left(\begin{array}{l} \cos \theta \cdot X\\ \cos \theta \cdot Y\\ \cos \theta \cdot Z \end{array}\right)+(1-\cos \theta) \cdot\frac{r_1X+r_2Y+r_3Z}{\sqrt{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}}\cdot \frac{1}{\sqrt{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}}\left(\begin{array}{l} r1 \\ r2 \\ r3 \end{array}\right)+\sin \theta \cdot\left(\begin{array}{l} r1 \\ r2 \\ r3 \end{array}\right)\cdot \frac{1}{\sqrt{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}}×\left(\begin{array}{l} X \\ Y \\ Z \end{array}\right) = cosθXcosθYcosθZ +(1cosθ)r12+r22+r32 r1X+r2Y+r3Zr12+r22+r32 1 r1r2r3 +sinθ r1r2r3 r12+r22+r32 1× XYZ

= ( cos ⁡ θ ⋅ X cos ⁡ θ ⋅ Y cos ⁡ θ ⋅ Z ) + ( 1 − cos ⁡ θ ) r 1 X + r 2 Y + r 3 Z r 1 2 + r 2 2 + r 3 2 ( r 1 r 2 r 3 ) + sin ⁡ θ r 1 2 + r 2 2 + r 3 2 ( r 2 Z − r 3 Y r 3 X − r 1 Z r 1 Y − r 2 X ) =\left(\begin{array}{c} \cos \theta \cdot X \\ \cos \theta \cdot Y \\ \cos \theta \cdot Z \end{array}\right)+(1-\cos \theta)\frac{r_{1} X+r_{2} Y+r_{3} Z}{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}\left(\begin{array}{l} r_{1} \\ r_{2} \\ r_{3} \end{array}\right)+\frac{\sin \theta}{\sqrt{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}}\left(\begin{array}{l} r_{2} Z-r_{3} Y \\ r_{3} X-r_{1} Z \\ r_{1} Y-r_{2} X \end{array}\right) = cosθXcosθYcosθZ +(1cosθ)r12+r22+r32r1X+r2Y+r3Z r1r2r3 +r12+r22+r32 sinθ r2Zr3Yr3Xr1Zr1Yr2X
平移后:
( X r t Y r t Z r t ) = ( cos ⁡ θ ⋅ X cos ⁡ θ ⋅ Y cos ⁡ θ ⋅ Z ) + ( 1 − cos ⁡ θ ) r 1 X + r 2 Y + r 3 Z r 1 2 + r 2 2 + r 3 2 ( r 1 r 2 r 3 ) + sin ⁡ θ r 1 2 + r 2 2 + r 3 2 ( r 2 Z − r 3 Y r 3 X − r 1 Z r 1 Y − r 2 X ) + ( t 1 t 2 t 3 ) \left(\begin{array}{l} X_{rt} \\ Y_{rt} \\ Z_{rt} \end{array}\right)=\left(\begin{array}{c} \cos \theta \cdot X \\ \cos \theta \cdot Y \\ \cos \theta \cdot Z \end{array}\right)+(1-\cos \theta)\frac{r_{1} X+r_{2} Y+r_{3} Z}{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}\left(\begin{array}{l} r_{1} \\ r_{2} \\ r_{3} \end{array}\right)+\frac{\sin \theta}{\sqrt{r_{1}^{2}+r_{2}^{2}+r_{3}^{2}}}\left(\begin{array}{l} r_{2} Z-r_{3} Y \\ r_{3} X-r_{1} Z \\ r_{1} Y-r_{2} X \end{array}\right)+\left(\begin{array}{l} t_{1} \\ t_{2} \\ t_{3} \end{array}\right) XrtYrtZrt = cosθXcosθYcosθZ +(1cosθ)r12+r22+r32r1X+r2Y+r3Z r1r2r3 +r12+r22+r32 sinθ r2Zr3Yr3Xr1Zr1Yr2X + t1t2t3

(这个公式没有考虑到畸变参数)重投影 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γfy0u0v01 XrtYrtZrt = fxXrt+Yrtγ+u0ZrtfyYrt+v0ZrtZrt
即: u ′ = f x ⋅ X r t + Y r t ⋅ γ + u 0 ⋅ Z r t Z r t = X r t Z r t ⋅ f x + Y r t Z r t ⋅ γ + u 0 v ′ = f y ⋅ Y r t + v 0 ⋅ Z r t Z r t = Y r t Z r t ⋅ f y + v 0 \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} u=ZrtfxXrt+Yrtγ+u0Zrt=ZrtXrtfx+ZrtYrtγ+u0v=ZrtfyYrt+v0Zrt=ZrtYrtfy+v0
完整原函数模型(含畸变):
单目相机标定实现--张正友标定法_第19张图片

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

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


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

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

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

代码实现:

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

}S_CameraOtherParameter;

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

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

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


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

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

        return uv;
    }

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

        //内参
        Eigen::Matrix3d intrinsicParam;

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



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

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

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

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

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

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


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


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

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

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

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

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




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


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

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

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

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

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



        }

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

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


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


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

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

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

        obj1 = f_y*y11+v_0;




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


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

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

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

            obj2 = f_y*y11+v_0;



        }

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

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

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

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


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

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

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


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

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

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

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

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

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

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

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

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



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





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

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

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

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


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

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

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

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

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


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

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

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


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

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


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


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

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

你可能感兴趣的:(视觉,算法)