张正友相机标定（全流程，含畸变，matlab源代码解析）
张正友标定的具体原理很多文章已经介绍，这里主要结合源代码对其中的基本原理及本人遇到的问题进行介绍。（仅介绍基本原理供本人复习，同时方便他人，如有问题，请及时指正勿喷）
1. 标定基本思路介绍
相机标定，即获取其内参、外参、畸变系数（内参与外参及相机成像模型的解释可以参考SLAM入门之视觉里程计）。
具体可以描述为
s m − = K [ R T ] M s\overset{-}{\mathop{m}}\,=K[\begin{matrix} R & T \\ \end{matrix}]M sm−=K[RT]M
其中s为世界坐标系到图像坐标系的尺度因子，K为内参矩阵，具体为
K = [ f x 0 c 1 0 f y c 2 0 0 1 ] K=\left[ \begin{matrix} {{f}_{x}} & 0 & {{c}_{1}} \\ 0 & {{f}_{y}} & {{c}_{2}} \\ 0 & 0 & 1 \\ \end{matrix} \right] K=⎣⎡fx000fy0c1c21⎦⎤
[R T]为旋转矩阵与平移向量，M为三维坐标点。
张正友标定方法的核心即是采用平面标定板，从而可以方便的将标定点的真值Z置为0，以此方便计算。
具体流程主要为
1、计算相机内参（由于采用平面标定板，可以很方面的计算出单应矩阵，然后根据单应矩阵计算出内参）
- 计算初始相机外参（计算出内参后，根据单应矩阵直接计算即可获得外参）
- 最大似然优化相机内参与外参（1、2得到的结果必定是带有误差的，因此需进行优化，此处误差函数不考虑畸变）
- 计算畸变系数（以上一步算到的结果作为真值计算畸变系数）
- 所有参数一起最大似然优化 （将所有参数内参、外参、畸变系数一起优化）
%% 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 m − = K [ R T ] M = H M s\overset{-}{\mathop{m}}\,=K[\begin{matrix} R & T \\ \end{matrix}]M\text{=}HM sm−=K[RT]M=HM
上面的公式若仅依靠这个变换自然是不成立的，但是考虑到Z为0，则可以变换为
根据上述公式，可以列出等式
将其变换为对应的矩阵（为方便计算，将 H 33 H_{33} H33视为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 H_{99} H99为1的约束，因此计算出来的结果应当添加一个尺寸因子，即计算出来的 H = λ ∗ K ∗ H = \lambda*K* H=λ∗K∗[r1 r2 T] ，可转为
k 1 ∗ [ H 1 H 2 H 3 ] = λ ∗ [ r 1 r 2 T ] k_1*[H_1 H_2 H_3] = \lambda*[r1 r2 T] k1∗[H1H2H3]=λ∗[r1r2T]
其中 H 1 , H 2 , H 3 H_1,H_2,H_3 H1,H2,H3为单应矩阵的列。基于此结果，以及旋转矩阵的定义特性，可以构建两个方程，即
为了方便计算，可将 k − T K − 1 k^{-T}K^{-1} k−TK−1视为一个矩阵B先行求解，可得
其中，由于B为 k − T K − 1 k^{-T}K^{-1} k−TK−1，可得 k − T K − 1 = ( k − T K − 1 ) T k^{-T}K^{-1}=(k^{-T}K^{-1})^T k−TK−1=(k−TK−1)T，则B为对称矩阵，可将B定义为
由上式可知B中有6个未知数，即最少需要3个H矩阵。同时，上述B中的式子如果计算出来，可以看出 B 12 B_{12} B12为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 ∗ K T B^{-1}=K*K^{T} B−1=K∗KT,即
所得结果也一致（暂时不知道区别）
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 = λ ∗ K ∗ H = \lambda*K* H=λ∗K∗[r1 r2 T] 可直接计算外参，即
K − 1 H = λ ∗ K^{-1}H = \lambda* K−1H=λ∗[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. 最大似然优化
根据上述计算出的内参、外参矩阵，构建最小重投影误差函数，优化内参外参。其中需要注意的几个点主要为
- 旋转矩阵应转换为向量
- 优化是参数输入进去应为一个向量
- 重投影误差输出可以为二维矩阵（即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 d i s , v d i s ) (u_{dis},v_{dis}) (udis,vdis)为畸变后的坐标，也即实际图像坐标根据内参反向映射到归一化平面。则可得
其中 r 2 = u 2 + v 2 r^2=u^2+v^2 r2=u2+v2
将其转化为方程，则是
可以写成
A D = d AD=d 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