MATLAB实现双目校准

MATLAB实现双目图像校准

在网上见的大多数双目图像校准都是MATLAB与OpenCV结合来实现的，经过两个月断断续续的学习，总算用纯MATLAB实现的图像校准算法。下面对这一段时间的成果进行总结和防止以后重新学习。

不废话，直接开始吧。。。。。。

说到双目系统首先必须要说一下双目模型。
首先我们要先只要三个坐标系：图像坐标系、相机坐标系、世界坐标系。

可参考http://www.cnblogs.com/dverdon/p/5609124.html
其中相机坐标转换为图像坐标：
其中u0、v0为图像平面的主点，dx、dy为相机的像元尺寸大小，u、v为图像坐标点、x、y为相机坐标点。

其中，Xc、Yc、Zc代表相机坐标系，X、Y、Z为世界坐标系。R为旋转矩阵，T为平移矩阵。
以上为理想状态下的双目视觉模型，但在实际中，相机的或多或少都存在畸变。因此在实际应用中要将畸变图像进行矫正。

世界坐标转换为相机坐标畸变矫正
相机的畸变模型公式：
可参考http://blog.csdn.net/wangxiaokun671903/article/details/37973365
http://blog.csdn.net/humanking7/article/details/45037239
上面两篇文章讲的很详细，我在这里就复制粘贴了。

综合以上内容，可以总结为一下内容（我很懒，就直接粘贴过来了）

该过程为世界坐标系转换为图像坐标系的过程

该过程为相机畸变处理过程

以上对原理进行了简单介绍，网上资料比我讲的详细，我这里只是防止忘记，做一个备份。

下面开始相机标定过程：这里我直接参考http://blog.csdn.net/dreamharding/article/details/53700166
这里我将最终的标定结果贴出来

开始上代码：

公式法：

主要参考http://blog.csdn.net/wangxiaokun671903/article/details/38017055和工具箱代码

clc;clear;

I1 = rgb2gray(imread('右图片路径'));
I2 = rgb2gray(imread('左图片路径'));
[m,n] = size(I1);
I11 = zeros(m,n)+255;
[m,n] = size(I2);
I22 = zeros(m,n)+255;

A1 = [665.32773         0       345.54178;...
        0           660.17940   249.92143;...
        0               0           1];
D1 = [0.22386   -0.93591   0.00725   0.00018  0.00000];

A2 = [688.30717         0       315.31975;...
        0           683.13738   264.40537;...
        0               0           1];
D2 = [0.27153   -1.00396   0.01107   0.00364  0.00000 ];

fx1 = A1(1,1);
fy1 = A1(2,2);
cx1 = A1(1,3);
cy1 = A1(2,3);
k11 = D1(1);
k12 = D1(2);
p11 = D1(3);
p12 = D1(4);
k13 = D1(5);

fx2 = A2(1,1);
fy2 = A2(2,2);
cx2 = A2(1,3);
cy2 = A2(2,3);
k21 = D2(1);
k22 = D2(2);
p21 = D2(3);
p22 = D2(4);
k23 = D2(5);

fx = (fx1 + fx2)/2;
fy = (fy1 + fy2)/2;
cx = (cx1 + cx2)/2;
cy = (cy1 + cy2)/2;

T = [-39.78384   1.21230  -40.44029];

om = [-0.02302   -0.00735  0.00089];

% 以下部分为直接拷贝工具箱源码内容 start
r_r = rodrigues(-om/2);
r_l = r_r';
t = r_r * T;

% Rotate both cameras so as to bring the translation vector in alignment with the (1;0;0) axis:
if abs(t(1)) > abs(t(2)),
    type_stereo = 0;
    uu = [1;0;0]; % Horizontal epipolar lines
else
    type_stereo = 1;
    uu = [0;1;0]; % Vertical epipolar lines
end;
if dot(uu,t)<0,
    uu = -uu; % Swtich side of the vector 
end;
ww = cross(t,uu);
ww = ww/norm(ww);
ww = acos(abs(dot(t,uu))/(norm(t)*norm(uu)))*ww;
R = rodrigues(ww);


% Global rotations to be applied to both views:
R1 = R * r_r;
R2 = R * r_l;

T_new = R1*T;
% 以上部分为直接拷贝工具箱源码内容 end

for v=1:m %640
    for u=1:n  %480
        x = (v - cx1)/fx1;
        y = (u - cy)/fy1;
        pos=[x;y;1]; %3x1
        pos=inv(R1)*pos; %旋转相机坐标系，3x1
        x1=pos(1,:)/pos(3,:);%归一化
        y1=pos(2,:)/pos(3,:);
        r=x1^2+y1^2;
        x2 = x1*(1+k11*r+k12*r^2+k13*r^3) + 2*p11*x1*y1+p12*(r+2*x1^2);
        y2 = y1*(1+k11*r+k12*r^2+k13*r^3) + 2*p12*x1*y1+p11*(r+2*y1^2);
        x3 = x2*fx1 +cx1;
        y3 = y2*fy1 +cy;
        if (x3>1 && x3<=n && y3>1 && y3<=m)
            h=y3;
            w=x3;
            I11(v,u)=(floor(w+1)-w)*(floor(h+1)-h)*I1(floor(h),floor(w))+(floor(w+1)-w)*(h-floor(h))*I1(floor(h+1),floor(w))+(w-floor(w))*(floor(h+1)-h)*I1(floor(h),floor(w+1))+(w-floor(w))*(h-floor(h))*I1(floor(h+1),floor(w+1));
        end
        if mod(v,50) == 0
            I11(v,u) = 255;
        end
    end
end

for v=1:m %640
    for u=1:n  %480
        x = (v - cx2)/fx2;
        y = (u - cy)/fy2;
        pos=[x;y;1]; %3x1
        pos=inv(R2)*pos; %旋转相机坐标系，3x1
        x1=pos(1,:)/pos(3,:);%归一化
        y1=pos(2,:)/pos(3,:);
        r=x1^2+y1^2;
        x2 = x1*(1+k21*r+k22*r^2+k23*r^3) + 2*p21*x1*y1+p22*(r+2*x1^2);
        y2 = y1*(1+k21*r+k22*r^2+k23*r^3) + 2*p22*x1*y1+p21*(r+2*y1^2);
        x3 = x2*fx2 +cx2;
        y3 = y2*fy2 +cy;
        if (x3>1 && x3<=n && y3>1 && y3<=m)
            h=y3;
            w=x3;
            I22(v,u)=(floor(w+1)-w)*(floor(h+1)-h)*I1(floor(h),floor(w))+(floor(w+1)-w)*(h-floor(h))*I1(floor(h+1),floor(w))+(w-floor(w))*(floor(h+1)-h)*I1(floor(h),floor(w+1))+(w-floor(w))*(h-floor(h))*I1(floor(h+1),floor(w+1));
        end
        if mod(v,50) == 0           % 画线
            I22(v,u) = 255;
        end
    end
end

figure;imshow(I11,[]);
figure;imshow(I22,[]);

该过程是直接根据公式进行编写。执行效率很慢。

直接法：
主要参考工具箱代码
这里首先我们需要对工具箱源代码进行修改。

在工具箱中找到rectify_stereo_pair.m，添加如在代码。

save result_left ind_new_left ind_1_left ind_2_left ind_3_left ind_4_left a1_left a2_left a3_left a4_left ;
save result_right ind_new_right ind_1_right ind_2_right ind_3_right ind_4_right a1_right a2_right a3_right a4_right ;

添加如图：

点击Rectifu the calibration images

最终会多出两个文件

这两个文件就是将原始图像直接转换为平行校准后图像。

clc;clear;
I = double(rgb2gray(imread('左图像路径')));
re = load('result_left.mat');
ind_new_left = re.ind_new_left;
a1_left = re.a1_left;
ind_1_left = re.ind_1_left;
a2_left = re.a2_left;
ind_2_left = re.ind_2_left;
a3_left = re.a3_left;
ind_3_left = re.ind_3_left;
a4_left = re.a4_left;
ind_4_left= re.ind_4_left;

Il = 255*ones(480,640);
Il(ind_new_left) = uint8(a1_left .* I(ind_1_left) + a2_left .* I(ind_2_left) + a3_left .* I(ind_3_left) + a4_left .* I(ind_4_left));

I = double(rgb2gray(imread('右图像路径')));
re = load('result_right.mat');
ind_new_right = re.ind_new_right;
a1_right = re.a1_right;
ind_1_right = re.ind_1_right;
a2_right = re.a2_right;
ind_2_right = re.ind_2_right;
a3_right = re.a3_right;
ind_3_right = re.ind_3_right;
a4_right = re.a4_right;
ind_4_right = re.ind_4_right;

Ir = 255*ones(480,640);
Ir(ind_new_left) = uint8(a1_left .* I(ind_1_left) + a2_left .* I(ind_2_left) + a3_left .* I(ind_3_left) + a4_left .* I(ind_4_left));

for i = 1:480
    for j = 1:640
        if mod(i,20)==0
            Il(i,j) = 255;
            Ir(i,j) = 255;
        end
    end
end

figure;imshow(Il,[]);
figure;imshow(Ir,[]);

该方法可以提高算法的执行效率。
今天先写到这里，下一步利用C++来实现双目校准过程。