1.相机标定

        真实物体通过相机可以在图像中显现，可以通过一个数学模型将物体的三维坐标和图像中物体的二维坐标一一对应，求解这个数学模型的过程就叫相机标定。

        从三维坐标（世界坐标系）到二维坐标（）又可以分为三个步骤：

（1）从世界坐标转换到相机坐标；

（2）从相机坐标转换到图像坐标；

（3）从图像坐标转换到像素坐标。

如上图所示：

（1）坐标系（Xw, Yw, Zw）为世界坐标系；

（2）坐标系（Xc, Yc, Zc）为相机坐标系；

（3）坐标系（x, y）为图像坐标系；

（4）坐标系（u, v）为像素坐标系；

1.1从世界坐标转换到相机坐标

        点P从世界坐标（Xpw, Ypw, Zpw）到相机坐标（Xpc, Ypc, Zpc）的变换包含了一个三维的旋转变换R以及一个平移变换t，齐次表达式为：

   

其中R为3×3大小的旋转矩阵，t为3×1的平移矩阵，变换矩阵P为3×4大小的矩阵：

1.2从相机坐标转换到图像坐标

        

则由P的相机坐标（Xpc, Ypc, Zpc）到图像坐标（xp, yp）有：

转换为矩阵：

fx, fy分别为x, y方向上的焦距。

1.3从图像坐标转换到像素坐标

        理想情况下图像坐标系和像素坐标系在同一平面上，所以这一步需要的是缩放到像素尺寸和平移到像素坐标系的原点，同时加上像素偏移的误差；

u,v为像素坐标，Sx，Sy为单位长度的像素数，c为像素偏移系数。

1.4相机内参、外参

由1.1,1.2,1.3可得世界坐标系到像素坐标系的转换表达式如下。

即：

得到

其中： 尺度系数，A是相机内参,R,t是相机外参

为图像的齐次坐标

是模型点的齐次坐标

1.5镜头畸变

1.5.1径向畸变

当光线在镜头边缘附近比在其光学中心弯曲得更多时，就会发生径向畸变。镜头越小，畸变越大。包含枕形（pincushion）、桶形（barrel）两种：

径向畸变通过径向畸变系数模型表示并校正：

其中：

x, y为未发生畸变的像素位置；

k1, k2, k3为径向畸变系数，一般两个系数就足够校准，但是对于大畸变如广角镜头就需要三个系数；

r^2 = x^2 + y^2.

1.5.2切向畸变

当镜头和像平面不平行时会发生切向畸变。切向畸变通过切向畸变系数模型表示并校正：

x, y为未发生畸变的像素位置；

k1, k2为切向畸变系数；

r^2 = x^2 + y^2.

1.6小结

（1）相机外参：世界坐标系到相机坐标系的变换矩阵（三轴旋转角度+三轴平移量总共6个参数）

（2）相机内参：焦距（fx, fy），光心/主点（cx, cy），倾斜系数（c）（5个参数）

（3）镜头畸变系数：径向畸变系数和切向畸变系数（k1, k2, p1, p2，4个参数）

2.calibratecamera求解相机参数关键步骤

2.1封闭解估计初始内参

（1）通过对象点和图像点由cvFindHomography（）函数计算单应性矩阵H；

此时，A中设定倾斜系数c=0，主点u0, v0由图像大小得到，因此现在的相机内参只有两个焦距未知量：

由上面两式有：

展开有得到以下8项（求解焦距只需要这几项）：

将下面两项带入上面四项有：

在源代码中即：

H[0] -= H[6]*a[2]; H[1] -= H[7]*a[2]; H[2] -= H[8]*a[2];
H[3] -= H[6]*a[5]; H[4] -= H[7]*a[5]; H[5] -= H[8]*a[5];

此时H经过更新（根据代码）为：

我们发现6个方程有8个未知量，要想解出焦距还需要两个方程，这两个方程就是正交矩阵R的两个约束：

结合上式可得：

则有两个新约束方程：

根据上式结合所有图像则可以得到方程组求解出两个焦距。代码为（代码中n是为了单位化向量r）：

for( j = 0; j < 3; j++ )
        {
            double t0 = H[j*3], t1 = H[j*3+1];
            h[j] = t0; v[j] = t1;
            d1[j] = (t0 + t1)*0.5;
            d2[j] = (t0 - t1)*0.5;
            n[0] += t0*t0; n[1] += t1*t1;
            n[2] += d1[j]*d1[j]; n[3] += d2[j]*d2[j];
        }

        for( j = 0; j < 4; j++ )
            n[j] = 1./std::sqrt(n[j]);

        for( j = 0; j < 3; j++ )
        {
            h[j] *= n[0]; v[j] *= n[1];
            d1[j] *= n[2]; d2[j] *= n[3];
        }

        Ap[0] = h[0]*v[0]; Ap[1] = h[1]*v[1];
        Ap[2] = d1[0]*d2[0]; Ap[3] = d1[1]*d2[1];
        bp[0] = -h[2]*v[2]; bp[1] = -d1[2]*d2[2];
    }

    /*
        cvSolve : 求解线性系统或者最小二乘法问题
        求解两个焦距参数
    */
    cvSolve( matA, _b, &_f, CV_NORMAL + CV_SVD );
    a[0] = std::sqrt(fabs(1./f[0]));
    a[4] = std::sqrt(fabs(1./f[1]));

2.2估计外参和畸变系数

        基于当前内参的初始值（来自 cvInitIntrinsicParams2D 或用户输入），对每张图像的 3D 世界点和 2D 图像点，使用 PnP 算法（Perspective-n-Point） 快速求解一个粗略的外参初始值（rvec 和 tvec）。
        PnP 算法的核心是：已知内参时，通过 3D-2D 点对应关系求解相机姿态（外参），属于线性 / 非线性求解（简化版），能快速得到可用的初始猜测。

        畸变系数的初始值通常直接设为 0 向量（假设镜头无畸变，这是最安全的初始假设）。

2.3LM迭代优化

        相机标定的本质是求解重投影误差最小化问题，即找到最优的内参、外参和畸变系数，使得 3D 世界点经相机投影后的 2D 点与实际检测到的图像点误差最小。cvCalibrateCamera2Internal 采用 Levenberg-Marquardt（LM）算法 实现这一优化，核心步骤如下：

LM最核心的是代价函数：所有图像中所有特征点的重投影误差平方和：

M 为图像数量，N 为每张图像的特征点数量；

为第 i 张图像第 j 个特征点的 3D 世界坐标；

 为第 i 张图像第 j 个特征点的 2D 图像坐标；

project()为相机投影函数（含畸变矫正），将 3D 点转化为 2D 像素点,即原理部分提到的世界坐标转换成像素坐标并加上畸变系数的内容。

LM 算法通过迭代调整优化变量 x，逐步减小代价函数 E(X)，具体步骤为：

1.初始化迭代参数：设定迭代次数上限（如 30 次）、收敛阈值（如参数增量的范数小于 (1e-7)）、阻尼因子 （初始值如 (1e-3)）。

2.计算雅可比矩阵 和误差向量：

对每个特征点，计算代价函数对所有优化变量的偏导数，组成雅可比矩阵 （每行对应一个特征点的误差对所有参数的偏导数）；

计算误差向量：每个元素为 （即重投影误差）。

3.构建正规方程组：
LM 算法的增量方程为：

是近似海森矩阵（Hessian）；

 是阻尼因子（平衡高斯 - 牛顿法和最速下降法）；

是待求解的参数增量。

4.求解参数增量 ：

调用线性方程组求解函数（如 cvSolve），使用 LU 分解或 SVD 方法求解上述正规方程组，得到。

5.参数更新与阻尼因子调整：

尝试更新参数：；

计算新的代价函数 ：

若（更新有效）：接受新参数 ，减小阻尼因子 （如 /=10），继续迭代；

若 （更新无效）：拒绝新参数，增大阻尼因子（如 *=10），重新求解增量方程。

6.迭代终止判断：重复步骤 2-5，直到满足以下条件之一：迭代次数达到上限；参数增量 的范数小于收敛阈值；代价函数 E(x)的变化量小于阈值。

        最后将优化后的内参矩阵 cameraMatrix、畸变系数distCoeffs 写入输出变量；将每张图像对应的最优外参（旋转向量 rvecs)、平移向量 （tvecs)写入输出数组。

2.4小结

        calibratecamera构建一个包含所有待求参数的向量，然后通过迭代调整这个向量，使得重投影误差（预测像素与实际像素的差值）的平方和最小化,通过迭代同时优化所有参数。