zynq FPGA 的双目视觉毕业设计（四）之matlab 摄像头畸变矫正算法实现

1.简述

     有很多大佬对摄像头畸变矫正理论原理进行了推理和实践，我就不洗了，推荐给你们吧

    1.双目相机的畸变校正以及平行校正（极线校正）的入门问题总结

     2. 畸变校正详解

2.我的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 = [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.矫正效果

欢迎关注我的公众号：芯王国，有更多的FPGA&数字IC的技术分享，还可以获取开源FPGA项目！