SFM之相机标定(二)

成像模型（Imaging Model）

现代一句城乡模型大致可以分为小孔成像相机和透镜成像相机。以下分别是两种成像模型的光路图。

在透镜成像模型中，我们依据物理学知识,已知透镜焦距 $f$ ，像距 $m$ ，物距 $n$ ，可以得到：

\frac{1}{f} = \frac{1}{m} + \frac{1}{n}

$\frac{1}{f}=\frac{1}{m} + \frac{1}{n}$
而在小孔成像模型中，我们将像的位置设置在“焦距”

f

$f$ 上，因此有：

\frac{1}{f} = \frac{1}{m}

$\frac{1}{f}=\frac{1}{m}$
一般地，由于物距远大于焦距，即 n>>f，所以 m≈f，此时可以用小孔模型代替透镜模型。

坐标系统（Coordinate System ）

在相机成像模型中，存在多个坐标系，分别有：像素坐标系，图像坐标系，相机坐标系以及世界坐标系。

为方面分析，如果将像平面转移至物体和相机之间，那么加上定义的三个坐标系，可以得到如下图所示的结构：

结合成像关系，可以得到相机坐标系和图像坐标系的关系：

x_{u} = f \frac{x_{c}}{z_{c}} y_{u} = f \frac{y_{c}}{z_{c}} z_{c} [\begin{matrix} x_{u} \\ y_{u} \\ 1 \end{matrix}] = [\begin{matrix} f & 0 & 0 & 0 \\ 0 & f & 0 & 0 \\ 0 & 0 & 1 & 0 \end{matrix}] [\begin{matrix} x_{c} \\ y_{c} \\ z_{c} \\ 1 \end{matrix}]

$x_u=f\frac{x_c}{z_c} \\ y_u=f\frac{y_c}{z_c}\\ z_c\begin{bmatrix} x_u \\y_u\\1 \end{bmatrix}= \begin{bmatrix} f &0&0&0 \\ 0&f&0&0\\0&0&1&0 \end{bmatrix} \begin{bmatrix} x_c \\y_c\\z_c\\1 \end{bmatrix}$
相片是由感光元件的作用而来，每个感光元件都有固定的尺寸大小(

d x, d y

$dx,dy$ )，一个感光元件代表图像中的一个像素。因此图像坐标系和像素坐标系有如下关系：

[\begin{matrix} X \\ Y \\ 1 \end{matrix}] = [\begin{matrix} \frac{1}{d x} & 0 & X_{0} \\ 0 & \frac{1}{d y} & Y_{0} \\ 0 & 0 & 1 \end{matrix}] [\begin{matrix} x_{u} \\ y_{u} \\ 1 \end{matrix}]

$\begin{bmatrix} X \\Y\\1 \end{bmatrix}= \begin{bmatrix} \frac{1}{dx} &0&X_0 \\ 0&\frac{1}{dy}&Y_0\\0&0&1 \end{bmatrix} \begin{bmatrix} x_u \\y_u\\1 \end{bmatrix}$
而世界坐标系与相机坐标系的关系和如何选取世界坐标系有关，通常两个坐标系的变换可以通过旋转和平移来实现变换（也就是常说的刚体变换）：

[\begin{matrix} x_{c} \\ y_{c} \\ z_{c} \end{matrix}] = R [\begin{matrix} x_{w} \\ y_{w} \\ z_{w} \end{matrix}] + T

$\begin{bmatrix} x_c \\y_c\\z_c \end{bmatrix}= R \begin{bmatrix} x_w \\y_w\\z_w \end{bmatrix}+T$

[\begin{matrix} x_{c} \\ y_{c} \\ z_{c} \\ 1 \end{matrix}] = [\begin{matrix} R & T \\ 0^{T} & 1 \end{matrix}] [\begin{matrix} x_{w} \\ y_{w} \\ z_{w} \\ 1 \end{matrix}] = [\begin{matrix} r_{11} & r_{12} & r_{13} & t_{x} \\ r_{21} & r_{22} & r_{23} & t_{y} \\ r_{31} & r_{32} & r_{33} & t_{z} \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} x_{w} \\ y_{w} \\ z_{w} \\ 1 \end{matrix}]

$\begin{bmatrix} x_c \\y_c\\z_c\\1 \end{bmatrix}= \begin{bmatrix} R&T\\0^T&1 \end{bmatrix} \begin{bmatrix} x_w \\y_w\\z_w\\1 \end{bmatrix}= \begin{bmatrix} r_{11}&r_{12}&r_{13}&t_x \\ r_{21}&r_{22}&r_{23}&t_y \\ r_{31}&r_{32}&r_{33}&t_z \\ 0&0&0&1 \\ \end{bmatrix} \begin{bmatrix} x_w \\y_w\\z_w\\1 \end{bmatrix}$

总结所的坐标系关系，可以得到像素坐标系和世界坐标系的关系（实际情况情况可能比这还复杂）：

z_{c} [\begin{matrix} X \\ Y \\ 1 \end{matrix}] = [\begin{matrix} \frac{1}{d x} & 0 & X_{0} \\ 0 & \frac{1}{d y} & Y_{0} \\ 0 & 0 & 1 \end{matrix}] [\begin{matrix} f & 0 & 0 & 0 \\ 0 & f & 0 & 0 \\ 0 & 0 & 1 & 0 \end{matrix}] [\begin{matrix} r_{11} & r_{12} & r_{13} & t_{x} \\ r_{21} & r_{22} & r_{23} & t_{y} \\ r_{31} & r_{32} & r_{33} & t_{z} \\ 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} x_{w} \\ y_{w} \\ z_{w} \\ 1 \end{matrix}]

$z_c \begin{bmatrix} X\\Y\\1 \end{bmatrix}= \begin{bmatrix} \frac{1}{dx} &0&X_0 \\ 0&\frac{1}{dy}&Y_0\\0&0&1 \end{bmatrix} \begin{bmatrix} f &0&0&0 \\ 0&f&0&0\\0&0&1&0 \end{bmatrix} \begin{bmatrix} r_{11}&r_{12}&r_{13}&t_x \\ r_{21}&r_{22}&r_{23}&t_y \\ r_{31}&r_{32}&r_{33}&t_z \\ 0&0&0&1 \\ \end{bmatrix} \begin{bmatrix} x_w \\y_w\\z_w\\1 \end{bmatrix}$
即：

s [\begin{matrix} X \\ Y \\ 1 \end{matrix}] = K [\begin{matrix} R, T \end{matrix}] [\begin{matrix} x_{w} \\ y_{w} \\ z_{w} \\ 1 \end{matrix}]

$s \begin{bmatrix} X\\Y\\1 \end{bmatrix}= K \begin{bmatrix} R , T \end{bmatrix} \begin{bmatrix} x_w \\y_w\\z_w\\1 \end{bmatrix}$
其中，

s

$s$ 为尺度因子，

K

$K$ 为相机内参矩阵，

R

$R$ 为旋转矩阵，

T

$T$ 为评议矩阵。

畸变校正

在成像时，理论上直线应该投影成为直线。但实际在透镜成像过程中，并非如此，这就是所谓的光学畸变。这种现象在远离像中心的部分畸变程度越大。畸变可以将其分为切向畸变和径向畸变。

理想的畸变模型下，理想的像素坐标与畸变像素的坐标关系如下：

x^{^{'}} = x + δ_{x r} + δ_{x d} y^{^{'}} = y + δ_{y r} + δ_{y d}

$x^{'} = x + \delta_{xr} + \delta_{xd}\\y^{'} = y + \delta_{yr} + \delta_{yd}$
径向畸变包括枕型畸变和桶型畸变，这主要是由于透镜的不太完美而造成的。其畸变模型可以表示为：

δ_{x r} = x (k_{1} r^{2} + k_{2} r^{4} + k_{3} r^{6} + K) δ_{y r} = y (k_{1} r^{2} + k_{2} r^{4} + k_{3} r^{6} + K)

$\delta_{xr} = x( k_1 r^2 + k_2 r^4 + k_3 r^6 + K )\\\delta_{yr} = y( k_1 r^2 + k_2 r^4 + k_3 r^6 + K )$
切向畸变包括离心畸变和薄透镜畸变等。薄透镜畸变主要发生在透镜组之中，而薄透镜畸变主要由于透镜倾斜而引发。其畸变模型可以表示为：

δ_{x d} = 2 p_{1} x y + p_{2} (r^{2} + 2 x^{2}) + K δ_{x d} = 2 p_{1} x y + p_{2} (r^{2} + 2 x^{2}) + K

$\delta_{xd} = 2p_1 x y + p_2(r^2 + 2 x^2) + K\\\delta_{xd} = 2p_1 x y + p_2(r^2 + 2 x^2) + K$
那么，可以推导出理想坐标和实际坐标的关系为：

[\begin{matrix} x^{^{'}} \\ y^{^{'}} \end{matrix}] = (1 + k_{1} r^{2} + k_{2} r^{4} + k_{3} r^{6}) [\begin{matrix} x \\ y \end{matrix}] + [\begin{matrix} 2 p_{1} x y + p_{2} (r^{2} + 2 x^{2}) \\ 2 p_{1} (r^{2} + 2 y^{2}) + 2 p_{2} x y \end{matrix}]

$\left[ \begin{matrix} x^{'} \\ y^{'} \end{matrix} \right] = (1 + k_1 r^2 + k_2 r^4 + k_3 r^6) \left[ \begin{matrix} x \\ y \end{matrix} \right] + \left[ \begin{matrix} 2p_1 x y + p_2(r^2 + 2 x^2) \\ 2p_1(r^2 + 2 y^2) + 2p_2 x y \end{matrix} \right]$

相机标定（Camera Calibration）

相机的标定主要的通过实验的方法来确定前述的一些参数，包括内参数矩阵，外参数矩阵，畸变矩阵等。

外参数矩阵：告诉你现实世界点(世界坐标)是怎样经过旋转和平移，然后落到另一个现实世界点(摄像机坐标)上。
内参数矩阵：告诉你上述那个点在1的基础上，如何继续经过摄像机的镜头、并通过针孔成像和电子转化而成为像素点。
畸变矩阵。告诉你为什么上面那个像素点并没有落在理论计算该落在的位置上，还产生了一定的偏移和变形。

张正友标定法

设三维世界坐标的点为 $[x_w,y_w,z_w,1]^T$ ，二维相机平面像素坐标为 $[X,Y,1]^T$ ，所以标定用的棋盘格平面到图像平面的单应性关系为：

$s [\begin{matrix} X \\ Y \\ 1 \end{matrix}] = K [\begin{matrix} R, T \end{matrix}] [\begin{matrix} x_{w} \\ y_{w} \\ z_{w} \\ 1 \end{matrix}]$ $s \begin{bmatrix} X\\Y\\1 \end{bmatrix}= K \begin{bmatrix} R , T \end{bmatrix} \begin{bmatrix} x_w \\y_w\\z_w\\1 \end{bmatrix}$
令其中的 $K$ 为：
$K = [\begin{array}{ccc} α & γ & X_{0} \\ 0 & β & Y_{0} \\ 0 & 0 & 1 \end{array}]$ $K=\left[ \begin{array}{ccc} \alpha & \gamma & X_0 \\ 0 & \beta & Y_0 \\ 0 & 0 & 1 \end{array} \right]$
当世界坐标系放在标定棋盘上时，此时 $z_w=0$ ，那么代入公式可以得到：
$s [\begin{matrix} X \\ Y \\ 1 \end{matrix}] = K [\begin{array}{cccc} r_{1} & r_{2} & r_{3} & t \end{array}] [\begin{matrix} x_{w} \\ y_{w} \\ 0 \\ 1 \end{matrix}] = K [\begin{array}{cccc} r_{1} & r_{2} & t \end{array}] [\begin{matrix} x_{w} \\ y_{w} \\ 1 \end{matrix}] = H [\begin{matrix} x_{w} \\ y_{w} \\ 1 \end{matrix}]$ $s\left[ \begin{array}{c} X \\ Y \\ 1 \end{array} \right] = K\left[ \begin{array}{cccc} r_1 & r_2 & r_3 & t \end{array} \right] \left[ \begin{array}{c} x_w \\ y_w \\ 0 \\ 1 \end{array} \right]=K \left[ \begin{array}{cccc} r_1 & r_2 & t \end{array} \right] \left[ \begin{array}{c} x_w \\ y_w \\ 1 \end{array} \right]=H \left[ \begin{array}{c} x_w \\ y_w \\ 1 \end{array} \right]$
即，称 $H$ 为单应性矩阵：
$s [\begin{matrix} u \\ v \\ 1 \end{matrix}] = H [\begin{matrix} X \\ Y \\ 1 \end{matrix}] H = [h_{1} h_{2} h 3] = λ K [r_{1} r_{2} t]$ $s \left[ \begin{array}{c} u \\ v \\ 1 \end{array} \right]= H \left[ \begin{array}{c} X \\ Y \\ 1 \end{array} \right]\\ H=[h_1\ h_2\ h3] = \lambda K[r_1\ r_2\ t]$

H是一个齐次矩阵，有一个元素为1，所以存在8个未知数，因此至少需要8个方程才能解得方程。每一个点对可以提供两个方程，因此至少需要四个对应的点对才能够得到一个解。

标定图像获取

1。图片放置的位置要能覆盖整个测量视场，在相机所能拍摄到的各个区域尽可能的获取图片。

2。图片的数量通常在15~25张之间，图像数量太少，容易导致标定参数不准确，但是太多容易影响标定时间。

3。标定板的成像尺寸应大致占整幅画面的1/4

4。标定板成像应该清晰，也就是图像质量要尽量好，不要有过曝、虚影、光照不均等情况

5。标定过程，相机的光圈、焦距不能发生改变，改变需要重新标定。

Matlab标定源码分析

主程序如下：

function calibration()
numImages = 5;
files = cell(1, 5);
for i = 1:numImages
    files{i} = fullfile('C:\\path\\to\\pics\\', sprintf('IMG_%d.jpg', i));
end
[imagePoints, boardSize] = detectCheckerboardPoints(files);\\检测棋盘格上的点
squareSize = 25; % in millimeters 设置棋盘格各自大小尺寸
worldPoints = generateCheckerboardPoints(boardSize, squareSize);\\生成点的世界坐标
cameraParams = estimateCameraParameters(imagePoints, worldPoints);\\估计出参数

角点检测的核心函数如下：

function [points, boardSize] = detectCheckerboard(I, sigma, peakThreshold)

%#codegen

[cxy, c45, Ix, Iy] = ...
    vision.internal.calibration.checkerboard.secondDerivCornerMetric(I, sigma);
[Ix2, Iy2, Ixy] = computeJacobianEntries(Ix, Iy);

points0 = vision.internal.calibration.checkerboard.find_peaks(cxy, peakThreshold);
scores0 = cxy(sub2ind(size(cxy), points0(:, 2), points0(:, 1)));
board0 = growCheckerboard(points0, scores0, Ix2, Iy2, Ixy, 0);

points45 = vision.internal.calibration.checkerboard.find_peaks(c45, peakThreshold);
scores45 = c45(sub2ind(size(c45), points45(:, 2), points45(:, 1)));
board45 = growCheckerboard(points45, scores45, Ix2, Iy2, Ixy, pi/4);

points = [];
boardSize = [0 0];
if board0.isValid && board0.Energy < board45.Energy
    board0 = orient(board0, I);
    [points, boardSize] = toPoints(board0);
    points = vision.internal.calibration.checkerboard.subPixelLocation(cxy, points);
elseif board45.isValid
    board45 = orient(board45, I);
    [points, boardSize] = toPoints(board45);
    points = vision.internal.calibration.checkerboard.subPixelLocation(c45, points);
end

end

世界坐标生成函数：

function worldPoints = generateCheckerboardPoints(boardSize, squareSize)
% 生成标定板上所检测的角点的世界坐标
%   返回一个 M x 2 矩阵，包含格子角点的x-y 坐标。角点 (0,0)对应板上左上方格的右下角点，
%   即该点为定义的世界坐标原点。boardSize 是一个含有2个元素的向量，指明了格子的数量和排布信息。返回的点
%   个数M = (boardSize(1)-1) * (boardSize(2)-1). squareSize 是一个表明格子尺寸（实际大小）的标量
%
%
% % offset the points to place the first point at lower-right corner of the
% % first square.

checkInputs(boardSize, squareSize);
boardSize = double(boardSize) - 1;
worldPoints = zeros(boardSize(1) * boardSize(2), 2);
k = 1;
for j = 0:boardSize(2)-1
    for i = 0:boardSize(1)-1
        worldPoints(k,1) = j * squareSize;
        worldPoints(k,2) = i * squareSize;
        k = k + 1;
    end
end

矩阵计算：

%--------------------------------------------------------------------------
function H = computeHomography(imagePoints, worldPoints)
% Compute projective transformation from worldPoints to imagePoints

H = fitgeotrans(worldPoints, imagePoints, 'projective');
H = (H.T)';
H = H / H(3,3);
%--------------------------------------------------------------------------
function V = computeV(homographies)
% Vb = 0

numImages = size(homographies, 3);
V = zeros(2 * numImages, 6);
for i = 1:numImages
    H = homographies(:, :, i)';
    V(i*2-1,:) = computeLittleV(H, 1, 2);
    V(i*2, :) = computeLittleV(H, 1, 1) - computeLittleV(H, 2, 2);
end

%--------------------------------------------------------------------------
function v = computeLittleV(H, i, j)
    v = [H(i,1)*H(j,1), H(i,1)*H(j,2)+H(i,2)*H(j,1), H(i,2)*H(j,2),...
         H(i,3)*H(j,1)+H(i,1)*H(j,3), H(i,3)*H(j,2)+H(i,2)*H(j,3), H(i,3)*H(j,3)];

%--------------------------------------------------------------------------     
function B = computeB(V)
% lambda * B = inv(A)' * inv(A), where A is the intrinsic matrix

[~, ~, U] = svd(V);
b = U(:, end);

% b = [B11, B12, B22, B13, B23, B33]
B = [b(1), b(2), b(4); b(2), b(3), b(5); b(4), b(5), b(6)];

%--------------------------------------------------------------------------
function A = computeIntrinsics(B)
% Compute the intrinsic matrix

cy = (B(1,2)*B(1,3) - B(1,1)*B(2,3)) / (B(1,1)*B(2,2)-B(1,2)^2);
lambda = B(3,3) - (B(1,3)^2 + cy * (B(1,2)*B(1,3) - B(1,1)*B(2,3))) / B(1,1);
fx = sqrt(lambda / B(1,1));
fy = sqrt(lambda * B(1,1) / (B(1,1) * B(2,2) - B(1,2)^2));
skew = -B(1,2) * fx^2 * fy / lambda;
cx = skew * cy / fx - B(1,3) * fx^2 / lambda;
A = vision.internal.calibration.constructIntrinsicMatrix(fx, fy, cx, cy, skew);
if ~isreal(A)
    error(message('vision:calibrate:complexCameraMatrix'));
end


function [rotationVectors, translationVectors] = ...
    computeExtrinsics(A, homographies)
% Compute translation and rotation vectors for all images

numImages = size(homographies, 3);
rotationVectors = zeros(3, numImages);
translationVectors = zeros(3, numImages); 
Ainv = inv(A);
for i = 1:numImages;
    H = homographies(:, :, i);
    h1 = H(:, 1);
    h2 = H(:, 2);
    h3 = H(:, 3);
    lambda = 1 / norm(Ainv * h1); %#ok

    % 3D rotation matrix
    r1 = lambda * Ainv * h1; %#ok
    r2 = lambda * Ainv * h2; %#ok
    r3 = cross(r1, r2);
    R = [r1,r2,r3];

    rotationVectors(:, i) = vision.internal.calibration.rodriguesMatrixToVector(R);

    % translation vector
    t = lambda * Ainv * h3;  %#ok
    translationVectors(:, i) = t;
end

rotationVectors = rotationVectors';
translationVectors = translationVectors';

参考资料（Reference）

[1] OpenCV实现SfM（一）: 相机模型
 [2] 相机的那些事儿 (二)成像模型
 [3]相机标定（Camera calibration）原理、步骤
 [4] 机器视觉的相机标定到底是什么？
[5] 张正友相机标定Opencv实现以及标定流程&&标定结果评价&&图像矫正流程解析（附标定程序和棋盘图）
[6] 张正友标定算法原理详解

单目视觉（4）：SFM之相机模型(二)