Commit 1594de3f authored by Oleg Dzhimiev's avatar Oleg Dzhimiev

combined ellipse matched

parent 3ece274b
...@@ -30,9 +30,19 @@ function x3dom_delta_markers(){ ...@@ -30,9 +30,19 @@ function x3dom_delta_markers(){
// just a coefficient // just a coefficient
var e2_k = 1/2; var e2_k = 1/2;
var e2_abc = [e2_k*1,e2_k*30,e2_k*1]; var e2_abc = [e2_k*1,e2_k*30,e2_k*1];
var e2_scale = e2_abc.join(","); //var e2_scale = e2_abc.join(",");
//x3dom_draw_ellipsoid("e2","green",e2_c.toString(),"",e2_scale); //x3dom_draw_ellipsoid("e2","green",e2_c.toString(),"",e2_scale);
x3dom_draw_ellipsoid_by_semiaxes_and_center("e2","green",{
O: e2_c,
Ox: new x3dom.fields.SFVec3f(e2_abc[0], 0, 0),
Oy: new x3dom.fields.SFVec3f( 0, e2_abc[1], 0),
Oz: new x3dom.fields.SFVec3f( 0, 0, e2_abc[2])
},0.0);
var RE2 = x3dom.fields.SFMatrix4f.identity();
//RE2.setValue(xa,ya,za);
// draw ellipsoid axes // draw ellipsoid axes
//var xa = new x3dom.fields.SFVec3f(1,0,0); //var xa = new x3dom.fields.SFVec3f(1,0,0);
//var ya = new x3dom.fields.SFVec3f(0,1,0); //var ya = new x3dom.fields.SFVec3f(0,1,0);
...@@ -62,8 +72,8 @@ function x3dom_delta_markers(){ ...@@ -62,8 +72,8 @@ function x3dom_delta_markers(){
var e1_d = x3dom_3d_distance(e1_c.x,e1_c.y,e1_c.z,false); var e1_d = x3dom_3d_distance(e1_c.x,e1_c.y,e1_c.z,false);
var e1_k = 20; var e1_k = 20;
var e1_abc = [e1_k*0.00038*e1_d,e1_k*0.00038*e1_d,2*e1_k*e1_d*e1_d/10000]; var e1_abc = [e1_k*0.00038*e1_d,e1_k*0.00038*e1_d,2*e1_k*e1_d*e1_d/10000];
var e1_scale = e1_abc.join(",");
console.log("e1 scale: "+e1_scale); console.log("e1 abc: "+e1_abc.join(" "));
// draw ellipsoid axes // draw ellipsoid axes
var e1_dir = e1_c; var e1_dir = e1_c;
...@@ -73,13 +83,15 @@ function x3dom_delta_markers(){ ...@@ -73,13 +83,15 @@ function x3dom_delta_markers(){
var ya = xa.cross(e1_dir); var ya = xa.cross(e1_dir);
var za = e1_dir.negate(); var za = e1_dir.negate();
x3dom_draw_line("xa","red", e2_c, e2_c.add(xa.multiply(e1_abc[0]))); // need to normalize?
x3dom_draw_line("ya","green",e2_c, e2_c.add(ya.multiply(e1_abc[1]))); /*
x3dom_draw_line("za","blue", e2_c, e2_c.add(za.multiply(e1_abc[2]))); xa = xa.normalize();
ya = ya.normalize();
za = za.normalize();
*/
//console.log(xa.toString()); var R0 = Data.camera.Matrices.R0;
//console.log(ya.toString()); var R0i = R0.inverse();
//console.log(za.toString());
// next construct rotation matrix // next construct rotation matrix
var RE1 = x3dom.fields.SFMatrix4f.identity(); var RE1 = x3dom.fields.SFMatrix4f.identity();
...@@ -87,204 +99,97 @@ function x3dom_delta_markers(){ ...@@ -87,204 +99,97 @@ function x3dom_delta_markers(){
console.log(RE1.toString()); console.log(RE1.toString());
var translation = new x3dom.fields.SFVec3f(0,0,0); x3dom_draw_ellipsoid_by_semiaxes_and_center("e1b","green",{
var scaleFactor = new x3dom.fields.SFVec3f(1,1,1); O: e2_c,
var rotation = new x3dom.fields.Quaternion(0,0,1,0); Ox: xa.multiply(e1_abc[0]),
var scaleOrientation = new x3dom.fields.Quaternion(0,0,1,0); Oy: ya.multiply(e1_abc[1]),
RE1.getTransform(translation, rotation, scaleFactor, scaleOrientation); Oz: za.multiply(e1_abc[2])
},0.0);
var q = rotation.toAxisAngle(); // now let's get to covariance matrix
var q_str = q[0].toString()+" "+q[1]; var JE1 = x3dom_ellipsoid_inertia_tensor_v2(1,e1_abc[0],e1_abc[1],e1_abc[2]);
var RE1xJE1 = RE1.mult(JE1);
x3dom_draw_ellipsoid("e1b","green",e2_c,q_str,e1_scale); var JE2 = x3dom_ellipsoid_inertia_tensor_v2(1,e2_abc[0],e2_abc[1],e2_abc[2]);
var RE2xJE2 = RE2.mult(JE2);
// now let's get to covariance matrix var C1 = RE1xJE1.mult(RE1xJE1.transpose());
var JE1 = x3dom_ellipsoid_inertia_tensor(1,e1_abc[0],e1_abc[1],e1_abc[2]); var C2 = RE2xJE2.mult(RE2xJE2.transpose());
var RE1i = RE1.inverse();
var RE1iJE1 = RE1i.mult(JE1);
var C = RE1iJE1.mult(RE1iJE1.transpose()); var C = C1.add(C2);
Cn = matrix_x3dom_to_numeric(C); Cn = matrix_x3dom_to_numeric(C);
Bn = numeric.eig(Cn); Bn = numeric.eig(Cn);
console.log(Bn); console.log(Bn);
var m = 1/5; var ee_a = Math.sqrt(Bn.lambda.x[0]);
var ee_b = Math.sqrt(Bn.lambda.x[1]);
var jxx = Math.sqrt(Bn.lambda.x[0]); var ee_c = Math.sqrt(Bn.lambda.x[2]);
var jyy = Math.sqrt(Bn.lambda.x[1]);
var jzz = Math.sqrt(Bn.lambda.x[2]);
var ee_a = Math.sqrt(1/2/m*(jyy+jzz-jxx));
var ee_b = Math.sqrt(1/2/m*(jxx+jzz-jyy));
var ee_c = Math.sqrt(1/2/m*(jxx+jyy-jzz));
console.log("e1 restored scale: "+[ee_a,ee_b,ee_c].join(" ")); console.log("e1 restored abc: "+[ee_a,ee_b,ee_c].join(" "));
var RE1n = new x3dom.fields.SFMatrix4f( var RE1n = new x3dom.fields.SFMatrix4f(
Bn.E.x[0][0], Bn.E.x[1][0], Bn.E.x[2][0], 0, Bn.E.x[0][0], Bn.E.x[0][1], Bn.E.x[0][2], 0,
Bn.E.x[0][1], Bn.E.x[1][1], Bn.E.x[2][1], 0, Bn.E.x[1][0], Bn.E.x[1][1], Bn.E.x[1][2], 0,
Bn.E.x[0][2], Bn.E.x[1][2], Bn.E.x[2][2], 0, Bn.E.x[2][0], Bn.E.x[2][1], Bn.E.x[2][2], 0,
0, 0, 0, 1 0, 0, 0, 1
); );
console.log(RE1n.toString()); console.log(RE1n.toString());
return 0; x3dom_draw_ellipsoid_by_semiaxes_and_center("e1c","red",{
O: e2_c,
//$(".my-markers").find("material").attr("transparency","0.5"); Ox: RE1n.e0().multiply(ee_a),
Oy: RE1n.e1().multiply(ee_b),
//$("#deltalink").remove(); Oz: RE1n.e2().multiply(ee_c)
},0.5);
var coords = [
"0 0 0",
p_w.x+" "+p_w.y+" "+p_w.z,
marker.x+" "+marker.y+" "+marker.z,
"0 0 0",
].join(" ");
var html = [
'<group id="deltalink" class="deltalink">',
' <switch whichChoice="0">',
' <transform>',
' <shape>',
' <appearance>',
' <material emissiveColor="white" transparency="0.0"></material>',
' </appearance>',
' <lineset vertexCount="4" solid="true" ccw="true" lit="true">',
' <coordinate point="'+coords+'"></coordinate>',
' </lineset>',
' </shape>',
' </transform>',
' </switch>',
'</group>'
].join('\n');
var sphere_element = $(html); return 0;
$('scene',Scene.element).append(sphere_element);
//var d = x3dom_3d_distance(p_w.x,p_w.y,p_w.z,false); }
var d = x3dom_3d_distance(marker.x,marker.y,marker.z,false);
//console.log("Distance to pointe is "+d);
// z-axis /*
var mark_dir = new x3dom.fields.SFVec3f(marker.x,marker.y,marker.z); e is an object
mark_dir = mark_dir.normalize(); {
O: x3dom.fields.SFVec3f()
Ox: x3dom.fields.SFVec3f()
Oy: x3dom.fields.SFVec3f()
Oz: x3dom.fields.SFVec3f()
}
*/
function x3dom_draw_ellipsoid_by_semiaxes_and_center(id,color,e,transparency=0.5){
var world_y = new x3dom.fields.SFVec3f(0,1,0); var transl = e.O.toString();
var scale = [e.Ox.length(),e.Oy.length(),e.Oz.length()].join(" ");
var mark_x = mark_dir.cross(world_y); var xa = e.Ox.normalize();
var mark_y = mark_x.cross(mark_dir); var ya = e.Oy.normalize();
var mark_z = mark_dir.negate(); var za = e.Oz.normalize();
var M = x3dom.fields.SFMatrix4f.lookAt(new x3dom.fields.SFVec3f(0,0,0), mark_dir, mark_y); var R = x3dom.fields.SFMatrix4f.identity();
R.setValue(xa,ya,za);
var translation = new x3dom.fields.SFVec3f(0,0,0); var translation = new x3dom.fields.SFVec3f(0,0,0);
var scaleFactor = new x3dom.fields.SFVec3f(1,1,1); var scaleFactor = new x3dom.fields.SFVec3f(1,1,1);
var rotation = new x3dom.fields.Quaternion(0,0,1,0); var rotation = new x3dom.fields.Quaternion(0,0,1,0);
var scaleOrientation = new x3dom.fields.Quaternion(0,0,1,0); var scaleOrientation = new x3dom.fields.Quaternion(0,0,1,0);
M.getTransform(translation, rotation, scaleFactor, scaleOrientation);
console.log("Rotation matrix");
console.log(M.toString());
// draw vectors?!
var aa = rotation.toAxisAngle();
var aa_string = aa[0].toString()+" "+aa[1];
//
// now draw the 1st ellipse
//
mark_dir = new x3dom.fields.SFVec3f(marker.x,marker.y,marker.z);
var k = 20;
var e1_abc = [k*0.00038*d,k*0.00038*d,2*k*d*d/10000];
var model_scale = e1_abc.join(",");
// at marker
//x3dom_draw_ellipsoid("e2a","gold",mark_dir.toString(),aa_string,model_scale);
// overlayed
//x3dom_draw_ellipsoid("e1b","green",transl,aa_string,model_scale);
// now Covariance
// E1 (model, tilted):
var J1 = x3dom_ellipsoid_inertia_tensor(1,e1_abc[0],e1_abc[1],e1_abc[2]);
var R1 = M;
var R1i = R1.inverse();
var R1iJ1 = R1i.mult(J1);
// E2 (map, vertical):
var J2 = x3dom_ellipsoid_inertia_tensor(1,e2_abc[0],e2_abc[1],e2_abc[2]);
var R2 = new x3dom.fields.SFMatrix4f(
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1
);
var R2i = R2.inverse();
var R2iJ2 = R2i.mult(J2);
var C = R1iJ1.mult(R1iJ1.transpose());
//var C = R2iJ2.mult(R2iJ2.transpose());
//C = C.add(R2iJ2.mult(R2iJ2.transpose()));
Cnum = matrix_x3dom_to_numeric(C);
Bnum = numeric.eig(Cnum);
var point = new x3dom.fields.SFVec3f(p_w.x,p_w.y,p_w.z);
var RNEW = new x3dom.fields.SFMatrix4f(
Bnum.E.x[0][0], Bnum.E.x[1][0], Bnum.E.x[2][0], 0,
Bnum.E.x[0][1], Bnum.E.x[1][1], Bnum.E.x[2][1], 0,
Bnum.E.x[0][2], Bnum.E.x[1][2], Bnum.E.x[2][2], 0,
0, 0, 0, 1
);
console.log("R new");
console.log(Rnew.toString());
var xnew = new x3dom.fields.SFVec3f(Bnum.E.x[0][0],Bnum.E.x[1][0],Bnum.E.x[2][0]);
var ynew = new x3dom.fields.SFVec3f(Bnum.E.x[0][1],Bnum.E.x[1][1],Bnum.E.x[2][1]);
var znew = new x3dom.fields.SFVec3f(Bnum.E.x[0][2],Bnum.E.x[1][2],Bnum.E.x[2][2]);
var m = 1/5;
var jxx = Math.sqrt(Bnum.lambda.x[0]);
var jyy = Math.sqrt(Bnum.lambda.x[1]);
var jzz = Math.sqrt(Bnum.lambda.x[2]);
var ee_a = Math.sqrt(1/2/m*(jyy+jzz-jxx)); R.getTransform(translation, rotation, scaleFactor, scaleOrientation);
var ee_b = Math.sqrt(1/2/m*(jxx+jzz-jyy));
var ee_c = Math.sqrt(1/2/m*(jxx+jyy-jzz));
//x3dom_draw_line("yaxis","green",point, point.add(ynew.multiply(ee_b))); var q = rotation.toAxisAngle();
var q_str = q[0].toString()+" "+q[1];
x3dom_draw_line("xaxis","red", point, point.add(xnew.multiply(ee_a)));
x3dom_draw_line("yaxis","green",point, point.add(ynew.multiply(ee_b)));
x3dom_draw_line("zaxis","blue", point, point.add(znew.multiply(ee_c)));
console.log("Covariance matrix");
console.log(C.toString());
// now what if I get transform from this?
var translation = new x3dom.fields.SFVec3f(0,0,0);
var scaleFactor = new x3dom.fields.SFVec3f(1,1,1);
var rotation = new x3dom.fields.Quaternion(0,0,1,0);
var scaleOrientation = new x3dom.fields.Quaternion(0,0,1,0);
C.getTransform(translation, rotation, scaleFactor, scaleOrientation);
console.log("Covariance decomposition"); // draw semi-axes
console.log(translation.toString()); x3dom_draw_line("xa","red", e.O, e.O.add(e.Ox));
console.log(rotation.toString()); x3dom_draw_line("ya","green", e.O, e.O.add(e.Oy));
console.log(scaleFactor.toString()); x3dom_draw_line("za","blue", e.O, e.O.add(e.Oz));
console.log(scaleOrientation.toString());
x3dom_draw_ellipsoid(id,color,transl,q_str,scale,transparency);
} }
function x3dom_draw_ellipsoid(id,color,transl,rotat,scale){ function x3dom_draw_ellipsoid(id,color,transl,rotat,scale,transparency=0.5){
$("#"+id).remove(); $("#"+id).remove();
...@@ -296,7 +201,7 @@ function x3dom_draw_ellipsoid(id,color,transl,rotat,scale){ ...@@ -296,7 +201,7 @@ function x3dom_draw_ellipsoid(id,color,transl,rotat,scale){
' <transform scale="'+scale+'" >', ' <transform scale="'+scale+'" >',
' <shape>', ' <shape>',
' <appearance>', ' <appearance>',
' <material diffuseColor="'+color+'" transparency="0.5"></material>', ' <material diffuseColor="'+color+'" transparency="'+transparency+'"></material>',
' </appearance>', ' </appearance>',
' <sphere radius="1"></sphere>', ' <sphere radius="1"></sphere>',
' </shape>', ' </shape>',
...@@ -370,6 +275,17 @@ function x3dom_ellipsoid_inertia_tensor(m,a,b,c){ ...@@ -370,6 +275,17 @@ function x3dom_ellipsoid_inertia_tensor(m,a,b,c){
} }
function x3dom_ellipsoid_inertia_tensor_v2(m,a,b,c){
return new x3dom.fields.SFMatrix4f(
a, 0, 0, 0,
0, b, 0, 0,
0, 0, c, 0,
0, 0, 0, 1
);
}
// also from 4x4 to 3x3 // also from 4x4 to 3x3
function matrix_x3dom_to_numeric(m){ function matrix_x3dom_to_numeric(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