-
Notifications
You must be signed in to change notification settings - Fork 1
/
main_blackfly.m
128 lines (116 loc) · 3.91 KB
/
main_blackfly.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
% Author: Jared Strader
% File: main_blackfly.m
%
% Description: Computes the calibration parameters for the Blackfly S
% camera, then projects the example images on the unit sphere and
% reprojects the unit sphere to a perspective image with the specified
% orientation. The reprojectied image is not interpolated.
%
% x-axis -> right in the image aligned with u coordinates
% y-axis -> down in image aligned with v coordinates
% z-axis -> out of image (negative behind camera
%
% https://www.mathworks.com/help/vision/ug/fisheye-calibration-basics.html
% https://sites.google.com/site/scarabotix/ocamcalib-toolbox
%
clear all; close all; clc;
%% Calibrate Fisheye (Blackfly S)
% for i = 1:20
% imageFileName = sprintf('Image%d.jpg', i);
% imageFileNames{i} = fullfile(imageFileName);
% end
%
% [imagePoints,boardSize] = detectCheckerboardPoints(imageFileNames);
%
% squareSize = 22.225; % millimeters
% worldPoints = generateCheckerboardPoints(boardSize,squareSize);
%
% I = imread('Image1.jpg');
% imageSize = [size(I,1) size(I,2)];
% params = estimateFisheyeParameters(imagePoints,worldPoints,imageSize);
% save();
%% Compute Mapping from Image to Unit Sphere
% Parameters from Matlab Toolbox (code is above)
% load('blackfly_matlab_results.mat');
load('calibration_blackfly_matlab.mat');
nrows = params.Intrinsics.ImageSize(1);
ncols = params.Intrinsics.ImageSize(2);
a0 = params.Intrinsics.MappingCoefficients(1);
a2 = params.Intrinsics.MappingCoefficients(2);
a3 = params.Intrinsics.MappingCoefficients(3);
a4 = params.Intrinsics.MappingCoefficients(4);
xc = params.Intrinsics.DistortionCenter(1);
yc = params.Intrinsics.DistortionCenter(2);
c = params.Intrinsics.StretchMatrix(1,1);
d = params.Intrinsics.StretchMatrix(1,2);
e = params.Intrinsics.StretchMatrix(2,1);
% Create a grid containing the coordinates of every pixel in the image
[nv, nu] = meshgrid(1:nrows, 1:ncols);
n = [nu(:)';nv(:)'];
% Map all pixels to the unit sphere
num_points = size(n,2);
T = [xc;yc]*ones(1,num_points);
A = [c,d;e,1];
m = inv(A)*(n-T);
p = [a4, a3, a2, 0, a0];
M = [m(1,:) ; m(2,:) ; polyval(p,sqrt(m(1,:).^2+m(2,:).^2)) ];
M = normc(M);
% Plot normalized points
x=M(1,1:1:end);
y=M(2,1:1:end);
z=M(3,1:1:end);
% scatter3(x,y,z);
% axis equal
%% Compute Look-Up Table for Implementation
% fileID = fopen('blackfly_sphere_lut.txt','w');
% for i=1:size(M,2)
% fprintf(fileID, strcat([' '],[num2str(x(i))]) );
% fprintf(fileID, strcat([num2str(y(i))],[' ']));
% fprintf(fileID, strcat([num2str(z(i))],[' ']));
% end
% csvwrite('blackfly_sphere_lut.csv',M');
%% Project Image on Sphere
% Load and display image
figure;
I = im2double(imread('Image1.jpg'));
imshow(I);
% Permute and reshape image to match the ordering of the generated mesh
J = permute(I,[2,1,3]);
J = reshape(J,[],3);
% Display unit sphere with colored points
figure;
pcshow([x(:),y(:),z(:)],J);
xlabel('x');
ylabel('y');
zlabel('z');
%% Reproject to Perspective Image (using Logitech c920 parameters)
%load intrinsic parameters
% load('logitech_c920_calibration_640x480.mat');
load('calibration_logitech_c920_640x480.mat');
K = cameraParams.IntrinsicMatrix'; %transpose due to matlab convention
%choose orientation for perspective camera
thetax = -75*pi/180;
thetay = 0*pi/180;
thetaz = 90*pi/180;
Rx = [1,0,0;0,cos(thetax),-sin(thetax);0,sin(thetax),cos(thetax)];
Ry = [cos(thetay),0,sin(thetay);0,1,0;-sin(thetay),0,cos(thetay)];
Rz = [cos(thetaz),-sin(thetaz),0;sin(thetaz),cos(thetaz),0;0,0,1];
tf = Ry*Rx*Rz*M;
P = K*tf;
%reproject points in image
img = zeros(480,640,3);
for i=1:length(M) %this loop should be vectorized
if(tf(3,i)>0) %ignore points behind camera
pixel = P(:,i);
w = pixel(3);
u = round(pixel(1)/w); %must normalize homogenous coordinates
v = round(pixel(2)/w);
if(u>0 && v>0 && u<=640 && v<=480)
img(v,u,1) = J(i,1);
img(v,u,2) = J(i,2);
img(v,u,3) = J(i,3);
end
end
end
figure;
imshow(img);