square_pose.js
3.51 KB
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
129
130
131
132
133
134
var POS = POS || {};
POS.SquareFiducial = function(modelSize, focalLength, width, height){
this.modelSize = modelSize || 70;
this.focalLength = focalLength || 600;
this.u0 = width/2.0;
this.v0 = height/2.0;
this.au = this.focalLength;
this.av = this.focalLength;
this.setModelSize = function (modelSize) {
this.modelSize = modelSize;
};
this.setFocalLength = function (focal) {
this.focalLength = focal;
this.au = focal;
this.av = focal;
};
this.setMatrix = function (mat) {
this.au = mat[0];
this.av = mat[4];
this.u0 = mat[2];
this.v0 = mat[5];
};
this.setImageSize = function (width, height) {
this.u0 = width/2;
this.v0 = height/2;
};
};
POS.SquareFiducial.prototype.pose = function(imagePoints){
var u0 = this.u0;
var v0 = this.v0;
var au = this.au;
var av = this.av;
var ua = imagePoints[0].x ;
var va = imagePoints[0].y ;
var ub = imagePoints[1].x ;
var vb = imagePoints[1].y ;
var uc = imagePoints[2].x ;
var vc = imagePoints[2].y ;
var ud = imagePoints[3].x ;
var vd = imagePoints[3].y ;
var detm = uc*vd - vc*ud + ud*vb - ub*vd + ub*vc - uc*vb;
var ZA = 1;
var ZB = (ua * (vc - vd) + va * (ud - uc) + (uc*vd - ud*vc))/detm ;
var ZC = (ua * (vb - vd) + va * (ud - ub) - (ud*vb - ub*vd))/detm ;
var ZD = (ua * (vb - vc) + va * (uc - ub) + (ub*vc - uc*vb))/detm ;
var r1 = ZC/ZA ;
var r2 = ZD/ZB ;
var tmp11 = (r1*(ua - u0)-(uc -u0))/au ;
var tmp12 = (r1*(va - v0)-(vc -v0))/av ;
var tmp21 = (r2*(ud - u0)-(ub -u0))/au ;
var tmp22 = (r2*(vd - v0)-(vb -v0))/av ;
var ZpA = -this.modelSize / Math.sqrt((r1-1)*(r1-1) + tmp11*tmp11 + tmp12*tmp12) ;
var ZpB = -this.modelSize / Math.sqrt((r2-1)*(r2-1) + tmp21*tmp21 + tmp22*tmp22) ;
var ZpC = r1 * ZpA;
var ZpD = r2 * ZpB;
var XA = ZpA/au *(ua-u0);
var XB = ZpB/au *(ub-u0);
var XC = ZpC/au *(uc-u0);
var XD = ZpD/au *(ud-u0);
var YA = ZpA/av *(va-v0);
var YB = ZpB/av *(vb-v0);
var YC = ZpC/av *(vc-v0);
var YD = ZpD/av *(vd-v0);
var position = [];
position[0] = -(XA + XB + XC + XD)/4.0;
position[1] = (YA + YB + YC + YD)/4.0;
position[2] = (ZpA + ZpB + ZpC + ZpD)/4.0;
var AC = [ - XC + XA, YC - YA, ZpC - ZpA ];
var DB = [ - XB + XD, YB - YD, ZpB - ZpD ];
var AB = [ XB - XA, YB - YA, ZpB - ZpA ];
var DA = [ XA - XD, YA - YD, ZpA - ZpD ];
var sV = this.sumVectors(AC,DB);
var dV = this.diffVectors(DB,AC);
var r1Star = this.scalVector(sV, 1.0/this.normVector(sV));
var r2Star = this.scalVector(dV, 1.0/this.normVector(dV));
var r3Star = this.crossProduct(r1Star,r2Star);
var rotation = [ [ r1Star[0], r2Star[0], r3Star[0] ],
[ r1Star[1], r2Star[1], r3Star[1] ],
[ r1Star[2], r2Star[2], r3Star[2] ]];
//console.log(rotation);
return new POS.SimplePose(position, rotation);
};
POS.SquareFiducial.prototype.sumVectors = function(a,b) {
return [ a[0]+b[0], a[1]+b[1], a[2]+b[2] ];
};
POS.SquareFiducial.prototype.diffVectors = function (a,b) {
return [ a[0]-b[0], a[1]-b[1], a[2]-b[2] ];
};
POS.SquareFiducial.prototype.scalVector = function(a,s) {
return [ a[0]*s, a[1]*s, a[2]*s ];
};
POS.SquareFiducial.prototype.normVector = function (a) {
return Math.sqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
};
POS.SquareFiducial.prototype.crossProduct = function(a,b) {
return [ a[1]*b[2] - a[2]*b[1],
a[2]*b[0] - a[0]*b[2],
a[0]*b[1] - a[1]*b[0]
];
};
POS.SimplePose = function(pos, rot) {
this.position = pos;
this.rotation = rot;
};
export default POS;