Matlab相机标定例程中去畸变过程详解+程序代码实现（如何调用源程序）

最近在研究通过Matlab标定程序获得相机内参以及畸变参数之后，如何实现对一张新拍摄的照片进行去畸变。由于Matlab标定程序只能显示标定所用的图片的去畸变图像，并不能直接对由该相机拍摄的一张新图片进行去畸变，所以就想着如何从源程序入手，写一小段代码，脱离标定例程程序，直接调用去畸变源程序。

首先，为了确保调用的源程序可用，大家可以先下载这个工具箱，免费下载地址：http://www.vision.caltech.edu/bouguetj/calib_doc/download/index.html，该工具箱加压后，将其路径添加到Matlab中，可直接调用其中的一些源程序。（该工具箱不是Matlab自带的相机标定，具体使用可以参考：https://www.cnblogs.com/czaoth/p/6708631.html）而本次调用的去畸变源程序主要为工具箱中的rect函数和apply_distortion两个函数。（如果不想下载工具箱，直接赋值本文后面的源程序代码，创建该函数即可）

在已知内参矩阵和畸变参数的情况下去畸变的步骤：

1.首先将像素坐标系转换至相机坐标系

相信大家之前对相机标定都有一定的了解，其中内参矩阵是指摄像机坐标系到图像坐标系再到像素坐标系的转换矩阵。具体为：设摄像机坐标系下的齐次坐标为（x,y,z,1）,像素坐标系的齐次坐标为（u,v,1），则有

则（x,y,z,1)'  =  inv(A)*(u,v,1)'。

2.在相机坐标系下去畸变 

由畸变模型：

•xcorrected = x(1+k1r2+k2r4+k3r6)

•ycorrected = y(1+k1r2+k2r4+k3r6) 可得失真图像像素点(x,y)在去畸变之后的位置(xcorrected,ycorrected)，值得注意的是，该过程是对失真图像所有像素点进行操作，并不只是对提取的棋盘格角点进行操作。

其中r2 = x^2+y^2,k1,k2,k3为径向畸变参数，切向畸变在本次应用中不考虑，详细说明可以看这篇博客：https://blog.csdn.net/JennyBi/article/details/85764988

3.将校正后的相机坐标系转回像素坐标系

主要通过下式转换：

u = fx*xcorrected+cx;

v = fy*ycorrected+cy;

4.根据失真图像在校正后的像素坐标位置进行插值处理（赋值）

该程序里插值的方法主要是通过中心像素点左上角(left-up)、右上角(right-up)、左下角(left-down)、右下角的值(right-down)，对中心像素点的亮度值赋值。具体如下：

a1 = (1 - alpha_y).*(1 - alpha_x);
a2 = (1 - alpha_y).*alpha_x;
a3 = alpha_y .* (1 - alpha_x);
a4 = alpha_y .* alpha_x;

Irec(ind_new) = a1 .* I(ind_lu) + a2 .* I(ind_ru) + a3 .* I(ind_ld) + a4 .* I(ind_rd);

其中alpha_x由步骤三中的u计算得出：alpha_x=u-floor(u);同理，alpha_y=v-floor(v)

至此，去畸变完成。上述过程也就是rect函数的流程，下面给出具体代码参考。

load cameraParams;%加载标定的参数结果
intrinx = cameraParams.IntrinsicMatrix';
I = imread('abc_3.bmp');
I = I(:,:,2);%除此之外，如果对三个通道同时处理，那么出来的就是彩色图

fx = intrinx(1,1);
fy = intrinx(2,2);
fc = [fx fy];
cx = intrinx(1,3);
cy = intrinx(2,3);
cc = [cx cy];
alpha_c = 0;
k21 = cameraParams.RadialDistortion(1);
k22 = cameraParams.RadialDistortion(2);
k23 = cameraParams.RadialDistortion(3);
kc = [k21 k22 k23 0 0];%k1,k2,k3,p1,p2
KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2) ; 0 0 1];
[I2] = rect(double(I),eye(3),fc,cc,kc,alpha_c,KK);%去畸变函数

figure();
image(I2);
colormap(gray(256));
title('Undistorted image');

rect函数

function [Irec] = rect(I,R,f,c,k,alpha,KK_new);

if nargin < 5,
   k = [0;0;0;0;0];
   if nargin < 4,
      c = [0;0];
      if nargin < 3,
         f = [1;1];
         if nargin < 2,
            R = eye(3);
            if nargin < 1,
               error('ERROR: Need an image to rectify');
            end;
         end;
      end;
   end;
end;

if nargin < 7,
   if nargin < 6,
        KK_new = [f(1) 0 c(1);0 f(2) c(2);0 0 1];
   else
       KK_new = alpha; % the 6th argument is actually KK_new   
   end;
   alpha = 0;
end;

 

% Note: R is the motion of the points in space
% So: X2 = R*X where X: coord in the old reference frame, X2: coord in the new ref frame.

if ~exist('KK_new'),
   KK_new = [f(1) alpha*f(1) c(1);0 f(2) c(2);0 0 1];
end;

[nr,nc] = size(I);%畸变图像尺寸

Irec = 255*ones(nr,nc);%创建去畸变图像尺寸
%% 像素系到相机坐标系的转换
[mx,my] = meshgrid(1:nc, 1:nr);
px = reshape(mx',nc*nr,1);%将mx’按列顺序，将其变为nc*nr行，1列的矩阵
py = reshape(my',nc*nr,1);

rays = inv(KK_new)*[(px - 1)';(py - 1)';ones(1,length(px))];

% Rotation: (or affine transformation):

rays2 = R'*rays;

x = [rays2(1,:)./rays2(3,:);rays2(2,:)./rays2(3,:)];
%% 在相机坐标系下去畸变（主要是找失真像素点在未失真情况下上的坐标）

% Add distortion:
xd = apply_distortion(x,k);

%% 将相机坐标系转回像素坐标系
% Reconvert in pixels:

px2 = f(1)*(xd(1,:)+alpha*xd(2,:))+c(1);
py2 = f(2)*xd(2,:)+c(2);

%% 对像素值进行插值处理
% Interpolate between the closest pixels:

px_0 = floor(px2);

py_0 = floor(py2);
py_1 = py_0 + 1;

good_points = find((px_0 >= 0) & (px_0 <= (nc-2)) & (py_0 >= 0) & (py_0 <= (nr-2)));

px2 = px2(good_points);
py2 = py2(good_points);
px_0 = px_0(good_points);
py_0 = py_0(good_points);

alpha_x = px2 - px_0;
alpha_y = py2 - py_0;

a1 = (1 - alpha_y).*(1 - alpha_x);
a2 = (1 - alpha_y).*alpha_x;
a3 = alpha_y .* (1 - alpha_x);
a4 = alpha_y .* alpha_x;

ind_lu = px_0 * nr + py_0 + 1;
ind_ru = (px_0 + 1) * nr + py_0 + 1;
ind_ld = px_0 * nr + (py_0 + 1) + 1;
ind_rd = (px_0 + 1) * nr + (py_0 + 1) + 1;

ind_new = (px(good_points)-1)*nr + py(good_points);

 

Irec(ind_new) = a1 .* I(ind_lu) + a2 .* I(ind_ru) + a3 .* I(ind_ld) + a4 .* I(ind_rd);

 

return;

% Convert in indices:

fact = 3;

[XX,YY]= meshgrid(1:nc,1:nr);
[XXi,YYi]= meshgrid(1:1/fact:nc,1:1/fact:nr);

%tic;
Iinterp = interp2(XX,YY,I,XXi,YYi); 
%toc

[nri,nci] = size(Iinterp);

ind_col = round(fact*(f(1)*xd(1,:)+c(1)))+1;
ind_row = round(fact*(f(2)*xd(2,:)+c(2)))+1;

good_points = find((ind_col >=1)&(ind_col<=nci)&(ind_row >=1)& (ind_row <=nri));
 

apply_distort函数

function [xd,dxddk] = apply_distortion(x,k)

% Complete the distortion vector if you are using the simple distortion model:
length_k = length(k);
if length_k <5 ,
    k = [k ; zeros(5-length_k,1)];
end;

[m,n] = size(x);

% Add distortion:

r2 = x(1,:).^2 + x(2,:).^2;

r4 = r2.^2;

r6 = r2.^3;

% Radial distortion:

cdist = 1 + k(1) * r2 + k(2) * r4 + k(3) * r6;
% cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6;%原版

if nargout > 1,
    dcdistdk = [ r2' r4' zeros(n,2) r6'];
end;

xd1 = x .* (ones(2,1)*cdist);

coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3));

if nargout > 1,
    dxd1dk = zeros(2*n,5);
    dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk;
    dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk;
end;

% tangential distortion:

a1 = 2.*x(1,:).*x(2,:);
a2 = r2 + 2*x(1,:).^2;
a3 = r2 + 2*x(2,:).^2;

% delta_x = [k(3)*a1 + k(4)*a2 ;
%    k(3) * a3 + k(4)*a1];%原版
delta_x = [k(4)*a1 + k(5)*a2 ;
   k(4) * a3 + k(5)*a1];

aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3);
bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3);
cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3);

if nargout > 1,
    ddelta_xdk = zeros(2*n,5);
    ddelta_xdk(1:2:end,3) = a1';
    ddelta_xdk(1:2:end,4) = a2';
    ddelta_xdk(2:2:end,3) = a3';
    ddelta_xdk(2:2:end,4) = a1';
end;

xd = xd1 + delta_x;

if nargout > 1,
    dxddk = dxd1dk + ddelta_xdk ;
    if length_k < 5,
        dxddk = dxddk(:,1:length_k);
    end;
end;

return;

% Test of the Jacobians:

n = 10;

lk = 1;

x = 10*randn(2,n);
k = 0.5*randn(lk,1);

[xd,dxddk] = apply_distortion(x,k);

% Test on k: OK!!

dk = 0.001 * norm(k)*randn(lk,1);
k2 = k + dk;

[x2] = apply_distortion(x,k2);

x_pred = xd + reshape(dxddk * dk,2,n);

norm(x2-xd)/norm(x2 - x_pred)

 

实验结果 

 失真图像

去畸变之后