Skip to content

Commit 9e83b9a

Browse files
bredfeldtbredfeldt
committed
Merge branch 'master' of https://github.com/zeroeth/blimp_blobs 
2 parents 7179d96 + 7c30f95 commit 9e83b9a

File tree

3 files changed

+379
-0
lines changed

3 files changed

+379
-0
lines changed

‎matlab/fit3Dlocation.m

Lines changed: 159 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,159 @@
1+
%fit3Dlocation.m
2+
% modified from calib.m by
3+
% Jeremy Bredfeldt - Morgridge Institutes for Research
4+
% Sept 2013
5+
6+
%Trying to derive the camera calibration matrices from first principles
7+
%If we know the pose of the camera, then what are the camera matrices?
8+
%With this, we can ROM check the calibrations generated by the calibration routines
9+
10+
%Then can we use these matrices to triangulate points properly in 3D, based on 2 camera poses?
11+
12+
%Place the origin in the south, west, floor level corner of the atrium
13+
%Positive x = points east
14+
%Positive y = points north
15+
%Positive z = up
16+
%Rotations: Cameras start pointing in positive z direction, and rotated from there
17+
% Bottom of camera is pointing in neg y direction
18+
% Use right hand rule for direction of rotation
19+
20+
function err = fit3Dlocation(ccp,rxyz,c1xy,c2xy)
21+
%Atrium dimensions
22+
%39624 mm in x direction
23+
ax = 42610;
24+
%10947 mm in y direction
25+
ay = 8660;
26+
%15240 mm in z direction
27+
az = 15240;
28+
29+
%Camera sensor and lens
30+
%Focal length = 4.2 mm
31+
fx = 4.2;
32+
fy = fx;
33+
%Sensor = 4.54 mm X 3.42 mm
34+
sx = 4.54/2;
35+
sy = 3.42/2;
36+
%Image size = 1280 X 720 pixels
37+
ix = 1280;
38+
iy = 720;
39+
40+
%These are in the world's reference frame, to actually get camera matrices, these must be inverted.
41+
%--Translation--
42+
%Camera 1 (west side)
43+
% Cx1 = 0;
44+
% Cy1 = ay/2; %centered in y dim
45+
% Cz1 = az;
46+
Cx1 = ccp(1);
47+
Cy1 = ccp(2); %centered in y dim
48+
Cz1 = ccp(3);
49+
50+
C1 = [Cx1; Cy1; Cz1];
51+
52+
%Camera 2 (east side)
53+
% Cx2 = ax;
54+
% Cy2 = ay/2;
55+
% Cz2 = az;
56+
Cx2 = ccp(4);
57+
Cy2 = ccp(5);
58+
Cz2 = ccp(6);
59+
60+
C2 = [Cx2; Cy2; Cz2];
61+
62+
%--Rotation--
63+
%Camera 1
64+
% thx1 = 0;
65+
% thy1 = pi/1.5; %point 45 deg down
66+
% thz1 = 0;
67+
thx1 = ccp(7);
68+
thy1 = ccp(8);
69+
thz1 = ccp(9);
70+
Rcx1 = [1 0 0; 0 cos(thx1) -sin(thx1); 0 sin(thx1) cos(thx1)];
71+
Rcy1 = [cos(thy1) 0 sin(thy1); 0 1 0; -sin(thy1) 0 cos(thy1)];
72+
Rcz1 = [cos(thz1) -sin(thz1) 0; sin(thz1) cos(thz1) 0; 0 0 1];
73+
Rc1 = Rcx1*Rcy1*Rcz1;
74+
75+
%Camera 2
76+
% thx2 = 0;
77+
% thy2 = -pi/1.5; %point 45 deg down
78+
% thz2 = pi;
79+
thx2 = ccp(10);
80+
thy2 = ccp(11);
81+
thz2 = ccp(12);
82+
83+
Rcx2 = [1 0 0; 0 cos(thx2) -sin(thx2); 0 sin(thx2) cos(thx2)];
84+
Rcy2 = [cos(thy2) 0 sin(thy2); 0 1 0; -sin(thy2) 0 cos(thy2)];
85+
Rcz2 = [cos(thz2) -sin(thz2) 0; sin(thz2) cos(thz2) 0; 0 0 1];
86+
Rc2 = Rcx2*Rcy2*Rcz2;
87+
88+
%Get the actual camera matrices (inverse of the previously written matrices):
89+
R1 = Rc1';
90+
R2 = Rc2';
91+
t1 = -R1*C1; %origin in cam 1 ref frame
92+
t2 = -R2*C2; %origin in cam 2 ref frame
93+
94+
%--Extrinsic--
95+
E1 = [R1 t1];
96+
E2 = [R2 t2];
97+
98+
%--Intrinsic--
99+
%Assume no skew or translation, and fx and fy are same
100+
%Assume both cameras are the same
101+
K = [fx 0 0; 0 fy 0; 0 0 1];
102+
103+
%--Final Matrices--
104+
P1 = K*E1;
105+
P2 = K*E2;
106+
107+
108+
%now triangulate to find the 3d location again, using the camera matrices
109+
[nums,numc] = size(c1xy);
110+
111+
for i = 1:nums
112+
113+
exyz(i,1:3) = triangulateJYB(P1,c1xy(i,:),P2,c2xy(i,:));
114+
115+
end
116+
117+
118+
% disp(sprintf('real x= %4.2f,y = %4.2f, z = %4.2f \n',rxyz(end,1),rxyz(end,2),rxyz(end,3)));
119+
% disp(sprintf('Estd x= %4.2f,y = %4.2f, z = %4.2f \n',exyz(end,1),exyz(end,2),exyz(end,3)));
120+
% clr1= 'rgbmcky';
121+
% mark1 ='spbthos';
122+
% clr2= 'ykcmbgr';
123+
% mark2 ='xhtbps';
124+
125+
% figure(1);clf
126+
% for i = 1:nums
127+
% plot3(exyz(i,1),exyz(i,2),exyz(i,3),clr1(mod(i,7)+1),mark1(mod(i,7)+1)); hold on;
128+
% plot3(rxyz(i,1),rxyz(i,2),rxyz(i,3),clr2(mod(i,7)+1),mark1(mod(i,7)+1));hold on
129+
% end
130+
% clr1 = 'rgbmckyk';
131+
% clr2 = 'ykcmbgrk';
132+
% mark1 = 'sdvphx*o';
133+
% figure(1);clf; axis([0 45000 10000 20000]);
134+
% for i = 1%:nums
135+
% plot3(exyz(i,1),exyz(i,2),exyz(i,3),[clr1(i),mark1(i)]); hold on;
136+
% plot3(rxyz(i,1),rxyz(i,2),rxyz(i,3),[clr2(i),mark1(i)]);hold on
137+
% xlim([0 45000]);
138+
% ylim([0 10000]);
139+
% zlim([0 20000]);
140+
% %axis([0 45000 10000 20000]);
141+
% pause(0.5)
142+
% drawnow
143+
%
144+
% end
145+
146+
pf = 1;
147+
if ccp(4) > 60000
148+
pf = pf*10^3;
149+
end
150+
151+
err = pf*mean(sqrt(sum((exyz-rxyz).^2,2)));
152+
153+
154+
155+
disp(sprintf('fitting error is %d',err));
156+
157+
end
158+
159+

‎matlab/focp.m

Lines changed: 100 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,100 @@
1+
% part of blimp 3D tracking
2+
% focp.m (find optimized camera parameters)
3+
clear;clc;home
4+
5+
pd1 = 'C:\Users\yuming\Documents\GitHub\blimp_blobs\';
6+
filename = [pd1,'tracking_optimization.xlsx'];
7+
num = xlsread(filename,'A2:H8');
8+
chc = [1 4:5];
9+
rxyz = num(chc,6:8);
10+
c1xy = num(chc,1:2);
11+
c2xy = num(chc,3:4);
12+
13+
%%
14+
% Cx1 = ccp(1);
15+
% Cy1 = ccp(2); %centered in y dim
16+
% Cz1 = ccp(3);
17+
% Cx2 = ccp(4);
18+
% Cy2 = ccp(5);
19+
% Cz2 = ccp(6);
20+
% thx1 = ccp(7);
21+
% thy1 = ccp(8);
22+
% thz1 = ccp(9);
23+
% thx2 = ccp(10);
24+
% thy2 = ccp(11);
25+
% thz2 = ccp(12);
26+
27+
ax = 42610;
28+
%10947 mm in y direction
29+
ay = 8660;
30+
%15240 mm in z direction
31+
az = 15240;
32+
33+
%Camera sensor and lens
34+
%Focal length = 4.2 mm
35+
fx = 4.2;
36+
fy = fx;
37+
%Sensor = 4.54 mm X 3.42 mm
38+
sx = 4.54/2;
39+
sy = 3.42/2;
40+
%Image size = 1280 X 720 pixels
41+
ix = 1280;
42+
iy = 720;
43+
44+
%These are in the world's reference frame, to actually get camera matrices, these must be inverted.
45+
%--Translation--
46+
%Camera 1 (west side)
47+
Cx1 = 0;
48+
Cy1 = ay/2; %centered in y dim
49+
Cz1 = az;
50+
C1 = [Cx1; Cy1; Cz1];
51+
52+
%Camera 2 (east side)
53+
Cx2 = ax;
54+
Cy2 = ay/2;
55+
Cz2 = az;
56+
C2 = [Cx2; Cy2; Cz2];
57+
58+
%--Rotation--
59+
%Camera 1
60+
thx1 = 0;
61+
thy1 = pi/1.5; %point 45 deg down
62+
thz1 = 0;
63+
64+
%Camera 2
65+
thx2 = 0;
66+
thy2 = -pi/1.5; %point 45 deg down
67+
thz2 = pi;
68+
ccp = [0 ay/2 az ax ay/2 az 0 pi/2 0 0 pi/2 pi];
69+
ccpc(1,:) = ccp;
70+
OPTIONS = optimset('TolFun',1e-3,'MaxFunEvals',1e5,'MaxIter',1e5);
71+
ccp = fminsearch('fit3Dlocation',ccp,OPTIONS,rxyz,c1xy,c2xy);
72+
ccpc(2,:) = ccp;
73+
figure(2);clf; plot(ccpc(1,:),'r*'); hold on;
74+
plot(ccpc(2,:),'bo');
75+
76+
77+
exyz = get3Dlocation(ccp,c1xy,c2xy);
78+
[exyz rxyz]
79+
clr1 = 'rgbmckyk';
80+
clr2 = 'ykcmbgrk';
81+
mark1 = 'sdvphx*o';
82+
figure(1);clf; axis([0 45000 10000 20000]);
83+
[nums,numc] = size(c1xy);
84+
for i = 1:nums
85+
plot3(exyz(i,1),exyz(i,2),exyz(i,3),[clr1(i),mark1(i)]); hold on;
86+
plot3(rxyz(i,1),rxyz(i,2),rxyz(i,3),[clr2(i),mark1(i)]);hold on
87+
xlim([0 20000]);
88+
ylim([0 10000]);
89+
zlim([0 10000]);
90+
%axis([0 45000 10000 20000]);
91+
pause(0.5)
92+
drawnow
93+
94+
end
95+
96+
err = mean(sqrt(sum((exyz-rxyz).^2,2)));
97+
98+
disp(sprintf('fitting error is %d',err));
99+
100+

‎matlab/get3Dlocation.m

Lines changed: 120 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,120 @@
1+
%fit3Dlocation.m
2+
% modified from calib.m by
3+
% Jeremy Bredfeldt - Morgridge Institutes for Research
4+
% Sept 2013
5+
6+
%Trying to derive the camera calibration matrices from first principles
7+
%If we know the pose of the camera, then what are the camera matrices?
8+
%With this, we can ROM check the calibrations generated by the calibration routines
9+
10+
%Then can we use these matrices to triangulate points properly in 3D, based on 2 camera poses?
11+
12+
%Place the origin in the south, west, floor level corner of the atrium
13+
%Positive x = points east
14+
%Positive y = points north
15+
%Positive z = up
16+
%Rotations: Cameras start pointing in positive z direction, and rotated from there
17+
% Bottom of camera is pointing in neg y direction
18+
% Use right hand rule for direction of rotation
19+
20+
function exyz = get3Dlocation(ccp,c1xy,c2xy)
21+
%Atrium dimensions
22+
%39624 mm in x direction
23+
ax = 42610;
24+
%10947 mm in y direction
25+
ay = 8660;
26+
%15240 mm in z direction
27+
az = 15240;
28+
29+
%Camera sensor and lens
30+
%Focal length = 4.2 mm
31+
fx = 4.2;
32+
fy = fx;
33+
%Sensor = 4.54 mm X 3.42 mm
34+
sx = 4.54/2;
35+
sy = 3.42/2;
36+
%Image size = 1280 X 720 pixels
37+
ix = 1280;
38+
iy = 720;
39+
40+
%These are in the world's reference frame, to actually get camera matrices, these must be inverted.
41+
%--Translation--
42+
%Camera 1 (west side)
43+
% Cx1 = 0;
44+
% Cy1 = ay/2; %centered in y dim
45+
% Cz1 = az;
46+
Cx1 = ccp(1);
47+
Cy1 = ccp(2); %centered in y dim
48+
Cz1 = ccp(3);
49+
50+
C1 = [Cx1; Cy1; Cz1];
51+
52+
%Camera 2 (east side)
53+
% Cx2 = ax;
54+
% Cy2 = ay/2;
55+
% Cz2 = az;
56+
Cx2 = ccp(4);
57+
Cy2 = ccp(5);
58+
Cz2 = ccp(6);
59+
60+
C2 = [Cx2; Cy2; Cz2];
61+
62+
%--Rotation--
63+
%Camera 1
64+
% thx1 = 0;
65+
% thy1 = pi/1.5; %point 45 deg down
66+
% thz1 = 0;
67+
thx1 = ccp(7);
68+
thy1 = ccp(8);
69+
thz1 = ccp(9);
70+
Rcx1 = [1 0 0; 0 cos(thx1) -sin(thx1); 0 sin(thx1) cos(thx1)];
71+
Rcy1 = [cos(thy1) 0 sin(thy1); 0 1 0; -sin(thy1) 0 cos(thy1)];
72+
Rcz1 = [cos(thz1) -sin(thz1) 0; sin(thz1) cos(thz1) 0; 0 0 1];
73+
Rc1 = Rcx1*Rcy1*Rcz1;
74+
75+
%Camera 2
76+
% thx2 = 0;
77+
% thy2 = -pi/1.5; %point 45 deg down
78+
% thz2 = pi;
79+
thx2 = ccp(10);
80+
thy2 = ccp(11);
81+
thz2 = ccp(12);
82+
83+
Rcx2 = [1 0 0; 0 cos(thx2) -sin(thx2); 0 sin(thx2) cos(thx2)];
84+
Rcy2 = [cos(thy2) 0 sin(thy2); 0 1 0; -sin(thy2) 0 cos(thy2)];
85+
Rcz2 = [cos(thz2) -sin(thz2) 0; sin(thz2) cos(thz2) 0; 0 0 1];
86+
Rc2 = Rcx2*Rcy2*Rcz2;
87+
88+
%Get the actual camera matrices (inverse of the previously written matrices):
89+
R1 = Rc1';
90+
R2 = Rc2';
91+
t1 = -R1*C1; %origin in cam 1 ref frame
92+
t2 = -R2*C2; %origin in cam 2 ref frame
93+
94+
%--Extrinsic--
95+
E1 = [R1 t1];
96+
E2 = [R2 t2];
97+
98+
%--Intrinsic--
99+
%Assume no skew or translation, and fx and fy are same
100+
%Assume both cameras are the same
101+
K = [fx 0 0; 0 fy 0; 0 0 1];
102+
103+
%--Final Matrices--
104+
P1 = K*E1;
105+
P2 = K*E2;
106+
107+
108+
%now triangulate to find the 3d location again, using the camera matrices
109+
[nums,numc] = size(c1xy);
110+
111+
for i = 1:nums
112+
113+
exyz(i,1:3) = triangulateJYB(P1,c1xy(i,:),P2,c2xy(i,:));
114+
115+
end
116+
117+
118+
end
119+
120+

0 commit comments

Comments
 (0)