496 lines
14 KiB
JavaScript
496 lines
14 KiB
JavaScript
/*
|
|
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;
|
|
};
|