-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathcalibrationPP.maple~
125 lines (96 loc) · 3.18 KB
/
calibrationPP.maple~
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
with(LinearAlgebra):
with(CodeGeneration):
#P is the 3x4 projection matrix. If you want to map from plane to plane
#use a 3x3 matrix.
PP:=array(1..3,1..3,[[p1,p2,p3],[p4,p5,p6],[p7,p8,p9]]);
#Input the values of the sample points into the array [X,Y,Z,u,v]
#X,Y,Z measured in cm and
#u,v measured in pixle index
Sp := Array([
[35,-25,272,454],
[46,15,340,275],
[62,30,414,212],
[34,34,282,208],
[57,0,418,333],
[62,-24,450,446]
]);
#Sp := Array([
# [2.2,9,97,432],
# [17.5,3.5,214,90],
# [11.5,-8.5,493,214],
# [9.2,-0.9,313,273],
# [14.6,11.5,33,155],
# #[11.8,7,138,219],
# [18,-2.9,354,67],
# [5,-6.2,438,367]
# ]);
makeMatrix := proc(arr)
local X, Y, u, v, Xworld, ximg, projPt, eqvec, AAi, AAj;
#with(LinearAlgebra):
# Xworld is the world coordinates, that is where you told the arm to place
# the token
X := arr[1];
Y := arr[2];
u := arr[3];
v := arr[4];
Xworld:=Vector(<X,Y,1>);
#ximg is the image coordinate, where you detected the token in the image.
ximg:=Vector(<u,v,1>);
projPt := Multiply(convert(PP,Matrix),Xworld);
eqvec := CrossProduct(ximg,projPt);
# AAi is the 3x12 matrix of coefficients. I use a simple trick to get the
# coefficients: I take derivatives with respect to the unknowns.
AAi:=array(1..3,1..9,[
['diff(eqvec[1],PP[1,i])'$i=1..3,
'diff(eqvec[1],PP[2,i])'$i=1..3,
'diff(eqvec[1],PP[3,i])'$i=1..3],
['diff(eqvec[2],PP[1,i])'$i=1..3,
'diff(eqvec[2],PP[2,i])'$i=1..3,
'diff(eqvec[2],PP[3,i])'$i=1..3],
['diff(eqvec[3],PP[1,i])'$i=1..3,
'diff(eqvec[3],PP[2,i])'$i=1..3,
'diff(eqvec[3],PP[3,i])'$i=1..3]]);
AAj := convert(AAi,Matrix);
end proc;
# ============ loop through the input values and print them out ===============
printOut := proc(arr, s)
description "this proc loops through the generated 3x12 matrices and stacks them together";
local stacked;
for i from 1 to s do
#first assignment to stacked
if i = 1 then
stacked := makeMatrix(arr[i]);
else
#subsequent assignments to stacked
stacked := Matrix ([[stacked],[makeMatrix(arr[i])]]);
end if
end do;
end proc;
# ================== assign the values of PP =========================
assignPP := proc (Vmat)
local V;
V := Row(Vmat,Dimension(Vmat)[1]);
#V := Column(Vmat,Dimension(Vmat)[2]);
<<V[1]|V[2]|V[3]>,<V[4]|V[5]|V[6]>,<V[7]|V[8]|V[9]>>;
end proc;
# ================= generate the PP matrix and invert it to go from u,v to X,Y,Z ===================
size := Dimension(convert(Sp,Matrix))[1];
AAj := printOut(Sp,size);
S, Vt := SingularValues(AAj,output = ['S','Vt']):
S;
Column(Vt,9);
PPnew := assignPP(Vt);
# invert PPnew
PPinv := MatrixInverse(PPnew);
KK := <350, 389,1>;
JJ := Multiply(PPinv,KK);
#evalf(JJ[1]/JJ[4]);
LL := < JJ[1]/JJ[3],JJ[2]/JJ[3] >;
# ====================================
# The next statement produces the C code for it. You can edit the result
# a bit to fit in a typical MediaMath program (i.e. replace the ][ in the
# generated code with either , (comma) or +3*i, so that it places the
# results in the correct place in the matrix.
#C(AAi);
# After that you call SVD, find the smallest singular value and copy the
#corresponding singular vector into P