From 6d8ed03f2bc5ca2bf4a47b8de2dc331300b742d9 Mon Sep 17 00:00:00 2001 From: Julien Delnatte Date: Tue, 24 Apr 2018 13:08:02 +0200 Subject: [PATCH 1/3] allow using $INTENSITY variable in TOOL ON to get the laser intensity value --- src/lib/cam-gcode-laser-cut.js | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/lib/cam-gcode-laser-cut.js b/src/lib/cam-gcode-laser-cut.js index 58686a36..cddaeeaf 100644 --- a/src/lib/cam-gcode-laser-cut.js +++ b/src/lib/cam-gcode-laser-cut.js @@ -122,7 +122,8 @@ export function getLaserCutGcode(props) { gcode += 'G0 Z' + zHeight.toFixed(decimal) + '\r\n\r\n'; } - gcode += gcodeToolOn; + gcode += gcodeToolOn.split("$INTENSITY").join(laserOnS); + for (let i = 1; i < selectedPath.length; ++i) { if (i == 1 && gcodeLaserIntensitySeparateLine) gcode += laserOnS + '\n'; From aae179cd3991dd7d70ac39e10ee3e457834b9198 Mon Sep 17 00:00:00 2001 From: tibus Date: Wed, 25 Apr 2018 21:54:39 +0200 Subject: [PATCH 2/3] refactor gCode generation allowing marlin compatibility Create a GCode generator field for each machine. Default is for grbl. In cam-gcode-raster and cam-gcode-laser-cut, each gcode writing will pass inside the generator and will return the appropriate GCode. No modification in gCode writing with default generator but with the generator "marlin" this will change the way the laser will turn on and movements to be compatible with stock marlin on 3D printer or so. For tool on/off the "gCodeToolOn" will be insert at each Intensity change and you can use $INTENSITY variable inside gCodeToolOn to get back the intensity. By exemple, the GRBL gcode : G0 X10.0 S100 Will become on Marlin, with gCodeToolOn set to "M106 $INTENSITY" : M106 S100 G0 X10.0 In Raster mode, GRBL gcode G1 X10.0 Y20.0 S100 X11.0 Y20.0 S30 will become in marlin : M106 S100 G1 X10.0 Y20.0 M106 S30 G1 X11.0 Y20.0 --- src/components/settings.js | 3 +- src/lib/action2gcode/gcode-generator.js | 14 + .../generators/abstract-generator.js | 13 + .../generators/default-generator.js | 76 ++ .../generators/marlin-generator.js | 83 ++ src/lib/cam-gcode-laser-cut.js | 40 +- src/lib/cam-gcode-raster.js | 20 +- src/lib/js-aruco/aruco.js | 0 src/lib/js-aruco/cv.js | 0 src/lib/js-aruco/posit1.js | 990 ++++++++--------- src/lib/js-aruco/posit2.js | 994 +++++++++--------- src/lib/js-aruco/svd.js | 570 +++++----- src/lib/lw.raster2gcode/raster-to-gcode.js | 59 +- src/lib/workers/cam-lasercut.js | 7 +- src/reducers/settings.js | 1 + 15 files changed, 1551 insertions(+), 1319 deletions(-) create mode 100644 src/lib/action2gcode/gcode-generator.js create mode 100644 src/lib/action2gcode/generators/abstract-generator.js create mode 100644 src/lib/action2gcode/generators/default-generator.js create mode 100644 src/lib/action2gcode/generators/marlin-generator.js mode change 100755 => 100644 src/lib/js-aruco/aruco.js mode change 100755 => 100644 src/lib/js-aruco/cv.js mode change 100755 => 100644 src/lib/js-aruco/posit1.js mode change 100755 => 100644 src/lib/js-aruco/posit2.js mode change 100755 => 100644 src/lib/js-aruco/svd.js diff --git a/src/components/settings.js b/src/components/settings.js index fb67f335..4a440678 100644 --- a/src/components/settings.js +++ b/src/components/settings.js @@ -264,7 +264,8 @@ class Settings extends React.Component { - + + diff --git a/src/lib/action2gcode/gcode-generator.js b/src/lib/action2gcode/gcode-generator.js new file mode 100644 index 00000000..7d646932 --- /dev/null +++ b/src/lib/action2gcode/gcode-generator.js @@ -0,0 +1,14 @@ +'use strict'; + +import DefaultGenerator from "./generators/default-generator" +import MarlinGenerator from "./generators/marlin-generator" + +export function getGenerator(gcodeGenerator, settings) { + switch(gcodeGenerator){ + case "marlin" : + return new MarlinGenerator(settings); + case "default" : + default : + return new DefaultGenerator(settings); + } +} diff --git a/src/lib/action2gcode/generators/abstract-generator.js b/src/lib/action2gcode/generators/abstract-generator.js new file mode 100644 index 00000000..ea25066a --- /dev/null +++ b/src/lib/action2gcode/generators/abstract-generator.js @@ -0,0 +1,13 @@ + +// AbstractDriver class +class AbstractGenerator { + // Class constructor... + constructor(settings) { + this.settings = settings; + } + +} + +// Exports +export { AbstractGenerator } +export default AbstractGenerator diff --git a/src/lib/action2gcode/generators/default-generator.js b/src/lib/action2gcode/generators/default-generator.js new file mode 100644 index 00000000..fd76e584 --- /dev/null +++ b/src/lib/action2gcode/generators/default-generator.js @@ -0,0 +1,76 @@ + +import AbstractGenerator from "./abstract-generator" + +// AbstractDriver class +class DefaultGenerator extends AbstractGenerator{ + // Class constructor... + constructor(settings) { + super(settings); + } + + moveRapid(params, optimized=false){ + if(params == null) + return ""; + + let gcode = ""; + if(!optimized) gcode+="G0 "; + gcode += this.move(params); + return gcode; + } + + moveTool(params, optimized=false){ + if(params == null) + return ""; + + let gcode = ""; + if(!optimized) gcode+="G1 "; + gcode += this.move(params); + return gcode; + } + + toolOn(gcode, params){ + if(gcode == null) + return ""; + + if(params.hasOwnProperty("i")) + gcode = gcode.split("$INTENSITY").join(params.i); + return gcode; + } + + toolOff(gcode, params){ + if(gcode == null) + return ""; + + if(params.hasOwnProperty("i")) + gcode = gcode.split("$INTENSITY").join(params.i); + return gcode; + } + + move(params){ + let gcode = ""; + if(params.hasOwnProperty("x")) + gcode += ` X${params.x}`; + + if(params.hasOwnProperty("y")) + gcode += ` Y${params.y}`; + + if(params.hasOwnProperty("a")) + gcode += ` A${params.a}`; + + if(params.hasOwnProperty("i")) + gcode += ` ${params.i}`; + + if(params.hasOwnProperty("s")) + gcode += ` S${params.s}`; + + if(params.hasOwnProperty("f")) + gcode += ` F${params.f}`; + + return gcode.trim(); + } + +} + +// Exports +export { DefaultGenerator } +export default DefaultGenerator diff --git a/src/lib/action2gcode/generators/marlin-generator.js b/src/lib/action2gcode/generators/marlin-generator.js new file mode 100644 index 00000000..65871be6 --- /dev/null +++ b/src/lib/action2gcode/generators/marlin-generator.js @@ -0,0 +1,83 @@ + +import AbstractGenerator from "./abstract-generator" + +// AbstractDriver class +class MarlinGenerator extends AbstractGenerator{ + // Class constructor... + constructor(settings) { + super(settings); + } + + moveRapid(params, optimized=false){ + if(params == null) + return ""; + + return this.move("G0", params); + } + + moveTool(params, optimized=false){ + if(params == null) + return ""; + + return this.move("G1", params); + } + + toolOn(gcode, params){ + if(gcode == null) + return ""; + + if(params.hasOwnProperty("i")) + gcode = gcode.split("$INTENSITY").join(params.i); + return gcode; + } + + toolOff(gcode, params){ + if(gcode == null) + return ""; + + if(params.hasOwnProperty("i")) + gcode = gcode.split("$INTENSITY").join(params.i); + return gcode; + } + + move(prefix, params){ + let gcode = ""; + + if(params.hasOwnProperty("s")){ + if(this.settings.gcodeToolOn.indexOf("$INTENSITY") > -1){ + gcode += `${this.settings.gcodeToolOn.split("$INTENSITY").join(this.settings.gcodeLaserIntensity+params.s)}\r\n`; + }else{ + gcode += `${this.settings.gcodeToolOn} S${params.s}\r\n`; + } + } + + if(params.hasOwnProperty("i")){ + if(this.settings.gcodeToolOn.indexOf("$INTENSITY") > -1){ + gcode += `${this.settings.gcodeToolOn.split("$INTENSITY").join(params.i)}\r\n`; + }else{ + gcode += `${this.settings.gcodeToolOn} ${params.i}\r\n`; + } + } + + gcode += prefix; + + if(params.hasOwnProperty("x")) + gcode += ` X${params.x}`; + + if(params.hasOwnProperty("y")) + gcode += ` Y${params.y}`; + + if(params.hasOwnProperty("a")) + gcode += ` A${params.a}`; + + if(params.hasOwnProperty("f")) + gcode += ` F${params.f}`; + + return gcode.trim(); + } + +} + +// Exports +export { MarlinGenerator } +export default MarlinGenerator diff --git a/src/lib/cam-gcode-laser-cut.js b/src/lib/cam-gcode-laser-cut.js index cddaeeaf..a0526b24 100644 --- a/src/lib/cam-gcode-laser-cut.js +++ b/src/lib/cam-gcode-laser-cut.js @@ -17,6 +17,7 @@ import { dist, cut, fillPath, insideOutside, pocket, reduceCamPaths, separateTabs, vCarve } from './cam'; import { mmToClipperScale, offset, rawPathsToClipperPaths, union } from './mesh'; +import { getGenerator } from "./action2gcode/gcode-generator"; // Convert laser cut paths to gcode. // paths: Array of CamPath @@ -32,7 +33,7 @@ import { mmToClipperScale, offset, rawPathsToClipperPaths, union } from './mesh' // gcodeToolOff: Laser off (may be empty) // gcodeSMaxValue: Max S value export function getLaserCutGcode(props) { - let { paths, scale, offsetX, offsetY, decimal, cutFeed, laserPower, passes, + let { paths, generator, scale, offsetX, offsetY, decimal, cutFeed, laserPower, passes, useA, aAxisDiameter, tabGeometry, gcodeToolOn, gcodeToolOff, gcodeLaserIntensity, gcodeLaserIntensitySeparateLine, gcodeSMinValue, gcodeSMaxValue, @@ -59,7 +60,8 @@ export function getLaserCutGcode(props) { lastX = roundedX; lastY = adjustedY; lastA = roundedA; - return 'G0 X' + x.toFixed(decimal) + ' A' + a.toFixed(decimal); + //return 'G0 X' + x.toFixed(decimal) + ' A' + a.toFixed(decimal); + return {x: x.toFixed(decimal), a: a.toFixed(decimal)}; } else { let dx = roundedX - lastX, dy = adjustedY - lastY, da = roundedA - lastA; let travelTime = Math.sqrt(dx * dx + dy * dy) / cutFeed; @@ -69,17 +71,20 @@ export function getLaserCutGcode(props) { else if (da) f = Math.abs(da) / travelTime; else - return ''; + return null; + //return ''; lastX = roundedX; lastY = adjustedY; lastA = roundedA; - return 'G1 X' + x.toFixed(decimal) + ' A' + a.toFixed(decimal) + ' F' + f.toFixed(decimal); + return {x: x.toFixed(decimal), a: a.toFixed(decimal), f: f.toFixed(decimal)}; + //return 'G1 X' + x.toFixed(decimal) + ' A' + a.toFixed(decimal) + ' F' + f.toFixed(decimal); } } else { - if (rapid) + return {x: x.toFixed(decimal), y: y.toFixed(decimal)}; + /*if (rapid) return 'G0 X' + x.toFixed(decimal) + ' Y' + y.toFixed(decimal); else - return 'G1 X' + x.toFixed(decimal) + ' Y' + y.toFixed(decimal); + return 'G1 X' + x.toFixed(decimal) + ' Y' + y.toFixed(decimal);*/ } } @@ -113,7 +118,7 @@ export function getLaserCutGcode(props) { gcode += '; Skip tab\r\n'; continue; } - gcode += convertPoint(selectedPath[0], true) + '\r\n'; + gcode += generator.moveRapid(convertPoint(selectedPath[0], true)) + '\r\n'; if (useZ && !usedZposition) { usedZposition = true; @@ -122,22 +127,28 @@ export function getLaserCutGcode(props) { gcode += 'G0 Z' + zHeight.toFixed(decimal) + '\r\n\r\n'; } - gcode += gcodeToolOn.split("$INTENSITY").join(laserOnS); - + gcode += generator.toolOn(gcodeToolOn, {i:laserOnS}); + for (let i = 1; i < selectedPath.length; ++i) { if (i == 1 && gcodeLaserIntensitySeparateLine) gcode += laserOnS + '\n'; - gcode += convertPoint(selectedPath[i], false); + let action = convertPoint(selectedPath[i], false); + //gcode += convertPoint(selectedPath[i], false); if (i == 1 && !gcodeLaserIntensitySeparateLine) - gcode += ' ' + laserOnS; + action.i = laserOnS; + //gcode += ' ' + laserOnS; if (i == 1 && !useA) - gcode += ' F' + cutFeed; + action.f = cutFeed; + //gcode += ' F' + cutFeed; + gcode += generator.moveTool(action); gcode += '\r\n'; } - gcode += gcodeToolOff; + + gcode += generator.toolOff(gcodeToolOff, {i:laserOnS}); } } + if (useBlower) { if (useBlower.blowerOff) { gcode += `\r\n ${useBlower.blowerOff}; Disable Air assist\r\n`; @@ -230,7 +241,10 @@ export function getLaserCutGcodeFromOp(settings, opIndex, op, geometry, openGeom if (op.hookOperationStart.length) gcode += op.hookOperationStart; + let generator = getGenerator(settings.gcodeGenerator, settings); + gcode += getLaserCutGcode({ + generator: generator, paths: camPaths, scale: 1 / mmToClipperScale, offsetX: 0, diff --git a/src/lib/cam-gcode-raster.js b/src/lib/cam-gcode-raster.js index faecd1ed..fa860797 100644 --- a/src/lib/cam-gcode-raster.js +++ b/src/lib/cam-gcode-raster.js @@ -61,12 +61,14 @@ export function getLaserRasterGcodeFromOp(settings, opIndex, op, docsWithImages, // POSTPROCESS GCODE; const postProcessing = (gc) => { + let g = ''; let raster = ''; - + let firstMove = gc.find((line)=>{ return line.match(/^G[0-1]\s+[XYZ]/gi); }) + for (let line of gc) { if (op.useA) { line = line.replace(/Y(\s*-?[0-9\.]{1,})/gi, (str,float)=>{ @@ -98,7 +100,7 @@ export function getLaserRasterGcodeFromOp(settings, opIndex, op, docsWithImages, if (firstMove) { g+= `\r\n; First Move\r\n`; - g+= firstMove.replace(/^G[0-1]/gi,'G0').replace(/S[0\.]+/gi,''); + g+= firstMove.replace(/^G[0-1]/gi,'G0').replace(/S[0\.]+/gi,'')+'\r\n'; } if (settings.machineZEnabled) { @@ -107,8 +109,14 @@ export function getLaserRasterGcodeFromOp(settings, opIndex, op, docsWithImages, g += 'G0 Z' + zHeight.toFixed(settings.decimal || 3) + '\r\n'; } - if (settings.gcodeToolOn && settings.gcodeToolOn.length) - g += `${settings.gcodeToolOn} \r\n`; + if (settings.gcodeToolOn && settings.gcodeToolOn.length){ + if(settings.gcodeToolOn.indexOf("$INTENSITY") > -1){ + g += `${settings.gcodeToolOn.split("$INTENSITY").join(settings.gcodeLaserIntensity+settings.gcodeSMaxValue.toFixed(4))}\r\n`; + }else{ + g += `${settings.gcodeToolOn} \r\n`; + } + //g += `${settings.gcodeToolOn} \r\n`; + } g += raster; @@ -192,6 +200,10 @@ export function getLaserRasterGcodeFromOp(settings, opIndex, op, docsWithImages, verboseG: op.verboseGcode, diagonal: op.diagonal, overscan: op.overScan, + gcodeGenerator : settings.gcodeGenerator, + gcodeToolOn : settings.gcodeToolOn, + gcodeToolOff : settings.gcodeToolOff, + gcodeLaserIntensity: settings.gcodeLaserIntensity, nonBlocking: false, milling: false, filters: { diff --git a/src/lib/js-aruco/aruco.js b/src/lib/js-aruco/aruco.js old mode 100755 new mode 100644 diff --git a/src/lib/js-aruco/cv.js b/src/lib/js-aruco/cv.js old mode 100755 new mode 100644 diff --git a/src/lib/js-aruco/posit1.js b/src/lib/js-aruco/posit1.js old mode 100755 new mode 100644 index 8b83f9c5..634b5482 --- a/src/lib/js-aruco/posit1.js +++ b/src/lib/js-aruco/posit1.js @@ -1,496 +1,496 @@ -/* -Copyright (c) 2012 Juan Mellado - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. -*/ - -/* -References: -- "Iterative Pose Estimation using Coplanar Feature Points" - Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis - http://www.cfar.umd.edu/~daniel/daniel_papersfordownload/CoplanarPts.pdf -*/ - -import SVD from './svd.js'; - -var POS = POS || {}; - -POS.Posit = function(modelSize, focalLength){ - this.objectPoints = this.buildModel(modelSize); - this.focalLength = focalLength; - - this.objectVectors = []; - this.objectNormal = []; - this.objectMatrix = [[],[],[]]; - - this.init(); -}; - -POS.Posit.prototype.buildModel = function(modelSize){ - var half = modelSize / 2.0; - - return [ - [-half, half, 0.0], - [ half, half, 0.0], - [ half, -half, 0.0], - [-half, -half, 0.0] ]; -}; - -POS.Posit.prototype.init = function(){ - var np = this.objectPoints.length, - vectors = [], n = [], len = 0.0, row = 2, i; - - for (i = 0; i < np; ++ i){ - this.objectVectors[i] = [this.objectPoints[i][0] - this.objectPoints[0][0], - this.objectPoints[i][1] - this.objectPoints[0][1], - this.objectPoints[i][2] - this.objectPoints[0][2]]; - - vectors[i] = [this.objectVectors[i][0], - this.objectVectors[i][1], - this.objectVectors[i][2]]; - } - - while(0.0 === len){ - n[0] = this.objectVectors[1][1] * this.objectVectors[row][2] - - this.objectVectors[1][2] * this.objectVectors[row][1]; - n[1] = this.objectVectors[1][2] * this.objectVectors[row][0] - - this.objectVectors[1][0] * this.objectVectors[row][2]; - n[2] = this.objectVectors[1][0] * this.objectVectors[row][1] - - this.objectVectors[1][1] * this.objectVectors[row][0]; - - len = Math.sqrt(n[0] * n[0] + n[1] * n[1] + n[2] * n[2]); - - ++ row; - } - - for (i = 0; i < 3; ++ i){ - this.objectNormal[i] = n[i] / len; - } - - POS.pseudoInverse(vectors, np, this.objectMatrix); -}; - -POS.Posit.prototype.pose = function(imagePoints){ - var posRotation1 = [[],[],[]], posRotation2 = [[],[],[]], posTranslation = [], - rotation1 = [[],[],[]], rotation2 = [[],[],[]], translation1 = [], translation2 = [], - error1, error2, valid1, valid2, i, j; - - this.pos(imagePoints, posRotation1, posRotation2, posTranslation); - - valid1 = this.isValid(posRotation1, posTranslation); - if (valid1){ - error1 = this.iterate(imagePoints, posRotation1, posTranslation, rotation1, translation1); - }else{ - error1 = {euclidean: -1.0, pixels: -1, maximum: -1.0}; - } - - valid2 = this.isValid(posRotation2, posTranslation); - if (valid2){ - error2 = this.iterate(imagePoints, posRotation2, posTranslation, rotation2, translation2); - }else{ - error2 = {euclidean: -1.0, pixels: -1, maximum: -1.0}; - } - - for (i = 0; i < 3; ++ i){ - for (j = 0; j < 3; ++ j){ - if (valid1){ - translation1[i] -= rotation1[i][j] * this.objectPoints[0][j]; - } - if (valid2){ - translation2[i] -= rotation2[i][j] * this.objectPoints[0][j]; - } - } - } - - return error1.euclidean < error2.euclidean? - new POS.Pose(error1.pixels, rotation1, translation1, error2.pixels, rotation2, translation2): - new POS.Pose(error2.pixels, rotation2, translation2, error1.pixels, rotation1, translation1); -}; - -POS.Posit.prototype.pos = function(imagePoints, rotation1, rotation2, translation){ - var np = this.objectPoints.length, imageVectors = [], - i0 = [], j0 = [], ivec = [], jvec = [], row1 = [], row2 = [], row3 = [], - i0i0, j0j0, i0j0, delta, q, lambda, mu, scale, i, j; - - for (i = 0; i < np; ++ i){ - imageVectors[i] = [imagePoints[i].x - imagePoints[0].x, - imagePoints[i].y - imagePoints[0].y]; - } - - //i0 and j0 - for (i = 0; i < 3; ++ i){ - i0[i] = 0.0; - j0[i] = 0.0; - for (j = 0; j < np; ++ j){ - i0[i] += this.objectMatrix[i][j] * imageVectors[j][0]; - j0[i] += this.objectMatrix[i][j] * imageVectors[j][1]; - } - } - - i0i0 = i0[0] * i0[0] + i0[1] * i0[1] + i0[2] * i0[2]; - j0j0 = j0[0] * j0[0] + j0[1] * j0[1] + j0[2] * j0[2]; - i0j0 = i0[0] * j0[0] + i0[1] * j0[1] + i0[2] * j0[2]; - - //Lambda and mu - delta = (j0j0 - i0i0) * (j0j0 - i0i0) + 4.0 * (i0j0 * i0j0); - - if (j0j0 - i0i0 >= 0.0){ - q = (j0j0 - i0i0 + Math.sqrt(delta) ) / 2.0; - }else{ - q = (j0j0 - i0i0 - Math.sqrt(delta) ) / 2.0; - } - - if (q >= 0.0){ - lambda = Math.sqrt(q); - if (0.0 === lambda){ - mu = 0.0; - }else{ - mu = -i0j0 / lambda; - } - }else{ - lambda = Math.sqrt( -(i0j0 * i0j0) / q); - if (0.0 === lambda){ - mu = Math.sqrt(i0i0 - j0j0); - }else{ - mu = -i0j0 / lambda; - } - } - - //First rotation - for (i = 0; i < 3; ++ i){ - ivec[i] = i0[i] + lambda * this.objectNormal[i]; - jvec[i] = j0[i] + mu * this.objectNormal[i]; - } - - scale = Math.sqrt(ivec[0] * ivec[0] + ivec[1] * ivec[1] + ivec[2] * ivec[2]); - - for (i = 0; i < 3; ++ i){ - row1[i] = ivec[i] / scale; - row2[i] = jvec[i] / scale; - } - - row3[0] = row1[1] * row2[2] - row1[2] * row2[1]; - row3[1] = row1[2] * row2[0] - row1[0] * row2[2]; - row3[2] = row1[0] * row2[1] - row1[1] * row2[0]; - - for (i = 0; i < 3; ++ i){ - rotation1[0][i] = row1[i]; - rotation1[1][i] = row2[i]; - rotation1[2][i] = row3[i]; - } - - //Second rotation - for (i = 0; i < 3; ++ i){ - ivec[i] = i0[i] - lambda * this.objectNormal[i]; - jvec[i] = j0[i] - mu * this.objectNormal[i]; - } - - for (i = 0; i < 3; ++ i){ - row1[i] = ivec[i] / scale; - row2[i] = jvec[i] / scale; - } - - row3[0] = row1[1] * row2[2] - row1[2] * row2[1]; - row3[1] = row1[2] * row2[0] - row1[0] * row2[2]; - row3[2] = row1[0] * row2[1] - row1[1] * row2[0]; - - for (i = 0; i < 3; ++ i){ - rotation2[0][i] = row1[i]; - rotation2[1][i] = row2[i]; - rotation2[2][i] = row3[i]; - } - - //Translation - translation[0] = imagePoints[0].x / scale; - translation[1] = imagePoints[0].y / scale; - translation[2] = this.focalLength / scale; -}; - -POS.Posit.prototype.isValid = function(rotation, translation){ - var np = this.objectPoints.length, zmin = Infinity, i = 0, zi; - - for (; i < np; ++ i){ - zi = translation[2] + - (rotation[2][0] * this.objectVectors[i][0] + - rotation[2][1] * this.objectVectors[i][1] + - rotation[2][2] * this.objectVectors[i][2]); - if (zi < zmin){ - zmin = zi; - } - } - - return zmin >= 0.0; -}; - -POS.Posit.prototype.iterate = function(imagePoints, posRotation, posTranslation, rotation, translation){ - var np = this.objectPoints.length, - oldSopImagePoints = [], sopImagePoints = [], - rotation1 = [[],[],[]], rotation2 = [[],[],[]], - translation1 = [], translation2 = [], - converged = false, iteration = 0, - oldImageDifference, imageDifference, factor, - error, error1, error2, delta, i, j; - - for (i = 0; i < np; ++ i){ - oldSopImagePoints[i] = {x: imagePoints[i].x, - y: imagePoints[i].y}; - } - - for (i = 0; i < 3; ++ i){ - for (j = 0; j < 3; ++ j){ - rotation[i][j] = posRotation[i][j]; - } - translation[i] = posTranslation[i]; - } - - for (i = 0; i < np; ++ i){ - factor = 0.0; - for (j = 0; j < 3; ++ j){ - factor += this.objectVectors[i][j] * rotation[2][j] / translation[2]; - } - sopImagePoints[i] = {x: (1.0 + factor) * imagePoints[i].x, - y: (1.0 + factor) * imagePoints[i].y}; - } - - imageDifference = 0.0; - - for (i = 0; i < np; ++ i){ - imageDifference += Math.abs(sopImagePoints[i].x - oldSopImagePoints[i].x); - imageDifference += Math.abs(sopImagePoints[i].y - oldSopImagePoints[i].y); - } - - for (i = 0; i < 3; ++ i){ - translation1[i] = translation[i] - - (rotation[i][0] * this.objectPoints[0][0] + - rotation[i][1] * this.objectPoints[0][1] + - rotation[i][2] * this.objectPoints[0][2]); - } - - error = error1 = this.error(imagePoints, rotation, translation1); - - //Convergence - converged = (0.0 === error1.pixels) || (imageDifference < 0.01); - - while( iteration ++ < 100 && !converged ){ - - for (i = 0; i < np; ++ i){ - oldSopImagePoints[i].x = sopImagePoints[i].x; - oldSopImagePoints[i].y = sopImagePoints[i].y; - } - - this.pos(sopImagePoints, rotation1, rotation2, translation); - - for (i = 0; i < 3; ++ i){ - translation1[i] = translation[i] - - (rotation1[i][0] * this.objectPoints[0][0] + - rotation1[i][1] * this.objectPoints[0][1] + - rotation1[i][2] * this.objectPoints[0][2]); - - translation2[i] = translation[i] - - (rotation2[i][0] * this.objectPoints[0][0] + - rotation2[i][1] * this.objectPoints[0][1] + - rotation2[i][2] * this.objectPoints[0][2]); - } - - error1 = this.error(imagePoints, rotation1, translation1); - error2 = this.error(imagePoints, rotation2, translation2); - - if ( (error1.euclidean >= 0.0) && (error2.euclidean >= 0.0) ){ - if (error2.euclidean < error1.euclidean){ - error = error2; - for (i = 0; i < 3; ++ i){ - for (j = 0; j < 3; ++ j){ - rotation[i][j] = rotation2[i][j]; - } - } - }else{ - error = error1; - for (i = 0; i < 3; ++ i){ - for (j = 0; j < 3; ++ j){ - rotation[i][j] = rotation1[i][j]; - } - } - } - } - - if ( (error1.euclidean < 0.0) && (error2.euclidean >= 0.0) ){ - error = error2; - for (i = 0; i < 3; ++ i){ - for (j = 0; j < 3; ++ j){ - rotation[i][j] = rotation2[i][j]; - } - } - } - - if ( (error2.euclidean < 0.0) && (error1.euclidean >= 0.0) ){ - error = error1; - for (i = 0; i < 3; ++ i){ - for (j = 0; j < 3; ++ j){ - rotation[i][j] = rotation1[i][j]; - } - } - } - - for (i = 0; i < np; ++ i){ - factor = 0.0; - for (j = 0; j < 3; ++ j){ - factor += this.objectVectors[i][j] * rotation[2][j] / translation[2]; - } - sopImagePoints[i].x = (1.0 + factor) * imagePoints[i].x; - sopImagePoints[i].y = (1.0 + factor) * imagePoints[i].y; - } - - oldImageDifference = imageDifference; - imageDifference = 0.0; - - for (i = 0; i < np; ++ i){ - imageDifference += Math.abs(sopImagePoints[i].x - oldSopImagePoints[i].x); - imageDifference += Math.abs(sopImagePoints[i].y - oldSopImagePoints[i].y); - } - - delta = Math.abs(imageDifference - oldImageDifference); - - converged = (0.0 === error.pixels) || (delta < 0.01); - } - - return error; -}; - -POS.Posit.prototype.error = function(imagePoints, rotation, translation){ - var np = this.objectPoints.length, - move = [], projection = [], errorvec = [], - euclidean = 0.0, pixels = 0.0, maximum = 0.0, - i, j, k; - - if ( !this.isValid(rotation, translation) ){ - return {euclidean: -1.0, pixels: -1, maximum: -1.0}; - } - - for (i = 0; i < np; ++ i){ - move[i] = []; - for (j = 0; j < 3; ++ j){ - move[i][j] = translation[j]; - } - } - - for (i = 0; i < np; ++ i){ - for (j = 0; j < 3; ++ j){ - for (k = 0; k < 3; ++ k){ - move[i][j] += rotation[j][k] * this.objectPoints[i][k]; - } - } - } - - for (i = 0; i < np; ++ i){ - projection[i] = []; - for (j = 0; j < 2; ++ j){ - projection[i][j] = this.focalLength * move[i][j] / move[i][2]; - } - } - - for (i = 0; i < np; ++ i){ - errorvec[i] = [projection[i][0] - imagePoints[i].x, - projection[i][1] - imagePoints[i].y]; - } - - for (i = 0; i < np; ++ i){ - euclidean += Math.sqrt(errorvec[i][0] * errorvec[i][0] + - errorvec[i][1] * errorvec[i][1]); - - pixels += Math.abs( Math.round(projection[i][0]) - Math.round(imagePoints[i].x) ) + - Math.abs( Math.round(projection[i][1]) - Math.round(imagePoints[i].y) ); - - if (Math.abs(errorvec[i][0]) > maximum){ - maximum = Math.abs(errorvec[i][0]); - } - if (Math.abs(errorvec[i][1]) > maximum){ - maximum = Math.abs(errorvec[i][1]); - } - } - - return {euclidean: euclidean / np, pixels: pixels, maximum: maximum}; -}; - -POS.pseudoInverse = function(a, n, b){ - var w = [], v = [[],[],[]], s = [[],[],[]], - wmax = 0.0, cn = 0, - i, j, k; - - SVD.svdcmp(a, n, 3, w, v); - - for (i = 0; i < 3; ++ i){ - if (w[i] > wmax){ - wmax = w[i]; - } - } - - wmax *= 0.01; - - for (i = 0; i < 3; ++ i){ - if (w[i] < wmax){ - w[i] = 0.0; - } - } - - for (j = 0; j < 3; ++ j){ - if (0.0 === w[j]){ - ++ cn; - for (k = j; k < 2; ++ k){ - for (i = 0; i < n; ++ i){ - a[i][k] = a[i][k + 1]; - } - for (i = 0; i < 3; ++ i){ - v[i][k] = v[i][k + 1]; - } - } - } - } - - for (j = 0; j < 2; ++ j){ - if (0.0 === w[j]){ - w[j] = w[j + 1]; - } - } - - for (i = 0; i < 3; ++ i){ - for (j = 0; j < 3 - cn; ++ j){ - s[i][j] = v[i][j] / w[j]; - } - } - - for (i = 0; i < 3; ++ i){ - for (j = 0; j < n; ++ j){ - b[i][j] = 0.0; - for (k = 0; k < 3 - cn; ++ k){ - b[i][j] += s[i][k] * a[j][k]; - } - } - } -}; - -POS.Pose = function(error1, rotation1, translation1, error2, rotation2, translation2){ - this.bestError = error1; - this.bestRotation = rotation1; - this.bestTranslation = translation1; - this.alternativeError = error2; - this.alternativeRotation = rotation2; - this.alternativeTranslation = translation2; -}; - +/* +Copyright (c) 2012 Juan Mellado + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + +/* +References: +- "Iterative Pose Estimation using Coplanar Feature Points" + Denis Oberkampf, Daniel F. DeMenthon, Larry S. Davis + http://www.cfar.umd.edu/~daniel/daniel_papersfordownload/CoplanarPts.pdf +*/ + +import SVD from './svd.js'; + +var POS = POS || {}; + +POS.Posit = function(modelSize, focalLength){ + this.objectPoints = this.buildModel(modelSize); + this.focalLength = focalLength; + + this.objectVectors = []; + this.objectNormal = []; + this.objectMatrix = [[],[],[]]; + + this.init(); +}; + +POS.Posit.prototype.buildModel = function(modelSize){ + var half = modelSize / 2.0; + + return [ + [-half, half, 0.0], + [ half, half, 0.0], + [ half, -half, 0.0], + [-half, -half, 0.0] ]; +}; + +POS.Posit.prototype.init = function(){ + var np = this.objectPoints.length, + vectors = [], n = [], len = 0.0, row = 2, i; + + for (i = 0; i < np; ++ i){ + this.objectVectors[i] = [this.objectPoints[i][0] - this.objectPoints[0][0], + this.objectPoints[i][1] - this.objectPoints[0][1], + this.objectPoints[i][2] - this.objectPoints[0][2]]; + + vectors[i] = [this.objectVectors[i][0], + this.objectVectors[i][1], + this.objectVectors[i][2]]; + } + + while(0.0 === len){ + n[0] = this.objectVectors[1][1] * this.objectVectors[row][2] - + this.objectVectors[1][2] * this.objectVectors[row][1]; + n[1] = this.objectVectors[1][2] * this.objectVectors[row][0] - + this.objectVectors[1][0] * this.objectVectors[row][2]; + n[2] = this.objectVectors[1][0] * this.objectVectors[row][1] - + this.objectVectors[1][1] * this.objectVectors[row][0]; + + len = Math.sqrt(n[0] * n[0] + n[1] * n[1] + n[2] * n[2]); + + ++ row; + } + + for (i = 0; i < 3; ++ i){ + this.objectNormal[i] = n[i] / len; + } + + POS.pseudoInverse(vectors, np, this.objectMatrix); +}; + +POS.Posit.prototype.pose = function(imagePoints){ + var posRotation1 = [[],[],[]], posRotation2 = [[],[],[]], posTranslation = [], + rotation1 = [[],[],[]], rotation2 = [[],[],[]], translation1 = [], translation2 = [], + error1, error2, valid1, valid2, i, j; + + this.pos(imagePoints, posRotation1, posRotation2, posTranslation); + + valid1 = this.isValid(posRotation1, posTranslation); + if (valid1){ + error1 = this.iterate(imagePoints, posRotation1, posTranslation, rotation1, translation1); + }else{ + error1 = {euclidean: -1.0, pixels: -1, maximum: -1.0}; + } + + valid2 = this.isValid(posRotation2, posTranslation); + if (valid2){ + error2 = this.iterate(imagePoints, posRotation2, posTranslation, rotation2, translation2); + }else{ + error2 = {euclidean: -1.0, pixels: -1, maximum: -1.0}; + } + + for (i = 0; i < 3; ++ i){ + for (j = 0; j < 3; ++ j){ + if (valid1){ + translation1[i] -= rotation1[i][j] * this.objectPoints[0][j]; + } + if (valid2){ + translation2[i] -= rotation2[i][j] * this.objectPoints[0][j]; + } + } + } + + return error1.euclidean < error2.euclidean? + new POS.Pose(error1.pixels, rotation1, translation1, error2.pixels, rotation2, translation2): + new POS.Pose(error2.pixels, rotation2, translation2, error1.pixels, rotation1, translation1); +}; + +POS.Posit.prototype.pos = function(imagePoints, rotation1, rotation2, translation){ + var np = this.objectPoints.length, imageVectors = [], + i0 = [], j0 = [], ivec = [], jvec = [], row1 = [], row2 = [], row3 = [], + i0i0, j0j0, i0j0, delta, q, lambda, mu, scale, i, j; + + for (i = 0; i < np; ++ i){ + imageVectors[i] = [imagePoints[i].x - imagePoints[0].x, + imagePoints[i].y - imagePoints[0].y]; + } + + //i0 and j0 + for (i = 0; i < 3; ++ i){ + i0[i] = 0.0; + j0[i] = 0.0; + for (j = 0; j < np; ++ j){ + i0[i] += this.objectMatrix[i][j] * imageVectors[j][0]; + j0[i] += this.objectMatrix[i][j] * imageVectors[j][1]; + } + } + + i0i0 = i0[0] * i0[0] + i0[1] * i0[1] + i0[2] * i0[2]; + j0j0 = j0[0] * j0[0] + j0[1] * j0[1] + j0[2] * j0[2]; + i0j0 = i0[0] * j0[0] + i0[1] * j0[1] + i0[2] * j0[2]; + + //Lambda and mu + delta = (j0j0 - i0i0) * (j0j0 - i0i0) + 4.0 * (i0j0 * i0j0); + + if (j0j0 - i0i0 >= 0.0){ + q = (j0j0 - i0i0 + Math.sqrt(delta) ) / 2.0; + }else{ + q = (j0j0 - i0i0 - Math.sqrt(delta) ) / 2.0; + } + + if (q >= 0.0){ + lambda = Math.sqrt(q); + if (0.0 === lambda){ + mu = 0.0; + }else{ + mu = -i0j0 / lambda; + } + }else{ + lambda = Math.sqrt( -(i0j0 * i0j0) / q); + if (0.0 === lambda){ + mu = Math.sqrt(i0i0 - j0j0); + }else{ + mu = -i0j0 / lambda; + } + } + + //First rotation + for (i = 0; i < 3; ++ i){ + ivec[i] = i0[i] + lambda * this.objectNormal[i]; + jvec[i] = j0[i] + mu * this.objectNormal[i]; + } + + scale = Math.sqrt(ivec[0] * ivec[0] + ivec[1] * ivec[1] + ivec[2] * ivec[2]); + + for (i = 0; i < 3; ++ i){ + row1[i] = ivec[i] / scale; + row2[i] = jvec[i] / scale; + } + + row3[0] = row1[1] * row2[2] - row1[2] * row2[1]; + row3[1] = row1[2] * row2[0] - row1[0] * row2[2]; + row3[2] = row1[0] * row2[1] - row1[1] * row2[0]; + + for (i = 0; i < 3; ++ i){ + rotation1[0][i] = row1[i]; + rotation1[1][i] = row2[i]; + rotation1[2][i] = row3[i]; + } + + //Second rotation + for (i = 0; i < 3; ++ i){ + ivec[i] = i0[i] - lambda * this.objectNormal[i]; + jvec[i] = j0[i] - mu * this.objectNormal[i]; + } + + for (i = 0; i < 3; ++ i){ + row1[i] = ivec[i] / scale; + row2[i] = jvec[i] / scale; + } + + row3[0] = row1[1] * row2[2] - row1[2] * row2[1]; + row3[1] = row1[2] * row2[0] - row1[0] * row2[2]; + row3[2] = row1[0] * row2[1] - row1[1] * row2[0]; + + for (i = 0; i < 3; ++ i){ + rotation2[0][i] = row1[i]; + rotation2[1][i] = row2[i]; + rotation2[2][i] = row3[i]; + } + + //Translation + translation[0] = imagePoints[0].x / scale; + translation[1] = imagePoints[0].y / scale; + translation[2] = this.focalLength / scale; +}; + +POS.Posit.prototype.isValid = function(rotation, translation){ + var np = this.objectPoints.length, zmin = Infinity, i = 0, zi; + + for (; i < np; ++ i){ + zi = translation[2] + + (rotation[2][0] * this.objectVectors[i][0] + + rotation[2][1] * this.objectVectors[i][1] + + rotation[2][2] * this.objectVectors[i][2]); + if (zi < zmin){ + zmin = zi; + } + } + + return zmin >= 0.0; +}; + +POS.Posit.prototype.iterate = function(imagePoints, posRotation, posTranslation, rotation, translation){ + var np = this.objectPoints.length, + oldSopImagePoints = [], sopImagePoints = [], + rotation1 = [[],[],[]], rotation2 = [[],[],[]], + translation1 = [], translation2 = [], + converged = false, iteration = 0, + oldImageDifference, imageDifference, factor, + error, error1, error2, delta, i, j; + + for (i = 0; i < np; ++ i){ + oldSopImagePoints[i] = {x: imagePoints[i].x, + y: imagePoints[i].y}; + } + + for (i = 0; i < 3; ++ i){ + for (j = 0; j < 3; ++ j){ + rotation[i][j] = posRotation[i][j]; + } + translation[i] = posTranslation[i]; + } + + for (i = 0; i < np; ++ i){ + factor = 0.0; + for (j = 0; j < 3; ++ j){ + factor += this.objectVectors[i][j] * rotation[2][j] / translation[2]; + } + sopImagePoints[i] = {x: (1.0 + factor) * imagePoints[i].x, + y: (1.0 + factor) * imagePoints[i].y}; + } + + imageDifference = 0.0; + + for (i = 0; i < np; ++ i){ + imageDifference += Math.abs(sopImagePoints[i].x - oldSopImagePoints[i].x); + imageDifference += Math.abs(sopImagePoints[i].y - oldSopImagePoints[i].y); + } + + for (i = 0; i < 3; ++ i){ + translation1[i] = translation[i] - + (rotation[i][0] * this.objectPoints[0][0] + + rotation[i][1] * this.objectPoints[0][1] + + rotation[i][2] * this.objectPoints[0][2]); + } + + error = error1 = this.error(imagePoints, rotation, translation1); + + //Convergence + converged = (0.0 === error1.pixels) || (imageDifference < 0.01); + + while( iteration ++ < 100 && !converged ){ + + for (i = 0; i < np; ++ i){ + oldSopImagePoints[i].x = sopImagePoints[i].x; + oldSopImagePoints[i].y = sopImagePoints[i].y; + } + + this.pos(sopImagePoints, rotation1, rotation2, translation); + + for (i = 0; i < 3; ++ i){ + translation1[i] = translation[i] - + (rotation1[i][0] * this.objectPoints[0][0] + + rotation1[i][1] * this.objectPoints[0][1] + + rotation1[i][2] * this.objectPoints[0][2]); + + translation2[i] = translation[i] - + (rotation2[i][0] * this.objectPoints[0][0] + + rotation2[i][1] * this.objectPoints[0][1] + + rotation2[i][2] * this.objectPoints[0][2]); + } + + error1 = this.error(imagePoints, rotation1, translation1); + error2 = this.error(imagePoints, rotation2, translation2); + + if ( (error1.euclidean >= 0.0) && (error2.euclidean >= 0.0) ){ + if (error2.euclidean < error1.euclidean){ + error = error2; + for (i = 0; i < 3; ++ i){ + for (j = 0; j < 3; ++ j){ + rotation[i][j] = rotation2[i][j]; + } + } + }else{ + error = error1; + for (i = 0; i < 3; ++ i){ + for (j = 0; j < 3; ++ j){ + rotation[i][j] = rotation1[i][j]; + } + } + } + } + + if ( (error1.euclidean < 0.0) && (error2.euclidean >= 0.0) ){ + error = error2; + for (i = 0; i < 3; ++ i){ + for (j = 0; j < 3; ++ j){ + rotation[i][j] = rotation2[i][j]; + } + } + } + + if ( (error2.euclidean < 0.0) && (error1.euclidean >= 0.0) ){ + error = error1; + for (i = 0; i < 3; ++ i){ + for (j = 0; j < 3; ++ j){ + rotation[i][j] = rotation1[i][j]; + } + } + } + + for (i = 0; i < np; ++ i){ + factor = 0.0; + for (j = 0; j < 3; ++ j){ + factor += this.objectVectors[i][j] * rotation[2][j] / translation[2]; + } + sopImagePoints[i].x = (1.0 + factor) * imagePoints[i].x; + sopImagePoints[i].y = (1.0 + factor) * imagePoints[i].y; + } + + oldImageDifference = imageDifference; + imageDifference = 0.0; + + for (i = 0; i < np; ++ i){ + imageDifference += Math.abs(sopImagePoints[i].x - oldSopImagePoints[i].x); + imageDifference += Math.abs(sopImagePoints[i].y - oldSopImagePoints[i].y); + } + + delta = Math.abs(imageDifference - oldImageDifference); + + converged = (0.0 === error.pixels) || (delta < 0.01); + } + + return error; +}; + +POS.Posit.prototype.error = function(imagePoints, rotation, translation){ + var np = this.objectPoints.length, + move = [], projection = [], errorvec = [], + euclidean = 0.0, pixels = 0.0, maximum = 0.0, + i, j, k; + + if ( !this.isValid(rotation, translation) ){ + return {euclidean: -1.0, pixels: -1, maximum: -1.0}; + } + + for (i = 0; i < np; ++ i){ + move[i] = []; + for (j = 0; j < 3; ++ j){ + move[i][j] = translation[j]; + } + } + + for (i = 0; i < np; ++ i){ + for (j = 0; j < 3; ++ j){ + for (k = 0; k < 3; ++ k){ + move[i][j] += rotation[j][k] * this.objectPoints[i][k]; + } + } + } + + for (i = 0; i < np; ++ i){ + projection[i] = []; + for (j = 0; j < 2; ++ j){ + projection[i][j] = this.focalLength * move[i][j] / move[i][2]; + } + } + + for (i = 0; i < np; ++ i){ + errorvec[i] = [projection[i][0] - imagePoints[i].x, + projection[i][1] - imagePoints[i].y]; + } + + for (i = 0; i < np; ++ i){ + euclidean += Math.sqrt(errorvec[i][0] * errorvec[i][0] + + errorvec[i][1] * errorvec[i][1]); + + pixels += Math.abs( Math.round(projection[i][0]) - Math.round(imagePoints[i].x) ) + + Math.abs( Math.round(projection[i][1]) - Math.round(imagePoints[i].y) ); + + if (Math.abs(errorvec[i][0]) > maximum){ + maximum = Math.abs(errorvec[i][0]); + } + if (Math.abs(errorvec[i][1]) > maximum){ + maximum = Math.abs(errorvec[i][1]); + } + } + + return {euclidean: euclidean / np, pixels: pixels, maximum: maximum}; +}; + +POS.pseudoInverse = function(a, n, b){ + var w = [], v = [[],[],[]], s = [[],[],[]], + wmax = 0.0, cn = 0, + i, j, k; + + SVD.svdcmp(a, n, 3, w, v); + + for (i = 0; i < 3; ++ i){ + if (w[i] > wmax){ + wmax = w[i]; + } + } + + wmax *= 0.01; + + for (i = 0; i < 3; ++ i){ + if (w[i] < wmax){ + w[i] = 0.0; + } + } + + for (j = 0; j < 3; ++ j){ + if (0.0 === w[j]){ + ++ cn; + for (k = j; k < 2; ++ k){ + for (i = 0; i < n; ++ i){ + a[i][k] = a[i][k + 1]; + } + for (i = 0; i < 3; ++ i){ + v[i][k] = v[i][k + 1]; + } + } + } + } + + for (j = 0; j < 2; ++ j){ + if (0.0 === w[j]){ + w[j] = w[j + 1]; + } + } + + for (i = 0; i < 3; ++ i){ + for (j = 0; j < 3 - cn; ++ j){ + s[i][j] = v[i][j] / w[j]; + } + } + + for (i = 0; i < 3; ++ i){ + for (j = 0; j < n; ++ j){ + b[i][j] = 0.0; + for (k = 0; k < 3 - cn; ++ k){ + b[i][j] += s[i][k] * a[j][k]; + } + } + } +}; + +POS.Pose = function(error1, rotation1, translation1, error2, rotation2, translation2){ + this.bestError = error1; + this.bestRotation = rotation1; + this.bestTranslation = translation1; + this.alternativeError = error2; + this.alternativeRotation = rotation2; + this.alternativeTranslation = translation2; +}; + export default POS; \ No newline at end of file diff --git a/src/lib/js-aruco/posit2.js b/src/lib/js-aruco/posit2.js old mode 100755 new mode 100644 index e14a3b0c..f3121cc8 --- a/src/lib/js-aruco/posit2.js +++ b/src/lib/js-aruco/posit2.js @@ -1,498 +1,498 @@ -/* -Copyright (c) 2012 Juan Mellado - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. -*/ - -/* -References: -- "3D Pose Estimation" - Andrew Kirillow - http://www.aforgenet.com/articles/posit/ -*/ -import SVD from './svd.js'; - -var POS = POS || {}; - -POS.Posit = function(modelSize, focalLength){ - this.model = this.buildModel(modelSize); - this.focalLength = focalLength; - - this.init(); -}; - -POS.Posit.prototype.buildModel = function(modelSize){ - var half = modelSize / 2.0; - - return [ - new Vec3(-half, half, 0.0), - new Vec3( half, half, 0.0), - new Vec3( half, -half, 0.0), - new Vec3(-half, -half, 0.0) ]; -}; - -POS.Posit.prototype.init = function(){ - var d = new Vec3(), v = new Mat3(), u; - - this.modelVectors = Mat3.fromRows( - Vec3.sub(this.model[1], this.model[0]), - Vec3.sub(this.model[2], this.model[0]), - Vec3.sub(this.model[3], this.model[0]) ); - - u = Mat3.clone(this.modelVectors); - - SVD.svdcmp(u.m, 3, 3, d.v, v.m); - - this.modelPseudoInverse = Mat3.mult( - Mat3.mult(v, Mat3.fromDiagonal( Vec3.inverse(d) ) ), Mat3.transpose(u) ); - - this.modelNormal = v.column( d.minIndex() ); -}; - -POS.Posit.prototype.pose = function(points){ - var eps = new Vec3(1.0, 1.0, 1.0), - rotation1 = new Mat3(), rotation2 = new Mat3(), - translation1 = new Vec3(), translation2 = new Vec3(), - error1, error2; - - this.pos(points, eps, rotation1, rotation2, translation1, translation2); - - error1 = this.iterate(points, rotation1, translation1); - error2 = this.iterate(points, rotation2, translation2); - - return error1 < error2? - new POS.Pose(error1, rotation1.m, translation1.v, error2, rotation2.m, translation2.v): - new POS.Pose(error2, rotation2.m, translation2.v, error1, rotation1.m, translation1.v); -}; - -POS.Posit.prototype.pos = function(points, eps, rotation1, rotation2, translation1, translation2){ - var xi = new Vec3(points[1].x, points[2].x, points[3].x), - yi = new Vec3(points[1].y, points[2].y, points[3].y), - xs = Vec3.addScalar( Vec3.mult(xi, eps), -points[0].x), - ys = Vec3.addScalar( Vec3.mult(yi, eps), -points[0].y), - i0 = Mat3.multVector(this.modelPseudoInverse, xs), - j0 = Mat3.multVector(this.modelPseudoInverse, ys), - s = j0.square() - i0.square(), - ij = Vec3.dot(i0, j0), - r = 0.0, theta = 0.0, - i, j, k, inorm, jnorm, scale, temp, lambda, mu; - - if (0.0 === s){ - r = Math.sqrt( Math.abs(2.0 * ij) ); - theta = (-Math.PI / 2.0) * (ij < 0.0? -1: (ij > 0.0? 1.0: 0.0) ); - }else{ - r = Math.sqrt( Math.sqrt(s * s + 4.0 * ij * ij) ); - theta = Math.atan(-2.0 * ij / s); - if (s < 0.0){ - theta += Math.PI; - } - theta /= 2.0; - } - - lambda = r * Math.cos(theta); - mu = r * Math.sin(theta); - - //First possible rotation/translation - i = Vec3.add(i0, Vec3.multScalar(this.modelNormal, lambda) ); - j = Vec3.add(j0, Vec3.multScalar(this.modelNormal, mu) ); - inorm = i.normalize(); - jnorm = j.normalize(); - k = Vec3.cross(i, j); - rotation1.copy( Mat3.fromRows(i, j, k) ); - - scale = (inorm + jnorm) / 2.0; - temp = Mat3.multVector(rotation1, this.model[0]); - translation1.v = [ - points[0].x / scale - temp.v[0], - points[0].y / scale - temp.v[1], - this.focalLength / scale]; - - //Second possible rotation/translation - i = Vec3.sub(i0, Vec3.multScalar(this.modelNormal, lambda) ); - j = Vec3.sub(j0, Vec3.multScalar(this.modelNormal, mu) ); - inorm = i.normalize(); - jnorm = j.normalize(); - k = Vec3.cross(i, j); - rotation2.copy( Mat3.fromRows(i, j, k) ); - - scale = (inorm + jnorm) / 2.0; - temp = Mat3.multVector(rotation2, this.model[0]); - translation2.v = [ - points[0].x / scale - temp.v[0], - points[0].y / scale - temp.v[1], - this.focalLength / scale]; -}; - -POS.Posit.prototype.iterate = function(points, rotation, translation){ - var prevError = Infinity, - rotation1 = new Mat3(), rotation2 = new Mat3(), - translation1 = new Vec3(), translation2 = new Vec3(), - i = 0, eps, error, error1, error2; - - for (; i < 100; ++ i){ - eps = Vec3.addScalar( Vec3.multScalar( - Mat3.multVector( this.modelVectors, rotation.row(2) ), 1.0 / translation.v[2]), 1.0); - - this.pos(points, eps, rotation1, rotation2, translation1, translation2); - - error1 = this.getError(points, rotation1, translation1); - error2 = this.getError(points, rotation2, translation2); - - if (error1 < error2){ - rotation.copy(rotation1); - translation.copy(translation1); - error = error1; - }else{ - rotation.copy(rotation2); - translation.copy(translation2); - error = error2; - } - - if ( (error <= 2.0) || (error > prevError) ){ - break; - } - - prevError = error; - } - - return error; -}; - -POS.Posit.prototype.getError = function(points, rotation, translation){ - var v1 = Vec3.add( Mat3.multVector(rotation, this.model[0]), translation), - v2 = Vec3.add( Mat3.multVector(rotation, this.model[1]), translation), - v3 = Vec3.add( Mat3.multVector(rotation, this.model[2]), translation), - v4 = Vec3.add( Mat3.multVector(rotation, this.model[3]), translation), - modeled, ia1, ia2, ia3, ia4, ma1, ma2, ma3, ma4; - - v1 = v1.v; v2 = v2.v; v3 = v3.v; v4 = v4.v; - - v1[0] *= this.focalLength / v1[2]; - v1[1] *= this.focalLength / v1[2]; - v2[0] *= this.focalLength / v2[2]; - v2[1] *= this.focalLength / v2[2]; - v3[0] *= this.focalLength / v3[2]; - v3[1] *= this.focalLength / v3[2]; - v4[0] *= this.focalLength / v4[2]; - v4[1] *= this.focalLength / v4[2]; - - modeled = [ - {x: v1[0], y: v1[1]}, - {x: v2[0], y: v2[1]}, - {x: v3[0], y: v3[1]}, - {x: v4[0], y: v4[1]} - ]; - - ia1 = this.angle( points[0], points[1], points[3] ); - ia2 = this.angle( points[1], points[2], points[0] ); - ia3 = this.angle( points[2], points[3], points[1] ); - ia4 = this.angle( points[3], points[0], points[2] ); - - ma1 = this.angle( modeled[0], modeled[1], modeled[3] ); - ma2 = this.angle( modeled[1], modeled[2], modeled[0] ); - ma3 = this.angle( modeled[2], modeled[3], modeled[1] ); - ma4 = this.angle( modeled[3], modeled[0], modeled[2] ); - - return ( Math.abs(ia1 - ma1) + - Math.abs(ia2 - ma2) + - Math.abs(ia3 - ma3) + - Math.abs(ia4 - ma4) ) / 4.0; -}; - -POS.Posit.prototype.angle = function(a, b, c){ - var x1 = b.x - a.x, y1 = b.y - a.y, - x2 = c.x - a.x, y2 = c.y - a.y; - - return Math.acos( (x1 * x2 + y1 * y2) / - (Math.sqrt(x1 * x1 + y1 * y1) * Math.sqrt(x2 * x2 + y2 * y2) ) ) * 180.0 / Math.PI; -}; - -POS.Pose = function(error1, rotation1, translation1, error2, rotation2, translation2){ - this.bestError = error1; - this.bestRotation = rotation1; - this.bestTranslation = translation1; - this.alternativeError = error2; - this.alternativeRotation = rotation2; - this.alternativeTranslation = translation2; -}; - -var Vec3 = function(x, y, z){ - this.v = [x || 0.0, y || 0.0, z || 0.0]; -}; - -Vec3.prototype.copy = function(a){ - var v = this.v; - - a = a.v; - - v[0] = a[0]; - v[1] = a[1]; - v[2] = a[2]; - - return this; -}; - -Vec3.add = function(a, b){ - var vector = new Vec3(), v = vector.v; - - a = a.v; b = b.v; - - v[0] = a[0] + b[0]; - v[1] = a[1] + b[1]; - v[2] = a[2] + b[2]; - - return vector; -}; - -Vec3.sub = function(a, b){ - var vector = new Vec3(), v = vector.v; - - a = a.v; b = b.v; - - v[0] = a[0] - b[0]; - v[1] = a[1] - b[1]; - v[2] = a[2] - b[2]; - - return vector; -}; - -Vec3.mult = function(a, b){ - var vector = new Vec3(), v = vector.v; - - a = a.v; b = b.v; - - v[0] = a[0] * b[0]; - v[1] = a[1] * b[1]; - v[2] = a[2] * b[2]; - - return vector; -}; - -Vec3.addScalar = function(a, b){ - var vector = new Vec3(), v = vector.v; - - a = a.v; - - v[0] = a[0] + b; - v[1] = a[1] + b; - v[2] = a[2] + b; - - return vector; -}; - -Vec3.multScalar = function(a, b){ - var vector = new Vec3(), v = vector.v; - - a = a.v; - - v[0] = a[0] * b; - v[1] = a[1] * b; - v[2] = a[2] * b; - - return vector; -}; - -Vec3.dot = function(a, b){ - a = a.v; b = b.v; - - return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; -}; - -Vec3.cross = function(a, b){ - a = a.v; b = b.v; - - return new Vec3( - a[1] * b[2] - a[2] * b[1], - a[2] * b[0] - a[0] * b[2], - a[0] * b[1] - a[1] * b[0]); -}; - -Vec3.prototype.normalize = function(){ - var v = this.v, - len = Math.sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); - - if (len > 0.0){ - v[0] /= len; - v[1] /= len; - v[2] /= len; - } - - return len; -}; - -Vec3.inverse = function(a){ - var vector = new Vec3(), v = vector.v; - - a = a.v; - - if (a[0] !== 0.0){ - v[0] = 1.0 / a[0]; - } - if (a[1] !== 0.0){ - v[1] = 1.0 / a[1]; - } - if (a[2] !== 0.0){ - v[2] = 1.0 / a[2]; - } - - return vector; -}; - -Vec3.prototype.square = function(){ - var v = this.v; - - return v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; -}; - -Vec3.prototype.minIndex = function(){ - var v = this.v; - - return v[0] < v[1]? (v[0] < v[2]? 0: 2): (v[1] < v[2]? 1: 2); -}; - -var Mat3 = function(){ - this.m = [ [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0], - [0.0, 0.0, 0.0] ]; -}; - -Mat3.clone = function(a){ - var matrix = new Mat3(), m = matrix.m; - - a = a.m; - - m[0][0] = a[0][0]; - m[0][1] = a[0][1]; - m[0][2] = a[0][2]; - m[1][0] = a[1][0]; - m[1][1] = a[1][1]; - m[1][2] = a[1][2]; - m[2][0] = a[2][0]; - m[2][1] = a[2][1]; - m[2][2] = a[2][2]; - - return matrix; -}; - -Mat3.prototype.copy = function(a){ - var m = this.m; - - a = a.m; - - m[0][0] = a[0][0]; - m[0][1] = a[0][1]; - m[0][2] = a[0][2]; - m[1][0] = a[1][0]; - m[1][1] = a[1][1]; - m[1][2] = a[1][2]; - m[2][0] = a[2][0]; - m[2][1] = a[2][1]; - m[2][2] = a[2][2]; - - return this; -}; - -Mat3.fromRows = function(a, b, c){ - var matrix = new Mat3(), m = matrix.m; - - a = a.v; b = b.v; c = c.v; - - m[0][0] = a[0]; - m[0][1] = a[1]; - m[0][2] = a[2]; - m[1][0] = b[0]; - m[1][1] = b[1]; - m[1][2] = b[2]; - m[2][0] = c[0]; - m[2][1] = c[1]; - m[2][2] = c[2]; - - return matrix; -}; - -Mat3.fromDiagonal = function(a){ - var matrix = new Mat3(), m = matrix.m; - - a = a.v; - - m[0][0] = a[0]; - m[1][1] = a[1]; - m[2][2] = a[2]; - - return matrix; -}; - -Mat3.transpose = function(a){ - var matrix = new Mat3(), m = matrix.m; - - a = a.m; - - m[0][0] = a[0][0]; - m[0][1] = a[1][0]; - m[0][2] = a[2][0]; - m[1][0] = a[0][1]; - m[1][1] = a[1][1]; - m[1][2] = a[2][1]; - m[2][0] = a[0][2]; - m[2][1] = a[1][2]; - m[2][2] = a[2][2]; - - return matrix; -}; - -Mat3.mult = function(a, b){ - var matrix = new Mat3(), m = matrix.m; - - a = a.m; b = b.m; - - m[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0] + a[0][2] * b[2][0]; - m[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1] + a[0][2] * b[2][1]; - m[0][2] = a[0][0] * b[0][2] + a[0][1] * b[1][2] + a[0][2] * b[2][2]; - m[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0] + a[1][2] * b[2][0]; - m[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1] + a[1][2] * b[2][1]; - m[1][2] = a[1][0] * b[0][2] + a[1][1] * b[1][2] + a[1][2] * b[2][2]; - m[2][0] = a[2][0] * b[0][0] + a[2][1] * b[1][0] + a[2][2] * b[2][0]; - m[2][1] = a[2][0] * b[0][1] + a[2][1] * b[1][1] + a[2][2] * b[2][1]; - m[2][2] = a[2][0] * b[0][2] + a[2][1] * b[1][2] + a[2][2] * b[2][2]; - - return matrix; -}; - -Mat3.multVector = function(m, a){ - m = m.m; a = a.v; - - return new Vec3( - m[0][0] * a[0] + m[0][1] * a[1] + m[0][2] * a[2], - m[1][0] * a[0] + m[1][1] * a[1] + m[1][2] * a[2], - m[2][0] * a[0] + m[2][1] * a[1] + m[2][2] * a[2]); -}; - -Mat3.prototype.column = function(index){ - var m = this.m; - - return new Vec3( m[0][index], m[1][index], m[2][index] ); -}; - -Mat3.prototype.row = function(index){ - var m = this.m; - - return new Vec3( m[index][0], m[index][1], m[index][2] ); -}; - +/* +Copyright (c) 2012 Juan Mellado + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + +/* +References: +- "3D Pose Estimation" + Andrew Kirillow + http://www.aforgenet.com/articles/posit/ +*/ +import SVD from './svd.js'; + +var POS = POS || {}; + +POS.Posit = function(modelSize, focalLength){ + this.model = this.buildModel(modelSize); + this.focalLength = focalLength; + + this.init(); +}; + +POS.Posit.prototype.buildModel = function(modelSize){ + var half = modelSize / 2.0; + + return [ + new Vec3(-half, half, 0.0), + new Vec3( half, half, 0.0), + new Vec3( half, -half, 0.0), + new Vec3(-half, -half, 0.0) ]; +}; + +POS.Posit.prototype.init = function(){ + var d = new Vec3(), v = new Mat3(), u; + + this.modelVectors = Mat3.fromRows( + Vec3.sub(this.model[1], this.model[0]), + Vec3.sub(this.model[2], this.model[0]), + Vec3.sub(this.model[3], this.model[0]) ); + + u = Mat3.clone(this.modelVectors); + + SVD.svdcmp(u.m, 3, 3, d.v, v.m); + + this.modelPseudoInverse = Mat3.mult( + Mat3.mult(v, Mat3.fromDiagonal( Vec3.inverse(d) ) ), Mat3.transpose(u) ); + + this.modelNormal = v.column( d.minIndex() ); +}; + +POS.Posit.prototype.pose = function(points){ + var eps = new Vec3(1.0, 1.0, 1.0), + rotation1 = new Mat3(), rotation2 = new Mat3(), + translation1 = new Vec3(), translation2 = new Vec3(), + error1, error2; + + this.pos(points, eps, rotation1, rotation2, translation1, translation2); + + error1 = this.iterate(points, rotation1, translation1); + error2 = this.iterate(points, rotation2, translation2); + + return error1 < error2? + new POS.Pose(error1, rotation1.m, translation1.v, error2, rotation2.m, translation2.v): + new POS.Pose(error2, rotation2.m, translation2.v, error1, rotation1.m, translation1.v); +}; + +POS.Posit.prototype.pos = function(points, eps, rotation1, rotation2, translation1, translation2){ + var xi = new Vec3(points[1].x, points[2].x, points[3].x), + yi = new Vec3(points[1].y, points[2].y, points[3].y), + xs = Vec3.addScalar( Vec3.mult(xi, eps), -points[0].x), + ys = Vec3.addScalar( Vec3.mult(yi, eps), -points[0].y), + i0 = Mat3.multVector(this.modelPseudoInverse, xs), + j0 = Mat3.multVector(this.modelPseudoInverse, ys), + s = j0.square() - i0.square(), + ij = Vec3.dot(i0, j0), + r = 0.0, theta = 0.0, + i, j, k, inorm, jnorm, scale, temp, lambda, mu; + + if (0.0 === s){ + r = Math.sqrt( Math.abs(2.0 * ij) ); + theta = (-Math.PI / 2.0) * (ij < 0.0? -1: (ij > 0.0? 1.0: 0.0) ); + }else{ + r = Math.sqrt( Math.sqrt(s * s + 4.0 * ij * ij) ); + theta = Math.atan(-2.0 * ij / s); + if (s < 0.0){ + theta += Math.PI; + } + theta /= 2.0; + } + + lambda = r * Math.cos(theta); + mu = r * Math.sin(theta); + + //First possible rotation/translation + i = Vec3.add(i0, Vec3.multScalar(this.modelNormal, lambda) ); + j = Vec3.add(j0, Vec3.multScalar(this.modelNormal, mu) ); + inorm = i.normalize(); + jnorm = j.normalize(); + k = Vec3.cross(i, j); + rotation1.copy( Mat3.fromRows(i, j, k) ); + + scale = (inorm + jnorm) / 2.0; + temp = Mat3.multVector(rotation1, this.model[0]); + translation1.v = [ + points[0].x / scale - temp.v[0], + points[0].y / scale - temp.v[1], + this.focalLength / scale]; + + //Second possible rotation/translation + i = Vec3.sub(i0, Vec3.multScalar(this.modelNormal, lambda) ); + j = Vec3.sub(j0, Vec3.multScalar(this.modelNormal, mu) ); + inorm = i.normalize(); + jnorm = j.normalize(); + k = Vec3.cross(i, j); + rotation2.copy( Mat3.fromRows(i, j, k) ); + + scale = (inorm + jnorm) / 2.0; + temp = Mat3.multVector(rotation2, this.model[0]); + translation2.v = [ + points[0].x / scale - temp.v[0], + points[0].y / scale - temp.v[1], + this.focalLength / scale]; +}; + +POS.Posit.prototype.iterate = function(points, rotation, translation){ + var prevError = Infinity, + rotation1 = new Mat3(), rotation2 = new Mat3(), + translation1 = new Vec3(), translation2 = new Vec3(), + i = 0, eps, error, error1, error2; + + for (; i < 100; ++ i){ + eps = Vec3.addScalar( Vec3.multScalar( + Mat3.multVector( this.modelVectors, rotation.row(2) ), 1.0 / translation.v[2]), 1.0); + + this.pos(points, eps, rotation1, rotation2, translation1, translation2); + + error1 = this.getError(points, rotation1, translation1); + error2 = this.getError(points, rotation2, translation2); + + if (error1 < error2){ + rotation.copy(rotation1); + translation.copy(translation1); + error = error1; + }else{ + rotation.copy(rotation2); + translation.copy(translation2); + error = error2; + } + + if ( (error <= 2.0) || (error > prevError) ){ + break; + } + + prevError = error; + } + + return error; +}; + +POS.Posit.prototype.getError = function(points, rotation, translation){ + var v1 = Vec3.add( Mat3.multVector(rotation, this.model[0]), translation), + v2 = Vec3.add( Mat3.multVector(rotation, this.model[1]), translation), + v3 = Vec3.add( Mat3.multVector(rotation, this.model[2]), translation), + v4 = Vec3.add( Mat3.multVector(rotation, this.model[3]), translation), + modeled, ia1, ia2, ia3, ia4, ma1, ma2, ma3, ma4; + + v1 = v1.v; v2 = v2.v; v3 = v3.v; v4 = v4.v; + + v1[0] *= this.focalLength / v1[2]; + v1[1] *= this.focalLength / v1[2]; + v2[0] *= this.focalLength / v2[2]; + v2[1] *= this.focalLength / v2[2]; + v3[0] *= this.focalLength / v3[2]; + v3[1] *= this.focalLength / v3[2]; + v4[0] *= this.focalLength / v4[2]; + v4[1] *= this.focalLength / v4[2]; + + modeled = [ + {x: v1[0], y: v1[1]}, + {x: v2[0], y: v2[1]}, + {x: v3[0], y: v3[1]}, + {x: v4[0], y: v4[1]} + ]; + + ia1 = this.angle( points[0], points[1], points[3] ); + ia2 = this.angle( points[1], points[2], points[0] ); + ia3 = this.angle( points[2], points[3], points[1] ); + ia4 = this.angle( points[3], points[0], points[2] ); + + ma1 = this.angle( modeled[0], modeled[1], modeled[3] ); + ma2 = this.angle( modeled[1], modeled[2], modeled[0] ); + ma3 = this.angle( modeled[2], modeled[3], modeled[1] ); + ma4 = this.angle( modeled[3], modeled[0], modeled[2] ); + + return ( Math.abs(ia1 - ma1) + + Math.abs(ia2 - ma2) + + Math.abs(ia3 - ma3) + + Math.abs(ia4 - ma4) ) / 4.0; +}; + +POS.Posit.prototype.angle = function(a, b, c){ + var x1 = b.x - a.x, y1 = b.y - a.y, + x2 = c.x - a.x, y2 = c.y - a.y; + + return Math.acos( (x1 * x2 + y1 * y2) / + (Math.sqrt(x1 * x1 + y1 * y1) * Math.sqrt(x2 * x2 + y2 * y2) ) ) * 180.0 / Math.PI; +}; + +POS.Pose = function(error1, rotation1, translation1, error2, rotation2, translation2){ + this.bestError = error1; + this.bestRotation = rotation1; + this.bestTranslation = translation1; + this.alternativeError = error2; + this.alternativeRotation = rotation2; + this.alternativeTranslation = translation2; +}; + +var Vec3 = function(x, y, z){ + this.v = [x || 0.0, y || 0.0, z || 0.0]; +}; + +Vec3.prototype.copy = function(a){ + var v = this.v; + + a = a.v; + + v[0] = a[0]; + v[1] = a[1]; + v[2] = a[2]; + + return this; +}; + +Vec3.add = function(a, b){ + var vector = new Vec3(), v = vector.v; + + a = a.v; b = b.v; + + v[0] = a[0] + b[0]; + v[1] = a[1] + b[1]; + v[2] = a[2] + b[2]; + + return vector; +}; + +Vec3.sub = function(a, b){ + var vector = new Vec3(), v = vector.v; + + a = a.v; b = b.v; + + v[0] = a[0] - b[0]; + v[1] = a[1] - b[1]; + v[2] = a[2] - b[2]; + + return vector; +}; + +Vec3.mult = function(a, b){ + var vector = new Vec3(), v = vector.v; + + a = a.v; b = b.v; + + v[0] = a[0] * b[0]; + v[1] = a[1] * b[1]; + v[2] = a[2] * b[2]; + + return vector; +}; + +Vec3.addScalar = function(a, b){ + var vector = new Vec3(), v = vector.v; + + a = a.v; + + v[0] = a[0] + b; + v[1] = a[1] + b; + v[2] = a[2] + b; + + return vector; +}; + +Vec3.multScalar = function(a, b){ + var vector = new Vec3(), v = vector.v; + + a = a.v; + + v[0] = a[0] * b; + v[1] = a[1] * b; + v[2] = a[2] * b; + + return vector; +}; + +Vec3.dot = function(a, b){ + a = a.v; b = b.v; + + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; +}; + +Vec3.cross = function(a, b){ + a = a.v; b = b.v; + + return new Vec3( + a[1] * b[2] - a[2] * b[1], + a[2] * b[0] - a[0] * b[2], + a[0] * b[1] - a[1] * b[0]); +}; + +Vec3.prototype.normalize = function(){ + var v = this.v, + len = Math.sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); + + if (len > 0.0){ + v[0] /= len; + v[1] /= len; + v[2] /= len; + } + + return len; +}; + +Vec3.inverse = function(a){ + var vector = new Vec3(), v = vector.v; + + a = a.v; + + if (a[0] !== 0.0){ + v[0] = 1.0 / a[0]; + } + if (a[1] !== 0.0){ + v[1] = 1.0 / a[1]; + } + if (a[2] !== 0.0){ + v[2] = 1.0 / a[2]; + } + + return vector; +}; + +Vec3.prototype.square = function(){ + var v = this.v; + + return v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; +}; + +Vec3.prototype.minIndex = function(){ + var v = this.v; + + return v[0] < v[1]? (v[0] < v[2]? 0: 2): (v[1] < v[2]? 1: 2); +}; + +var Mat3 = function(){ + this.m = [ [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0] ]; +}; + +Mat3.clone = function(a){ + var matrix = new Mat3(), m = matrix.m; + + a = a.m; + + m[0][0] = a[0][0]; + m[0][1] = a[0][1]; + m[0][2] = a[0][2]; + m[1][0] = a[1][0]; + m[1][1] = a[1][1]; + m[1][2] = a[1][2]; + m[2][0] = a[2][0]; + m[2][1] = a[2][1]; + m[2][2] = a[2][2]; + + return matrix; +}; + +Mat3.prototype.copy = function(a){ + var m = this.m; + + a = a.m; + + m[0][0] = a[0][0]; + m[0][1] = a[0][1]; + m[0][2] = a[0][2]; + m[1][0] = a[1][0]; + m[1][1] = a[1][1]; + m[1][2] = a[1][2]; + m[2][0] = a[2][0]; + m[2][1] = a[2][1]; + m[2][2] = a[2][2]; + + return this; +}; + +Mat3.fromRows = function(a, b, c){ + var matrix = new Mat3(), m = matrix.m; + + a = a.v; b = b.v; c = c.v; + + m[0][0] = a[0]; + m[0][1] = a[1]; + m[0][2] = a[2]; + m[1][0] = b[0]; + m[1][1] = b[1]; + m[1][2] = b[2]; + m[2][0] = c[0]; + m[2][1] = c[1]; + m[2][2] = c[2]; + + return matrix; +}; + +Mat3.fromDiagonal = function(a){ + var matrix = new Mat3(), m = matrix.m; + + a = a.v; + + m[0][0] = a[0]; + m[1][1] = a[1]; + m[2][2] = a[2]; + + return matrix; +}; + +Mat3.transpose = function(a){ + var matrix = new Mat3(), m = matrix.m; + + a = a.m; + + m[0][0] = a[0][0]; + m[0][1] = a[1][0]; + m[0][2] = a[2][0]; + m[1][0] = a[0][1]; + m[1][1] = a[1][1]; + m[1][2] = a[2][1]; + m[2][0] = a[0][2]; + m[2][1] = a[1][2]; + m[2][2] = a[2][2]; + + return matrix; +}; + +Mat3.mult = function(a, b){ + var matrix = new Mat3(), m = matrix.m; + + a = a.m; b = b.m; + + m[0][0] = a[0][0] * b[0][0] + a[0][1] * b[1][0] + a[0][2] * b[2][0]; + m[0][1] = a[0][0] * b[0][1] + a[0][1] * b[1][1] + a[0][2] * b[2][1]; + m[0][2] = a[0][0] * b[0][2] + a[0][1] * b[1][2] + a[0][2] * b[2][2]; + m[1][0] = a[1][0] * b[0][0] + a[1][1] * b[1][0] + a[1][2] * b[2][0]; + m[1][1] = a[1][0] * b[0][1] + a[1][1] * b[1][1] + a[1][2] * b[2][1]; + m[1][2] = a[1][0] * b[0][2] + a[1][1] * b[1][2] + a[1][2] * b[2][2]; + m[2][0] = a[2][0] * b[0][0] + a[2][1] * b[1][0] + a[2][2] * b[2][0]; + m[2][1] = a[2][0] * b[0][1] + a[2][1] * b[1][1] + a[2][2] * b[2][1]; + m[2][2] = a[2][0] * b[0][2] + a[2][1] * b[1][2] + a[2][2] * b[2][2]; + + return matrix; +}; + +Mat3.multVector = function(m, a){ + m = m.m; a = a.v; + + return new Vec3( + m[0][0] * a[0] + m[0][1] * a[1] + m[0][2] * a[2], + m[1][0] * a[0] + m[1][1] * a[1] + m[1][2] * a[2], + m[2][0] * a[0] + m[2][1] * a[1] + m[2][2] * a[2]); +}; + +Mat3.prototype.column = function(index){ + var m = this.m; + + return new Vec3( m[0][index], m[1][index], m[2][index] ); +}; + +Mat3.prototype.row = function(index){ + var m = this.m; + + return new Vec3( m[index][0], m[index][1], m[index][2] ); +}; + export default POS; \ No newline at end of file diff --git a/src/lib/js-aruco/svd.js b/src/lib/js-aruco/svd.js old mode 100755 new mode 100644 index a68432d7..d3d0a50b --- a/src/lib/js-aruco/svd.js +++ b/src/lib/js-aruco/svd.js @@ -1,286 +1,286 @@ -/* -Copyright (c) 2012 Juan Mellado - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in -all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN -THE SOFTWARE. -*/ - -/* -References: -- "Numerical Recipes in C - Second Edition" - http://www.nr.com/ -*/ - -var SVD = SVD || {}; - -SVD.svdcmp = function(a, m, n, w, v){ - var flag, i, its, j, jj, k, l, nm, - anorm = 0.0, c, f, g = 0.0, h, s, scale = 0.0, x, y, z, rv1 = []; - - //Householder reduction to bidiagonal form - for (i = 0; i < n; ++ i){ - l = i + 1; - rv1[i] = scale * g; - g = s = scale = 0.0; - if (i < m){ - for (k = i; k < m; ++ k){ - scale += Math.abs( a[k][i] ); - } - if (0.0 !== scale){ - for (k = i; k < m; ++ k){ - a[k][i] /= scale; - s += a[k][i] * a[k][i]; - } - f = a[i][i]; - g = -SVD.sign( Math.sqrt(s), f ); - h = f * g - s; - a[i][i] = f - g; - for (j = l; j < n; ++ j){ - for (s = 0.0, k = i; k < m; ++ k){ - s += a[k][i] * a[k][j]; - } - f = s / h; - for (k = i; k < m; ++ k){ - a[k][j] += f * a[k][i]; - } - } - for (k = i; k < m; ++ k){ - a[k][i] *= scale; - } - } - } - w[i] = scale * g; - g = s = scale = 0.0; - if ( (i < m) && (i !== n - 1) ){ - for (k = l; k < n; ++ k){ - scale += Math.abs( a[i][k] ); - } - if (0.0 !== scale){ - for (k = l; k < n; ++ k){ - a[i][k] /= scale; - s += a[i][k] * a[i][k]; - } - f = a[i][l]; - g = -SVD.sign( Math.sqrt(s), f ); - h = f * g - s; - a[i][l] = f - g; - for (k = l; k < n; ++ k){ - rv1[k] = a[i][k] / h; - } - for (j = l; j < m; ++ j){ - for (s = 0.0, k = l; k < n; ++ k){ - s += a[j][k] * a[i][k]; - } - for (k = l; k < n; ++ k){ - a[j][k] += s * rv1[k]; - } - } - for (k = l; k < n; ++ k){ - a[i][k] *= scale; - } - } - } - anorm = Math.max(anorm, ( Math.abs( w[i] ) + Math.abs( rv1[i] ) ) ); - } - - //Acumulation of right-hand transformation - for (i = n - 1; i >= 0; -- i){ - if (i < n - 1){ - if (0.0 !== g){ - for (j = l; j < n; ++ j){ - v[j][i] = ( a[i][j] / a[i][l] ) / g; - } - for (j = l; j < n; ++ j){ - for (s = 0.0, k = l; k < n; ++ k){ - s += a[i][k] * v[k][j]; - } - for (k = l; k < n; ++ k){ - v[k][j] += s * v[k][i]; - } - } - } - for (j = l; j < n; ++ j){ - v[i][j] = v[j][i] = 0.0; - } - } - v[i][i] = 1.0; - g = rv1[i]; - l = i; - } - - //Acumulation of left-hand transformation - for (i = Math.min(n, m) - 1; i >= 0; -- i){ - l = i + 1; - g = w[i]; - for (j = l; j < n; ++ j){ - a[i][j] = 0.0; - } - if (0.0 !== g){ - g = 1.0 / g; - for (j = l; j < n; ++ j){ - for (s = 0.0, k = l; k < m; ++ k){ - s += a[k][i] * a[k][j]; - } - f = (s / a[i][i]) * g; - for (k = i; k < m; ++ k){ - a[k][j] += f * a[k][i]; - } - } - for (j = i; j < m; ++ j){ - a[j][i] *= g; - } - }else{ - for (j = i; j < m; ++ j){ - a[j][i] = 0.0; - } - } - ++ a[i][i]; - } - - //Diagonalization of the bidiagonal form - for (k = n - 1; k >= 0; -- k){ - for (its = 1; its <= 30; ++ its){ - flag = true; - for (l = k; l >= 0; -- l){ - nm = l - 1; - if ( Math.abs( rv1[l] ) + anorm === anorm ){ - flag = false; - break; - } - if ( Math.abs( w[nm] ) + anorm === anorm ){ - break; - } - } - if (flag){ - c = 0.0; - s = 1.0; - for (i = l; i <= k; ++ i){ - f = s * rv1[i]; - if ( Math.abs(f) + anorm === anorm ){ - break; - } - g = w[i]; - h = SVD.pythag(f, g); - w[i] = h; - h = 1.0 / h; - c = g * h; - s = -f * h; - for (j = 1; j <= m; ++ j){ - y = a[j][nm]; - z = a[j][i]; - a[j][nm] = y * c + z * s; - a[j][i] = z * c - y * s; - } - } - } - - //Convergence - z = w[k]; - if (l === k){ - if (z < 0.0){ - w[k] = -z; - for (j = 0; j < n; ++ j){ - v[j][k] = -v[j][k]; - } - } - break; - } - - if (30 === its){ - return false; - } - - //Shift from bottom 2-by-2 minor - x = w[l]; - nm = k - 1; - y = w[nm]; - g = rv1[nm]; - h = rv1[k]; - f = ( (y - z) * (y + z) + (g - h) * (g + h) ) / (2.0 * h * y); - g = SVD.pythag( f, 1.0 ); - f = ( (x - z) * (x + z) + h * ( (y / (f + SVD.sign(g, f) ) ) - h) ) / x; - - //Next QR transformation - c = s = 1.0; - for (j = l; j <= nm; ++ j){ - i = j + 1; - g = rv1[i]; - y = w[i]; - h = s * g; - g = c * g; - z = SVD.pythag(f, h); - rv1[j] = z; - c = f / z; - s = h / z; - f = x * c + g * s; - g = g * c - x * s; - h = y * s; - y *= c; - for (jj = 0; jj < n; ++ jj){ - x = v[jj][j]; - z = v[jj][i]; - v[jj][j] = x * c + z * s; - v[jj][i] = z * c - x * s; - } - z = SVD.pythag(f, h); - w[j] = z; - if (0.0 !== z){ - z = 1.0 / z; - c = f * z; - s = h * z; - } - f = c * g + s * y; - x = c * y - s * g; - for (jj = 0; jj < m; ++ jj){ - y = a[jj][j]; - z = a[jj][i]; - a[jj][j] = y * c + z * s; - a[jj][i] = z * c - y * s; - } - } - rv1[l] = 0.0; - rv1[k] = f; - w[k] = x; - } - } - - return true; -}; - -SVD.pythag = function(a, b){ - var at = Math.abs(a), bt = Math.abs(b), ct; - - if (at > bt){ - ct = bt / at; - return at * Math.sqrt(1.0 + ct * ct); - } - - if (0.0 === bt){ - return 0.0; - } - - ct = at / bt; - return bt * Math.sqrt(1.0 + ct * ct); -}; - -SVD.sign = function(a, b){ - return b >= 0.0? Math.abs(a): -Math.abs(a); -}; - - +/* +Copyright (c) 2012 Juan Mellado + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + +/* +References: +- "Numerical Recipes in C - Second Edition" + http://www.nr.com/ +*/ + +var SVD = SVD || {}; + +SVD.svdcmp = function(a, m, n, w, v){ + var flag, i, its, j, jj, k, l, nm, + anorm = 0.0, c, f, g = 0.0, h, s, scale = 0.0, x, y, z, rv1 = []; + + //Householder reduction to bidiagonal form + for (i = 0; i < n; ++ i){ + l = i + 1; + rv1[i] = scale * g; + g = s = scale = 0.0; + if (i < m){ + for (k = i; k < m; ++ k){ + scale += Math.abs( a[k][i] ); + } + if (0.0 !== scale){ + for (k = i; k < m; ++ k){ + a[k][i] /= scale; + s += a[k][i] * a[k][i]; + } + f = a[i][i]; + g = -SVD.sign( Math.sqrt(s), f ); + h = f * g - s; + a[i][i] = f - g; + for (j = l; j < n; ++ j){ + for (s = 0.0, k = i; k < m; ++ k){ + s += a[k][i] * a[k][j]; + } + f = s / h; + for (k = i; k < m; ++ k){ + a[k][j] += f * a[k][i]; + } + } + for (k = i; k < m; ++ k){ + a[k][i] *= scale; + } + } + } + w[i] = scale * g; + g = s = scale = 0.0; + if ( (i < m) && (i !== n - 1) ){ + for (k = l; k < n; ++ k){ + scale += Math.abs( a[i][k] ); + } + if (0.0 !== scale){ + for (k = l; k < n; ++ k){ + a[i][k] /= scale; + s += a[i][k] * a[i][k]; + } + f = a[i][l]; + g = -SVD.sign( Math.sqrt(s), f ); + h = f * g - s; + a[i][l] = f - g; + for (k = l; k < n; ++ k){ + rv1[k] = a[i][k] / h; + } + for (j = l; j < m; ++ j){ + for (s = 0.0, k = l; k < n; ++ k){ + s += a[j][k] * a[i][k]; + } + for (k = l; k < n; ++ k){ + a[j][k] += s * rv1[k]; + } + } + for (k = l; k < n; ++ k){ + a[i][k] *= scale; + } + } + } + anorm = Math.max(anorm, ( Math.abs( w[i] ) + Math.abs( rv1[i] ) ) ); + } + + //Acumulation of right-hand transformation + for (i = n - 1; i >= 0; -- i){ + if (i < n - 1){ + if (0.0 !== g){ + for (j = l; j < n; ++ j){ + v[j][i] = ( a[i][j] / a[i][l] ) / g; + } + for (j = l; j < n; ++ j){ + for (s = 0.0, k = l; k < n; ++ k){ + s += a[i][k] * v[k][j]; + } + for (k = l; k < n; ++ k){ + v[k][j] += s * v[k][i]; + } + } + } + for (j = l; j < n; ++ j){ + v[i][j] = v[j][i] = 0.0; + } + } + v[i][i] = 1.0; + g = rv1[i]; + l = i; + } + + //Acumulation of left-hand transformation + for (i = Math.min(n, m) - 1; i >= 0; -- i){ + l = i + 1; + g = w[i]; + for (j = l; j < n; ++ j){ + a[i][j] = 0.0; + } + if (0.0 !== g){ + g = 1.0 / g; + for (j = l; j < n; ++ j){ + for (s = 0.0, k = l; k < m; ++ k){ + s += a[k][i] * a[k][j]; + } + f = (s / a[i][i]) * g; + for (k = i; k < m; ++ k){ + a[k][j] += f * a[k][i]; + } + } + for (j = i; j < m; ++ j){ + a[j][i] *= g; + } + }else{ + for (j = i; j < m; ++ j){ + a[j][i] = 0.0; + } + } + ++ a[i][i]; + } + + //Diagonalization of the bidiagonal form + for (k = n - 1; k >= 0; -- k){ + for (its = 1; its <= 30; ++ its){ + flag = true; + for (l = k; l >= 0; -- l){ + nm = l - 1; + if ( Math.abs( rv1[l] ) + anorm === anorm ){ + flag = false; + break; + } + if ( Math.abs( w[nm] ) + anorm === anorm ){ + break; + } + } + if (flag){ + c = 0.0; + s = 1.0; + for (i = l; i <= k; ++ i){ + f = s * rv1[i]; + if ( Math.abs(f) + anorm === anorm ){ + break; + } + g = w[i]; + h = SVD.pythag(f, g); + w[i] = h; + h = 1.0 / h; + c = g * h; + s = -f * h; + for (j = 1; j <= m; ++ j){ + y = a[j][nm]; + z = a[j][i]; + a[j][nm] = y * c + z * s; + a[j][i] = z * c - y * s; + } + } + } + + //Convergence + z = w[k]; + if (l === k){ + if (z < 0.0){ + w[k] = -z; + for (j = 0; j < n; ++ j){ + v[j][k] = -v[j][k]; + } + } + break; + } + + if (30 === its){ + return false; + } + + //Shift from bottom 2-by-2 minor + x = w[l]; + nm = k - 1; + y = w[nm]; + g = rv1[nm]; + h = rv1[k]; + f = ( (y - z) * (y + z) + (g - h) * (g + h) ) / (2.0 * h * y); + g = SVD.pythag( f, 1.0 ); + f = ( (x - z) * (x + z) + h * ( (y / (f + SVD.sign(g, f) ) ) - h) ) / x; + + //Next QR transformation + c = s = 1.0; + for (j = l; j <= nm; ++ j){ + i = j + 1; + g = rv1[i]; + y = w[i]; + h = s * g; + g = c * g; + z = SVD.pythag(f, h); + rv1[j] = z; + c = f / z; + s = h / z; + f = x * c + g * s; + g = g * c - x * s; + h = y * s; + y *= c; + for (jj = 0; jj < n; ++ jj){ + x = v[jj][j]; + z = v[jj][i]; + v[jj][j] = x * c + z * s; + v[jj][i] = z * c - x * s; + } + z = SVD.pythag(f, h); + w[j] = z; + if (0.0 !== z){ + z = 1.0 / z; + c = f * z; + s = h * z; + } + f = c * g + s * y; + x = c * y - s * g; + for (jj = 0; jj < m; ++ jj){ + y = a[jj][j]; + z = a[jj][i]; + a[jj][j] = y * c + z * s; + a[jj][i] = z * c - y * s; + } + } + rv1[l] = 0.0; + rv1[k] = f; + w[k] = x; + } + } + + return true; +}; + +SVD.pythag = function(a, b){ + var at = Math.abs(a), bt = Math.abs(b), ct; + + if (at > bt){ + ct = bt / at; + return at * Math.sqrt(1.0 + ct * ct); + } + + if (0.0 === bt){ + return 0.0; + } + + ct = at / bt; + return bt * Math.sqrt(1.0 + ct * ct); +}; + +SVD.sign = function(a, b){ + return b >= 0.0? Math.abs(a): -Math.abs(a); +}; + + export default SVD; \ No newline at end of file diff --git a/src/lib/lw.raster2gcode/raster-to-gcode.js b/src/lib/lw.raster2gcode/raster-to-gcode.js index 51596be2..b565f079 100644 --- a/src/lib/lw.raster2gcode/raster-to-gcode.js +++ b/src/lib/lw.raster2gcode/raster-to-gcode.js @@ -1,4 +1,5 @@ import CanvasGrid from './canvas-grid' +import { getGenerator } from "../action2gcode/gcode-generator" // RasterToGcode class class RasterToGcode extends CanvasGrid { @@ -32,6 +33,8 @@ class RasterToGcode extends CanvasGrid { precision: { X: 2, Y: 2, S: 4 }, // Number of decimals for each commands + gcodeGenerator: "default", + nonBlocking: true, // Use setTimeout to avoid blocking the UI filters: { @@ -98,10 +101,6 @@ class RasterToGcode extends CanvasGrid { // Output size in millimeters this.outputSize = { width : 0, height: 0 } - // G0 command - this.G1 = ['G', 1] - this.G0 = ['G', this.burnWhite ? 1 : 0] - // Calculate beam offset this.beamOffset = this.toolDiameter * 1000 / 2000 @@ -165,6 +164,12 @@ class RasterToGcode extends CanvasGrid { // Defaults settings settings = settings || {} + this.generator = getGenerator(this.gcodeGenerator, this); + + // G0 command + this.G1 = this.generator.moveTool; + this.G0 = this.burnWhite ? this.generator.moveTool : this.generator.moveRapid; + // Register user callbacks this._registerUserCallbacks(settings) @@ -252,29 +257,34 @@ class RasterToGcode extends CanvasGrid { // Compute and return a command, return null if not changed _command(name, value) { + //console.log("_command", value); // If the value argument is an object if (typeof value === 'object') { // Computed commands line let commands = Array.prototype.slice.call(arguments) - let command, line = [] + let command, line = [], params = {}; // for each command - for (var i = 0, il = commands.length; i < il; i++) { + for (var i = 1, il = commands.length; i < il; i++) { command = this._command.apply(this, commands[i]) command && line.push(command) + command && (params[command[0].toLowerCase()] = command[1]); } // Return the line if not empty - return line.length ? line.join(' ') : null + let result = line.length ? name.call(this.generator, params, name == this.lastCommands["function"]).split("\r\n") : null; + this.lastCommands["function"] = name; + return result; } // Format the value + //console.log("value", value); value = value.toFixed(this.precision[name] || 0) // If the value was changed or if verbose mode on if (this.verboseG || value !== this.lastCommands[name]) { - this.lastCommands[name] = value - return name + value + this.lastCommands[name] = value; + return [name, value]; } // No change @@ -312,7 +322,7 @@ class RasterToGcode extends CanvasGrid { } // Commands - point.G = point.s ? ['G', 1] : this.G0 + point.G = point.s ? this.G1 : this.G0 point.X = (point.x * this.toolDiameter) + this.offsets.X point.Y = (point.y * this.toolDiameter) + this.offsets.Y point.S = this._mapPixelPower(point.s) @@ -470,7 +480,7 @@ class RasterToGcode extends CanvasGrid { let addCommand = (...args) => { command = this._command(...args) - command && gcode.push(command) + command && Array.prototype.push.apply(gcode, command) } // Get first point @@ -481,15 +491,15 @@ class RasterToGcode extends CanvasGrid { let pass = (passNum) => { // Move to start of the line - addCommand(['G', 0], ['Z', this.zSafe]) - addCommand(['G', 0], ['X', point.X], ['Y', point.Y]) - addCommand(['G', 0], ['Z', this.zSurface]) + addCommand(this.generator.moveRapid, ['Z', this.zSafe]) + addCommand(this.generator.moveRapid, ['X', point.X], ['Y', point.Y]) + addCommand(this.generator.moveRapid, ['Z', this.zSurface]) // For each point on the line while (point) { if (point.S) { if (plung) { - addCommand(['G', 0], ['Z', this.zSurface]) + addCommand(this.generator.moveRapid, ['Z', this.zSurface]) plung = false } @@ -501,17 +511,17 @@ class RasterToGcode extends CanvasGrid { Z = Math.max(Z, -zMax) } - addCommand(['G', 1], ['Z', this.zSurface + Z]) - addCommand(['G', 1], ['X', point.X], ['Y', point.Y]) + addCommand(this.generator.moveTool, ['Z', this.zSurface + Z]) + addCommand(this.generator.moveTool, ['X', point.X], ['Y', point.Y]) } else { if (plung) { - addCommand(['G', 1], ['Z', this.zSurface]) + addCommand(this.generator.moveTool, ['Z', this.zSurface]) plung = false } - addCommand(['G', 0], ['Z', this.zSafe]) - addCommand(['G', 0], ['X', point.X], ['Y', point.Y]) + addCommand(this.generator.moveRapid, ['Z', this.zSafe]) + addCommand(this.generator.moveRapid, ['X', point.X], ['Y', point.Y]) } if (point.lastWhite || point.lastColored) { @@ -523,8 +533,8 @@ class RasterToGcode extends CanvasGrid { } // Move to Z safe - addCommand(['G', 1], ['Z', this.zSurface]) - addCommand(['G', 0], ['Z', this.zSafe]) + addCommand(this.generator.moveTool, ['Z', this.zSurface]) + addCommand(this.generator.moveRapid, ['Z', this.zSafe]) } for (var i = 1; i <= this.passes; i++) { @@ -587,13 +597,14 @@ class RasterToGcode extends CanvasGrid { let addCommand = (...args) => { command = this._command(...args) - command && gcode.push(command) + command && Array.prototype.push.apply(gcode, command) } // Get first point point = this._getPoint(index) // Move to start of the line + addCommand(this.G0, ['X', point.X], ['Y', point.Y], ['S', 0]) // Get next point @@ -602,6 +613,8 @@ class RasterToGcode extends CanvasGrid { // For each point on the line while (point) { // Burn to next point + + //gcode.push(point.G.call(this.generator, {x: point.X.toFixed(this.precision["X"]), y: point.Y.toFixed(this.precision["Y"]), s:point.S.toFixed(this.precision["S"])})); addCommand(point.G, ['X', point.X], ['Y', point.Y], ['S', point.S]) // Get next point diff --git a/src/lib/workers/cam-lasercut.js b/src/lib/workers/cam-lasercut.js index 16d1e181..8fb682f7 100644 --- a/src/lib/workers/cam-lasercut.js +++ b/src/lib/workers/cam-lasercut.js @@ -21,6 +21,11 @@ onmessage = (event) => { self.close(); }; - getLaserCutGcodeFromOp.apply(this, [settings, opIndex, op, geometry, openGeometry, tabGeometry, showAlert, done, progress]) + try{ + getLaserCutGcodeFromOp.apply(this, [settings, opIndex, op, geometry, openGeometry, tabGeometry, showAlert, done, progress]) + }catch(e){ + console.error(e); + postMessage(JSON.stringify({ event: "onError", errors:[{ message:e, level:10 }]})); + } } \ No newline at end of file diff --git a/src/reducers/settings.js b/src/reducers/settings.js index ff7ab2e3..6b703d4c 100644 --- a/src/reducers/settings.js +++ b/src/reducers/settings.js @@ -119,6 +119,7 @@ export const SETTINGS_INITIALSTATE = { gcodeStart: "G21 ; Set units to mm\r\nG90 ; Absolute positioning\r\n", gcodeEnd: "M5 ; Switch tool offEnd\r\n", gcodeHoming: "", + gcodeGenerator: "default", gcodeToolOn: "", gcodeToolOff: "", gcodeLaserIntensity: 'S', From b64c5d571a9e5df7d64142c12c8ff3f0e3647954 Mon Sep 17 00:00:00 2001 From: tibus Date: Thu, 26 Apr 2018 19:39:49 +0200 Subject: [PATCH 3/3] change back permission to 755 --- src/lib/js-aruco/aruco.js | 0 src/lib/js-aruco/cv.js | 0 src/lib/js-aruco/posit1.js | 0 src/lib/js-aruco/posit2.js | 0 src/lib/js-aruco/svd.js | 0 5 files changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 src/lib/js-aruco/aruco.js mode change 100644 => 100755 src/lib/js-aruco/cv.js mode change 100644 => 100755 src/lib/js-aruco/posit1.js mode change 100644 => 100755 src/lib/js-aruco/posit2.js mode change 100644 => 100755 src/lib/js-aruco/svd.js diff --git a/src/lib/js-aruco/aruco.js b/src/lib/js-aruco/aruco.js old mode 100644 new mode 100755 diff --git a/src/lib/js-aruco/cv.js b/src/lib/js-aruco/cv.js old mode 100644 new mode 100755 diff --git a/src/lib/js-aruco/posit1.js b/src/lib/js-aruco/posit1.js old mode 100644 new mode 100755 diff --git a/src/lib/js-aruco/posit2.js b/src/lib/js-aruco/posit2.js old mode 100644 new mode 100755 diff --git a/src/lib/js-aruco/svd.js b/src/lib/js-aruco/svd.js old mode 100644 new mode 100755