js-aruco2/src/posit1.js

JavaScript porting of the ArUco library supporting multiple dictionaries including ARUCO_MIP_36h12 and custom dictionaries for augmented reality marker detection

/*
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
*/

var POS = POS || {};
this.POS = POS;

var SVD = this.SVD || require('./svd').SVD;

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;
};