Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- //////////////////////////////////////////////////////////////////////////
- // //
- // This is a generated file. You can view the original //
- // source in your browser if your browser supports source maps. //
- // //
- // If you are using Chrome, open the Developer Tools and click the gear //
- // icon in its lower right corner. In the General Settings panel, turn //
- // on 'Enable source maps'. //
- // //
- // If you are using Firefox 23, go to `about:config` and set the //
- // `devtools.debugger.source-maps-enabled` preference to true. //
- // (The preference should be on by default in Firefox 24; versions //
- // older than 23 do not support source maps.) //
- // //
- //////////////////////////////////////////////////////////////////////////
- (function () {
- /* Imports */
- var Meteor = Package.meteor.Meteor;
- (function () {
- ////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // //
- // packages/js-aruco/aruco.js //
- // //
- ////////////////////////////////////////////////////////////////////////////////////////////////////////////
- //
- /* // 1
- Copyright (c) 2011 Juan Mellado // 2
- // 3
- Permission is hereby granted, free of charge, to any person obtaining a copy // 4
- of this software and associated documentation files (the "Software"), to deal // 5
- in the Software without restriction, including without limitation the rights // 6
- to use, copy, modify, merge, publish, distribute, sublicense, and/or sell // 7
- copies of the Software, and to permit persons to whom the Software is // 8
- furnished to do so, subject to the following conditions: // 9
- // 10
- The above copyright notice and this permission notice shall be included in // 11
- all copies or substantial portions of the Software. // 12
- // 13
- THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR // 14
- IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, // 15
- FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE // 16
- AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER // 17
- LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, // 18
- OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // 19
- THE SOFTWARE. // 20
- */ // 21
- // 22
- /* // 23
- References: // 24
- - "ArUco: a minimal library for Augmented Reality applications based on OpenCv" // 25
- http://www.uco.es/investiga/grupos/ava/node/26 // 26
- */ // 27
- // 28
- var AR = AR || {}; // 29
- // 30
- AR.Marker = function(id, corners){ // 31
- this.id = id; // 32
- this.corners = corners; // 33
- }; // 34
- // 35
- AR.Detector = function(){ // 36
- this.grey = new CV.Image(); // 37
- this.thres = new CV.Image(); // 38
- this.homography = new CV.Image(); // 39
- this.binary = []; // 40
- this.contours = []; // 41
- this.polys = []; // 42
- this.candidates = []; // 43
- }; // 44
- // 45
- AR.Detector.prototype.detect = function(image){ // 46
- CV.grayscale(image, this.grey); // 47
- CV.adaptiveThreshold(this.grey, this.thres, 2, 7); // 48
- // 49
- this.contours = CV.findContours(this.thres, this.binary); // 50
- // 51
- this.candidates = this.findCandidates(this.contours, image.width * 0.20, 0.05, 10); // 52
- this.candidates = this.clockwiseCorners(this.candidates); // 53
- this.candidates = this.notTooNear(this.candidates, 10); // 54
- // 55
- return this.findMarkers(this.grey, this.candidates, 49); // 56
- }; // 57
- // 58
- AR.Detector.prototype.findCandidates = function(contours, minSize, epsilon, minLength){ // 59
- var candidates = [], len = contours.length, contour, poly, i; // 60
- // 61
- this.polys = []; // 62
- // 63
- for (i = 0; i < len; ++ i){ // 64
- contour = contours[i]; // 65
- // 66
- if (contour.length >= minSize){ // 67
- poly = CV.approxPolyDP(contour, contour.length * epsilon); // 68
- // 69
- this.polys.push(poly); // 70
- // 71
- if ( (4 === poly.length) && ( CV.isContourConvex(poly) ) ){ // 72
- // 73
- if ( CV.minEdgeLength(poly) >= minLength){ // 74
- candidates.push(poly); // 75
- } // 76
- } // 77
- } // 78
- } // 79
- // 80
- return candidates; // 81
- }; // 82
- // 83
- AR.Detector.prototype.clockwiseCorners = function(candidates){ // 84
- var len = candidates.length, dx1, dx2, dy1, dy2, swap, i; // 85
- // 86
- for (i = 0; i < len; ++ i){ // 87
- dx1 = candidates[i][1].x - candidates[i][0].x; // 88
- dy1 = candidates[i][1].y - candidates[i][0].y; // 89
- dx2 = candidates[i][2].x - candidates[i][0].x; // 90
- dy2 = candidates[i][2].y - candidates[i][0].y; // 91
- // 92
- if ( (dx1 * dy2 - dy1 * dx2) < 0){ // 93
- swap = candidates[i][1]; // 94
- candidates[i][1] = candidates[i][3]; // 95
- candidates[i][3] = swap; // 96
- } // 97
- } // 98
- // 99
- return candidates; // 100
- }; // 101
- // 102
- AR.Detector.prototype.notTooNear = function(candidates, minDist){ // 103
- var notTooNear = [], len = candidates.length, dist, dx, dy, i, j, k; // 104
- // 105
- for (i = 0; i < len; ++ i){ // 106
- // 107
- for (j = i + 1; j < len; ++ j){ // 108
- dist = 0; // 109
- // 110
- for (k = 0; k < 4; ++ k){ // 111
- dx = candidates[i][k].x - candidates[j][k].x; // 112
- dy = candidates[i][k].y - candidates[j][k].y; // 113
- // 114
- dist += dx * dx + dy * dy; // 115
- } // 116
- // 117
- if ( (dist / 4) < (minDist * minDist) ){ // 118
- // 119
- if ( CV.perimeter( candidates[i] ) < CV.perimeter( candidates[j] ) ){ // 120
- candidates[i].tooNear = true; // 121
- }else{ // 122
- candidates[j].tooNear = true; // 123
- } // 124
- } // 125
- } // 126
- } // 127
- // 128
- for (i = 0; i < len; ++ i){ // 129
- if ( !candidates[i].tooNear ){ // 130
- notTooNear.push( candidates[i] ); // 131
- } // 132
- } // 133
- // 134
- return notTooNear; // 135
- }; // 136
- // 137
- AR.Detector.prototype.findMarkers = function(imageSrc, candidates, warpSize){ // 138
- var markers = [], len = candidates.length, candidate, marker, i; // 139
- // 140
- for (i = 0; i < len; ++ i){ // 141
- candidate = candidates[i]; // 142
- // 143
- CV.warp(imageSrc, this.homography, candidate, warpSize); // 144
- // 145
- CV.threshold(this.homography, this.homography, CV.otsu(this.homography) ); // 146
- // 147
- marker = this.getMarker(this.homography, candidate); // 148
- if (marker){ // 149
- markers.push(marker); // 150
- } // 151
- } // 152
- // 153
- return markers; // 154
- }; // 155
- // 156
- AR.Detector.prototype.getMarker = function(imageSrc, candidate){ // 157
- var width = (imageSrc.width / 7) >>> 0, // 158
- minZero = (width * width) >> 1, // 159
- bits = [], rotations = [], distances = [], // 160
- square, pair, inc, i, j; // 161
- // 162
- for (i = 0; i < 7; ++ i){ // 163
- inc = (0 === i || 6 === i)? 1: 6; // 164
- // 165
- for (j = 0; j < 7; j += inc){ // 166
- square = {x: j * width, y: i * width, width: width, height: width}; // 167
- if ( CV.countNonZero(imageSrc, square) > minZero){ // 168
- return null; // 169
- } // 170
- } // 171
- } // 172
- // 173
- for (i = 0; i < 5; ++ i){ // 174
- bits[i] = []; // 175
- // 176
- for (j = 0; j < 5; ++ j){ // 177
- square = {x: (j + 1) * width, y: (i + 1) * width, width: width, height: width}; // 178
- // 179
- bits[i][j] = CV.countNonZero(imageSrc, square) > minZero? 1: 0; // 180
- } // 181
- } // 182
- // 183
- rotations[0] = bits; // 184
- distances[0] = this.hammingDistance( rotations[0] ); // 185
- // 186
- pair = {first: distances[0], second: 0}; // 187
- // 188
- for (i = 1; i < 4; ++ i){ // 189
- rotations[i] = this.rotate( rotations[i - 1] ); // 190
- distances[i] = this.hammingDistance( rotations[i] ); // 191
- // 192
- if (distances[i] < pair.first){ // 193
- pair.first = distances[i]; // 194
- pair.second = i; // 195
- } // 196
- } // 197
- // 198
- if (0 !== pair.first){ // 199
- return null; // 200
- } // 201
- // 202
- return new AR.Marker( // 203
- this.mat2id( rotations[pair.second] ), // 204
- this.rotate2(candidate, 4 - pair.second) ); // 205
- }; // 206
- // 207
- AR.Detector.prototype.hammingDistance = function(bits){ // 208
- var ids = [ [1,0,0,0,0], [1,0,1,1,1], [0,1,0,0,1], [0,1,1,1,0] ], // 209
- dist = 0, sum, minSum, i, j, k; // 210
- // 211
- for (i = 0; i < 5; ++ i){ // 212
- minSum = Infinity; // 213
- // 214
- for (j = 0; j < 4; ++ j){ // 215
- sum = 0; // 216
- // 217
- for (k = 0; k < 5; ++ k){ // 218
- sum += bits[i][k] === ids[j][k]? 0: 1; // 219
- } // 220
- // 221
- if (sum < minSum){ // 222
- minSum = sum; // 223
- } // 224
- } // 225
- // 226
- dist += minSum; // 227
- } // 228
- // 229
- return dist; // 230
- }; // 231
- // 232
- AR.Detector.prototype.mat2id = function(bits){ // 233
- var id = 0, i; // 234
- // 235
- for (i = 0; i < 5; ++ i){ // 236
- id <<= 1; // 237
- id |= bits[i][1]; // 238
- id <<= 1; // 239
- id |= bits[i][3]; // 240
- } // 241
- // 242
- return id; // 243
- }; // 244
- // 245
- AR.Detector.prototype.rotate = function(src){ // 246
- var dst = [], len = src.length, i, j; // 247
- // 248
- for (i = 0; i < len; ++ i){ // 249
- dst[i] = []; // 250
- for (j = 0; j < src[i].length; ++ j){ // 251
- dst[i][j] = src[src[i].length - j - 1][i]; // 252
- } // 253
- } // 254
- // 255
- return dst; // 256
- }; // 257
- // 258
- AR.Detector.prototype.rotate2 = function(src, rotation){ // 259
- var dst = [], len = src.length, i; // 260
- // 261
- for (i = 0; i < len; ++ i){ // 262
- dst[i] = src[ (rotation + i) % len ]; // 263
- } // 264
- // 265
- return dst; // 266
- }; // 267
- // 268
- ////////////////////////////////////////////////////////////////////////////////////////////////////////////
- }).call(this);
- (function () {
- ////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // //
- // packages/js-aruco/cv.js //
- // //
- ////////////////////////////////////////////////////////////////////////////////////////////////////////////
- //
- /* // 1
- Copyright (c) 2011 Juan Mellado // 2
- // 3
- Permission is hereby granted, free of charge, to any person obtaining a copy // 4
- of this software and associated documentation files (the "Software"), to deal // 5
- in the Software without restriction, including without limitation the rights // 6
- to use, copy, modify, merge, publish, distribute, sublicense, and/or sell // 7
- copies of the Software, and to permit persons to whom the Software is // 8
- furnished to do so, subject to the following conditions: // 9
- // 10
- The above copyright notice and this permission notice shall be included in // 11
- all copies or substantial portions of the Software. // 12
- // 13
- THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR // 14
- IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, // 15
- FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE // 16
- AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER // 17
- LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, // 18
- OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN // 19
- THE SOFTWARE. // 20
- */ // 21
- // 22
- /* // 23
- References: // 24
- - "OpenCV: Open Computer Vision Library" // 25
- http://sourceforge.net/projects/opencvlibrary/ // 26
- - "Stack Blur: Fast But Goodlooking" // 27
- http://incubator.quasimondo.com/processing/fast_blur_deluxe.php // 28
- */ // 29
- // 30
- var CV = CV || {}; // 31
- // 32
- CV.Image = function(width, height, data){ // 33
- this.width = width || 0; // 34
- this.height = height || 0; // 35
- this.data = data || []; // 36
- }; // 37
- // 38
- CV.grayscale = function(imageSrc, imageDst){ // 39
- var src = imageSrc.data, dst = imageDst.data, len = src.length, // 40
- i = 0, j = 0; // 41
- // 42
- for (; i < len; i += 4){ // 43
- dst[j ++] = // 44
- (src[i] * 0.299 + src[i + 1] * 0.587 + src[i + 2] * 0.114 + 0.5) & 0xff; // 45
- } // 46
- // 47
- imageDst.width = imageSrc.width; // 48
- imageDst.height = imageSrc.height; // 49
- // 50
- return imageDst; // 51
- }; // 52
- // 53
- CV.threshold = function(imageSrc, imageDst, threshold){ // 54
- var src = imageSrc.data, dst = imageDst.data, // 55
- len = src.length, tab = [], i; // 56
- // 57
- for (i = 0; i < 256; ++ i){ // 58
- tab[i] = i <= threshold? 0: 255; // 59
- } // 60
- // 61
- for (i = 0; i < len; ++ i){ // 62
- dst[i] = tab[ src[i] ]; // 63
- } // 64
- // 65
- imageDst.width = imageSrc.width; // 66
- imageDst.height = imageSrc.height; // 67
- // 68
- return imageDst; // 69
- }; // 70
- // 71
- CV.adaptiveThreshold = function(imageSrc, imageDst, kernelSize, threshold){ // 72
- var src = imageSrc.data, dst = imageDst.data, len = src.length, tab = [], i; // 73
- // 74
- CV.stackBoxBlur(imageSrc, imageDst, kernelSize); // 75
- // 76
- for (i = 0; i < 768; ++ i){ // 77
- tab[i] = (i - 255 <= -threshold)? 255: 0; // 78
- } // 79
- // 80
- for (i = 0; i < len; ++ i){ // 81
- dst[i] = tab[ src[i] - dst[i] + 255 ]; // 82
- } // 83
- // 84
- imageDst.width = imageSrc.width; // 85
- imageDst.height = imageSrc.height; // 86
- // 87
- return imageDst; // 88
- }; // 89
- // 90
- CV.otsu = function(imageSrc){ // 91
- var src = imageSrc.data, len = src.length, hist = [], // 92
- threshold = 0, sum = 0, sumB = 0, wB = 0, wF = 0, max = 0, // 93
- mu, between, i; // 94
- // 95
- for (i = 0; i < 256; ++ i){ // 96
- hist[i] = 0; // 97
- } // 98
- // 99
- for (i = 0; i < len; ++ i){ // 100
- hist[ src[i] ] ++; // 101
- } // 102
- // 103
- for (i = 0; i < 256; ++ i){ // 104
- sum += hist[i] * i; // 105
- } // 106
- // 107
- for (i = 0; i < 256; ++ i){ // 108
- wB += hist[i]; // 109
- if (0 !== wB){ // 110
- // 111
- wF = len - wB; // 112
- if (0 === wF){ // 113
- break; // 114
- } // 115
- // 116
- sumB += hist[i] * i; // 117
- // 118
- mu = (sumB / wB) - ( (sum - sumB) / wF ); // 119
- // 120
- between = wB * wF * mu * mu; // 121
- // 122
- if (between > max){ // 123
- max = between; // 124
- threshold = i; // 125
- } // 126
- } // 127
- } // 128
- // 129
- return threshold; // 130
- }; // 131
- // 132
- CV.stackBoxBlurMult = // 133
- [1, 171, 205, 293, 57, 373, 79, 137, 241, 27, 391, 357, 41, 19, 283, 265]; // 134
- // 135
- CV.stackBoxBlurShift = // 136
- [0, 9, 10, 11, 9, 12, 10, 11, 12, 9, 13, 13, 10, 9, 13, 13]; // 137
- // 138
- CV.BlurStack = function(){ // 139
- this.color = 0; // 140
- this.next = null; // 141
- }; // 142
- // 143
- CV.stackBoxBlur = function(imageSrc, imageDst, kernelSize){ // 144
- var src = imageSrc.data, dst = imageDst.data, // 145
- height = imageSrc.height, width = imageSrc.width, // 146
- heightMinus1 = height - 1, widthMinus1 = width - 1, // 147
- size = kernelSize + kernelSize + 1, radius = kernelSize + 1, // 148
- mult = CV.stackBoxBlurMult[kernelSize], // 149
- shift = CV.stackBoxBlurShift[kernelSize], // 150
- stack, stackStart, color, sum, pos, start, p, x, y, i; // 151
- // 152
- stack = stackStart = new CV.BlurStack(); // 153
- for (i = 1; i < size; ++ i){ // 154
- stack = stack.next = new CV.BlurStack(); // 155
- } // 156
- stack.next = stackStart; // 157
- // 158
- pos = 0; // 159
- // 160
- for (y = 0; y < height; ++ y){ // 161
- start = pos; // 162
- // 163
- color = src[pos]; // 164
- sum = radius * color; // 165
- // 166
- stack = stackStart; // 167
- for (i = 0; i < radius; ++ i){ // 168
- stack.color = color; // 169
- stack = stack.next; // 170
- } // 171
- for (i = 1; i < radius; ++ i){ // 172
- stack.color = src[pos + i]; // 173
- sum += stack.color; // 174
- stack = stack.next; // 175
- } // 176
- // 177
- stack = stackStart; // 178
- for (x = 0; x < width; ++ x){ // 179
- dst[pos ++] = (sum * mult) >>> shift; // 180
- // 181
- p = x + radius; // 182
- p = start + (p < widthMinus1? p: widthMinus1); // 183
- sum -= stack.color - src[p]; // 184
- // 185
- stack.color = src[p]; // 186
- stack = stack.next; // 187
- } // 188
- } // 189
- // 190
- for (x = 0; x < width; ++ x){ // 191
- pos = x; // 192
- start = pos + width; // 193
- // 194
- color = dst[pos]; // 195
- sum = radius * color; // 196
- // 197
- stack = stackStart; // 198
- for (i = 0; i < radius; ++ i){ // 199
- stack.color = color; // 200
- stack = stack.next; // 201
- } // 202
- for (i = 1; i < radius; ++ i){ // 203
- stack.color = dst[start]; // 204
- sum += stack.color; // 205
- stack = stack.next; // 206
- // 207
- start += width; // 208
- } // 209
- // 210
- stack = stackStart; // 211
- for (y = 0; y < height; ++ y){ // 212
- dst[pos] = (sum * mult) >>> shift; // 213
- // 214
- p = y + radius; // 215
- p = x + ( (p < heightMinus1? p: heightMinus1) * width ); // 216
- sum -= stack.color - dst[p]; // 217
- // 218
- stack.color = dst[p]; // 219
- stack = stack.next; // 220
- // 221
- pos += width; // 222
- } // 223
- } // 224
- // 225
- return imageDst; // 226
- }; // 227
- // 228
- CV.gaussianBlur = function(imageSrc, imageDst, imageMean, kernelSize){ // 229
- var kernel = CV.gaussianKernel(kernelSize); // 230
- // 231
- imageDst.width = imageSrc.width; // 232
- imageDst.height = imageSrc.height; // 233
- // 234
- imageMean.width = imageSrc.width; // 235
- imageMean.height = imageSrc.height; // 236
- // 237
- CV.gaussianBlurFilter(imageSrc, imageMean, kernel, true); // 238
- CV.gaussianBlurFilter(imageMean, imageDst, kernel, false); // 239
- // 240
- return imageDst; // 241
- }; // 242
- // 243
- CV.gaussianBlurFilter = function(imageSrc, imageDst, kernel, horizontal){ // 244
- var src = imageSrc.data, dst = imageDst.data, // 245
- height = imageSrc.height, width = imageSrc.width, // 246
- pos = 0, limit = kernel.length >> 1, // 247
- cur, value, i, j, k; // 248
- // 249
- for (i = 0; i < height; ++ i){ // 250
- // 251
- for (j = 0; j < width; ++ j){ // 252
- value = 0.0; // 253
- // 254
- for (k = -limit; k <= limit; ++ k){ // 255
- // 256
- if (horizontal){ // 257
- cur = pos + k; // 258
- if (j + k < 0){ // 259
- cur = pos; // 260
- } // 261
- else if (j + k >= width){ // 262
- cur = pos; // 263
- } // 264
- }else{ // 265
- cur = pos + (k * width); // 266
- if (i + k < 0){ // 267
- cur = pos; // 268
- } // 269
- else if (i + k >= height){ // 270
- cur = pos; // 271
- } // 272
- } // 273
- // 274
- value += kernel[limit + k] * src[cur]; // 275
- } // 276
- // 277
- dst[pos ++] = horizontal? value: (value + 0.5) & 0xff; // 278
- } // 279
- } // 280
- // 281
- return imageDst; // 282
- }; // 283
- // 284
- CV.gaussianKernel = function(kernelSize){ // 285
- var tab = // 286
- [ [1], // 287
- [0.25, 0.5, 0.25], // 288
- [0.0625, 0.25, 0.375, 0.25, 0.0625], // 289
- [0.03125, 0.109375, 0.21875, 0.28125, 0.21875, 0.109375, 0.03125] ], // 290
- kernel = [], center, sigma, scale2X, sum, x, i; // 291
- // 292
- if ( (kernelSize <= 7) && (kernelSize % 2 === 1) ){ // 293
- kernel = tab[kernelSize >> 1]; // 294
- }else{ // 295
- center = (kernelSize - 1.0) * 0.5; // 296
- sigma = 0.8 + (0.3 * (center - 1.0) ); // 297
- scale2X = -0.5 / (sigma * sigma); // 298
- sum = 0.0; // 299
- for (i = 0; i < kernelSize; ++ i){ // 300
- x = i - center; // 301
- sum += kernel[i] = Math.exp(scale2X * x * x); // 302
- } // 303
- sum = 1 / sum; // 304
- for (i = 0; i < kernelSize; ++ i){ // 305
- kernel[i] *= sum; // 306
- } // 307
- } // 308
- // 309
- return kernel; // 310
- }; // 311
- // 312
- CV.findContours = function(imageSrc, binary){ // 313
- var width = imageSrc.width, height = imageSrc.height, contours = [], // 314
- src, deltas, pos, pix, nbd, outer, hole, i, j; // 315
- // 316
- src = CV.binaryBorder(imageSrc, binary); // 317
- // 318
- deltas = CV.neighborhoodDeltas(width + 2); // 319
- // 320
- pos = width + 3; // 321
- nbd = 1; // 322
- // 323
- for (i = 0; i < height; ++ i, pos += 2){ // 324
- // 325
- for (j = 0; j < width; ++ j, ++ pos){ // 326
- pix = src[pos]; // 327
- // 328
- if (0 !== pix){ // 329
- outer = hole = false; // 330
- // 331
- if (1 === pix && 0 === src[pos - 1]){ // 332
- outer = true; // 333
- } // 334
- else if (pix >= 1 && 0 === src[pos + 1]){ // 335
- hole = true; // 336
- } // 337
- // 338
- if (outer || hole){ // 339
- ++ nbd; // 340
- // 341
- contours.push( CV.borderFollowing(src, pos, nbd, {x: j, y: i}, hole, deltas) ); // 342
- } // 343
- } // 344
- } // 345
- } // 346
- // 347
- return contours; // 348
- }; // 349
- // 350
- CV.borderFollowing = function(src, pos, nbd, point, hole, deltas){ // 351
- var contour = [], pos1, pos3, pos4, s, s_end, s_prev; // 352
- // 353
- contour.hole = hole; // 354
- // 355
- s = s_end = hole? 0: 4; // 356
- do{ // 357
- s = (s - 1) & 7; // 358
- pos1 = pos + deltas[s]; // 359
- if (src[pos1] !== 0){ // 360
- break; // 361
- } // 362
- }while(s !== s_end); // 363
- // 364
- if (s === s_end){ // 365
- src[pos] = -nbd; // 366
- contour.push( {x: point.x, y: point.y} ); // 367
- // 368
- }else{ // 369
- pos3 = pos; // 370
- s_prev = s ^ 4; // 371
- // 372
- while(true){ // 373
- s_end = s; // 374
- // 375
- do{ // 376
- pos4 = pos3 + deltas[++ s]; // 377
- }while(src[pos4] === 0); // 378
- // 379
- s &= 7; // 380
- // 381
- if ( ( (s - 1) >>> 0) < (s_end >>> 0) ){ // 382
- src[pos3] = -nbd; // 383
- } // 384
- else if (src[pos3] === 1){ // 385
- src[pos3] = nbd; // 386
- } // 387
- // 388
- contour.push( {x: point.x, y: point.y} ); // 389
- // 390
- s_prev = s; // 391
- // 392
- point.x += CV.neighborhood[s][0]; // 393
- point.y += CV.neighborhood[s][1]; // 394
- // 395
- if ( (pos4 === pos) && (pos3 === pos1) ){ // 396
- break; // 397
- } // 398
- // 399
- pos3 = pos4; // 400
- s = (s + 4) & 7; // 401
- } // 402
- } // 403
- // 404
- return contour; // 405
- }; // 406
- // 407
- CV.neighborhood = // 408
- [ [1, 0], [1, -1], [0, -1], [-1, -1], [-1, 0], [-1, 1], [0, 1], [1, 1] ]; // 409
- // 410
- CV.neighborhoodDeltas = function(width){ // 411
- var deltas = [], len = CV.neighborhood.length, i = 0; // 412
- // 413
- for (; i < len; ++ i){ // 414
- deltas[i] = CV.neighborhood[i][0] + (CV.neighborhood[i][1] * width); // 415
- } // 416
- // 417
- return deltas.concat(deltas); // 418
- }; // 419
- // 420
- CV.approxPolyDP = function(contour, epsilon){ // 421
- var slice = {start_index: 0, end_index: 0}, // 422
- right_slice = {start_index: 0, end_index: 0}, // 423
- poly = [], stack = [], len = contour.length, // 424
- pt, start_pt, end_pt, dist, max_dist, le_eps, // 425
- dx, dy, i, j, k; // 426
- // 427
- epsilon *= epsilon; // 428
- // 429
- k = 0; // 430
- // 431
- for (i = 0; i < 3; ++ i){ // 432
- max_dist = 0; // 433
- // 434
- k = (k + right_slice.start_index) % len; // 435
- start_pt = contour[k]; // 436
- if (++ k === len) {k = 0;} // 437
- // 438
- for (j = 1; j < len; ++ j){ // 439
- pt = contour[k]; // 440
- if (++ k === len) {k = 0;} // 441
- // 442
- dx = pt.x - start_pt.x; // 443
- dy = pt.y - start_pt.y; // 444
- dist = dx * dx + dy * dy; // 445
- // 446
- if (dist > max_dist){ // 447
- max_dist = dist; // 448
- right_slice.start_index = j; // 449
- } // 450
- } // 451
- } // 452
- // 453
- if (max_dist <= epsilon){ // 454
- poly.push( {x: start_pt.x, y: start_pt.y} ); // 455
- // 456
- }else{ // 457
- slice.start_index = k; // 458
- slice.end_index = (right_slice.start_index += slice.start_index); // 459
- // 460
- right_slice.start_index -= right_slice.start_index >= len? len: 0; // 461
- right_slice.end_index = slice.start_index; // 462
- if (right_slice.end_index < right_slice.start_index){ // 463
- right_slice.end_index += len; // 464
- } // 465
- // 466
- stack.push( {start_index: right_slice.start_index, end_index: right_slice.end_index} ); // 467
- stack.push( {start_index: slice.start_index, end_index: slice.end_index} ); // 468
- } // 469
- // 470
- while(stack.length !== 0){ // 471
- slice = stack.pop(); // 472
- // 473
- end_pt = contour[slice.end_index % len]; // 474
- start_pt = contour[k = slice.start_index % len]; // 475
- if (++ k === len) {k = 0;} // 476
- // 477
- if (slice.end_index <= slice.start_index + 1){ // 478
- le_eps = true; // 479
- // 480
- }else{ // 481
- max_dist = 0; // 482
- // 483
- dx = end_pt.x - start_pt.x; // 484
- dy = end_pt.y - start_pt.y; // 485
- // 486
- for (i = slice.start_index + 1; i < slice.end_index; ++ i){ // 487
- pt = contour[k]; // 488
- if (++ k === len) {k = 0;} // 489
- // 490
- dist = Math.abs( (pt.y - start_pt.y) * dx - (pt.x - start_pt.x) * dy); // 491
- // 492
- if (dist > max_dist){ // 493
- max_dist = dist; // 494
- right_slice.start_index = i; // 495
- } // 496
- } // 497
- // 498
- le_eps = max_dist * max_dist <= epsilon * (dx * dx + dy * dy); // 499
- } // 500
- // 501
- if (le_eps){ // 502
- poly.push( {x: start_pt.x, y: start_pt.y} ); // 503
- // 504
- }else{ // 505
- right_slice.end_index = slice.end_index; // 506
- slice.end_index = right_slice.start_index; // 507
- // 508
- stack.push( {start_index: right_slice.start_index, end_index: right_slice.end_index} ); // 509
- stack.push( {start_index: slice.start_index, end_index: slice.end_index} ); // 510
- } // 511
- } // 512
- // 513
- return poly; // 514
- }; // 515
- // 516
- CV.warp = function(imageSrc, imageDst, contour, warpSize){ // 517
- var src = imageSrc.data, dst = imageDst.data, // 518
- width = imageSrc.width, height = imageSrc.height, // 519
- pos = 0, // 520
- sx1, sx2, dx1, dx2, sy1, sy2, dy1, dy2, p1, p2, p3, p4, // 521
- m, r, s, t, u, v, w, x, y, i, j; // 522
- // 523
- m = CV.getPerspectiveTransform(contour, warpSize - 1); // 524
- // 525
- r = m[8]; // 526
- s = m[2]; // 527
- t = m[5]; // 528
- // 529
- for (i = 0; i < warpSize; ++ i){ // 530
- r += m[7]; // 531
- s += m[1]; // 532
- t += m[4]; // 533
- // 534
- u = r; // 535
- v = s; // 536
- w = t; // 537
- // 538
- for (j = 0; j < warpSize; ++ j){ // 539
- u += m[6]; // 540
- v += m[0]; // 541
- w += m[3]; // 542
- // 543
- x = v / u; // 544
- y = w / u; // 545
- // 546
- sx1 = x >>> 0; // 547
- sx2 = (sx1 === width - 1)? sx1: sx1 + 1; // 548
- dx1 = x - sx1; // 549
- dx2 = 1.0 - dx1; // 550
- // 551
- sy1 = y >>> 0; // 552
- sy2 = (sy1 === height - 1)? sy1: sy1 + 1; // 553
- dy1 = y - sy1; // 554
- dy2 = 1.0 - dy1; // 555
- // 556
- p1 = p2 = sy1 * width; // 557
- p3 = p4 = sy2 * width; // 558
- // 559
- dst[pos ++] = // 560
- (dy2 * (dx2 * src[p1 + sx1] + dx1 * src[p2 + sx2]) + // 561
- dy1 * (dx2 * src[p3 + sx1] + dx1 * src[p4 + sx2]) ) & 0xff; // 562
- // 563
- } // 564
- } // 565
- // 566
- imageDst.width = warpSize; // 567
- imageDst.height = warpSize; // 568
- // 569
- return imageDst; // 570
- }; // 571
- // 572
- CV.getPerspectiveTransform = function(src, size){ // 573
- var rq = CV.square2quad(src); // 574
- // 575
- rq[0] /= size; // 576
- rq[1] /= size; // 577
- rq[3] /= size; // 578
- rq[4] /= size; // 579
- rq[6] /= size; // 580
- rq[7] /= size; // 581
- // 582
- return rq; // 583
- }; // 584
- // 585
- CV.square2quad = function(src){ // 586
- var sq = [], px, py, dx1, dx2, dy1, dy2, den; // 587
- // 588
- px = src[0].x - src[1].x + src[2].x - src[3].x; // 589
- py = src[0].y - src[1].y + src[2].y - src[3].y; // 590
- // 591
- if (0 === px && 0 === py){ // 592
- sq[0] = src[1].x - src[0].x; // 593
- sq[1] = src[2].x - src[1].x; // 594
- sq[2] = src[0].x; // 595
- sq[3] = src[1].y - src[0].y; // 596
- sq[4] = src[2].y - src[1].y; // 597
- sq[5] = src[0].y; // 598
- sq[6] = 0; // 599
- sq[7] = 0; // 600
- sq[8] = 1; // 601
- // 602
- }else{ // 603
- dx1 = src[1].x - src[2].x; // 604
- dx2 = src[3].x - src[2].x; // 605
- dy1 = src[1].y - src[2].y; // 606
- dy2 = src[3].y - src[2].y; // 607
- den = dx1 * dy2 - dx2 * dy1; // 608
- // 609
- sq[6] = (px * dy2 - dx2 * py) / den; // 610
- sq[7] = (dx1 * py - px * dy1) / den; // 611
- sq[8] = 1; // 612
- sq[0] = src[1].x - src[0].x + sq[6] * src[1].x; // 613
- sq[1] = src[3].x - src[0].x + sq[7] * src[3].x; // 614
- sq[2] = src[0].x; // 615
- sq[3] = src[1].y - src[0].y + sq[6] * src[1].y; // 616
- sq[4] = src[3].y - src[0].y + sq[7] * src[3].y; // 617
- sq[5] = src[0].y; // 618
- } // 619
- // 620
- return sq; // 621
- }; // 622
- // 623
- CV.isContourConvex = function(contour){ // 624
- var orientation = 0, convex = true, // 625
- len = contour.length, i = 0, j = 0, // 626
- cur_pt, prev_pt, dxdy0, dydx0, dx0, dy0, dx, dy; // 627
- // 628
- prev_pt = contour[len - 1]; // 629
- cur_pt = contour[0]; // 630
- // 631
- dx0 = cur_pt.x - prev_pt.x; // 632
- dy0 = cur_pt.y - prev_pt.y; // 633
- // 634
- for (; i < len; ++ i){ // 635
- if (++ j === len) {j = 0;} // 636
- // 637
- prev_pt = cur_pt; // 638
- cur_pt = contour[j]; // 639
- // 640
- dx = cur_pt.x - prev_pt.x; // 641
- dy = cur_pt.y - prev_pt.y; // 642
- dxdy0 = dx * dy0; // 643
- dydx0 = dy * dx0; // 644
- // 645
- orientation |= dydx0 > dxdy0? 1: (dydx0 < dxdy0? 2: 3); // 646
- // 647
- if (3 === orientation){ // 648
- convex = false; // 649
- break; // 650
- } // 651
- // 652
- dx0 = dx; // 653
- dy0 = dy; // 654
- } // 655
- // 656
- return convex; // 657
- }; // 658
- // 659
- CV.perimeter = function(poly){ // 660
- var len = poly.length, i = 0, j = len - 1, // 661
- p = 0.0, dx, dy; // 662
- // 663
- for (; i < len; j = i ++){ // 664
- dx = poly[i].x - poly[j].x; // 665
- dy = poly[i].y - poly[j].y; // 666
- // 667
- p += Math.sqrt(dx * dx + dy * dy) ; // 668
- } // 669
- // 670
- return p; // 671
- }; // 672
- // 673
- CV.minEdgeLength = function(poly){ // 674
- var len = poly.length, i = 0, j = len - 1, // 675
- min = Infinity, d, dx, dy; // 676
- // 677
- for (; i < len; j = i ++){ // 678
- dx = poly[i].x - poly[j].x; // 679
- dy = poly[i].y - poly[j].y; // 680
- // 681
- d = dx * dx + dy * dy; // 682
- // 683
- if (d < min){ // 684
- min = d; // 685
- } // 686
- } // 687
- // 688
- return Math.sqrt(min); // 689
- }; // 690
- // 691
- CV.countNonZero = function(imageSrc, square){ // 692
- var src = imageSrc.data, height = square.height, width = square.width, // 693
- pos = square.x + (square.y * imageSrc.width), // 694
- span = imageSrc.width - width, // 695
- nz = 0, i, j; // 696
- // 697
- for (i = 0; i < height; ++ i){ // 698
- // 699
- for (j = 0; j < width; ++ j){ // 700
- // 701
- if ( 0 !== src[pos ++] ){ // 702
- ++ nz; // 703
- } // 704
- } // 705
- // 706
- pos += span; // 707
- } // 708
- // 709
- return nz; // 710
- }; // 711
- // 712
- CV.binaryBorder = function(imageSrc, dst){ // 713
- var src = imageSrc.data, height = imageSrc.height, width = imageSrc.width, // 714
- posSrc = 0, posDst = 0, i, j; // 715
- // 716
- for (j = -2; j < width; ++ j){ // 717
- dst[posDst ++] = 0; // 718
- } // 719
- // 720
- for (i = 0; i < height; ++ i){ // 721
- dst[posDst ++] = 0; // 722
- // 723
- for (j = 0; j < width; ++ j){ // 724
- dst[posDst ++] = (0 === src[posSrc ++]? 0: 1); // 725
- } // 726
- // 727
- dst[posDst ++] = 0; // 728
- } // 729
- // 730
- for (j = -2; j < width; ++ j){ // 731
- dst[posDst ++] = 0; // 732
- } // 733
- // 734
- return dst; // 735
- }; // 736
- // 737
- ////////////////////////////////////////////////////////////////////////////////////////////////////////////
- }).call(this);
- (function () {
- ////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // //
- // packages/js-aruco/posit1.js //
- // //
- ////////////////////////////////////////////////////////////////////////////////////////////////////////////
- //
- /*
- // 1
- Copyright (c) 2012 Juan Mellado
- // 2
- // 3
- Permission is hereby granted, free of charge, to any person obtaining a copy
- // 4
- of this software and associated documentation files (the "Software"), to deal
- // 5
- in the Software without restriction, including without limitation the rights
- // 6
- to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- // 7
- copies of the Software, and to permit persons to whom the Software is
- // 8
- furnished to do so, subject to the following conditions:
- // 9
- // 10
- The above copyright notice and this permission notice shall be included in
- // 11
- all copies or substantial portions of the Software.
- // 12
- // 13
- THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- // 14
- IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- // 15
- FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- // 16
- AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- // 17
- LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- // 18
- OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- // 19
- THE SOFTWARE.
- // 20
- */
- // 21
- // 22
- /*
- // 23
- References:
- // 24
- - "Iterative Pose Estimation using Coplanar Feature Points"
- // 25
- Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis
- // 26
- http://www.cfar.umd.edu/~daniel/daniel_papersfordownload/CoplanarPts.pdf
- // 27
- */
- // 28
- // 29
- var POS = POS || {};
- // 30
- // 31
- POS.Posit = function(modelSize, focalLength){
- // 32
- this.objectPoints = this.buildModel(modelSize);
- // 33
- this.focalLength = focalLength;
- // 34
- // 35
- this.objectVectors = [];
- // 36
- this.objectNormal = [];
- // 37
- this.objectMatrix = [[],[],[]];
- // 38
- // 39
- this.init();
- // 40
- };
- // 41
- // 42
- POS.Posit.prototype.buildModel = function(modelSize){
- // 43
- var half = modelSize / 2.0;
- // 44
- // 45
- return [
- // 46
- [-half, half, 0.0],
- // 47
- [ half, half, 0.0],
- // 48
- [ half, -half, 0.0],
- // 49
- [-half, -half, 0.0] ];
- // 50
- };
- // 51
- // 52
- POS.Posit.prototype.init = function(){
- // 53
- var np = this.objectPoints.length,
- // 54
- vectors = [], n = [], len = 0.0, row = 2, i;
- // 55
- // 56
- for (i = 0; i < np; ++ i){
- // 57
- this.objectVectors[i] = [this.objectPoints[i][0] - this.objectPoints[0][0],
- // 58
- this.objectPoints[i][1] - this.objectPoints[0][1],
- // 59
- this.objectPoints[i][2] - this.objectPoints[0][2]];
- // 60
- // 61
- vectors[i] = [this.objectVectors[i][0],
- // 62
- this.objectVectors[i][1],
- // 63
- this.objectVectors[i][2]];
- // 64
- }
- // 65
- // 66
- while(0.0 === len){
- // 67
- n[0] = this.objectVectors[1][1] * this.objectVectors[row][2] -
- // 68
- this.objectVectors[1][2] * this.objectVectors[row][1];
- // 69
- n[1] = this.objectVectors[1][2] * this.objectVectors[row][0] -
- // 70
- this.objectVectors[1][0] * this.objectVectors[row][2];
- // 71
- n[2] = this.objectVectors[1][0] * this.objectVectors[row][1] -
- // 72
- this.objectVectors[1][1] * this.objectVectors[row][0];
- // 73
- // 74
- len = Math.sqrt(n[0] * n[0] + n[1] * n[1] + n[2] * n[2]);
- // 75
- // 76
- ++ row;
- // 77
- }
- // 78
- // 79
- for (i = 0; i < 3; ++ i){
- // 80
- this.objectNormal[i] = n[i] / len;
- // 81
- }
- // 82
- // 83
- POS.pseudoInverse(vectors, np, this.objectMatrix);
- // 84
- };
- // 85
- // 86
- POS.Posit.prototype.pose = function(imagePoints){
- // 87
- var posRotation1 = [[],[],[]], posRotation2 = [[],[],[]], posTranslation = [],
- // 88
- rotation1 = [[],[],[]], rotation2 = [[],[],[]], translation1 = [], translation2 = [],
- // 89
- error1, error2, valid1, valid2, i, j;
- // 90
- // 91
- this.pos(imagePoints, posRotation1, posRotation2, posTranslation);
- // 92
- // 93
- valid1 = this.isValid(posRotation1, posTranslation);
- // 94
- if (valid1){
- // 95
- error1 = this.iterate(imagePoints, posRotation1, posTranslation, rotation1, translation1);
- // 96
- }else{
- // 97
- error1 = {euclidean: -1.0, pixels: -1, maximum: -1.0};
- // 98
- }
- // 99
- // 100
- valid2 = this.isValid(posRotation2, posTranslation);
- // 101
- if (valid2){
- // 102
- error2 = this.iterate(imagePoints, posRotation2, posTranslation, rotation2, translation2);
- // 103
- }else{
- // 104
- error2 = {euclidean: -1.0, pixels: -1, maximum: -1.0};
- // 105
- }
- // 106
- // 107
- for (i = 0; i < 3; ++ i){
- // 108
- for (j = 0; j < 3; ++ j){
- // 109
- if (valid1){
- // 110
- translation1[i] -= rotation1[i][j] * this.objectPoints[0][j];
- // 111
- }
- // 112
- if (valid2){
- // 113
- translation2[i] -= rotation2[i][j] * this.objectPoints[0][j];
- // 114
- }
- // 115
- }
- // 116
- }
- // 117
- // 118
- return error1.euclidean < error2.euclidean?
- // 119
- new POS.Pose(error1.pixels, rotation1, translation1, error2.pixels, rotation2, translation2):
- // 120
- new POS.Pose(error2.pixels, rotation2, translation2, error1.pixels, rotation1, translation1);
- // 121
- };
- // 122
- // 123
- POS.Posit.prototype.pos = function(imagePoints, rotation1, rotation2, translation){
- // 124
- var np = this.objectPoints.length, imageVectors = [],
- // 125
- i0 = [], j0 = [], ivec = [], jvec = [], row1 = [], row2 = [], row3 = [],
- // 126
- i0i0, j0j0, i0j0, delta, q, lambda, mu, scale, i, j;
- // 127
- // 128
- for (i = 0; i < np; ++ i){
- // 129
- imageVectors[i] = [imagePoints[i].x - imagePoints[0].x,
- // 130
- imagePoints[i].y - imagePoints[0].y];
- // 131
- }
- // 132
- // 133
- //i0 and j0
- // 134
- for (i = 0; i < 3; ++ i){
- // 135
- i0[i] = 0.0;
- // 136
- j0[i] = 0.0;
- // 137
- for (j = 0; j < np; ++ j){
- // 138
- i0[i] += this.objectMatrix[i][j] * imageVectors[j][0];
- // 139
- j0[i] += this.objectMatrix[i][j] * imageVectors[j][1];
- // 140
- }
- // 141
- }
- // 142
- // 143
- i0i0 = i0[0] * i0[0] + i0[1] * i0[1] + i0[2] * i0[2];
- // 144
- j0j0 = j0[0] * j0[0] + j0[1] * j0[1] + j0[2] * j0[2];
- // 145
- i0j0 = i0[0] * j0[0] + i0[1] * j0[1] + i0[2] * j0[2];
- // 146
- // 147
- //Lambda and mu
- // 148
- delta = (j0j0 - i0i0) * (j0j0 - i0i0) + 4.0 * (i0j0 * i0j0);
- // 149
- // 150
- if (j0j0 - i0i0 >= 0.0){
- // 151
- q = (j0j0 - i0i0 + Math.sqrt(delta) ) / 2.0;
- // 152
- }else{
- // 153
- q = (j0j0 - i0i0 - Math.sqrt(delta) ) / 2.0;
- // 154
- }
- // 155
- // 156
- if (q >= 0.0){
- // 157
- lambda = Math.sqrt(q);
- // 158
- if (0.0 === lambda){
- // 159
- mu = 0.0;
- // 160
- }else{
- // 161
- mu = -i0j0 / lambda;
- // 162
- }
- // 163
- }else{
- // 164
- lambda = Math.sqrt( -(i0j0 * i0j0) / q);
- // 165
- if (0.0 === lambda){
- // 166
- mu = Math.sqrt(i0i0 - j0j0);
- // 167
- }else{
- // 168
- mu = -i0j0 / lambda;
- // 169
- }
- // 170
- }
- // 171
- // 172
- //First rotation
- // 173
- for (i = 0; i < 3; ++ i){
- // 174
- ivec[i] = i0[i] + lambda * this.objectNormal[i];
- // 175
- jvec[i] = j0[i] + mu * this.objectNormal[i];
- // 176
- }
- // 177
- // 178
- scale = Math.sqrt(ivec[0] * ivec[0] + ivec[1] * ivec[1] + ivec[2] * ivec[2]);
- // 179
- // 180
- for (i = 0; i < 3; ++ i){
- // 181
- row1[i] = ivec[i] / scale;
- // 182
- row2[i] = jvec[i] / scale;
- // 183
- }
- // 184
- // 185
- row3[0] = row1[1] * row2[2] - row1[2] * row2[1];
- // 186
- row3[1] = row1[2] * row2[0] - row1[0] * row2[2];
- // 187
- row3[2] = row1[0] * row2[1] - row1[1] * row2[0];
- // 188
- // 189
- for (i = 0; i < 3; ++ i){
- // 190
- rotation1[0][i] = row1[i];
- // 191
- rotation1[1][i] = row2[i];
- // 192
- rotation1[2][i] = row3[i];
- // 193
- }
- // 194
- // 195
- //Second rotation
- // 196
- for (i = 0; i < 3; ++ i){
- // 197
- ivec[i] = i0[i] - lambda * this.objectNormal[i];
- // 198
- jvec[i] = j0[i] - mu * this.objectNormal[i];
- // 199
- }
- // 200
- // 201
- for (i = 0; i < 3; ++ i){
- // 202
- row1[i] = ivec[i] / scale;
- // 203
- row2[i] = jvec[i] / scale;
- // 204
- }
- // 205
- // 206
- row3[0] = row1[1] * row2[2] - row1[2] * row2[1];
- // 207
- row3[1] = row1[2] * row2[0] - row1[0] * row2[2];
- // 208
- row3[2] = row1[0] * row2[1] - row1[1] * row2[0];
- // 209
- // 210
- for (i = 0; i < 3; ++ i){
- // 211
- rotation2[0][i] = row1[i];
- // 212
- rotation2[1][i] = row2[i];
- // 213
- rotation2[2][i] = row3[i];
- // 214
- }
- // 215
- // 216
- //Translation
- // 217
- translation[0] = imagePoints[0].x / scale;
- // 218
- translation[1] = imagePoints[0].y / scale;
- // 219
- translation[2] = this.focalLength / scale;
- // 220
- };
- // 221
- // 222
- POS.Posit.prototype.isValid = function(rotation, translation){
- // 223
- var np = this.objectPoints.length, zmin = Infinity, i = 0, zi;
- // 224
- // 225
- for (; i < np; ++ i){
- // 226
- zi = translation[2] +
- // 227
- (rotation[2][0] * this.objectVectors[i][0] +
- // 228
- rotation[2][1] * this.objectVectors[i][1] +
- // 229
- rotation[2][2] * this.objectVectors[i][2]);
- // 230
- if (zi < zmin){
- // 231
- zmin = zi;
- // 232
- }
- // 233
- }
- // 234
- // 235
- return zmin >= 0.0;
- // 236
- };
- // 237
- // 238
- POS.Posit.prototype.iterate = function(imagePoints, posRotation, posTranslation, rotation, translation){
- // 239
- var np = this.objectPoints.length,
- // 240
- oldSopImagePoints = [], sopImagePoints = [],
- // 241
- rotation1 = [[],[],[]], rotation2 = [[],[],[]],
- // 242
- translation1 = [], translation2 = [],
- // 243
- converged = false, iteration = 0,
- // 244
- oldImageDifference, imageDifference, factor,
- // 245
- error, error1, error2, delta, i, j;
- // 246
- // 247
- for (i = 0; i < np; ++ i){
- // 248
- oldSopImagePoints[i] = {x: imagePoints[i].x,
- // 249
- y: imagePoints[i].y};
- // 250
- }
- // 251
- // 252
- for (i = 0; i < 3; ++ i){
- // 253
- for (j = 0; j < 3; ++ j){
- // 254
- rotation[i][j] = posRotation[i][j];
- // 255
- }
- // 256
- translation[i] = posTranslation[i];
- // 257
- }
- // 258
- // 259
- for (i = 0; i < np; ++ i){
- // 260
- factor = 0.0;
- // 261
- for (j = 0; j < 3; ++ j){
- // 262
- factor += this.objectVectors[i][j] * rotation[2][j] / translation[2];
- // 263
- }
- // 264
- sopImagePoints[i] = {x: (1.0 + factor) * imagePoints[i].x,
- // 265
- y: (1.0 + factor) * imagePoints[i].y};
- // 266
- }
- // 267
- // 268
- imageDifference = 0.0;
- // 269
- // 270
- for (i = 0; i < np; ++ i){
- // 271
- imageDifference += Math.abs(sopImagePoints[i].x - oldSopImagePoints[i].x);
- // 272
- imageDifference += Math.abs(sopImagePoints[i].y - oldSopImagePoints[i].y);
- // 273
- }
- // 274
- // 275
- for (i = 0; i < 3; ++ i){
- // 276
- translation1[i] = translation[i] -
- // 277
- (rotation[i][0] * this.objectPoints[0][0] +
- // 278
- rotation[i][1] * this.objectPoints[0][1] +
- // 279
- rotation[i][2] * this.objectPoints[0][2]);
- // 280
- }
- // 281
- // 282
- error1 = this.error(imagePoints, rotation, translation1);
- // 283
- // 284
- //Convergence
- // 285
- converged = (0.0 === error1.pixels) || (imageDifference < 0.01);
- // 286
- // 287
- while( iteration ++ < 100 && !converged ){
- // 288
- // 289
- for (i = 0; i < np; ++ i){
- // 290
- oldSopImagePoints[i].x = sopImagePoints[i].x;
- // 291
- oldSopImagePoints[i].y = sopImagePoints[i].y;
- // 292
- }
- // 293
- // 294
- this.pos(sopImagePoints, rotation1, rotation2, translation);
- // 295
- // 296
- for (i = 0; i < 3; ++ i){
- // 297
- translation1[i] = translation[i] -
- // 298
- (rotation1[i][0] * this.objectPoints[0][0] +
- // 299
- rotation1[i][1] * this.objectPoints[0][1] +
- // 300
- rotation1[i][2] * this.objectPoints[0][2]);
- // 301
- // 302
- translation2[i] = translation[i] -
- // 303
- (rotation2[i][0] * this.objectPoints[0][0] +
- // 304
- rotation2[i][1] * this.objectPoints[0][1] +
- // 305
- rotation2[i][2] * this.objectPoints[0][2]);
- // 306
- }
- // 307
- // 308
- error1 = this.error(imagePoints, rotation1, translation1);
- // 309
- error2 = this.error(imagePoints, rotation2, translation2);
- // 310
- // 311
- if ( (error1.euclidean >= 0.0) && (error2.euclidean >= 0.0) ){
- // 312
- if (error2.euclidean < error1.euclidean){
- // 313
- error = error2;
- // 314
- for (i = 0; i < 3; ++ i){
- // 315
- for (j = 0; j < 3; ++ j){
- // 316
- rotation[i][j] = rotation2[i][j];
- // 317
- }
- // 318
- }
- // 319
- }else{
- // 320
- error = error1;
- // 321
- for (i = 0; i < 3; ++ i){
- // 322
- for (j = 0; j < 3; ++ j){
- // 323
- rotation[i][j] = rotation1[i][j];
- // 324
- }
- // 325
- }
- // 326
- }
- // 327
- }
- // 328
- // 329
- if ( (error1.euclidean < 0.0) && (error2.euclidean >= 0.0) ){
- // 330
- error = error2;
- // 331
- for (i = 0; i < 3; ++ i){
- // 332
- for (j = 0; j < 3; ++ j){
- // 333
- rotation[i][j] = rotation2[i][j];
- // 334
- }
- // 335
- }
- // 336
- }
- // 337
- // 338
- if ( (error2.euclidean < 0.0) && (error1.euclidean >= 0.0) ){
- // 339
- error = error1;
- // 340
- for (i = 0; i < 3; ++ i){
- // 341
- for (j = 0; j < 3; ++ j){
- // 342
- rotation[i][j] = rotation1[i][j];
- // 343
- }
- // 344
- }
- // 345
- }
- // 346
- // 347
- for (i = 0; i < np; ++ i){
- // 348
- factor = 0.0;
- // 349
- for (j = 0; j < 3; ++ j){
- // 350
- factor += this.objectVectors[i][j] * rotation[2][j] / translation[2];
- // 351
- }
- // 352
- sopImagePoints[i].x = (1.0 + factor) * imagePoints[i].x;
- // 353
- sopImagePoints[i].y = (1.0 + factor) * imagePoints[i].y;
- // 354
- }
- // 355
- // 356
- oldImageDifference = imageDifference;
- // 357
- imageDifference = 0.0;
- // 358
- // 359
- for (i = 0; i < np; ++ i){
- // 360
- imageDifference += Math.abs(sopImagePoints[i].x - oldSopImagePoints[i].x);
- // 361
- imageDifference += Math.abs(sopImagePoints[i].y - oldSopImagePoints[i].y);
- // 362
- }
- // 363
- // 364
- delta = Math.abs(imageDifference - oldImageDifference);
- // 365
- // 366
- converged = (0.0 === error.pixels) || (delta < 0.01);
- // 367
- }
- // 368
- // 369
- return error;
- // 370
- };
- // 371
- // 372
- POS.Posit.prototype.error = function(imagePoints, rotation, translation){
- // 373
- var np = this.objectPoints.length,
- // 374
- move = [], projection = [], errorvec = [],
- // 375
- euclidean = 0.0, pixels = 0.0, maximum = 0.0,
- // 376
- i, j, k;
- // 377
- // 378
- if ( !this.isValid(rotation, translation) ){
- // 379
- return {euclidean: -1.0, pixels: -1, maximum: -1.0};
- // 380
- }
- // 381
- // 382
- for (i = 0; i < np; ++ i){
- // 383
- move[i] = [];
- // 384
- for (j = 0; j < 3; ++ j){
- // 385
- move[i][j] = translation[j];
- // 386
- }
- // 387
- }
- // 388
- // 389
- for (i = 0; i < np; ++ i){
- // 390
- for (j = 0; j < 3; ++ j){
- // 391
- for (k = 0; k < 3; ++ k){
- // 392
- move[i][j] += rotation[j][k] * this.objectPoints[i][k];
- // 393
- }
- // 394
- }
- // 395
- }
- // 396
- // 397
- for (i = 0; i < np; ++ i){
- // 398
- projection[i] = [];
- // 399
- for (j = 0; j < 2; ++ j){
- // 400
- projection[i][j] = this.focalLength * move[i][j] / move[i][2];
- // 401
- }
- // 402
- }
- // 403
- // 404
- for (i = 0; i < np; ++ i){
- // 405
- errorvec[i] = [projection[i][0] - imagePoints[i].x,
- // 406
- projection[i][1] - imagePoints[i].y];
- // 407
- }
- // 408
- // 409
- for (i = 0; i < np; ++ i){
- // 410
- euclidean += Math.sqrt(errorvec[i][0] * errorvec[i][0] +
- // 411
- errorvec[i][1] * errorvec[i][1]);
- // 412
- // 413
- pixels += Math.abs( Math.round(projection[i][0]) - Math.round(imagePoints[i].x) ) +
- // 414
- Math.abs( Math.round(projection[i][1]) - Math.round(imagePoints[i].y) );
- // 415
- // 416
- if (Math.abs(errorvec[i][0]) > maximum){
- // 417
- maximum = Math.abs(errorvec[i][0]);
- // 418
- }
- // 419
- if (Math.abs(errorvec[i][1]) > maximum){
- // 420
- maximum = Math.abs(errorvec[i][1]);
- // 421
- }
- // 422
- }
- // 423
- // 424
- return {euclidean: euclidean / np, pixels: pixels, maximum: maximum};
- // 425
- };
- // 426
- // 427
- POS.pseudoInverse = function(a, n, b){
- // 428
- var w = [], v = [[],[],[]], s = [[],[],[]],
- // 429
- wmax = 0.0, cn = 0,
- // 430
- i, j, k;
- // 431
- // 432
- SVD.svdcmp(a, n, 3, w, v);
- // 433
- // 434
- for (i = 0; i < 3; ++ i){
- // 435
- if (w[i] > wmax){
- // 436
- wmax = w[i];
- // 437
- }
- // 438
- }
- // 439
- // 440
- wmax *= 0.01;
- // 441
- // 442
- for (i = 0; i < 3; ++ i){
- // 443
- if (w[i] < wmax){
- // 444
- w[i] = 0.0;
- // 445
- }
- // 446
- }
- // 447
- // 448
- for (j = 0; j < 3; ++ j){
- // 449
- if (0.0 === w[j]){
- // 450
- ++ cn;
- // 451
- for (k = j; k < 2; ++ k){
- // 452
- for (i = 0; i < n; ++ i){
- // 453
- a[i][k] = a[i][k + 1];
- // 454
- }
- // 455
- for (i = 0; i < 3; ++ i){
- // 456
- v[i][k] = v[i][k + 1];
- // 457
- }
- // 458
- }
- // 459
- }
- // 460
- }
- // 461
- // 462
- for (j = 0; j < 2; ++ j){
- // 463
- if (0.0 === w[j]){
- // 464
- w[j] = w[j + 1];
- // 465
- }
- // 466
- }
- // 467
- // 468
- for (i = 0; i < 3; ++ i){
- // 469
- for (j = 0; j < 3 - cn; ++ j){
- // 470
- s[i][j] = v[i][j] / w[j];
- // 471
- }
- // 472
- }
- // 473
- // 474
- for (i = 0; i < 3; ++ i){
- // 475
- for (j = 0; j < n; ++ j){
- // 476
- b[i][j] = 0.0;
- // 477
- for (k = 0; k < 3 - cn; ++ k){
- // 478
- b[i][j] += s[i][k] * a[j][k];
- // 479
- }
- // 480
- }
- // 481
- }
- // 482
- };
- // 483
- // 484
- POS.Pose = function(error1, rotation1, translation1, error2, rotation2, translation2){
- // 485
- this.bestError = error1;
- // 486
- this.bestRotation = rotation1;
- // 487
- this.bestTranslation = translation1;
- // 488
- this.alternativeError = error2;
- // 489
- this.alternativeRotation = rotation2;
- // 490
- this.alternativeTranslation = translation2;
- // 491
- };
- // 492
- // 493
- ////////////////////////////////////////////////////////////////////////////////////////////////////////////
- }).call(this);
- (function () {
- ////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // //
- // packages/js-aruco/posit2.js //
- // //
- ////////////////////////////////////////////////////////////////////////////////////////////////////////////
- //
- /*
- // 1
- Copyright (c) 2012 Juan Mellado
- // 2
- // 3
- Permission is hereby granted, free of charge, to any person obtaining a copy
- // 4
- of this software and associated documentation files (the "Software"), to deal
- // 5
- in the Software without restriction, including without limitation the rights
- // 6
- to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- // 7
- copies of the Software, and to permit persons to whom the Software is
- // 8
- furnished to do so, subject to the following conditions:
- // 9
- // 10
- The above copyright notice and this permission notice shall be included in
- // 11
- all copies or substantial portions of the Software.
- // 12
- // 13
- THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- // 14
- IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- // 15
- FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- // 16
- AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- // 17
- LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- // 18
- OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- // 19
- THE SOFTWARE.
- // 20
- */
- // 21
- // 22
- /*
- // 23
- References:
- // 24
- - "3D Pose Estimation"
- // 25
- Andrew Kirillow
- // 26
- http://www.aforgenet.com/articles/posit/
- // 27
- */
- // 28
- // 29
- var POS = POS || {};
- // 30
- // 31
- POS.Posit = function(modelSize, focalLength){
- // 32
- this.model = this.buildModel(modelSize);
- // 33
- this.focalLength = focalLength;
- // 34
- // 35
- this.init();
- // 36
- };
- // 37
- // 38
- POS.Posit.prototype.buildModel = function(modelSize){
- // 39
- var half = modelSize / 2.0;
- // 40
- // 41
- return [
- // 42
- new Vec3(-half, half, 0.0),
- // 43
- new Vec3( half, half, 0.0),
- // 44
- new Vec3( half, -half, 0.0),
- // 45
- new Vec3(-half, -half, 0.0) ];
- // 46
- };
- // 47
- // 48
- POS.Posit.prototype.init = function(){
- // 49
- var d = new Vec3(), v = new Mat3(), u;
- // 50
- // 51
- this.modelVectors = Mat3.fromRows(
- // 52
- Vec3.sub(this.model[1], this.model[0]),
- // 53
- Vec3.sub(this.model[2], this.model[0]),
- // 54
- Vec3.sub(this.model[3], this.model[0]) );
- // 55
- // 56
- u = Mat3.clone(this.modelVectors);
- // 57
- // 58
- SVD.svdcmp(u.m, 3, 3, d.v, v.m);
- // 59
- // 60
- this.modelPseudoInverse = Mat3.mult(
- // 61
- Mat3.mult(v, Mat3.fromDiagonal( Vec3.inverse(d) ) ), Mat3.transpose(u) );
- // 62
- // 63
- this.modelNormal = v.column( d.minIndex() );
- // 64
- };
- // 65
- // 66
- POS.Posit.prototype.pose = function(points){
- // 67
- var eps = new Vec3(1.0, 1.0, 1.0),
- // 68
- rotation1 = new Mat3(), rotation2 = new Mat3(),
- // 69
- translation1 = new Vec3(), translation2 = new Vec3(),
- // 70
- error1, error2;
- // 71
- // 72
- this.pos(points, eps, rotation1, rotation2, translation1, translation2);
- // 73
- // 74
- error1 = this.iterate(points, rotation1, translation1);
- // 75
- error2 = this.iterate(points, rotation2, translation2);
- // 76
- // 77
- return error1 < error2?
- // 78
- new POS.Pose(error1, rotation1.m, translation1.v, error2, rotation2.m, translation2.v):
- // 79
- new POS.Pose(error2, rotation2.m, translation2.v, error1, rotation1.m, translation1.v);
- // 80
- };
- // 81
- // 82
- POS.Posit.prototype.pos = function(points, eps, rotation1, rotation2, translation1, translation2){
- // 83
- var xi = new Vec3(points[1].x, points[2].x, points[3].x),
- // 84
- yi = new Vec3(points[1].y, points[2].y, points[3].y),
- // 85
- xs = Vec3.addScalar( Vec3.mult(xi, eps), -points[0].x),
- // 86
- ys = Vec3.addScalar( Vec3.mult(yi, eps), -points[0].y),
- // 87
- i0 = Mat3.multVector(this.modelPseudoInverse, xs),
- // 88
- j0 = Mat3.multVector(this.modelPseudoInverse, ys),
- // 89
- s = j0.square() - i0.square(),
- // 90
- ij = Vec3.dot(i0, j0),
- // 91
- r = 0.0, theta = 0.0,
- // 92
- i, j, k, inorm, jnorm, scale, temp, lambda, mu;
- // 93
- // 94
- if (0.0 === s){
- // 95
- r = Math.sqrt( Math.abs(2.0 * ij) );
- // 96
- theta = (-Math.PI / 2.0) * (ij < 0.0? -1: (ij > 0.0? 1.0: 0.0) );
- // 97
- }else{
- // 98
- r = Math.sqrt( Math.sqrt(s * s + 4.0 * ij * ij) );
- // 99
- theta = Math.atan(-2.0 * ij / s);
- // 100
- if (s < 0.0){
- // 101
- theta += Math.PI;
- // 102
- }
- // 103
- theta /= 2.0;
- // 104
- }
- // 105
- // 106
- lambda = r * Math.cos(theta);
- // 107
- mu = r * Math.sin(theta);
- // 108
- // 109
- //First possible rotation/translation
- // 110
- i = Vec3.add(i0, Vec3.multScalar(this.modelNormal, lambda) );
- // 111
- j = Vec3.add(j0, Vec3.multScalar(this.modelNormal, mu) );
- // 112
- inorm = i.normalize();
- // 113
- jnorm = j.normalize();
- // 114
- k = Vec3.cross(i, j);
- // 115
- rotation1.copy( Mat3.fromRows(i, j, k) );
- // 116
- // 117
- scale = (inorm + jnorm) / 2.0;
- // 118
- temp = Mat3.multVector(rotation1, this.model[0]);
- // 119
- translation1.v = [
- // 120
- points[0].x / scale - temp.v[0],
- // 121
- points[0].y / scale - temp.v[1],
- // 122
- this.focalLength / scale];
- // 123
- // 124
- //Second possible rotation/translation
- // 125
- i = Vec3.sub(i0, Vec3.multScalar(this.modelNormal, lambda) );
- // 126
- j = Vec3.sub(j0, Vec3.multScalar(this.modelNormal, mu) );
- // 127
- inorm = i.normalize();
- // 128
- jnorm = j.normalize();
- // 129
- k = Vec3.cross(i, j);
- // 130
- rotation2.copy( Mat3.fromRows(i, j, k) );
- // 131
- // 132
- scale = (inorm + jnorm) / 2.0;
- // 133
- temp = Mat3.multVector(rotation2, this.model[0]);
- // 134
- translation2.v = [
- // 135
- points[0].x / scale - temp.v[0],
- // 136
- points[0].y / scale - temp.v[1],
- // 137
- this.focalLength / scale];
- // 138
- };
- // 139
- // 140
- POS.Posit.prototype.iterate = function(points, rotation, translation){
- // 141
- var prevError = Infinity,
- // 142
- rotation1 = new Mat3(), rotation2 = new Mat3(),
- // 143
- translation1 = new Vec3(), translation2 = new Vec3(),
- // 144
- i = 0, eps, error, error1, error2;
- // 145
- // 146
- for (; i < 100; ++ i){
- // 147
- eps = Vec3.addScalar( Vec3.multScalar(
- // 148
- Mat3.multVector( this.modelVectors, rotation.row(2) ), 1.0 / translation.v[2]), 1.0);
- // 149
- // 150
- this.pos(points, eps, rotation1, rotation2, translation1, translation2);
- // 151
- // 152
- error1 = this.getError(points, rotation1, translation1);
- // 153
- error2 = this.getError(points, rotation2, translation2);
- // 154
- // 155
- if (error1 < error2){
- // 156
- rotation.copy(rotation1);
- // 157
- translation.copy(translation1);
- // 158
- error = error1;
- // 159
- }else{
- // 160
- rotation.copy(rotation2);
- // 161
- translation.copy(translation2);
- // 162
- error = error2;
- // 163
- }
- // 164
- // 165
- if ( (error <= 2.0) || (error > prevError) ){
- // 166
- break;
- // 167
- }
- // 168
- // 169
- prevError = error;
- // 170
- }
- // 171
- // 172
- return error;
- // 173
- };
- // 174
- // 175
- POS.Posit.prototype.getError = function(points, rotation, translation){
- // 176
- var v1 = Vec3.add( Mat3.multVector(rotation, this.model[0]), translation),
- // 177
- v2 = Vec3.add( Mat3.multVector(rotation, this.model[1]), translation),
- // 178
- v3 = Vec3.add( Mat3.multVector(rotation, this.model[2]), translation),
- // 179
- v4 = Vec3.add( Mat3.multVector(rotation, this.model[3]), translation),
- // 180
- modeled, ia1, ia2, ia3, ia4, ma1, ma2, ma3, ma4;
- // 181
- // 182
- v1 = v1.v; v2 = v2.v; v3 = v3.v; v4 = v4.v;
- // 183
- // 184
- v1[0] *= this.focalLength / v1[2];
- // 185
- v1[1] *= this.focalLength / v1[2];
- // 186
- v2[0] *= this.focalLength / v2[2];
- // 187
- v2[1] *= this.focalLength / v2[2];
- // 188
- v3[0] *= this.focalLength / v3[2];
- // 189
- v3[1] *= this.focalLength / v3[2];
- // 190
- v4[0] *= this.focalLength / v4[2];
- // 191
- v4[1] *= this.focalLength / v4[2];
- // 192
- // 193
- modeled = [
- // 194
- {x: v1[0], y: v1[1]},
- // 195
- {x: v2[0], y: v2[1]},
- // 196
- {x: v3[0], y: v3[1]},
- // 197
- {x: v4[0], y: v4[1]}
- // 198
- ];
- // 199
- // 200
- ia1 = this.angle( points[0], points[1], points[3] );
- // 201
- ia2 = this.angle( points[1], points[2], points[0] );
- // 202
- ia3 = this.angle( points[2], points[3], points[1] );
- // 203
- ia4 = this.angle( points[3], points[0], points[2] );
- // 204
- // 205
- ma1 = this.angle( modeled[0], modeled[1], modeled[3] );
- // 206
- ma2 = this.angle( modeled[1], modeled[2], modeled[0] );
- // 207
- ma3 = this.angle( modeled[2], modeled[3], modeled[1] );
- // 208
- ma4 = this.angle( modeled[3], modeled[0], modeled[2] );
- // 209
- // 210
- return ( Math.abs(ia1 - ma1) +
- // 211
- Math.abs(ia2 - ma2) +
- // 212
- Math.abs(ia3 - ma3) +
- // 213
- Math.abs(ia4 - ma4) ) / 4.0;
- // 214
- };
- // 215
- // 216
- POS.Posit.prototype.angle = function(a, b, c){
- // 217
- var x1 = b.x - a.x, y1 = b.y - a.y,
- // 218
- x2 = c.x - a.x, y2 = c.y - a.y;
- // 219
- // 220
- return Math.acos( (x1 * x2 + y1 * y2) /
- // 221
- (Math.sqrt(x1 * x1 + y1 * y1) * Math.sqrt(x2 * x2 + y2 * y2) ) ) * 180.0 / Math.PI;
- // 222
- };
- // 223
- // 224
- POS.Pose = function(error1, rotation1, translation1, error2, rotation2, translation2){
- // 225
- this.bestError = error1;
- // 226
- this.bestRotation = rotation1;
- // 227
- this.bestTranslation = translation1;
- // 228
- this.alternativeError = error2;
- // 229
- this.alternativeRotation = rotation2;
- // 230
- this.alternativeTranslation = translation2;
- // 231
- };
- // 232
- // 233
- var Vec3 = function(x, y, z){
- // 234
- this.v = [x || 0.0, y || 0.0, z || 0.0];
- // 235
- };
- // 236
- // 237
- Vec3.prototype.copy = function(a){
- // 238
- var v = this.v;
- // 239
- // 240
- a = a.v;
- // 241
- // 242
- v[0] = a[0];
- // 243
- v[1] = a[1];
- // 244
- v[2] = a[2];
- // 245
- // 246
- return this;
- // 247
- };
- // 248
- // 249
- Vec3.add = function(a, b){
- // 250
- var vector = new Vec3(), v = vector.v;
- // 251
- // 252
- a = a.v; b = b.v;
- // 253
- // 254
- v[0] = a[0] + b[0];
- // 255
- v[1] = a[1] + b[1];
- // 256
- v[2] = a[2] + b[2];
- // 257
- // 258
- return vector;
- // 259
- };
- // 260
- // 261
- Vec3.sub = function(a, b){
- // 262
- var vector = new Vec3(), v = vector.v;
- // 263
- // 264
- a = a.v; b = b.v;
- // 265
- // 266
- v[0] = a[0] - b[0];
- // 267
- v[1] = a[1] - b[1];
- // 268
- v[2] = a[2] - b[2];
- // 269
- // 270
- return vector;
- // 271
- };
- // 272
- // 273
- Vec3.mult = function(a, b){
- // 274
- var vector = new Vec3(), v = vector.v;
- // 275
- // 276
- a = a.v; b = b.v;
- // 277
- // 278
- v[0] = a[0] * b[0];
- // 279
- v[1] = a[1] * b[1];
- // 280
- v[2] = a[2] * b[2];
- // 281
- // 282
- return vector;
- // 283
- };
- // 284
- // 285
- Vec3.addScalar = function(a, b){
- // 286
- var vector = new Vec3(), v = vector.v;
- // 287
- // 288
- a = a.v;
- // 289
- // 290
- v[0] = a[0] + b;
- // 291
- v[1] = a[1] + b;
- // 292
- v[2] = a[2] + b;
- // 293
- // 294
- return vector;
- // 295
- };
- // 296
- // 297
- Vec3.multScalar = function(a, b){
- // 298
- var vector = new Vec3(), v = vector.v;
- // 299
- // 300
- a = a.v;
- // 301
- // 302
- v[0] = a[0] * b;
- // 303
- v[1] = a[1] * b;
- // 304
- v[2] = a[2] * b;
- // 305
- // 306
- return vector;
- // 307
- };
- // 308
- // 309
- Vec3.dot = function(a, b){
- // 310
- a = a.v; b = b.v;
- // 311
- // 312
- return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
- // 313
- };
- // 314
- // 315
- Vec3.cross = function(a, b){
- // 316
- a = a.v; b = b.v;
- // 317
- // 318
- return new Vec3(
- // 319
- a[1] * b[2] - a[2] * b[1],
- // 320
- a[2] * b[0] - a[0] * b[2],
- // 321
- a[0] * b[1] - a[1] * b[0]);
- // 322
- };
- // 323
- // 324
- Vec3.prototype.normalize = function(){
- // 325
- var v = this.v,
- // 326
- len = Math.sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
- // 327
- // 328
- if (len > 0.0){
- // 329
- v[0] /= len;
- // 330
- v[1] /= len;
- // 331
- v[2] /= len;
- // 332
- }
- // 333
- // 334
- return len;
- // 335
- };
- // 336
- // 337
- Vec3.inverse = function(a){
- // 338
- var vector = new Vec3(), v = vector.v;
- // 339
- // 340
- a = a.v;
- // 341
- // 342
- if (a[0] !== 0.0){
- // 343
- v[0] = 1.0 / a[0];
- // 344
- }
- // 345
- if (a[1] !== 0.0){
- // 346
- v[1] = 1.0 / a[1];
- // 347
- }
- // 348
- if (a[2] !== 0.0){
- // 349
- v[2] = 1.0 / a[2];
- // 350
- }
- // 351
- // 352
- return vector;
- // 353
- };
- // 354
- // 355
- Vec3.prototype.square = function(){
- // 356
- var v = this.v;
- // 357
- // 358
- return v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
- // 359
- };
- // 360
- // 361
- Vec3.prototype.minIndex = function(){
- // 362
- var v = this.v;
- // 363
- // 364
- return v[0] < v[1]? (v[0] < v[2]? 0: 2): (v[1] < v[2]? 1: 2);
- // 365
- };
- // 366
- // 367
- var Mat3 = function(){
- // 368
- this.m = [ [0.0, 0.0, 0.0],
- // 369
- [0.0, 0.0, 0.0],
- // 370
- [0.0, 0.0, 0.0] ];
- // 371
- };
- // 372
- // 373
- Mat3.clone = function(a){
- // 374
- var matrix = new Mat3(), m = matrix.m;
- // 375
- // 376
- a = a.m;
- // 377
- // 378
- m[0][0] = a[0][0];
- // 379
- m[0][1] = a[0][1];
- // 380
- m[0][2] = a[0][2];
- // 381
- m[1][0] = a[1][0];
- // 382
- m[1][1] = a[1][1];
- // 383
- m[1][2] = a[1][2];
- // 384
- m[2][0] = a[2][0];
- // 385
- m[2][1] = a[2][1];
- // 386
- m[2][2] = a[2][2];
- // 387
- // 388
- return matrix;
- // 389
- };
- // 390
- // 391
- Mat3.prototype.copy = function(a){
- // 392
- var m = this.m;
- // 393
- // 394
- a = a.m;
- // 395
- // 396
- m[0][0] = a[0][0];
- // 397
- m[0][1] = a[0][1];
- // 398
- m[0][2] = a[0][2];
- // 399
- m[1][0] = a[1][0];
- // 400
- m[1][1] = a[1][1];
- // 401
- m[1][2] = a[1][2];
- // 402
- m[2][0] = a[2][0];
- // 403
- m[2][1] = a[2][1];
- // 404
- m[2][2] = a[2][2];
- // 405
- // 406
- return this;
- // 407
- };
- // 408
- // 409
- Mat3.fromRows = function(a, b, c){
- // 410
- var matrix = new Mat3(), m = matrix.m;
- // 411
- // 412
- a = a.v; b = b.v; c = c.v;
- // 413
- // 414
- m[0][0] = a[0];
- // 415
- m[0][1] = a[1];
- // 416
- m[0][2] = a[2];
- // 417
- m[1][0] = b[0];
- // 418
- m[1][1] = b[1];
- // 419
- m[1][2] = b[2];
- // 420
- m[2][0] = c[0];
- // 421
- m[2][1] = c[1];
- // 422
- m[2][2] = c[2];
- // 423
- // 424
- return matrix;
- // 425
- };
- // 426
- // 427
- Mat3.fromDiagonal = function(a){
- // 428
- var matrix = new Mat3(), m = matrix.m;
- // 429
- // 430
- a = a.v;
- // 431
- // 432
- m[0][0] = a[0];
- // 433
- m[1][1] = a[1];
- // 434
- m[2][2] = a[2];
- // 435
- // 436
- return matrix;
- // 437
- };
- // 438
- // 439
- Mat3.transpose = function(a){
- // 440
- var matrix = new Mat3(), m = matrix.m;
- // 441
- // 442
- a = a.m;
- // 443
- // 444
- m[0][0] = a[0][0];
- // 445
- m[0][1] = a[1][0];
- // 446
- m[0][2] = a[2][0];
- // 447
- m[1][0] = a[0][1];
- // 448
- m[1][1] = a[1][1];
- // 449
- m[1][2] = a[2][1];
- // 450
- m[2][0] = a[0][2];
- // 451
- m[2][1] = a[1][2];
- // 452
- m[2][2] = a[2][2];
- // 453
- // 454
- return matrix;
- // 455
- };
- // 456
- // 457
- Mat3.mult = function(a, b){
- // 458
- var matrix = new Mat3(), m = matrix.m;
- // 459
- // 460
- a = a.m; b = b.m;
- // 461
- // 462
- m[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0] + a[0][2] * b[2][0];
- // 463
- m[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1] + a[0][2] * b[2][1];
- // 464
- m[0][2] = a[0][0] * b[0][2] + a[0][1] * b[1][2] + a[0][2] * b[2][2];
- // 465
- m[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0] + a[1][2] * b[2][0];
- // 466
- m[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1] + a[1][2] * b[2][1];
- // 467
- m[1][2] = a[1][0] * b[0][2] + a[1][1] * b[1][2] + a[1][2] * b[2][2];
- // 468
- m[2][0] = a[2][0] * b[0][0] + a[2][1] * b[1][0] + a[2][2] * b[2][0];
- // 469
- m[2][1] = a[2][0] * b[0][1] + a[2][1] * b[1][1] + a[2][2] * b[2][1];
- // 470
- m[2][2] = a[2][0] * b[0][2] + a[2][1] * b[1][2] + a[2][2] * b[2][2];
- // 471
- // 472
- return matrix;
- // 473
- };
- // 474
- // 475
- Mat3.multVector = function(m, a){
- // 476
- m = m.m; a = a.v;
- // 477
- // 478
- return new Vec3(
- // 479
- m[0][0] * a[0] + m[0][1] * a[1] + m[0][2] * a[2],
- // 480
- m[1][0] * a[0] + m[1][1] * a[1] + m[1][2] * a[2],
- // 481
- m[2][0] * a[0] + m[2][1] * a[1] + m[2][2] * a[2]);
- // 482
- };
- // 483
- // 484
- Mat3.prototype.column = function(index){
- // 485
- var m = this.m;
- // 486
- // 487
- return new Vec3( m[0][index], m[1][index], m[2][index] );
- // 488
- };
- // 489
- // 490
- Mat3.prototype.row = function(index){
- // 491
- var m = this.m;
- // 492
- // 493
- return new Vec3( m[index][0], m[index][1], m[index][2] );
- // 494
- };
- // 495
- // 496
- ////////////////////////////////////////////////////////////////////////////////////////////////////////////
- }).call(this);
- (function () {
- ////////////////////////////////////////////////////////////////////////////////////////////////////////////
- // //
- // packages/js-aruco/svd.js //
- // //
- ////////////////////////////////////////////////////////////////////////////////////////////////////////////
- //
- /*
- // 1
- Copyright (c) 2012 Juan Mellado
- // 2
- // 3
- Permission is hereby granted, free of charge, to any person obtaining a copy
- // 4
- of this software and associated documentation files (the "Software"), to deal
- // 5
- in the Software without restriction, including without limitation the rights
- // 6
- to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- // 7
- copies of the Software, and to permit persons to whom the Software is
- // 8
- furnished to do so, subject to the following conditions:
- // 9
- // 10
- The above copyright notice and this permission notice shall be included in
- // 11
- all copies or substantial portions of the Software.
- // 12
- // 13
- THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- // 14
- IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- // 15
- FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- // 16
- AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- // 17
- LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- // 18
- OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- // 19
- THE SOFTWARE.
- // 20
- */
- // 21
- // 22
- /*
- // 23
- References:
- // 24
- - "Numerical Recipes in C - Second Edition"
- // 25
- http://www.nr.com/
- // 26
- */
- // 27
- // 28
- var SVD = SVD || {};
- // 29
- // 30
- SVD.svdcmp = function(a, m, n, w, v){
- // 31
- var flag, i, its, j, jj, k, l, nm,
- // 32
- anorm = 0.0, c, f, g = 0.0, h, s, scale = 0.0, x, y, z, rv1 = [];
- // 33
- // 34
- //Householder reduction to bidiagonal form
- // 35
- for (i = 0; i < n; ++ i){
- // 36
- l = i + 1;
- // 37
- rv1[i] = scale * g;
- // 38
- g = s = scale = 0.0;
- // 39
- if (i < m){
- // 40
- for (k = i; k < m; ++ k){
- // 41
- scale += Math.abs( a[k][i] );
- // 42
- }
- // 43
- if (0.0 !== scale){
- // 44
- for (k = i; k < m; ++ k){
- // 45
- a[k][i] /= scale;
- // 46
- s += a[k][i] * a[k][i];
- // 47
- }
- // 48
- f = a[i][i];
- // 49
- g = -SVD.sign( Math.sqrt(s), f );
- // 50
- h = f * g - s;
- // 51
- a[i][i] = f - g;
- // 52
- for (j = l; j < n; ++ j){
- // 53
- for (s = 0.0, k = i; k < m; ++ k){
- // 54
- s += a[k][i] * a[k][j];
- // 55
- }
- // 56
- f = s / h;
- // 57
- for (k = i; k < m; ++ k){
- // 58
- a[k][j] += f * a[k][i];
- // 59
- }
- // 60
- }
- // 61
- for (k = i; k < m; ++ k){
- // 62
- a[k][i] *= scale;
- // 63
- }
- // 64
- }
- // 65
- }
- // 66
- w[i] = scale * g;
- // 67
- g = s = scale = 0.0;
- // 68
- if ( (i < m) && (i !== n - 1) ){
- // 69
- for (k = l; k < n; ++ k){
- // 70
- scale += Math.abs( a[i][k] );
- // 71
- }
- // 72
- if (0.0 !== scale){
- // 73
- for (k = l; k < n; ++ k){
- // 74
- a[i][k] /= scale;
- // 75
- s += a[i][k] * a[i][k];
- // 76
- }
- // 77
- f = a[i][l];
- // 78
- g = -SVD.sign( Math.sqrt(s), f );
- // 79
- h = f * g - s;
- // 80
- a[i][l] = f - g;
- // 81
- for (k = l; k < n; ++ k){
- // 82
- rv1[k] = a[i][k] / h;
- // 83
- }
- // 84
- for (j = l; j < m; ++ j){
- // 85
- for (s = 0.0, k = l; k < n; ++ k){
- // 86
- s += a[j][k] * a[i][k];
- // 87
- }
- // 88
- for (k = l; k < n; ++ k){
- // 89
- a[j][k] += s * rv1[k];
- // 90
- }
- // 91
- }
- // 92
- for (k = l; k < n; ++ k){
- // 93
- a[i][k] *= scale;
- // 94
- }
- // 95
- }
- // 96
- }
- // 97
- anorm = Math.max(anorm, ( Math.abs( w[i] ) + Math.abs( rv1[i] ) ) );
- // 98
- }
- // 99
- // 100
- //Acumulation of right-hand transformation
- // 101
- for (i = n - 1; i >= 0; -- i){
- // 102
- if (i < n - 1){
- // 103
- if (0.0 !== g){
- // 104
- for (j = l; j < n; ++ j){
- // 105
- v[j][i] = ( a[i][j] / a[i][l] ) / g;
- // 106
- }
- // 107
- for (j = l; j < n; ++ j){
- // 108
- for (s = 0.0, k = l; k < n; ++ k){
- // 109
- s += a[i][k] * v[k][j];
- // 110
- }
- // 111
- for (k = l; k < n; ++ k){
- // 112
- v[k][j] += s * v[k][i];
- // 113
- }
- // 114
- }
- // 115
- }
- // 116
- for (j = l; j < n; ++ j){
- // 117
- v[i][j] = v[j][i] = 0.0;
- // 118
- }
- // 119
- }
- // 120
- v[i][i] = 1.0;
- // 121
- g = rv1[i];
- // 122
- l = i;
- // 123
- }
- // 124
- // 125
- //Acumulation of left-hand transformation
- // 126
- for (i = Math.min(n, m) - 1; i >= 0; -- i){
- // 127
- l = i + 1;
- // 128
- g = w[i];
- // 129
- for (j = l; j < n; ++ j){
- // 130
- a[i][j] = 0.0;
- // 131
- }
- // 132
- if (0.0 !== g){
- // 133
- g = 1.0 / g;
- // 134
- for (j = l; j < n; ++ j){
- // 135
- for (s = 0.0, k = l; k < m; ++ k){
- // 136
- s += a[k][i] * a[k][j];
- // 137
- }
- // 138
- f = (s / a[i][i]) * g;
- // 139
- for (k = i; k < m; ++ k){
- // 140
- a[k][j] += f * a[k][i];
- // 141
- }
- // 142
- }
- // 143
- for (j = i; j < m; ++ j){
- // 144
- a[j][i] *= g;
- // 145
- }
- // 146
- }else{
- // 147
- for (j = i; j < m; ++ j){
- // 148
- a[j][i] = 0.0;
- // 149
- }
- // 150
- }
- // 151
- ++ a[i][i];
- // 152
- }
- // 153
- // 154
- //Diagonalization of the bidiagonal form
- // 155
- for (k = n - 1; k >= 0; -- k){
- // 156
- for (its = 1; its <= 30; ++ its){
- // 157
- flag = true;
- // 158
- for (l = k; l >= 0; -- l){
- // 159
- nm = l - 1;
- // 160
- if ( Math.abs( rv1[l] ) + anorm === anorm ){
- // 161
- flag = false;
- // 162
- break;
- // 163
- }
- // 164
- if ( Math.abs( w[nm] ) + anorm === anorm ){
- // 165
- break;
- // 166
- }
- // 167
- }
- // 168
- if (flag){
- // 169
- c = 0.0;
- // 170
- s = 1.0;
- // 171
- for (i = l; i <= k; ++ i){
- // 172
- f = s * rv1[i];
- // 173
- if ( Math.abs(f) + anorm === anorm ){
- // 174
- break;
- // 175
- }
- // 176
- g = w[i];
- // 177
- h = SVD.pythag(f, g);
- // 178
- w[i] = h;
- // 179
- h = 1.0 / h;
- // 180
- c = g * h;
- // 181
- s = -f * h;
- // 182
- for (j = 1; j <= m; ++ j){
- // 183
- y = a[j][nm];
- // 184
- z = a[j][i];
- // 185
- a[j][nm] = y * c + z * s;
- // 186
- a[j][i] = z * c - y * s;
- // 187
- }
- // 188
- }
- // 189
- }
- // 190
- // 191
- //Convergence
- // 192
- z = w[k];
- // 193
- if (l === k){
- // 194
- if (z < 0.0){
- // 195
- w[k] = -z;
- // 196
- for (j = 0; j < n; ++ j){
- // 197
- v[j][k] = -v[j][k];
- // 198
- }
- // 199
- }
- // 200
- break;
- // 201
- }
- // 202
- // 203
- if (30 === its){
- // 204
- return false;
- // 205
- }
- // 206
- // 207
- //Shift from bottom 2-by-2 minor
- // 208
- x = w[l];
- // 209
- nm = k - 1;
- // 210
- y = w[nm];
- // 211
- g = rv1[nm];
- // 212
- h = rv1[k];
- // 213
- f = ( (y - z) * (y + z) + (g - h) * (g + h) ) / (2.0 * h * y);
- // 214
- g = SVD.pythag( f, 1.0 );
- // 215
- f = ( (x - z) * (x + z) + h * ( (y / (f + SVD.sign(g, f) ) ) - h) ) / x;
- // 216
- // 217
- //Next QR transformation
- // 218
- c = s = 1.0;
- // 219
- for (j = l; j <= nm; ++ j){
- // 220
- i = j + 1;
- // 221
- g = rv1[i];
- // 222
- y = w[i];
- // 223
- h = s * g;
- // 224
- g = c * g;
- // 225
- z = SVD.pythag(f, h);
- // 226
- rv1[j] = z;
- // 227
- c = f / z;
- // 228
- s = h / z;
- // 229
- f = x * c + g * s;
- // 230
- g = g * c - x * s;
- // 231
- h = y * s;
- // 232
- y *= c;
- // 233
- for (jj = 0; jj < n; ++ jj){
- // 234
- x = v[jj][j];
- // 235
- z = v[jj][i];
- // 236
- v[jj][j] = x * c + z * s;
- // 237
- v[jj][i] = z * c - x * s;
- // 238
- }
- // 239
- z = SVD.pythag(f, h);
- // 240
- w[j] = z;
- // 241
- if (0.0 !== z){
- // 242
- z = 1.0 / z;
- // 243
- c = f * z;
- // 244
- s = h * z;
- // 245
- }
- // 246
- f = c * g + s * y;
- // 247
- x = c * y - s * g;
- // 248
- for (jj = 0; jj < m; ++ jj){
- // 249
- y = a[jj][j];
- // 250
- z = a[jj][i];
- // 251
- a[jj][j] = y * c + z * s;
- // 252
- a[jj][i] = z * c - y * s;
- // 253
- }
- // 254
- }
- // 255
- rv1[l] = 0.0;
- // 256
- rv1[k] = f;
- // 257
- w[k] = x;
- // 258
- }
- // 259
- }
- // 260
- // 261
- return true;
- // 262
- };
- // 263
- // 264
- SVD.pythag = function(a, b){
- // 265
- var at = Math.abs(a), bt = Math.abs(b), ct;
- // 266
- // 267
- if (at > bt){
- // 268
- ct = bt / at;
- // 269
- return at * Math.sqrt(1.0 + ct * ct);
- // 270
- }
- // 271
- // 272
- if (0.0 === bt){
- // 273
- return 0.0;
- // 274
- }
- // 275
- // 276
- ct = at / bt;
- // 277
- return bt * Math.sqrt(1.0 + ct * ct);
- // 278
- };
- // 279
- // 280
- SVD.sign = function(a, b){
- // 281
- return b >= 0.0? Math.abs(a): -Math.abs(a);
- // 282
- };
- // 283
- // 284
- ////////////////////////////////////////////////////////////////////////////////////////////////////////////
- }).call(this);
- /* Exports */
- if (typeof Package === 'undefined') Package = {};
- Package['js-aruco'] = {};
- })();
Add Comment
Please, Sign In to add comment