Skip to content

X
x3domlet
Commit 7454bb71 authored by Oleg Dzhimiev's avatar Oleg Dzhimiev
Browse files
Options

testing derivatives

parent 8760fe49
Hide whitespace changes
Inline Side-by-side
Showing with 300 additions and 23 deletions
js/x3dom_deltas.js
View file @ 7454bb71
......@@ -29,6 +29,17 @@ function x3dom_delta_markers(){
var Error_vector = get_E(marker_i,parameters);
var dx = 1;
var nd = nd_E_heading(marker_i,parameters,0.00001);
console.log("Numerical dE/dh (vector) = "+nd.toString());
var ad = ad_E_heading(marker_i,parameters);
console.log("Analytical dE/dh (vector) = "+ad.toString());
var ad2 = ad2_E_heading(marker_i,parameters);
console.log("Analytical2 dE/dh (vector) = "+ad2.toString());
var C = get_C(marker_i,parameters);
var Ci = C.inverse();
......@@ -36,9 +47,11 @@ function x3dom_delta_markers(){
var AtA = A.transpose().mult(A);
/*
console.log("Test: C^-1 = AtA ?");
console.log("Ci: "+Ci.toString());
console.log("AtA: "+AtA.toString());
*/
/*
var e1_c = new x3dom.fields.SFVec3f(marker.x,marker.y,marker.z);
......@@ -65,7 +78,240 @@ function get_E(mark,pars){
}
// ad_ == analytical derivative
function ad_E_heading(mark,pars){
var A = get_A(mark,pars);
var dA = dA_heading(mark,pars);
var v = get_v(mark,pars);
var dv = dv_heading(mark,pars);
console.log("AD1:");
console.log("dA1: "+dA.toString());
console.log("dv1: "+dv.toString());
var part1 = A.multMatrixVec(dv);
var part2 = dA.multMatrixVec(v);
var result = part1.add(part2);
return result;
}
// ad_ == analytical derivative
function ad2_E_heading(mark,pars){
var A = get_A(mark,pars);
var dA = get_dA(mark,pars,dHTR_dheading);
var v = get_v(mark,pars);
var dv = get_dv(mark,pars,dHTR_dheading);
console.log("AD2:");
console.log("dA2: "+dA.toString());
console.log("dv2: "+dv.toString());
var part1 = A.multMatrixVec(dv);
var part2 = dA.multMatrixVec(v);
var result = part1.add(part2);
return result;
}
// nd_ == numerical derivative
function nd_E_heading(mark,pars,dx=0.001){
// in degrees
//var dx = 0.01;
var p1 = JSON.parse(JSON.stringify(pars));
var p2 = JSON.parse(JSON.stringify(pars));
p2.heading += dx;
//console.log(p1);
//console.log(p2);
var y1 = get_E(mark,p1);
var y2 = get_E(mark,p2);
var dy = y2.subtract(y1).divide(dx);
return dy;
}
function dC_heading(mark,pars){
var M1 = dC1_heading(mark,pars);
var M2 = dC2_heading(mark,pars);
var dC = M1.add(M2);
return dC;
}
function dC1_heading(mark,pars){
var RE1 = get_RE1(mark,pars);
var dRE1 = dRE1_heading(mark,pars);
var JE1 = get_JE1(mark,pars);
var JE1xJE1t = JE1.mult(JE1.transpose());
var P1 = dRE1.mult(JE1xJE1t).mult(RE1);
var P2 = RE1.mult(JE1xJE1t).mult(dRE1);
var RES = P1.add(P2);
return RES;
}
function dC2_heading(mark,pars){
var M = x3dom.fields.SFMatrix4f.zeroMatrix();
return M;
}
/*
function get_dC1(mark,pars){
var RE1 = get_RE1(mark,pars);
var dRE1 = get_dRE1(mark,pars);
var JE1 = get_JE1(mark,pars);
var JE1xJE1t = JE1.mult(JE1.transpose());
var P1 = dRE1.mult(JE1xJE1t).mult(RE1);
var P2 = RE1.mult(JE1xJE1t).mult(dRE1);
var RES = P1.add(P2);
return RES;
}
function get_dC2(mark,pars){
var M = x3dom.fields.SFMatrix4f.zeroMatrix();
return M;
}
*/
function dRE1_heading(mark,pars){
// a'
var e1_model = new x3dom.fields.SFVec3f(mark.model.x,mark.model.y,mark.model.z);
var e1_world = get_e1_w(mark,pars);
var a = e1_world;
var heading = pars.heading*RPD;
var tilt = (pars.tilt-90)*RPD;
var roll = pars.roll*RPD;
var T = x3dom_toYawPitchRoll();
var M = x3dom.fields.SFMatrix4f.zeroMatrix();
M.add(dHTR(heading,tilt,roll));
//M.add(HdTR(heading,tilt,roll));
//M.add(HTdR(heading,tilt,roll));
var d_e1_world = M.mult(T).multMatrixVec(e1_model).multiply(RPD);
var da = d_e1_world;
var dR1 = new x3dom.fields.SFMatrix4f(
-da.z, -da.x*a.z-a.x*da.z, da.x, 0,
0, 2*a.x*da.x+2*a.z*da.z, da.y, 0,
da.x, -da.y*a.z-a.y*da.z, da.z, 0,
0, 0, 0, 1
);
return dR1;
}
function dA_heading(mark,pars){
var C = get_C(mark,pars);
var dC = dC_heading(mark,pars);
console.log("dC(dA_heading): "+dC.toString());
Cn = matrix_x3dom_to_numeric(C);
Bn = numeric.eig(Cn);
var a = Bn.lambda.x[0];
var b = Bn.lambda.x[1];
var c = Bn.lambda.x[2];
var sa = Math.sqrt(a);
var sb = Math.sqrt(b);
var sc = Math.sqrt(c);
var vec0 = new x3dom.fields.SFVec3f(Bn.E.x[0][0],Bn.E.x[0][1],Bn.E.x[0][2]);
var vec1 = new x3dom.fields.SFVec3f(Bn.E.x[1][0],Bn.E.x[1][1],Bn.E.x[1][2]);
var vec2 = new x3dom.fields.SFVec3f(Bn.E.x[2][0],Bn.E.x[2][1],Bn.E.x[2][2]);
var R = new x3dom.fields.SFMatrix4f(
vec0.x, vec1.x, vec2.x, 0,
vec0.y, vec1.y, vec2.y, 0,
vec0.z, vec1.z, vec2.z, 0,
0, 0, 0, 1
);
var J = new x3dom.fields.SFMatrix4f(
sa, 0, 0, 0,
0, sb, 0, 0,
0, 0, sc, 0,
0, 0, 0, 1
);
var Ji = J.inverse();
var da = vec0.dot(dC.multMatrixVec(vec0));
var db = vec1.dot(dC.multMatrixVec(vec1));
var dc = vec2.dot(dC.multMatrixVec(vec2));
var dJi = new x3dom.fields.SFMatrix4f(
0.5/sa*da, 0, 0, 0,
0, 0.5/sb*db, 0, 0,
0, 0, 0.5/sc*dc, 0,
0, 0, 0, 1
);
var dvec0 = get_dvec(C,dC,a,da,vec0);
var dvec1 = get_dvec(C,dC,b,db,vec1);
var dvec2 = get_dvec(C,dC,c,dc,vec2);
var dR = new x3dom.fields.SFMatrix4f(
dvec0.x, dvec1.x, dvec2.x, 0,
dvec0.y, dvec1.y, dvec2.y, 0,
dvec0.z, dvec1.z, dvec2.z, 0,
0, 0, 0, 1
);
// Q = JiR
// dQ = dJi x R + Ji x dR
// dQ = dJi x R + Ji x dR
var dA = dJi.mult(R).add(Ji.mult(dR));
// ok
return dA;
}
// J1
function get_JE1(mark,pars){
......@@ -135,8 +381,8 @@ function get_C(mark,pars){
var JE2 = get_JE2(mark,pars);
var RE2 = get_RE2(mark,pars);
// debugging
/*
var e2_world = get_e2_w(mark,pars);
var M2W = get_e1_M2W(mark,pars);
var W2M = M2W.inverse();
......@@ -154,6 +400,7 @@ function get_C(mark,pars){
Oy: W2M.multMatrixVec(RE2.e1().multiply(JE2._11)),
Oz: W2M.multMatrixVec(RE2.e2().multiply(JE2._22))
},transparency=0.3);
*/
// now let's get to covariance matrix
var RE1xJE1 = RE1.mult(JE1);
......@@ -176,10 +423,11 @@ function get_C(mark,pars){
}
function get_dC(mark,pars){
// general - can do via callback
function get_dC(mark,pars,callback){
var M1 = get_dC1(mark,pars);
var M2 = get_dC2(mark,pars);
var M1 = get_dC1(mark,pars,callback);
var M2 = get_dC2(mark,pars,callback);
var dC = M1.add(M2);
......@@ -229,16 +477,20 @@ function get_A(mark,pars){
var M2W = get_e1_M2W(mark,pars);
var W2M = M2W.inverse();
/*
x3dom_draw_ellipsoid_by_semiaxes_and_center("ec1","red",{
O: W2M.multMatrixVec(e2_world),
Ox: W2M.multMatrixVec(R.transpose().e0().multiply(sa)),
Oy: W2M.multMatrixVec(R.transpose().e1().multiply(sb)),
Oz: W2M.multMatrixVec(R.transpose().e2().multiply(sc))
},transparency=0.6);
*/
/*
console.log("Eigen test:");
console.log("C = RAR^-1: "+C.toString());
console.log("A: "+J.mult(J).toString());
*/
// incorrect
var T1 = R.mult(J).mult(J).mult(R.inverse());
......@@ -254,17 +506,20 @@ function get_A(mark,pars){
}
//E = Av
function get_dA(mark,pars){
// callback is the needed derivative function
function get_dA(mark,pars,callback){
var C = get_C(mark,pars);
var dC = get_dC(mark,pars);
var dC = get_dC(mark,pars,callback);
console.log("dC(get_dA): "+dC.toString());
Cn = matrix_x3dom_to_numeric(C);
Bn = numeric.eig(Cn);
var a = Math.sqrt(Bn.lambda.x[0]);
var b = Math.sqrt(Bn.lambda.x[1]);
var c = Math.sqrt(Bn.lambda.x[2]);
var a = Bn.lambda.x[0];
var b = Bn.lambda.x[1];
var c = Bn.lambda.x[2];
var sa = Math.sqrt(a);
var sb = Math.sqrt(b);
......@@ -290,7 +545,6 @@ function get_dA(mark,pars){
var Ji = J.inverse();
var da = vec0.dot(dC.multMatrixVec(vec0));
var db = vec1.dot(dC.multMatrixVec(vec1));
var dc = vec2.dot(dC.multMatrixVec(vec2));
......@@ -302,9 +556,9 @@ function get_dA(mark,pars){
0, 0, 0, 1
);
var dvec0 = get_dvec(C,dC,a,da);
var dvec1 = get_dvec(C,dC,b,db);
var dvec2 = get_dvec(C,dC,c,dc);
var dvec0 = get_dvec(C,dC,a,da,vec0);
var dvec1 = get_dvec(C,dC,b,db,vec1);
var dvec2 = get_dvec(C,dC,c,dc,vec2);
var dR = new x3dom.fields.SFMatrix4f(
dvec0.x, dvec1.x, dvec2.x, 0,
......@@ -316,6 +570,8 @@ function get_dA(mark,pars){
// Q = JiR
// dQ = dJi x R + Ji x dR
// dQ = dJi x R + Ji x dR
var dA = dJi.mult(R).add(Ji.mult(dR));
// ok
......@@ -326,8 +582,8 @@ function get_dA(mark,pars){
function get_dvec(A,dA,lambda,dlambda,vec){
var I = x3dom.fields.SFMatrix4f.identity();
var M1 = A.subtract(I.multiply(lambda)).inverse();
var M2 = I.multiply(dlambda).subtract(dA);
var M1 = A.add(I.multiply(lambda).negate()).inverse();
var M2 = I.multiply(dlambda).add(dA.negate());
var dvec = M1.mult(M2).multMatrixVec(vec);
return dvec;
......@@ -359,9 +615,16 @@ function get_v(mark,pars){
return v;
}
function get_dv(mark,pars){
function get_dv(mark,pars,callback){
return new x3dom.fields.SFVec3f(0,0,0);
//return new x3dom.fields.SFVec3f(0,0,0);
var M = callback(mark,pars);
var T = x3dom_toYawPitchRoll();
var e1_model = new x3dom.fields.SFVec3f(mark.model.x,mark.model.y,mark.model.z);
var dv = M.mult(T).multMatrixVec(e1_model);
return dv;
}
......@@ -447,7 +710,7 @@ function dv_roll(mark,pars){
}
function get_dRE1(mark,pars){
function get_dRE1(mark,pars,callback){
// a'
var e1_model = new x3dom.fields.SFVec3f(mark.model.x,mark.model.y,mark.model.z);
......@@ -460,12 +723,16 @@ function get_dRE1(mark,pars){
var T = x3dom_toYawPitchRoll();
/*
var M = x3dom.fields.SFMatrix4f.zeroMatrix();
M.add(dHTR(heading,tilt,roll));
M.add(HdTR(heading,tilt,roll));
M.add(HTdR(heading,tilt,roll));
*/
var d_e1_world = M.mult(T).multMatrixVec(e1_model).multiply(RPD);
var M = callback(mark,pars);
var d_e1_world = M.mult(T).multMatrixVec(e1_model);
var da = d_e1_world;
......@@ -480,10 +747,21 @@ function get_dRE1(mark,pars){
}
function get_dC1(mark,pars){
function dHTR_dheading(mark,pars){
var heading = pars.heading*RPD;
var tilt = (pars.tilt-90)*RPD;
var roll = pars.roll*RPD;
var M = dHTR(heading,tilt,roll).multiply(RPD);
return M;
}
function get_dC1(mark,pars,callback){
var RE1 = get_RE1(mark,pars);
var dRE1 = get_dRE1(mark,pars);
var dRE1 = get_dRE1(mark,pars,callback);
var JE1 = get_JE1(mark,pars);
var JE1xJE1t = JE1.mult(JE1.transpose());
......@@ -497,10 +775,9 @@ function get_dC1(mark,pars){
}
function get_dC2(mark,pars){
function get_dC2(mark,pars,callback){
var M = x3dom.fields.SFMatrix4f.zeroMatrix();
return M;
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment