mind-ar
Version:
web augmented reality framework
308 lines (255 loc) • 10.1 kB
JavaScript
// try to implement https://hal.inria.fr/inria-00174036/PDF/RR-6303.pdf
import {Matrix, inverse} from 'ml-matrix';
import {SVD} from "svd-js";
import {solveHomography} from '../utils/homography';
import {computeScreenCoordiate} from "./utils";
const opposites_of_minors = (M, row, col) => {
let x1 = col === 0? 1: 0;
let x2 = col === 2? 1: 2;
let y1 = row === 0? 1: 0;
let y2 = row === 2? 1: 2;
return M[y1][x2] * M[y2][x1] - M[y1][x1] * M[y2][x2];
}
const findRmatFrom_tstar_n = (H, tstar, n, v) => {
// computes R = H( I - (2/v)*te_star*ne_t )
const twoDivV = 2 / v;
const tmp = [
[1 - twoDivV * tstar[0] * n[0], 0 - twoDivV * tstar[0] * n[1], 0 - twoDivV * tstar[0] * n[2]],
[0 - twoDivV * tstar[1] * n[0], 1 - twoDivV * tstar[1] * n[1], 0 - twoDivV * tstar[1] * n[2]],
[0 - twoDivV * tstar[2] * n[0], 0 - twoDivV * tstar[2] * n[1], 1 - twoDivV * tstar[2] * n[2]],
];
const R = [[0,0,0], [0,0,0], [0,0,0]];
for (let i = 0; i < 3; i++) {
for (let j = 0; j < 3; j++) {
for (let k = 0; k < 3; k++) {
R[i][j] += H[i][k] * tmp[k][j];
}
}
}
//const R = H.mmul( new Matrix(tmp));
return R;
}
const estimate = ({screenCoords, worldCoords, projectionTransform}) => {
const Harray = solveHomography(worldCoords.map((p) => [p.x, p.y]), screenCoords.map((p) => [p.x, p.y]));
const G = new Matrix([
[Harray[0], Harray[1], Harray[2]],
[Harray[3], Harray[4], Harray[5]],
[Harray[6], Harray[7], Harray[8]],
]);
const K = new Matrix(projectionTransform);
const KInv = inverse(K);
const KInvArr = KInv.to2DArray();
const KArr = K.to2DArray();
const Hhat = KInv.mmul(G).mmul(K);
const { q } = SVD(Hhat.to2DArray());
const H = Hhat.div(q[1]);
const HTH = H.transpose().mmul(H);
const S = Matrix.sub(HTH, Matrix.eye(3, 3)).to2DArray();
console.log("G", G);
console.log("svd q", q);
console.log("Hhat", Hhat);
console.log("H", H);
console.log("HTH", HTH);
console.log("S", S);
// M00, M11, M22
const M00 = opposites_of_minors(S, 0, 0);
const M11 = opposites_of_minors(S, 1, 1);
const M22 = opposites_of_minors(S, 2, 2);
const rtM00 = Math.sqrt(M00);
const rtM11 = Math.sqrt(M11);
const rtM22 = Math.sqrt(M22);
// M01, M12, M02
const M01 = opposites_of_minors(S, 0, 1);
const e01 = M01 >= 0? 1: -1;
const M12 = opposites_of_minors(S, 1, 2);
const e12 = M12 >= 0? 1: -1;
const M02 = opposites_of_minors(S, 0, 2);
const e02 = M02 >= 0? 1: -1;
let maxIndex = 0;
if (Math.abs(S[1][1]) > Math.abs(S[maxIndex][maxIndex])) maxIndex = 1;
if (Math.abs(S[2][2]) > Math.abs(S[maxIndex][maxIndex])) maxIndex = 2;
console.log("rtM00", rtM00, rtM11, rtM22);
console.log("M01", M01, M12, M02, e01, e12, e02);
let npa = [0, 0, 0];
let npb = [0, 0, 0];
console.log("max index", maxIndex);
if (maxIndex === 0) {
npa[0] = npb[0] = S[0][0];
npa[1] = S[0][1] + rtM22;
npb[1] = S[0][1] - rtM22;
npa[2] = S[0][2] + e12 * rtM11;
npb[2] = S[0][2] - e12 * rtM11;
} else if (maxIndex === 1) {
npa[0] = S[0][1] + rtM22;
npb[0] = S[0][1] - rtM22;
npa[1] = npb[1] = S[1][1];
npa[2] = S[1][2] - e02 * rtM00;
npb[2] = S[1][2] + e02 * rtM00;
} else if (maxIndex === 2) {
npa[0] = S[0][2] + e01 * rtM11;
npb[0] = S[0][2] - e01 * rtM11;
npa[1] = S[1][2] + rtM00;
npb[1] = S[1][2] - rtM00;
npa[2] = npb[2] = S[2][2];
}
console.log("npa", npa);
console.log("npb", npb);
const traceS = S[0][0] + S[1][1] + S[2][2];
const v = 2.0 * Math.sqrt(1 + traceS - M00 - M11 - M22);
const ESii = S[maxIndex][maxIndex] >= 0? 1: -1
const r_2 = 2 + traceS + v;
const nt_2 = 2 + traceS - v;
const r = Math.sqrt(r_2);
const n_t = Math.sqrt(nt_2);
console.log("r n_t", r, n_t);
const npaNorm = Math.sqrt(npa[0] * npa[0] + npa[1] * npa[1] + npa[2] * npa[2]);
const npbNorm = Math.sqrt(npb[0] * npb[0] + npb[1] * npb[1] + npb[2] * npb[2]);
const na = [npa[0] / npaNorm, npa[1] / npaNorm, npa[2] / npaNorm];
const nb = [npb[0] / npbNorm, npb[1] / npbNorm, npb[2] / npbNorm];
console.log("na nb", na, nb);
const half_nt = 0.5 * n_t;
const esii_t_r = ESii * r;
const ta_star = [];
for (let i = 0; i < 3; i++) {
ta_star[i] = half_nt * (esii_t_r * nb[i] - n_t * na[i]);
}
const tb_star = [];
for (let i = 0; i < 3; i++) {
tb_star[i] = half_nt * (esii_t_r * na[i] - n_t * nb[i]);
}
const HArr = H.to2DArray();
console.log("ta_star", ta_star, tb_star);
/*
"""solutions = []
# Ra, ta
R = findRmatFrom_tstar_n(H, ta_star, na, v)
t = R.dot(ta_star)
solutions.append((R, t, na))
# Ra, -ta
solutions.append((R, -t, -na))
# Rb, tb
R = findRmatFrom_tstar_n(H, tb_star, nb, v)
t = R.dot(tb_star)
solutions.append((R, t, nb))
# Rb, -tb
solutions.append((R, -t, -nb))
*/
const findT = (R1, ta_star) => {
const t = [
R1[0][0] * ta_star[0] + R1[0][1] * ta_star[1] + R1[0][2] * ta_star[2],
R1[1][0] * ta_star[0] + R1[1][1] * ta_star[1] + R1[1][2] * ta_star[2],
R1[2][0] * ta_star[0] + R1[2][1] * ta_star[1] + R1[2][2] * ta_star[2]
]
return t;
}
const Ra = findRmatFrom_tstar_n(HArr, ta_star, na, v);
const ta = findT(Ra, ta_star);
const nta = [-ta[0], -ta[1], -ta[2]];
console.log("RaTRa", (new Matrix(Ra)).transpose().mmul((new Matrix(Ra))));
const Rb = findRmatFrom_tstar_n(HArr, tb_star, nb, v);
const tb = findT(Rb, tb_star);
const ntb = [-tb[0], -tb[1], -tb[2]];
const findModelViewProjectionTransform = (R, t) => {
const transform = [
[R[0][0], R[0][1], R[0][2], t[0]],
[R[1][0], R[1][1], R[1][2], t[1]],
[R[2][0], R[2][1], R[2][2], t[2]],
];
return transform;
const modelViewProjectionTransform = [[],[],[]];
for (let j = 0; j < 3; j++ ) {
for (let i = 0; i < 4; i++) {
modelViewProjectionTransform[j][i] = KArr[j][0] * transform[0][i]
+ KArr[j][1] * transform[1][i]
+ KArr[j][2] * transform[2][i];
}
}
return modelViewProjectionTransform;
}
console.log("Ra ta", Ra, ta);
console.log("Rb tb", Rb, tb);
const tnT = new Matrix([
[ta[0] * na[0], ta[0] * na[1], ta[0] * na[2]],
[ta[1] * na[0], ta[1] * na[1], ta[1] * na[2]],
[ta[2] * na[0], ta[2] * na[1], ta[2] * na[2]],
]);
const RtnT = (new Matrix(Ra)).add(tnT);
console.log("tnT", tnT);
console.log("RtnT", RtnT);
const modelViewProjectionTransforms = [];
modelViewProjectionTransforms.push(findModelViewProjectionTransform(Ra, ta));
modelViewProjectionTransforms.push(findModelViewProjectionTransform(Ra, nta));
modelViewProjectionTransforms.push(findModelViewProjectionTransform(Rb, tb));
modelViewProjectionTransforms.push(findModelViewProjectionTransform(Rb, ntb));
const applyMatrix = (K, pt) => {
let kx = K[0][0] * pt[0] + K[0][1] * pt[1] + K[0][2];
let ky = K[1][0] * pt[0] + K[1][1] * pt[1] + K[1][2];
let kz = K[2][0] * pt[0] + K[2][1] * pt[1] + K[2][2];
kx /= kz;
ky /= kz;
return [kx, ky];
}
for (let s = 0; s < modelViewProjectionTransforms.length; s++) {
console.log("solution", s);
const modelViewProjectionTransform = modelViewProjectionTransforms[s];
for (let i = 0; i < worldCoords.length; i++) {
let world = applyMatrix(KInvArr, [worldCoords[i].x, worldCoords[i].y]);
let world2 = applyMatrix(RtnT.to2DArray(), world);
let screen = applyMatrix(KInvArr, [screenCoords[i].x, screenCoords[i].y]);
console.log("map", worldCoords[i], screenCoords[i]);
console.log("mapped", world, world2, screen);
//const mapped = computeScreenCoordiate(modelViewProjectionTransform, worldCoords[i].x, worldCoords[i].y, 0);
//console.log("mapped", worldCoords[i], screenCoords[i], mapped);
//console.log("mapped", worldCoords[i], screenCoords[i], kx2, ky2, mapped);
}
}
for (let s = 0; s < modelViewProjectionTransforms.length; s++) {
console.log("mvp solution", s);
const modelViewProjectionTransform = modelViewProjectionTransforms[s];
for (let i = 0; i < worldCoords.length; i++) {
let world = applyMatrix(KInvArr, [worldCoords[i].x, worldCoords[i].y]);
let screen = applyMatrix(KInvArr, [screenCoords[i].x, screenCoords[i].y]);
//const mapped = computeScreenCoordiate(modelViewProjectionTransform, worldCoords[i].x, worldCoords[i].y, 0);
const mapped = computeScreenCoordiate(modelViewProjectionTransform, world[0], world[1], 0);
console.log("mapped", worldCoords[i], screenCoords[i], world, screen, mapped);
}
}
return null;
/*
const R1 = findRmatFrom_tstar_n(HArr, ta_star, na, v);
const R2 = findRmatFrom_tstar_n(HArr, tb_star, nb, v);
console.log("R1", R1);
console.log("R2", R2);
const t = [
R1[0][0] * ta_star[0] + R1[0][1] * ta_star[1] + R1[0][2] * ta_star[2],
R1[1][0] * ta_star[0] + R1[1][1] * ta_star[1] + R1[1][2] * ta_star[2],
R1[2][0] * ta_star[0] + R1[2][1] * ta_star[1] + R1[2][2] * ta_star[2]
]
const R = R2;
const modelViewProjectionTransform = [
[R[0][0], R[0][1], R[0][2], t[0]],
[R[1][0], R[1][1], R[1][2], t[0]],
[R[2][0], R[2][1], R[2][2], t[0]],
];
*/
for (let i = 0; i < worldCoords.length; i++) {
const mapped = computeScreenCoordiate(modelViewProjectionTransform, worldCoords[i].x, worldCoords[i].y, 0);
console.log("mapped", worldCoords[i], screenCoords[i], mapped);
}
// this is the full computation if the projectTransform does not look like the expected format, but more computations
const modelViewTransform = [[],[],[]];
for (let j = 0; j < 3; j++ ) {
for (let i = 0; i < 4; i++) {
modelViewTransform[j][i] = KInvArr[j][0] * modelViewProjectionTransform[0][i]
+ KInvArr[j][1] * modelViewProjectionTransform[1][i]
+ KInvArr[j][2] * modelViewProjectionTransform[2][i];
}
}
console.log("KInvArr", KInvArr);
console.log("modelViewProjectionTransform", modelViewProjectionTransform);
console.log("modelViewTransform", modelViewTransform);
return modelViewTransform;
}
export {
estimate,
}