张正友相机标定（全流程，含畸变，matlab源代码解析）

张正友标定的具体原理很多文章已经介绍，这里主要结合源代码对其中的基本原理及本人遇到的问题进行介绍。（仅介绍基本原理供本人复习，同时方便他人，如有问题，请及时指正勿喷）

1. 标定基本思路介绍

相机标定，即获取其内参、外参、畸变系数（内参与外参及相机成像模型的解释可以参考SLAM入门之视觉里程计）。
具体可以描述为
$$s\stackrel{-}{m}=K[\begin{array}{cc}R& T\end{array}]M$$
其中s为世界坐标系到图像坐标系的尺度因子，K为内参矩阵，具体为
$$K=\left[\begin{array}{ccc}{f}_x& 0& c_1\\ 0& f_y& c_2\\ 0& 0& 1\end{array}\right]$$
[R T]为旋转矩阵与平移向量，M为三维坐标点。

张正友标定方法的核心即是采用平面标定板，从而可以方便的将标定点的真值Z置为0，以此方便计算。

具体流程主要为
1、计算相机内参（由于采用平面标定板，可以很方面的计算出单应矩阵，然后根据单应矩阵计算出内参）

2. 计算初始相机外参（计算出内参后，根据单应矩阵直接计算即可获得外参）
3. 最大似然优化相机内参与外参（1、2得到的结果必定是带有误差的，因此需进行优化，此处误差函数不考虑畸变）
4. 计算畸变系数（以上一步算到的结果作为真值计算畸变系数）
5. 所有参数一起最大似然优化 （将所有参数内参、外参、畸变系数一起优化）

%% 1. 计算初始相机内参
K=[];
D=[];
RT_list=[];
[K,H_list] = CalInitK(CaliBoard,img_points);

%% 2. 计算初始相机外参
RT_list = CalInitRT(K,H_list);

%% 3. 最大似然优化 
[K,RT_list] = MinReprojErrorKRT(K,RT_list,CaliBoard,img_points);

%% 4. 计算畸变系数 
%   D : 畸变参数,分别为k1,k2,k3,p1,p2
D = CalDAll(K,RT_list,CaliBoard,img_points);

%% 5. 所有参数一起最大似然优化 
[K,D,RT_list] = MinReprojErrorKDRT(K,D,RT_list,CaliBoard,img_points);

2. 相机内参计算

function [K,H_list] = CalInitK(CaliBoard,img_points)
% 相机初始内参计算
% 1.计算单应矩阵
% 2.根据旋转矩阵的特性，构建方程
% 3.根据旋转矩阵的特性，构建方程


% inputs,
%   CaliBoard : 标定板真实坐标点
%   img_points :相机拍摄到的图像提取到的标定板点
%
% outputs,
%   K : 相机内参
%   H_list : 单应矩阵序列

K = [];
H_list = [];
%% 1.计算单应矩阵
for i = 1:size(img_points,1)
    img_points_tmp = cell2mat(img_points(i));
    index = img_points_tmp(:,1);
    tmp1 = CaliBoard(index,1:2);
    tmp2 = img_points_tmp(:,2:3);
    H = CalH(tmp1,tmp2);
    H_list(i,:,:) = H;
end

%% 2.计算lambda*K^{-T}*k^{-1},lambda为放缩因子
B = CalB(H_list);

%% 3.根据B=lambda*K^{-T}*k^{-1}计算K，lambda为放缩因子
v0 = (B(1,2)*B(1,3)-B(1,1)*B(2,3))/(B(1,1)*B(2,2)-B(1,2)*B(1,2));
B_inv = inv(B); 
lambda = B_inv(3,3);
B_inv = B_inv/lambda;
c_x = B_inv(1,3);
c_y = B_inv(2,3);
f_x = sqrt((B_inv(1,1)-c_x*c_x));
f_y = sqrt((B_inv(2,2)-c_y*c_y));
K=[f_x,0,c_x;
    0,f_y,c_y;
    0,0,1];

end

2.1 单应矩阵计算

为方便计算，张正友采用了平面标定板，因此内参矩阵与旋转平移矩阵的乘积可以视作单应矩阵H即可得
$$s\stackrel{-}{m}=K[\begin{array}{cc}R& T\end{array}]M\text{=}HM$$

上面的公式若仅依靠这个变换自然是不成立的，但是考虑到Z为0，则可以变换为

根据上述公式，可以列出等式

将其变换为对应的矩阵（为方便计算，将$H_{33}$视为1）
则可得

根据多个对应点构建多个上述方程，即可求解单应矩阵H。如上述矩阵所述，一个对应点可构建2个方程，H矩阵总共有8个未知数，则至少需要4个对应点。

function H = CalH(CaliBoard,img_points)
% % 相机单应矩阵计算
% 为方便计算，张正友采用了平面标定板，因此内参矩阵与旋转平移矩阵的乘积可以视作单应矩阵H即可得
% sm=K[r1 r2 T]M,将其K[r1 r2 T]视为H，重构方程可得P1=H*P，s为尺度因子，暂时隐藏
% 根据P1=H*P得到两个方程，将H视为向量，构建方程
% 转变成A*X=B的矩阵形式
% P1(X1,Y1),表示img_points
% P(X,Y)，表示CaliBoard
% inputs,
%   CaliBoard : 标定板真实坐标点
%   img_points :相机拍摄到的图像提取到的标定板点（单张图像）
%
% outputs,
%   H : 单应矩阵

H=[];
XX1 = CaliBoard(:,1).*img_points(:,1);
X1Y = img_points(:,1).*CaliBoard(:,2);
XY1 = CaliBoard(:,1).*img_points(:,2);
YY1 = CaliBoard(:,2).*img_points(:,2);

A = zeros(size(CaliBoard,1)*2,8);
for i=1:size(CaliBoard,1)
    A(2*i-1,1:3) = [CaliBoard(i,1:2),1];
    A(2*i-1,7:8) = [-XX1(i,1),-X1Y(i,1)];
    A(2*i,4:6) = [CaliBoard(i,1:2),1];
    A(2*i,7:8) = [-XY1(i,1),-YY1(i,1)];
end
B=reshape(img_points.',[size(CaliBoard,1)*size(CaliBoard,2),1]);

H = A\B;
H=[H;1];
H = reshape(H,[3,3])';
end

2.2 基于单应矩阵计算相机内参

如2.1中所说，单应矩阵H=K[r1 r2 T]，但因为计算H时添加了$H_{99}$为1的约束，因此计算出来的结果应当添加一个尺寸因子，即计算出来的$H=\lambda \ast K\ast$[r1 r2 T] ，可转为
$$k_1\ast [H_1{H}_2{H}_3]=\lambda \ast [r1r2T]$$
其中$H_1,H_2,H_3$为单应矩阵的列。基于此结果，以及旋转矩阵的定义特性，可以构建两个方程，即

为了方便计算，可将$k^{-T}{K}^{-1}$视为一个矩阵B先行求解，可得

其中，由于B为$k^{-T}{K}^{-1}$，可得$k^{-T}{K}^{-1}=(k^{-T}{K}^{-1}{)}^T$，则B为对称矩阵，可将B定义为

由上式可知B中有6个未知数，即最少需要3个H矩阵。同时，上述B中的式子如果计算出来，可以看出$B_{12}$为0，因此，增加了一个约束条件，最少只需要两个H即可计算出结果。（此处为考虑下面n=2的情况）
附：关于标定图片数量n和内参获取的关系（来自参考文献[2]）

• 如果 n>=3，就可以得到唯一解b（带有一个比例因子λ）。
• 如果 n=2，也就是只有两张标定图片，那么我们可以设置内参中的γ=0（γ表示由于制造误差产生的两个坐标轴偏斜参数，通常很小，可忽略），将前面式子（搬运到下图）中γ=0可以看到对应 B12=0，换句话说，就是增加了一个约束条件：[0, 1, 0, 0, 0, 0]b = 0。（此处从运算角度上不理解，但若仅考虑未知数的数目，假设K只有4个未知数，则2个H足够计算）
• 如果n=1，只能假设u0, v0已知（位于图像中心）且 γ=0，这样只能解出fx, fy两个参数。
  
  基于此，即可计算出B的值。（公式图片来自参考文献[2]）
  
  然后基于B的值直接进行求解即可计算出内参。
  
  附：此处本人采用直接计算出B的逆矩阵，得到$B^{-1}=K\ast K^T$,即
  
  所得结果也一致（暂时不知道区别）

function B = CalB(H_list)
% 相机内参K^{-T}*k^{-1}计算
% 根据旋转矩阵的性质，每个单应矩阵可以得到两个方程
% 最终得到 v_{12}*b=0,(v_{11}-v_{22})*b=0,其中b为[B11,B12,B13,B22,B23,B33]^T
% v为~{{v}_{ij}}={{\left[ \begin{matrix}
%    \begin{matrix}
%    {{h}_{i1}}{{h}_{j1}} & {{h}_{i1}}{{h}_{j2}}+{{h}_{i2}}{{h}_{j1}} & {{h}_{i2}}{{h}_{j2}}  \\
% \end{matrix} & \begin{matrix}
%    {{h}_{i3}}{{h}_{j1}}+{{h}_{i1}}{{h}_{j3}} & {{h}_{i3}}{{h}_{j2}}+{{h}_{i2}}{{h}_{j3}} & {{h}_{i3}}{{h}_{j3}}  \\
% \end{matrix}  \\
% \end{matrix} \right]}^{T}}

% inputs,
%   H_list : 单应矩阵列表
%
% outputs,
%   B : K^{-T}*k^{-1}

B=[];

% 计算v
for i=1:size(H_list,1)
    h = squeeze(H_list(i,:,:));
    h=h'; % 公式计算时考虑按照列划分的1,2,3，所以需转置一下
    tmp_v(1,1) = h(1,1)*h(2,1);
    tmp_v(1,2) = h(1,2)*h(2,1)+h(1,1)*h(2,2);
    tmp_v(1,3) = h(1,3)*h(2,1)+h(1,1)*h(2,3);
    tmp_v(1,4) = h(1,2)*h(2,2);
    tmp_v(1,5) = h(1,3)*h(2,2)+h(1,2)*h(2,3);
    tmp_v(1,6) = h(1,3)*h(2,3);
    
    tmp_v(2,1) = h(1,1)*h(1,1)-h(2,1)*h(2,1);
    tmp_v(2,2) = 2*(h(1,2)*h(1,1)-h(2,2)*h(2,1));
    tmp_v(2,3) = 2*(h(1,3)*h(1,1)-h(2,3)*h(2,1));
    tmp_v(2,4) = h(1,2)*h(1,2)-h(2,2)*h(2,2);
    tmp_v(2,5) = 2*(h(1,3)*h(1,2)-h(2,3)*h(2,2));
    tmp_v(2,6) = h(1,3)*h(1,3)-h(2,3)*h(2,3);

    v(2*i-1:2*i,:) = tmp_v;
end

[U,S,V] = JOS_SVD(v);
b=V(:,6)';
B =[b(1,1),b(1,2),b(1,3);
    b(1,2),b(1,4),b(1,5);
     b(1,3),b(1,5),b(1,6)];
end

3 计算相机外参

相机内参知道后，根据公式$H=\lambda \ast K\ast$[r1 r2 T] 可直接计算外参，即
$K^{-1}H=\lambda \ast$[r1 r2 T]
由于旋转矩阵每列的模为1，可直接计算出外参矩阵。

function [RT_list] = CalInitRT(K,H_list)
% 相机初始内参计算
% r1 = lambda*M^-1*h_1,lambda与求解内参时的不同，
% 个人理解求第二个尺度因子时需添加R为1的约束，求第一个时H本身是up to scale的，无关紧要。
% r2 = lambda*M^-1*h_2
% r3 = r1 x r2
% t = lambda*M_1*h_3
%
% inputs,
%   K : 相机内参
%   H_list :单应矩阵序列
%
% outputs,
%   RT_list : 相机外参序列

RT_list = [];
K_inv = inv(K);
for i=1:size(H_list,1)
    h = squeeze(H_list(i,:,:));
    lambda = 1/norm(K_inv*h(:,1));
    r1 = lambda*K_inv*h(:,1);
    r2 = lambda*K_inv*h(:,2);
    r3 = cross(r1,r2);
    t = lambda*K_inv*h(:,3);
    rt = [r1,r2,r3,t];
    RT_list(i,:,:) = rt;
end
end

4. 最大似然优化

根据上述计算出的内参、外参矩阵，构建最小重投影误差函数，优化内参外参。其中需要注意的几个点主要为

1. 旋转矩阵应转换为向量
2. 优化是参数输入进去应为一个向量
3. 重投影误差输出可以为二维矩阵（即X,Y两个方向）

function [K_new,RT_list_new] = MinReprojErrorKRT(K,RT_list,CaliBoard,img_points)
% 最小化重投影误差函数
% inputs,
%   K : 待优化相机内参
%   RT_list : 待优化旋转平移矩阵
%   CaliBoard : 标定板真实坐标点
%   img_points :相机拍摄到的图像提取到的标定板点
%
% outputs,
%   K_new : 优化后的相机内参
%   RT_list : 优化后的旋转平移矩阵

K_new=[];
RT_list_new=[];

% 构建变量及初始值
X_init (1:4) = [K(1,1),K(2,2),K(1,3),K(2,3)];
for i=1:size(RT_list,1)
    X_init (i*6-1:i*6+1) = rodrigues(squeeze(RT_list(i,:,1:3)));
    X_init (2+i*6:4+i*6) = RT_list(i,:,4)';
end

% 优化参数
options = optimoptions(@lsqnonlin,'Algorithm','levenberg-marquardt');
f = @(x)ReprojErrorKRT(x,CaliBoard,img_points);
[result,resnorm,residual ] = lsqnonlin(f,X_init,[],[],options);

%结果重构
K_new = K;
K_new(1,1) = result(1);
K_new(2,2) = result(2);
K_new(1,3) = result(3);
K_new(2,3) = result(4);
for i=1:size(RT_list,1)
    RT_list_new(i,:,1:3) = rodrigues(result (i*6-1:i*6+1));
    RT_list_new(i,:,4) =  result (2+i*6:4+i*6);
end

end

function erros_list = ReprojErrorKRT(X,CaliBoard,img_points)
% 重投影误差函数
% inputs,
%   X: 待优化的参数，
%       X(1:4)，f_x,f_y,c_x,c_y
%       X(5:7) r
%       X(8:10) t
%       以此类推，直到结束
%   CaliBoard : 标定板真实坐标点
%   img_points :相机拍摄到的图像提取到的标定板点
%
% outputs,
%   erros_list : 重投影误差(x,y两个方向，二维矩阵)

repro= 0;
repro_list=[];
erros_list=[];
K=[X(1),0,X(3);
    0,X(2),X(4);
    0,0,1];
CaliBoard = [CaliBoard(:,1:2),ones(size(CaliBoard,1),1)];
for i=1:size(img_points,1)
    img_points_tmp = cell2mat(img_points(i));
    index = img_points_tmp(:,1);
    CaliBoard_tmp = CaliBoard(index,:);
    R= rodrigues( X (i*6-1:i*6+1) );
    T= X (2+i*6:4+i*6)';
    RT = [R(:,1:2),T];
    img_points_proj = K*RT*CaliBoard_tmp';
    img_points_proj(1,:)=img_points_proj(1,:)./img_points_proj(3,:);
    img_points_proj(2,:)=img_points_proj(2,:)./img_points_proj(3,:);
    img_points_proj(3,:)=1;
    img_points_proj=img_points_proj';
    error =squeeze(img_points_tmp(:,2:3))-img_points_proj(:,1:2);
    erros_list=[erros_list;error];
    
    % 调试用
    erros_dis =vecnorm(error,2,2);
    repro = repro+sum(erros_dis.*erros_dis);
    repro_list=[repro_list;sum(erros_dis.*erros_dis)];
end
end

5. 计算畸变系数

在不考虑畸变系数的情况下优化完内外参数后，可以计算畸变系数。需要注明，畸变变换是在归一化平面进行的，因此计算时可以直接将坐标变换到归一化坐标系方便进行计算，也方便进行理解。
设（u,v）为标定板根据外参投影到归一化平面上的点坐标，$(u_{dis},v_{dis})$为畸变后的坐标，也即实际图像坐标根据内参反向映射到归一化平面。则可得

其中$r^2=u^2+v^2$
将其转化为方程，则是

可以写成
$$AD=d$$
则可直接计算出畸变参数。
附：注意事项

• 注意变换是在归一化坐标系，所以最好的办法就是转换到归一化坐标系进行计算，不然容易发生错误。

function D = CalDAll(K,RT_list,CaliBoard,img_points)
% 畸变参数计算函数，径向畸变与切向畸变

%将函数记成A*D=d
% inputs,
%   K : 相机内参
%   RT_list : 旋转平移矩阵
%   CaliBoard : 标定板真实坐标点
%   img_points :相机拍摄到的图像提取到的标定板点
%
% outputs,
%   dis : 畸变参数,分别为k1,k2,k3,p1,p2

img_points_all = [];
CaliBoard = [CaliBoard(:,1:2),ones(size(CaliBoard,1),1)];
img_points__proj_norm_all = [];

% 重投影获取归一化坐标点
for i=1:size(img_points,1)
    
    img_points_tmp = cell2mat(img_points(i));
    RT = [squeeze(RT_list(i,:,1:2)),squeeze(RT_list(i,:,4))'];
    
    %获取所有图像点
    img_points_all=[img_points_all;squeeze(img_points_tmp(:,2:3))];
    
    % 理想点在归一化坐标系上的点 x
    img_points_proj_norm = RT*CaliBoard';
    img_points_proj_norm(1,:)=img_points_proj_norm(1,:)./img_points_proj_norm(3,:);
    img_points_proj_norm(2,:)=img_points_proj_norm(2,:)./img_points_proj_norm(3,:);
    img_points_proj_norm(3,:)=1;
    img_points_proj_norm=img_points_proj_norm';
    img_points__proj_norm_all= [img_points__proj_norm_all;img_points_proj_norm(:,1:2)];
end


% 畸变后的点在归一化坐标系上的点 x_dis
img_points_norm_all = inv(K)*[img_points_all,ones(size(img_points_all,1),1)]';
img_points_norm_all =img_points_norm_all';
% 计算D,d
r2 = sum(img_points__proj_norm_all.*img_points__proj_norm_all,2);
r4 = r2.*r2;
r6 = r2.*r4;

u = img_points__proj_norm_all(:,1);
v = img_points__proj_norm_all(:,2);
u_dis = img_points_norm_all(:,1);
v_dis = img_points_norm_all(:,2);

for i=1:size(img_points_all,1)
    A(2*i-1,1) = u(i,1)*r2(i,1);
    A(2*i-1,2) = u(i,1)*r4(i,1);
    A(2*i-1,3) = u(i,1)*r6(i,1);
    A(2*i-1,4) = 2*u(i,1)*v(i,1);
    A(2*i-1,5) = r2(i,1)+2*u(i,1)*u(i,1);
    
    A(2*i,1) = v(i,1)*r2(i,1);
    A(2*i,2) = v(i,1)*r4(i,1);
    A(2*i,3) = v(i,1)*r6(i,1);
    A(2*i,4) = r2(i,1)+2*v(i,1)*v(i,1);
    A(2*i,5) = 2*u(i,1)*v(i,1);
    
    d(2*i-1,1) = u_dis(i,1)-u(i,1);
    d(2*i,1) = v_dis(i,1)-v(i,1);
end

D = A\d;

end

6. 考虑畸变系数的优化

与4中基本一致，只是投影时考虑到了畸变参数。

function [K_new,D_new,RT_list_new] = MinReprojErrorKDRT(K,D,RT_list,CaliBoard,img_points)
% 最小化重投影误差函数
% inputs,
%   K : 待优化相机内参
%   D: 待优化的相机畸变参数
%   RT_list : 待优化旋转平移矩阵
%   CaliBoard : 标定板真实坐标点
%   img_points :相机拍摄到的图像提取到的标定板点
%
% outputs,
%   K_new : 优化后的相机内参
%   D_new: 优化后的相机畸变参数
%   RT_list : 优化后的旋转平移矩阵

K_new=[];
RT_list_new=[];
D_new= [];
% 构建变量及初始值
X_init (1:4) = [K(1,1),K(2,2),K(1,3),K(2,3)];
X_init(5:9) = D;
for i=1:size(RT_list,1)
    X_init (i*6+4:i*6+6) = rodrigues(squeeze(RT_list(i,:,1:3)));
    X_init (7+i*6:9+i*6) = RT_list(i,:,4)';
end

% 优化参数
options = optimoptions(@lsqnonlin,'Algorithm','levenberg-marquardt');
f = @(x)ReprojErrorKDRT(x,CaliBoard,img_points);
[result,resnorm,residual ] = lsqnonlin(f,X_init,[],[],options);

%结果重构
K_new = K;
K_new(1,1) = result(1);
K_new(2,2) = result(2);
K_new(1,3) = result(3);
K_new(2,3) = result(4);
D_new = result(5:9);
for i=1:size(RT_list,1)
    RT_list_new(i,:,1:3) = rodrigues(result (i*6+4:i*6+6));
    RT_list_new(i,:,4) =  result (7+i*6:9+i*6);
end

end

function error = ReprojErrorKDRT(X,CaliBoard,img_points)
% 重投影误差函数,考虑畸变参数
% inputs,
%   X: 待优化的参数，
%       X(1:4)，f_x,f_y,c_x,c_y
%       X(5:9),k1,k2,k3,p1,p2
%       X(10:12) r
%       X(12:14) t
%       以此类推，直到结束
%   CaliBoard : 标定板真实坐标点
%   img_points :相机拍摄到的图像提取到的标定板点
%
% outputs,
%   erros_list : 重投影误差(x,y两个方向，二维矩阵)

repro= 0;
repro_list=[];
K=[X(1),0,X(3);
    0,X(2),X(4);
    0,0,1];
k1 = X(5);
k2 = X(6);
k3 = X(7);
p1 = X(8);
p2 = X(9);

CaliBoard = [CaliBoard(:,1:2),ones(size(CaliBoard,1),1)];
img_points_proj_norm_all=[];
img_points_all=[];
% 重投影获取像素坐标点
for i=1:size(img_points,1)
    img_points_tmp = cell2mat(img_points(i));
    index = img_points_tmp(:,1);
    CaliBoard_tmp = CaliBoard(index,:);
    
    R= rodrigues( X (i*6+4:i*6+6) );
    T= X (7+i*6:9+i*6)';
    RT = [R(:,1:2),T];
    
    % 理想点在归一化坐标系上的点 x
    img_points_proj_norm = RT*CaliBoard';
    img_points_proj_norm(1,:)=img_points_proj_norm(1,:)./img_points_proj_norm(3,:);
    img_points_proj_norm(2,:)=img_points_proj_norm(2,:)./img_points_proj_norm(3,:);
    img_points_proj_norm(3,:)=1;
    img_points_proj_norm=img_points_proj_norm';
    img_points_proj_norm_all= [img_points_proj_norm_all;img_points_proj_norm(:,1:2)];
    %获取所有图像点
    img_points_all=[img_points_all;squeeze(img_points_tmp(:,2:3))];end

% 计算D,d
r2 = sum(img_points_proj_norm_all.*img_points_proj_norm_all,2);
r4 = r2.*r2;
r6 = r2.*r4;

x = img_points_proj_norm_all(:,1);
y = img_points_proj_norm_all(:,2);

x_dis = x.*(1+k1.*r2+k2.*r4+k3.*r6)+2*p1*(x.*y)+p2*(r2+2*x.*x);
y_dis = y.*(1+k1.*r2+k2.*r4+k3.*r6)+2*p2*(x.*y)+p1*(r2+2*y.*y);

img_points_proj_all = K*[x_dis,y_dis,ones(size(x_dis,1),1)]';
img_points_proj_all = img_points_proj_all';
error = img_points_proj_all(:,1:2)-img_points_all;
end

参考文献

[1] 张正友标定算法原理详解.https://www.jianshu.com/p/9d2fe4c2e3b7
[2]张正友标定法-完整学习笔记-从原理到实战. https://zhuanlan.zhihu.com/p/136827980
[3]张正友标定法.https://www.cnblogs.com/linzzz98/articles/13604279.html

相关资源代码

部分库可以下载看
https://download.csdn.net/download/zhangpan333/85445628