MATLAB 双目视觉极线矫正算法实现

 matlab的源码：

clear;clc;close all;
 
 
image_left = imread('./IMAG_L1.BMP');
image_right= imread('./IMAG_R1.BMP');
 
[H, W, C] = size(image_left);
 
 
%内参
A_L = [ 429.99849   0.0         376.35001;
        0.0         427.66238    235.38535;
        0.0         0.0         1.0];
  
A_R = [ 422.52402   0.0         387.14095;
        0.0         421.20306   210.78373;
        0.0         0.0         1.0];
   
fx1 = A_L(1,1);
fy1 = A_L(2,2);
cx1 = A_L(1,3);
cy1 = A_L(2,3);
 
fx2 = A_R(1,1);
fy2 = A_R(2,2);
cx2 = A_R(1,3);
cy2 = A_R(2,3);
 
fx = (fx1 + fx2)/2;
fy = (fy1 + fy2)/2;
cx = (cx1 + cx2)/2;
cy = (cy1 + cy2)/2;
 
AA = [fx    0   cx;
      0     fy  cy;
      0     0    1];
%外参
 
 
 
D_L = [  -0.02811   0.01014   -0.00971   -0.00288  0.00000 ];
D_R = [  -0.01148   -0.00157   -0.01033   0.00455  0.00000 ];
 
 
 
k11 = D_L(1,1);
k12 = D_L(1,2);
k13 = D_L(1,4);
p11 = D_L(1,5);
p12 = D_L(1,3);
 
k21 = D_R(1,1);
k22 = D_R(1,2);
k23 = D_R(1,4);
p21 = D_R(1,5);
p22 = D_R(1,3);
 
%旋转与平移
 
om = [ 0.00214   -0.03862  -0.00286] ;
 
R_r = rodrigues(om/2);
R_l = -R_r';
 
 
for v = 11 : H-10
    for u = 26 : W-2
        
        u1 = (u - cx)/fx;
        v1 = (v - cy)/fy;
        
        pos1 = inv(R_l)*[u1 v1 1]';
        pos2 = inv(R_r)*[u1 v1 1]';
        
        
        
        x1 = pos1(1,:)/pos1(3,:);
        y1 = pos1(2,:)/pos1(3,:);
        r1 = x1^2 + y1^2;
        
        
        x2 = pos2(1,:)/pos2(3,:);
        y2 = pos2(2,:)/pos2(3,:);
        r2 = x2^2 + y2^2;
        
        xx1 =  x1*(1 + k11*r1 + k12*r1^2 + k13*r1^3) + 2*p11*x1*y1 + p12*(r1 + 2*x1^2) ;
        yy1 =  y1*(1 + k11*r1 + k12*r1^2 + k13*r1^3) + 2*p12*x1*y1 + p11*(r1 + 2*y1^2) ;
        
        
        xx2 =  x2*(1 + k21*r2 + k22*r2^2 +k23*r2^3) + 2*p21*x2*y2 + p22*(r2 + 2*x2^2) ;
        yy2 =  y2*(1 + k21*r2 + k22*r2^2 +k23*r2^3) + 2*p22*x2*y2 + p21*(r2 + 2*y2^2) ;
        
        xxx1 = xx1*fx1 + cx1;
        yyy1 = yy1*fy1 + cy1;
        
        xxx2 = xx2*fx2 + cx2;
        yyy2 = yy2*fy2 + cy2;
        
         if( xxx1 > 1 && yyy1 >1 &&  xxx1 <= 752 && yyy1 <=480)
            w1 = xxx1;
            h1 = yyy1;
 
            
            new_image_L(v-10,u-25)=  (floor(w1+1)-w1) * (floor(h1+1)-h1) * image_left(floor(h1),floor(w1)) + (floor(w1+1)-w1) * (h1-floor(h1)) * image_left(floor(h1+1),floor(w1)) + (w1-floor(w1)) * (floor(h1+1)-h1) * image_left(floor(h1),floor(w1+1) ) + (w1-floor(w1)) * (h1-floor(h1)) * image_left(floor(h1+1),floor(w1+1));
         end
        
         if(xxx2 > 1 && yyy2 >1 &&  xxx2 <= 752 && yyy2 <=480)
            w2 = xxx2;
            h2 = yyy2;
            
            new_image_R(v-10,u-25)=  (floor(w2+1)-w2) * (floor(h2+1)-h2) * image_right(floor(h2),floor(w2)) + (floor(w2+1)-w2) * (h2-floor(h2)) * image_right(floor(h2+1),floor(w2)) + (w2-floor(w2)) * (floor(h2+1)-h2) * image_right(floor(h2),floor(w2+1) ) + (w2-floor(w2)) * (h2-floor(h2)) * image_right(floor(h2+1),floor(w2+1));
         end
    end
end
size(new_image_L)
size(new_image_R)
 
ori_image = [image_left,image_right];
cor_image = [new_image_L,new_image_R];
[e, f, g] = size(ori_image);
 
subplot(2,1,1);imshow(ori_image);
title('校正之前');
 
M = 10;  % 水平分量
N = 10;  % 垂直分量
lw = 0.001;  % 划线宽度
mx= ones(1,M+1);
my = linspace(1,e,M+1);
% 画水平线
for k = 1:M+1
    line([mx(k) f*mx(k)],[my(k) my(k)],'color','g','LineWidth',lw);
end
nx = linspace(1,f,N+1);
ny = ones(1,N+1);
% 画垂直线
for k = 1:N+1
    line([nx(k) nx(k)],[ny(k) e*ny(k)],'color','g','LineWidth',lw);
end
 
 
subplot(2,1,2);imshow(cor_image);
title('校正之后');
 
M = 10;  % 水平分量
N = 10;  % 垂直分量
lw = 0.001;  % 划线宽度
mx= ones(1,M+1);
my = linspace(1,e,M+1);
% 画水平线
for k = 1:M+1
    line([mx(k) f*mx(k)],[my(k) my(k)],'color','g','LineWidth',lw);
end
nx = linspace(1,f,N+1);
ny = ones(1,N+1);
% 画垂直线
for k = 1:N+1
    line([nx(k) nx(k)],[ny(k) e*ny(k)],'color','g','LineWidth',lw);
end
 
 
 快去试试吧！