Commit 6ce3a7a8 authored by Oleg Dzhimiev's avatar Oleg Dzhimiev

alignment code clean up, todo: error reporting and mode on/off by checkbox

parent f2be9efd
...@@ -188,7 +188,7 @@ html, body, #x3d_wrapper { ...@@ -188,7 +188,7 @@ html, body, #x3d_wrapper {
} }
#align_button{ #align_button{
background-image:url('images/align.png'); background-image:url('images/ic_explore_black_48dp_1x.png');
background-size: 32px 32px; background-size: 32px 32px;
background-repeat: no-repeat; background-repeat: no-repeat;
...@@ -307,4 +307,18 @@ html, body, #x3d_wrapper { ...@@ -307,4 +307,18 @@ html, body, #x3d_wrapper {
user-select:none; user-select:none;
} }
#aa1_dialog{
position:absolute;
top:2px;
left:2px;
background: rgba(256,256,256,1);
padding:5px;
}
#aa1_dialog td{
padding: 0px 5px;
}
#aa1_dialog button{
margin: 0px 5px;
}
...@@ -3,7 +3,7 @@ function align_init(){ ...@@ -3,7 +3,7 @@ function align_init(){
$("#align_button").on("click",function(){ $("#align_button").on("click",function(){
//align_heading(); //align_heading();
x3dom_align_0(); x3dom_align_GN();
}); });
/* /*
...@@ -14,38 +14,59 @@ function align_init(){ ...@@ -14,38 +14,59 @@ function align_init(){
} }
function test_markers_set1(){ /*
* Check which markers need to be moved to have both 3d and map coorinates
* returns false if yes
* true - all's fine
*/
function check_markers(){
Data.camera.kml.latitude = 40.7233861; var c1,c2;
Data.camera.kml.longitude = -111.9328843; var result = true;
Data.camera.kml.heading = 62;
Data.markers = [ for(var i=0;i<Data.markers.length;i++){
{align:{ latitude: 40.72362633635111, longitude: -111.93257600069047, x:-9.079290749776595, y:-14.27794573338788, z: -32.46383785654424 }},
{align:{ latitude: 40.7234408473505, longitude: -111.93217568099502, x:23.90413018819188, y:-16.192438967265613, z: -53.91987886096472 }},
{align:{ latitude: 40.7239048229759, longitude: -111.93186186254026, x:-8.800276069589225, y:-17.382935178801347, z:-100.34033327103612 }}
];
} c1 = Data.markers[i].d_map;
c2 = Data.markers[i].d_x3d;
if (c1.toString().indexOf("drag")!=-1){
console.log("error: marker "+i+": drag over map. Mouse over shows a marker info.");
result = false;
}
if (c2.toString().indexOf("drag")!=-1){
console.log("error: marker "+i+": drag over 3D scene. Mouse over shows a marker info.");
result = false;
}
}
function x3dom_align_0(){ return result;
//get all points }
//console.log("Base");
//console.log(Data.camera);
//console.log("Markers");
//console.log(Data.markers);
//test_markers_set1(); /*
* run the Gauss-Newton algorithm iterations
*/
function x3dom_align_GN(){
// need at least 3 points
if (Data.markers != undefined){
if (Data.markers.length<3){
console.log("Alignment error: place at least 3 points");
return -1;
}
}else{
console.log("Alignment error: place at least 3 points");
return -1;
}
//var base = Data.camera; if (!check_markers()){
// base.x console.log("Alignment error: marker has not been moved over 3D or Map");
// base.y return -2;
// base.z }
// base.latitude
// base.longitude
//initial? // initial approximation:
var x0 = Data.camera.kml.latitude; var x0 = Data.camera.kml.latitude;
var y0 = Data.camera.kml.longitude; var y0 = Data.camera.kml.longitude;
var h0 = Data.camera.kml.heading; var h0 = Data.camera.kml.heading;
...@@ -56,31 +77,6 @@ function x3dom_align_0(){ ...@@ -56,31 +77,6 @@ function x3dom_align_0(){
//test_AxV(); //test_AxV();
//test_Ainv(); //test_Ainv();
//return -1;
/*
console.log("Initial approximation: "+x0+" "+y0+" "+h0);
for(var i=0;i<Data.markers.length;i++){
console.log("Marker "+i+": "+Data.markers[i].align.latitude+" "+Data.markers[i].align.longitude+" "+Data.markers[i].align.x+" "+Data.markers[i].align.y+" "+Data.markers[i].align.z);
//console.log(f1_3d_i(i,x0,y0,h0)+" - "+f2_map_i(i,x0,y0,h0)+" = "+r_i(i,x0,y0,h0)+" also, final bearing: "+f2_map_i_inverse(i,x0,y0,h0));
//console.log(f1_3d_i(i,x0,y0,h0)+" - "+f2_map_i(i,x0,y0,h0)+" = "+r_i(i,x0,y0,h0));
}
*/
/*
for(var i=0;i<Data.markers.length;i++){
//console.log(Data.markers[i].align.latitude+" "+Data.markers[i].align.longitude+" "+Data.markers[i].align.x+" "+Data.markers[i].align.y+" "+Data.markers[i].align.z);
//console.log(f1_3d_i(i,x0,y0,h0)+" - "+f2_map_i(i,x0,y0,h0)+" = "+r_i(i,x0,y0,h0)+" also, final bearing: "+f2_map_i_inverse(i,x0,y0,h0));
console.log(f1_3d_i(i,x0,y0,h0)+" - "+f2_map_i(i,x0,y0,h0)+" = "+r_i(i,x0,y0,h0));
}
console.log("Begin");
*/
var ε = 0.000000001; var ε = 0.000000001;
var iterate = true; var iterate = true;
...@@ -88,9 +84,6 @@ function x3dom_align_0(){ ...@@ -88,9 +84,6 @@ function x3dom_align_0(){
var result = 0; var result = 0;
var xyh = [x0,y0,h0]; var xyh = [x0,y0,h0];
//console.log("Iteration 0, initial: "+xyh[0]+" "+xyh[1]+" "+xyh[2]);
//console.log("Error function value: "+sigma(xyh[0],xyh[1],xyh[2]));
while(iterate){ while(iterate){
counter++; counter++;
...@@ -113,7 +106,7 @@ function x3dom_align_0(){ ...@@ -113,7 +106,7 @@ function x3dom_align_0(){
//console.log("Error function value: "+sigma(xyh_new[0],xyh_new[1],xyh_new[2])); //console.log("Error function value: "+sigma(xyh_new[0],xyh_new[1],xyh_new[2]));
if (counter==150){ if (counter==1000){
iterate = false; iterate = false;
} }
...@@ -129,147 +122,46 @@ function x3dom_align_0(){ ...@@ -129,147 +122,46 @@ function x3dom_align_0(){
} }
function apply_alignment_dialog(xyh0,xyh1,c,e){ /*
* calc the next iteration values
var d = $("<div>",{id:"aa1_dialog"}); */
var dc = $([
'<div>Alignment algorithm results</div>',
'<br/>',
'<div>Error: <b>'+e+' &deg;</b></div>',
'<div>Iterations: <b>'+c+'</b></div>',
'<div>',
'<table>',
' <tr>',
' <th></th>',
' <th>Latitude</th>',
' <th>Longitude</th>',
' <th>Heading</th>',
' </tr>',
' <tr>',
' <th>old</th>',
' <td>'+xyh0[0]+'</td>',
' <td>'+xyh0[1]+'</td>',
' <td>'+xyh0[2]+'</td>',
' </tr>',
' <tr>',
' <th>new</th>',
' <td>'+xyh1[0]+'</td>',
' <td>'+xyh1[1]+'</td>',
' <td>'+xyh1[2]+'</td>',
' </tr>',
'</table>',
'</div>',
'<br/>'
].join('\n'));
d.append(dc);
var b1 = $("<button>").html("apply");
b1.on('click',function(){
apply_alignment(xyh1);
b2.click();
});
var b2 = $("<button>").html("cancel");
b2.on('click',function(){
$("#aa1_dialog").remove();
});
d.append($("<div>").append(b1).append(b2));
$("body").append(d);
}
function apply_alignment(xyh){
var Camera = Map.marker;
Data.camera.heading = xyh[2];
Data.camera.latitude = xyh[0];
Data.camera.longitude = xyh[1];
Data.camera.kml.heading = xyh[2];
Data.camera.kml.latitude = xyh[0];
Data.camera.kml.longitude = xyh[1];
//update initial location and heading
x3d_initial_camera_placement();
}
function sigma(x,y,h){
var sum = 0
for(var i=0;i<Data.markers.length;i++){
sum += r_i(i,x,y,h)*r_i(i,x,y,h);
}
sum = Math.sqrt(sum/Data.markers.length);
return sum;
}
function GaussNewtonAlgorithm(x,y,h){ function GaussNewtonAlgorithm(x,y,h){
var J = Jacobian(x,y,h); var J = Jacobian(x,y,h);
var Jt = At(J); var Jt = At(J);
//console.log(J);
//console.log(Jt);
var JtJ = AxB(Jt,J); var JtJ = AxB(Jt,J);
//console.log(JtJ);
//console.log("Determinant: "+Adet(JtJ));
var JtJi = Ainv(JtJ); var JtJi = Ainv(JtJ);
//console.log(AxB(JtJ,JtJi));
var JtJixJt = AxB(JtJi,Jt); var JtJixJt = AxB(JtJi,Jt);
// console.log(J);
// console.log(JtJ);
// console.log(JtJi);
// console.log("testing JtJ x JtJ_inv");
// console.log(AxB(JtJi,JtJ));
// console.log(AxB(JtJ,JtJi));
// console.log(Jt);
// console.log(JtJixJt);
//console.log("JtJixJt");
//console.log(JtJixJt);
var Vr = []; var Vr = [];
for(var i=0;i<Data.markers.length;i++){ for(var i=0;i<Data.markers.length;i++){
Vr[i] = r_i(i,x,y,h); Vr[i] = r_i(i,x,y,h);
} }
//console.log("Vr");
//console.log(Vr);
//console.log("Vr");
//console.log(Vr);
var d = AxV(JtJixJt,Vr); var d = AxV(JtJixJt,Vr);
//console.log("JtJixJt x Vr");
//console.log(d);
//var k = 1/10;
var k = 1; var k = 1;
return [x-k*d[0], y-k*d[1], h-k*d[2]]; return [x-k*d[0], y-k*d[1], h-k*d[2]];
} }
/*
* sum of squared residuals - criterion for stopping
*/
function sigma(x,y,h){
var sum = 0
for(var i=0;i<Data.markers.length;i++){
sum += r_i(i,x,y,h)*r_i(i,x,y,h);
}
sum = Math.sqrt(sum/Data.markers.length);
return sum;
}
/*
* heading in degrees from 3D model
*/
function f1_3d_i(i,x,y,h){ function f1_3d_i(i,x,y,h){
var base = Data.camera; var base = Data.camera;
var mark = Data.markers[i]; var mark = Data.markers[i];
...@@ -278,6 +170,9 @@ function f1_3d_i(i,x,y,h){ ...@@ -278,6 +170,9 @@ function f1_3d_i(i,x,y,h){
return res; return res;
} }
/*
* heading in degrees from map
*/
function f2_map_i(i,x,y,h){ function f2_map_i(i,x,y,h){
var mark = Data.markers[i]; var mark = Data.markers[i];
...@@ -307,43 +202,16 @@ function f2_map_i(i,x,y,h){ ...@@ -307,43 +202,16 @@ function f2_map_i(i,x,y,h){
} }
function f2_map_i_inverse(i,x,y,h){ /*
* residuals function
var mark = Data.markers[i]; */
var p1_ll = new L.LatLng(mark.align.latitude,mark.align.longitude);
var p2_ll = new L.LatLng(x,y);
//console.log(p1_ll);
//console.log(p2_ll);
p1_ll.lat = p1_ll.lat*Math.PI/180;
p1_ll.lng = p1_ll.lng*Math.PI/180;
p2_ll.lat = p2_ll.lat*Math.PI/180;
p2_ll.lng = p2_ll.lng*Math.PI/180;
var dlat = p2_ll.lat-p1_ll.lat;
var dlon = p2_ll.lng-p1_ll.lng;
var dy = Math.sin(dlon)*Math.cos(p2_ll.lat);
var dx = Math.cos(p1_ll.lat)*Math.sin(p2_ll.lat)-Math.sin(p1_ll.lat)*Math.cos(p2_ll.lat)*Math.cos(dlon);
//console.log("dy = "+dy+" dx = "+dx);
var res = 180/Math.PI*Math.atan2(dy,dx);
return res;
}
function r_i(i,x,y,h){ function r_i(i,x,y,h){
return (f1_3d_i(i,x,y,h)-f2_map_i(i,x,y,h)); return (f1_3d_i(i,x,y,h)-f2_map_i(i,x,y,h));
} }
function dr_dh_i(i,x,y,h){ /*
return 1; * dr/dx(i)
} */
function dr_dx_i(i,x,y,h){ function dr_dx_i(i,x,y,h){
var mark = Data.markers[i]; var mark = Data.markers[i];
...@@ -372,7 +240,9 @@ function dr_dx_i(i,x,y,h){ ...@@ -372,7 +240,9 @@ function dr_dx_i(i,x,y,h){
return res; return res;
} }
/*
* dr/dy(i)
*/
function dr_dy_i(i,x,y,h){ function dr_dy_i(i,x,y,h){
var mark = Data.markers[i]; var mark = Data.markers[i];
...@@ -401,7 +271,16 @@ function dr_dy_i(i,x,y,h){ ...@@ -401,7 +271,16 @@ function dr_dy_i(i,x,y,h){
return res; return res;
} }
/*
* dr/dh(i)
*/
function dr_dh_i(i,x,y,h){
return 1;
}
/*
* Jacobi matrix
*/
function Jacobian(x,y,h){ function Jacobian(x,y,h){
var J = []; var J = [];
...@@ -412,6 +291,7 @@ function Jacobian(x,y,h){ ...@@ -412,6 +291,7 @@ function Jacobian(x,y,h){
var mark = Data.markers[i]; var mark = Data.markers[i];
J[i]=[ dr_dx_i(i,x,y,h), dr_dy_i(i,x,y,h), dr_dh_i(i,x,y,h)]; J[i]=[ dr_dx_i(i,x,y,h), dr_dy_i(i,x,y,h), dr_dh_i(i,x,y,h)];
/* /*
e0 = 0.000000001; e0 = 0.000000001;
e1 = 0.000000001; e1 = 0.000000001;
...@@ -425,27 +305,23 @@ function Jacobian(x,y,h){ ...@@ -425,27 +305,23 @@ function Jacobian(x,y,h){
dri_dy_num = (r_i(i,x ,y+e1,h )-r_i(i,x ,y-e1,h ))/e1/2; dri_dy_num = (r_i(i,x ,y+e1,h )-r_i(i,x ,y-e1,h ))/e1/2;
dri_dh_num = (r_i(i,x ,y ,h+e2)-r_i(i,x ,y ,h-e2))/e2/2; dri_dh_num = (r_i(i,x ,y ,h+e2)-r_i(i,x ,y ,h-e2))/e2/2;
console.log("CAL: "+dri_dx_cal.toFixed(10)+" "+dri_dy_cal.toFixed(10)+" "+dri_dh_cal.toFixed(4)); console.log("CALC: "+dri_dx_cal.toFixed(10)+" "+dri_dy_cal.toFixed(10)+" "+dri_dh_cal.toFixed(4));
console.log("NUM: "+dri_dx_num.toFixed(10)+" "+dri_dy_num.toFixed(10)+" "+dri_dh_num.toFixed(4)); console.log("NUME: "+dri_dx_num.toFixed(10)+" "+dri_dy_num.toFixed(10)+" "+dri_dh_num.toFixed(4));
*/ */
} }
return J; return J;
} }
/* /*
1 1 1 * Utility functions
1 1 1 1 1 1 1 1 */
1 1 1 1 1 1 1 1
1 1 1 1 1 1 1 1
1 1 1
*/
//tested
function AxB(A,B){
//A.length == B[0].length /*
//A[0].length == B.length * AxB, any dimensions
*/
function AxB(A,B){
var m1 = A.length; var m1 = A.length;
var n1 = A[0].length; var n1 = A[0].length;
...@@ -474,7 +350,9 @@ function AxB(A,B){ ...@@ -474,7 +350,9 @@ function AxB(A,B){
} }
//tested /*
* Determinant for 3x3 only
*/
function Adet(A){ function Adet(A){
var m = A.length; var m = A.length;
...@@ -495,7 +373,9 @@ function Adet(A){ ...@@ -495,7 +373,9 @@ function Adet(A){
} }
//tested /*
* Inverted matrix for 3x3 only
*/
function Ainv(A){ function Ainv(A){
var m = A.length; var m = A.length;
...@@ -520,7 +400,9 @@ function Ainv(A){ ...@@ -520,7 +400,9 @@ function Ainv(A){
]; ];
} }
//tested /*
* Transposed matrix, any dimensions
*/
function At(A){ function At(A){
var R = []; var R = [];
...@@ -534,7 +416,9 @@ function At(A){ ...@@ -534,7 +416,9 @@ function At(A){
return R; return R;
} }
//tested /*
* Matrix x Vector - any dimensions
*/
function AxV(A,V){ function AxV(A,V){
var Vr = []; var Vr = [];
...@@ -560,41 +444,108 @@ function AxV(A,V){ ...@@ -560,41 +444,108 @@ function AxV(A,V){
return Vr; return Vr;
} }
function align_heading(){ /*
* ui dialog to apply or cancel results
*/
function apply_alignment_dialog(xyh0,xyh1,c,e){
var d = $("<div>",{id:"aa1_dialog"});
var dc = $([
'<div>Alignment algorithm results</div>',
'<br/>',
'<div>Error: <b>'+e+' &deg;</b></div>',
'<div>Iterations: <b>'+c+'</b></div>',
'<div>',
'<table>',
' <tr>',
' <th></th>',
' <th>Latitude</th>',
' <th>Longitude</th>',
' <th>Heading</th>',
' </tr>',
' <tr>',
' <th>old</th>',
' <td>'+xyh0[0]+'</td>',
' <td>'+xyh0[1]+'</td>',
' <td>'+xyh0[2]+'</td>',
' </tr>',
' <tr>',
' <th>new</th>',
' <td>'+xyh1[0]+'</td>',
' <td>'+xyh1[1]+'</td>',
' <td>'+xyh1[2]+'</td>',
' </tr>',
'</table>',
'</div>',
'<br/>'
].join('\n'));
d.append(dc);
var b1 = $("<button>").html("apply");
b1.on('click',function(){
apply_alignment(xyh1);
b2.click();
});
var b2 = $("<button>").html("cancel");
b2.on('click',function(){
$("#aa1_dialog").remove();
});
d.append($("<div>").append(b1).append(b2));
$("body").append(d);
}
/*
* actual apply function
*/
function apply_alignment(xyh){
// find selected markers var Camera = Map.marker;
// pick the first one?
// align?!
console.log("heading");
var map_markers = Map.marker._measureMarkers; Data.camera.heading = xyh[2];
var selected_markers = []; Data.camera.latitude = xyh[0];
Data.camera.longitude = xyh[1];
map_markers.forEach(function(c,i){ Data.camera.kml.heading = xyh[2];
if (selected_markers.length<2){ Data.camera.kml.latitude = xyh[0];
if (c._selected){ Data.camera.kml.longitude = xyh[1];
selected_markers.push(c); //update initial location and heading
}
}
});
if (selected_markers.length<2){ Map.marker.setHeading(xyh[2]);
console.log("select 2 markers"); Map.marker.setBasePoint(new L.LatLng(xyh[0],xyh[1]));
Scene.showMessage("messagewindow","error: select 2 markers","red"); Map.marker._syncMeasureMarkersToBasePoint();
}
//update on 3d
var p1_ll = Camera._latlng;
for(var i=0;i<Data.markers.length;i++){
var p2_ll = Camera._measureMarkers[i]._latlng;
leaf_update_x3dom_marker(p1_ll,p2_ll,i);
}
console.log(selected_markers); x3d_initial_camera_placement();
} }
/*
* not used
*/
function align_roll(){ function align_roll(){
console.log("roll"); console.log("roll");
} }
/*
* not used
*/
function align_tilt(){ function align_tilt(){
console.log("tilt"); console.log("tilt");
...@@ -687,3 +638,16 @@ function test_AxB(){ ...@@ -687,3 +638,16 @@ function test_AxB(){
console.log("testing AxB: end"); console.log("testing AxB: end");
} }
function test_markers_set1(){
Data.camera.kml.latitude = 40.7233861;
Data.camera.kml.longitude = -111.9328843;
Data.camera.kml.heading = 62;
Data.markers = [
{align:{ latitude: 40.72362633635111, longitude: -111.93257600069047, x:-9.079290749776595, y:-14.27794573338788, z: -32.46383785654424 }},
{align:{ latitude: 40.7234408473505, longitude: -111.93217568099502, x:23.90413018819188, y:-16.192438967265613, z: -53.91987886096472 }},
{align:{ latitude: 40.7239048229759, longitude: -111.93186186254026, x:-8.800276069589225, y:-17.382935178801347, z:-100.34033327103612 }}
];
}
...@@ -58,7 +58,7 @@ var SETTINGS = { ...@@ -58,7 +58,7 @@ var SETTINGS = {
'path' : "1487451413_967079", 'path' : "1487451413_967079",
'version': "v1", 'version': "v1",
'experimental': false, 'experimental': false,
'edit': false, 'edit': true,
'files': { 'files': {
'x3d':"", 'x3d':"",
'x3d_background':"", 'x3d_background':"",
...@@ -722,28 +722,33 @@ function leaf_drag_marker(){ ...@@ -722,28 +722,33 @@ function leaf_drag_marker(){
var p1_ll = Camera._latlng; var p1_ll = Camera._latlng;
var p2_ll = Camera.draggedMarker._latlng; var p2_ll = Camera.draggedMarker._latlng;
var mark = Data.markers[index]; leaf_update_x3dom_marker(p1_ll,p2_ll,index);
X3DOMObject.displayMarkInfo(index);
mark.latitude = p2_ll.lat; }
mark.longitude = p2_ll.lng;
mark.align.latitude = mark.latitude; }
mark.align.longitude = mark.longitude;
var distance = p1_ll.distanceTo(p2_ll); function leaf_update_x3dom_marker(p1_ll,p2_ll,index){
var dp_w = x3dom_delta_map2scene(p1_ll,p2_ll); var mark = Data.markers[index];
mark.x = dp_w.x; mark.latitude = p2_ll.lat;
mark.z = dp_w.z; mark.longitude = p2_ll.lng;
mark.d_map = distance; mark.align.latitude = mark.latitude;
mark.align.longitude = mark.longitude;
X3DOMObject.displayMarkInfo(index); var distance = p1_ll.distanceTo(p2_ll);
X3DOMObject.Marker.place(mark.x,mark.y,mark.z,"my-sph-"+index); var dp_w = x3dom_delta_map2scene(p1_ll,p2_ll);
} mark.x = dp_w.x;
mark.z = dp_w.z;
mark.d_map = distance;
X3DOMObject.Marker.place(mark.x,mark.y,mark.z,"my-sph-"+index);
} }
......
...@@ -41,7 +41,7 @@ function convert_color_l2x(color){ ...@@ -41,7 +41,7 @@ function convert_color_l2x(color){
/* /*
* azimuth by geo coords * azimuth by geo coords
*/ */
/*
function getAzimuth2(p1,p2){ function getAzimuth2(p1,p2){
//p1 - start point //p1 - start point
...@@ -58,7 +58,7 @@ function getAzimuth2(p1,p2){ ...@@ -58,7 +58,7 @@ function getAzimuth2(p1,p2){
return azimuth; return azimuth;
} }
*/
/* /*
* azimuth by canvas coords * azimuth by canvas coords
*/ */
......
...@@ -64,7 +64,13 @@ ...@@ -64,7 +64,13 @@
<div id='help_wrapper'> <div id='help_wrapper'>
<div id='menu_button'></div> <div id='menu_button'></div>
<div id='help_button'>?</div> <div id='help_button'>?</div>
<div id='align_button' class='experimental'></div> <div id='align_button' title='Run alignment algorithm for camera heading and location using markers.
Instructions:
1. Use approximate location control on the map to change initial approximation for the algorithm.
2. Place at least 3 markers on the 3D model (ctrl+click) - drag to position more precisely.
3. Move all markers on the map to update their location - drag to position more precisely.
4. Click this button - the results will appear in a dialog window with "apply"/"cancel"
5. To save the result click upload button (Initial location and heading) in the menu.' class='edit'></div>
</div> </div>
<div id='info-wrapper'> <div id='info-wrapper'>
<div id='window-info'></div> <div id='window-info'></div>
......
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