Zynq FPGA binocular vision graduation design (four) matlab camera distortion correction algorithm implementation

1. Brief description

     There are many big guys who have reasoned and practiced the theoretical principles of camera distortion correction, I will not wash it, and I recommend it to you.

    1. Summary of the basic problems of distortion correction and parallel correction (polar line correction) of binocular camera

     2.  Detailed explanation of distortion correction

2. My matlab distortion correction source code

clear;clc;close all;


image_left = imread('./IMAG_L1.BMP');
image_right= imread('./IMAG_R1.BMP');

[H, W, C] = size(image_left);
%内参
A_L = [727.16981    0           375.50000;
      0             726.44894   239.50000;
      0             0           1       ];
  
A_R = [731.58976    0           381.09402;
       0            730.18323   246.16608;
       0            0           1       ];
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);
%外参

D_L = [ -0.50296   0.36519   -0.00268   0.00240  0.00000 ];
D_R = [ -0.46867   0.27536   -0.00327   0.00309  0.00000 ];

k11 = D_L(1,1);
k12 = D_L(1,2);
k13 = D_L(1,5);
p11 = D_L(1,3);
p12 = D_L(1,4);

k21 = D_R(1,1);
k22 = D_R(1,2);
k23 = D_R(1,5);
p21 = D_R(1,3);
p22 = D_R(1,4);

%旋转与平移
R = [ -0.00815   -0.00416  0.00418 ];
T = [ -104.30359   -3.38916  -5.23585 ];

for v = 1 : H
    for u = 1 : W  
        
        B_L = inv(A_L)*[u v 1]';
        B_R = inv(A_R)*[u v 1]';
        
        x1 = B_L(1,1);
        y1 = B_L(2,1);
        r1 = x1^2 + y1^2;
        
        x2 = B_R(1,1);
        y2 = B_R(2,1);
        r2 = x2^2 + y2^2;
        
        xx1 =  x1*(1 + k11*r1 + k12*r1^2 ) + 2*p11*x1*y1 + p12*(r1 + 2*x1^2) ;
        yy1 =  y1*(1 + k11*r1 + k12*r1^2 ) + 2*p12*x1*y1 + p11*(r1 + 2*y1^2) ;
        
        
        xx2 =  x2*(1 + k21*r2 + k22*r2^2 ) + 2*p21*x2*y2 + p22*(r2 + 2*x2^2) ;
        yy2 =  y2*(1 + k21*r2 + k22*r2^2 ) + 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 && xxx1<=W && yyy1>1 && yyy1<=H)
            w1 = xxx1;
            h1 = yyy1;
            new_image_L(v,u)=  (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 && xxx2<=W && yyy2>1 && yyy2<=H)
            w2 = xxx2;
            h2 = yyy2;
            new_image_R(v,u)=  (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


subplot(2,2,1);imshow(image_left);
title('左相机校正之前');
subplot(2,2,3);imshow(new_image_L);
title('左相机校正之后');

subplot(2,2,2);imshow(image_right);
title('右相机校正之前');
subplot(2,2,4);imshow(new_image_R);
title('右相机校正之后');

3. Correction effect

Welcome to follow my public account: Core Kingdom, there are more FPGA & digital IC technology sharing, and you can also get open source FPGA projects!