张正友相机标定(全流程,含畸变,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、2得到的结果必定是带有误差的,因此需进行优化,此处误差函数不考虑畸变)
  3. 计算畸变系数(以上一步算到的结果作为真值计算畸变系数)
  4. 所有参数一起最大似然优化 (将所有参数内参、外参、畸变系数一起优化)
%% 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)
则可得
张正友相机标定(全流程,含畸变,matlab源代码解析)_第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为单应矩阵的列。基于此结果,以及旋转矩阵的定义特性,可以构建两个方程,即
张正友相机标定(全流程,含畸变,matlab源代码解析)_第2张图片

为了方便计算,可将 k − T K − 1 k^{-T}K^{-1} kTK1视为一个矩阵B先行求解,可得
在这里插入图片描述
其中,由于B为 k − T K − 1 k^{-T}K^{-1} kTK1,可得 k − T K − 1 = ( k − T K − 1 ) T k^{-T}K^{-1}=(k^{-T}K^{-1})^T kTK1=(kTK1)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两个参数。
    张正友相机标定(全流程,含畸变,matlab源代码解析)_第3张图片
    基于此,即可计算出B的值。(公式图片来自参考文献[2])
    张正友相机标定(全流程,含畸变,matlab源代码解析)_第4张图片
    然后基于B的值直接进行求解即可计算出内参。
    张正友相机标定(全流程,含畸变,matlab源代码解析)_第5张图片
    附:此处本人采用直接计算出B的逆矩阵,得到 B − 1 = K ∗ K T B^{-1}=K*K^{T} B1=KKT,即
    在这里插入图片描述
    所得结果也一致(暂时不知道区别)
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* K1H=λ[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 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
将其转化为方程,则是
张正友相机标定(全流程,含畸变,matlab源代码解析)_第6张图片
可以写成
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

你可能感兴趣的:(双目视觉,matlab,开发语言)