-
Notifications
You must be signed in to change notification settings - Fork 8
/
Copy pathcalIntrinsicFrom3VanishingPts.m
65 lines (62 loc) · 7.42 KB
/
calIntrinsicFrom3VanishingPts.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
function K = calIntrinsicFrom3VanishingPts(vanishingPt1,vanishingPt2,vanishingPt3)
% Brief: 从三个消失点坐标求解相机内参K(代数求解方法)
% Details:
% 代数求解方法，已知三个消失点坐标(u1,u2,u3)',(v1,v2,v3)',(w1,w2,w3)',求取
% 内参K中的f,u0,v0. 根据正交约束，满足方程u'*(inv(K))'*inv(K)*v=0,三个消失点有
% 三对组合，对应三个独立方程，可求解焦距f，相机主点u0,v0.
% 理论详见[Geometry of a single camera](http://saurabhg.web.illinois.edu/teaching/ece549/sp2021/slides/lec14_calibration.pdf)
% 第50张幻灯片叙述.
% 条件：由三个消失点组成的三角形必定为锐角三角形
%
% syms f u0 v0 v1 v2 u1 u2 w1 w2 real
% assume(f>0)
% K = [f,0,u0;0,f,v0;0,0,1];
% v = [v1,v2,1]';
% u = [u1,u2,1]';
% w = [w1,w2,1]';
%
% express1 = simplify(v'*(inv(K))'*inv(K)*u);
% express2 = subs(express1,u,w);
% express3 = subs(express1,v,w);
% [f_,u0_,v0_,params,conditions] = solve([express1,express2,express3],[f,u0,v0],...
% ReturnConditions=true,...
% IgnoreAnalyticConstraints=true)
%
% Syntax:
% K = calIntrinsicFrom3VanishingPts(vanishingPt1,vanshingPt2,vanshingPt3)
%
% Inputs:
% vanishingPt1 - [1,3] size,[double] type,最后一列为1，齐次性
% vanishingPt2 - [1,3] size,[double] type,最后一列为1，齐次性
% vanishingPt3 - [1,3] size,[double] type,最后一列为1，齐次性
%
% Outputs:
% K - [3,3] size,[double] type,通用格式,[f,0,u0;0,f,v0;0,0,1]
%
% Example:
% None
%
% See also: None
% Author: cuixingxing
% Email: [email protected]
% Created: 22-Aug-2022 09:44:23
% Version history revision notes:
% None
% Implementation In Matlab R2022a
%
arguments
vanishingPt1 (1,3) double
vanishingPt2 (1,3) double
vanishingPt3 (1,3) double
end
u1 = vanishingPt1(1,1);u2 = vanishingPt1(1,2);u3 = vanishingPt1(1,3);
v1 = vanishingPt2(1,1);v2 = vanishingPt2(1,2);v3 = vanishingPt2(1,3);
w1 = vanishingPt3(1,1);w2 = vanishingPt3(1,2);w3 = vanishingPt3(1,3);
% 直接获取结果
f = (abs((u1*v2*w3 - u1*v3*w2 - u2*v1*w3 + u2*v3*w1 + u3*v1*w2 - u3*v2*w1))*abs(u3)*abs(v3)*abs(w3)*sqrt(- u1^4*v1^2*v3^2*w3^4 + 2*u1^4*v1*v3^3*w1*w3^3 - u1^4*v3^4*w1^2*w3^2 - 2*u1^3*u2*v1*v2*v3^2*w3^4 + 2*u1^3*u2*v1*v3^3*w2*w3^3 + 2*u1^3*u2*v2*v3^3*w1*w3^3 - 2*u1^3*u2*v3^4*w1*w2*w3^2 + 2*u1^3*u3*v1^3*v3*w3^4 - 2*u1^3*u3*v1^2*v3^2*w1*w3^3 + u1^3*u3*v1*v2^2*v3*w3^4 - 2*u1^3*u3*v1*v3^3*w1^2*w3^2 - u1^3*u3*v1*v3^3*w2^2*w3^2 - u1^3*u3*v2^2*v3^2*w1*w3^3 + 2*u1^3*u3*v3^4*w1^3*w3 + u1^3*u3*v3^4*w1*w2^2*w3 - u1^2*u2^2*v1^2*v3^2*w3^4 + 2*u1^2*u2^2*v1*v3^3*w1*w3^3 - u1^2*u2^2*v2^2*v3^2*w3^4 + 2*u1^2*u2^2*v2*v3^3*w2*w3^3 - u1^2*u2^2*v3^4*w1^2*w3^2 - u1^2*u2^2*v3^4*w2^2*w3^2 + 4*u1^2*u2*u3*v1^2*v2*v3*w3^4 - 2*u1^2*u2*u3*v1^2*v3^2*w2*w3^3 - 2*u1^2*u2*u3*v1*v2*v3^2*w1*w3^3 - 2*u1^2*u2*u3*v1*v3^3*w1*w2*w3^2 + u1^2*u2*u3*v2^3*v3*w3^4 - u1^2*u2*u3*v2^2*v3^2*w2*w3^3 - 2*u1^2*u2*u3*v2*v3^3*w1^2*w3^2 - u1^2*u2*u3*v2*v3^3*w2^2*w3^2 + 4*u1^2*u2*u3*v3^4*w1^2*w2*w3 + u1^2*u2*u3*v3^4*w2^3*w3 - u1^2*u3^2*v1^4*w3^4 - 2*u1^2*u3^2*v1^3*v3*w1*w3^3 - u1^2*u3^2*v1^2*v2^2*w3^4 - 2*u1^2*u3^2*v1^2*v2*v3*w2*w3^3 + 6*u1^2*u3^2*v1^2*v3^2*w1^2*w3^2 + 2*u1^2*u3^2*v1^2*v3^2*w2^2*w3^2 - u1^2*u3^2*v1*v2^2*v3*w1*w3^3 + 4*u1^2*u3^2*v1*v2*v3^2*w1*w2*w3^2 - 2*u1^2*u3^2*v1*v3^3*w1^3*w3 - u1^2*u3^2*v1*v3^3*w1*w2^2*w3 - u1^2*u3^2*v2^3*v3*w2*w3^3 + 2*u1^2*u3^2*v2^2*v3^2*w1^2*w3^2 + 2*u1^2*u3^2*v2^2*v3^2*w2^2*w3^2 - 2*u1^2*u3^2*v2*v3^3*w1^2*w2*w3 - u1^2*u3^2*v2*v3^3*w2^3*w3 - u1^2*u3^2*v3^4*w1^4 - u1^2*u3^2*v3^4*w1^2*w2^2 - 2*u1*u2^3*v1*v2*v3^2*w3^4 + 2*u1*u2^3*v1*v3^3*w2*w3^3 + 2*u1*u2^3*v2*v3^3*w1*w3^3 - 2*u1*u2^3*v3^4*w1*w2*w3^2 + u1*u2^2*u3*v1^3*v3*w3^4 - u1*u2^2*u3*v1^2*v3^2*w1*w3^3 + 4*u1*u2^2*u3*v1*v2^2*v3*w3^4 - 2*u1*u2^2*u3*v1*v2*v3^2*w2*w3^3 - u1*u2^2*u3*v1*v3^3*w1^2*w3^2 - 2*u1*u2^2*u3*v1*v3^3*w2^2*w3^2 - 2*u1*u2^2*u3*v2^2*v3^2*w1*w3^3 - 2*u1*u2^2*u3*v2*v3^3*w1*w2*w3^2 + u1*u2^2*u3*v3^4*w1^3*w3 + 4*u1*u2^2*u3*v3^4*w1*w2^2*w3 - 2*u1*u2*u3^2*v1^3*v2*w3^4 - 2*u1*u2*u3^2*v1^2*v2*v3*w1*w3^3 + 4*u1*u2*u3^2*v1^2*v3^2*w1*w2*w3^2 - 2*u1*u2*u3^2*v1*v2^3*w3^4 - 2*u1*u2*u3^2*v1*v2^2*v3*w2*w3^3 + 4*u1*u2*u3^2*v1*v2*v3^2*w1^2*w3^2 + 4*u1*u2*u3^2*v1*v2*v3^2*w2^2*w3^2 - 2*u1*u2*u3^2*v1*v3^3*w1^2*w2*w3 + 4*u1*u2*u3^2*v2^2*v3^2*w1*w2*w3^2 - 2*u1*u2*u3^2*v2*v3^3*w1*w2^2*w3 - 2*u1*u2*u3^2*v3^4*w1^3*w2 - 2*u1*u2*u3^2*v3^4*w1*w2^3 + 2*u1*u3^3*v1^4*w1*w3^3 + 2*u1*u3^3*v1^3*v2*w2*w3^3 - 2*u1*u3^3*v1^3*v3*w1^2*w3^2 - u1*u3^3*v1^3*v3*w2^2*w3^2 + 2*u1*u3^3*v1^2*v2^2*w1*w3^3 - 2*u1*u3^3*v1^2*v2*v3*w1*w2*w3^2 - 2*u1*u3^3*v1^2*v3^2*w1^3*w3 - u1*u3^3*v1^2*v3^2*w1*w2^2*w3 + 2*u1*u3^3*v1*v2^3*w2*w3^3 - u1*u3^3*v1*v2^2*v3*w1^2*w3^2 - 2*u1*u3^3*v1*v2^2*v3*w2^2*w3^2 - 2*u1*u3^3*v1*v2*v3^2*w1^2*w2*w3 + 2*u1*u3^3*v1*v3^3*w1^4 + 2*u1*u3^3*v1*v3^3*w1^2*w2^2 - u1*u3^3*v2^2*v3^2*w1^3*w3 - 2*u1*u3^3*v2^2*v3^2*w1*w2^2*w3 + 2*u1*u3^3*v2*v3^3*w1^3*w2 + 2*u1*u3^3*v2*v3^3*w1*w2^3 - u2^4*v2^2*v3^2*w3^4 + 2*u2^4*v2*v3^3*w2*w3^3 - u2^4*v3^4*w2^2*w3^2 + u2^3*u3*v1^2*v2*v3*w3^4 - u2^3*u3*v1^2*v3^2*w2*w3^3 + 2*u2^3*u3*v2^3*v3*w3^4 - 2*u2^3*u3*v2^2*v3^2*w2*w3^3 - u2^3*u3*v2*v3^3*w1^2*w3^2 - 2*u2^3*u3*v2*v3^3*w2^2*w3^2 + u2^3*u3*v3^4*w1^2*w2*w3 + 2*u2^3*u3*v3^4*w2^3*w3 - u2^2*u3^2*v1^3*v3*w1*w3^3 - u2^2*u3^2*v1^2*v2^2*w3^4 - u2^2*u3^2*v1^2*v2*v3*w2*w3^3 + 2*u2^2*u3^2*v1^2*v3^2*w1^2*w3^2 + 2*u2^2*u3^2*v1^2*v3^2*w2^2*w3^2 - 2*u2^2*u3^2*v1*v2^2*v3*w1*w3^3 + 4*u2^2*u3^2*v1*v2*v3^2*w1*w2*w3^2 - u2^2*u3^2*v1*v3^3*w1^3*w3 - 2*u2^2*u3^2*v1*v3^3*w1*w2^2*w3 - u2^2*u3^2*v2^4*w3^4 - 2*u2^2*u3^2*v2^3*v3*w2*w3^3 + 2*u2^2*u3^2*v2^2*v3^2*w1^2*w3^2 + 6*u2^2*u3^2*v2^2*v3^2*w2^2*w3^2 - u2^2*u3^2*v2*v3^3*w1^2*w2*w3 - 2*u2^2*u3^2*v2*v3^3*w2^3*w3 - u2^2*u3^2*v3^4*w1^2*w2^2 - u2^2*u3^2*v3^4*w2^4 + 2*u2*u3^3*v1^3*v2*w1*w3^3 + 2*u2*u3^3*v1^2*v2^2*w2*w3^3 - 2*u2*u3^3*v1^2*v2*v3*w1^2*w3^2 - u2*u3^3*v1^2*v2*v3*w2^2*w3^2 - 2*u2*u3^3*v1^2*v3^2*w1^2*w2*w3 - u2*u3^3*v1^2*v3^2*w2^3*w3 + 2*u2*u3^3*v1*v2^3*w1*w3^3 - 2*u2*u3^3*v1*v2^2*v3*w1*w2*w3^2 - 2*u2*u3^3*v1*v2*v3^2*w1*w2^2*w3 + 2*u2*u3^3*v1*v3^3*w1^3*w2 + 2*u2*u3^3*v1*v3^3*w1*w2^3 + 2*u2*u3^3*v2^4*w2*w3^3 - u2*u3^3*v2^3*v3*w1^2*w3^2 - 2*u2*u3^3*v2^3*v3*w2^2*w3^2 - u2*u3^3*v2^2*v3^2*w1^2*w2*w3 - 2*u2*u3^3*v2^2*v3^2*w2^3*w3 + 2*u2*u3^3*v2*v3^3*w1^2*w2^2 + 2*u2*u3^3*v2*v3^3*w2^4 - u3^4*v1^4*w1^2*w3^2 - 2*u3^4*v1^3*v2*w1*w2*w3^2 + 2*u3^4*v1^3*v3*w1^3*w3 + u3^4*v1^3*v3*w1*w2^2*w3 - u3^4*v1^2*v2^2*w1^2*w3^2 - u3^4*v1^2*v2^2*w2^2*w3^2 + 4*u3^4*v1^2*v2*v3*w1^2*w2*w3 + u3^4*v1^2*v2*v3*w2^3*w3 - u3^4*v1^2*v3^2*w1^4 - u3^4*v1^2*v3^2*w1^2*w2^2 - 2*u3^4*v1*v2^3*w1*w2*w3^2 + u3^4*v1*v2^2*v3*w1^3*w3 + 4*u3^4*v1*v2^2*v3*w1*w2^2*w3 - 2*u3^4*v1*v2*v3^2*w1^3*w2 - 2*u3^4*v1*v2*v3^2*w1*w2^3 - u3^4*v2^4*w2^2*w3^2 + u3^4*v2^3*v3*w1^2*w2*w3 + 2*u3^4*v2^3*v3*w2^3*w3 - u3^4*v2^2*v3^2*w1^2*w2^2 - u3^4*v2^2*v3^2*w2^4))/(u3^2*v3^2*w3^2*(u1*v2*w3 - u1*v3*w2 - u2*v1*w3 + u2*v3*w1 + u3*v1*w2 - u3*v2*w1)^2);
u0 = (- u2^2*v2*v3*w3^2 + u2^2*v3^2*w2*w3 + u2*u3*v2^2*w3^2 - u2*u3*v3^2*w2^2 + u1*w1*u2*v3^2*w3 - u1*v1*u2*v3*w3^2 - u3^2*v2^2*w2*w3 + u3^2*v2*v3*w2^2 - v1*w1*u3^2*v2*w3 + v1*w1*u3^2*v3*w2 + u1*v1*u3*v2*w3^2 - u1*w1*u3*v3^2*w2)/(u3*v3*w3*(u1*v2*w3 - u1*v3*w2 - u2*v1*w3 + u2*v3*w1 + u3*v1*w2 - u3*v2*w1));
v0 = -(- u1^2*v1*v3*w3^2 + u1^2*v3^2*w1*w3 + u1*u3*v1^2*w3^2 - u1*u3*v3^2*w1^2 + u2*w2*u1*v3^2*w3 - u2*v2*u1*v3*w3^2 - u3^2*v1^2*w1*w3 + u3^2*v1*v3*w1^2 - v2*w2*u3^2*v1*w3 + v2*w2*u3^2*v3*w1 + u2*v2*u3*v1*w3^2 - u2*w2*u3*v3^2*w1)/(u3*v3*w3*(u1*v2*w3 - u1*v3*w2 - u2*v1*w3 + u2*v3*w1 + u3*v1*w2 - u3*v2*w1));
K = [f,0,u0;
0,f,v0;
0,0,1];
end