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 {
}
#align_button{
background-image:url('images/align.png');
background-image:url('images/ic_explore_black_48dp_1x.png');
background-size: 32px 32px;
background-repeat: no-repeat;
......@@ -307,4 +307,18 @@ html, body, #x3d_wrapper {
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(){
$("#align_button").on("click",function(){
//align_heading();
x3dom_align_0();
x3dom_align_GN();
});
/*
......@@ -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;
Data.camera.kml.longitude = -111.9328843;
Data.camera.kml.heading = 62;
var c1,c2;
var result = true;
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 }}
];
for(var i=0;i<Data.markers.length;i++){
}
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;
}
function x3dom_align_0(){
if (c2.toString().indexOf("drag")!=-1){
console.log("error: marker "+i+": drag over 3D scene. Mouse over shows a marker info.");
result = false;
}
//get all points
//console.log("Base");
//console.log(Data.camera);
//console.log("Markers");
//console.log(Data.markers);
}
//test_markers_set1();
return result;
//var base = Data.camera;
// base.x
// base.y
// base.z
// base.latitude
// base.longitude
}
//initial?
/*
* 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;
}
if (!check_markers()){
console.log("Alignment error: marker has not been moved over 3D or Map");
return -2;
}
// initial approximation:
var x0 = Data.camera.kml.latitude;
var y0 = Data.camera.kml.longitude;
var h0 = Data.camera.kml.heading;
......@@ -56,31 +77,6 @@ function x3dom_align_0(){
//test_AxV();
//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 iterate = true;
......@@ -88,9 +84,6 @@ function x3dom_align_0(){
var result = 0;
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){
counter++;
......@@ -113,7 +106,7 @@ function x3dom_align_0(){
//console.log("Error function value: "+sigma(xyh_new[0],xyh_new[1],xyh_new[2]));
if (counter==150){
if (counter==1000){
iterate = false;
}
......@@ -129,147 +122,46 @@ function x3dom_align_0(){
}
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);
}
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;
}
/*
* calc the next iteration values
*/
function GaussNewtonAlgorithm(x,y,h){
var J = Jacobian(x,y,h);
var Jt = At(J);
//console.log(J);
//console.log(Jt);
var JtJ = AxB(Jt,J);
//console.log(JtJ);
//console.log("Determinant: "+Adet(JtJ));
var JtJi = Ainv(JtJ);
//console.log(AxB(JtJ,JtJi));
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 = [];
for(var i=0;i<Data.markers.length;i++){
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);
//console.log("JtJixJt x Vr");
//console.log(d);
//var k = 1/10;
var k = 1;
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){
var base = Data.camera;
var mark = Data.markers[i];
......@@ -278,6 +170,9 @@ function f1_3d_i(i,x,y,h){
return res;
}
/*
* heading in degrees from map
*/
function f2_map_i(i,x,y,h){
var mark = Data.markers[i];
......@@ -307,43 +202,16 @@ function f2_map_i(i,x,y,h){
}
function f2_map_i_inverse(i,x,y,h){
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;
}
/*
* residuals function
*/
function r_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){
var mark = Data.markers[i];
......@@ -372,7 +240,9 @@ function dr_dx_i(i,x,y,h){
return res;
}
/*
* dr/dy(i)
*/
function dr_dy_i(i,x,y,h){
var mark = Data.markers[i];
......@@ -401,7 +271,16 @@ function dr_dy_i(i,x,y,h){
return res;
}
/*
* dr/dh(i)
*/
function dr_dh_i(i,x,y,h){
return 1;
}
/*
* Jacobi matrix
*/
function Jacobian(x,y,h){
var J = [];
......@@ -412,6 +291,7 @@ function Jacobian(x,y,h){
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)];
/*
e0 = 0.000000001;
e1 = 0.000000001;
......@@ -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_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("NUM: "+dri_dx_num.toFixed(10)+" "+dri_dy_num.toFixed(10)+" "+dri_dh_num.toFixed(4));
console.log("CALC: "+dri_dx_cal.toFixed(10)+" "+dri_dy_cal.toFixed(10)+" "+dri_dh_cal.toFixed(4));
console.log("NUME: "+dri_dx_num.toFixed(10)+" "+dri_dy_num.toFixed(10)+" "+dri_dh_num.toFixed(4));
*/
}
return J;
}
/*
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
1 1 1
*/
//tested
function AxB(A,B){
* Utility functions
*/
//A.length == B[0].length
//A[0].length == B.length
/*
* AxB, any dimensions
*/
function AxB(A,B){
var m1 = A.length;
var n1 = A[0].length;
......@@ -474,7 +350,9 @@ function AxB(A,B){
}
//tested
/*
* Determinant for 3x3 only
*/
function Adet(A){
var m = A.length;
......@@ -495,7 +373,9 @@ function Adet(A){
}
//tested
/*
* Inverted matrix for 3x3 only
*/
function Ainv(A){
var m = A.length;
......@@ -520,7 +400,9 @@ function Ainv(A){
];
}
//tested
/*
* Transposed matrix, any dimensions
*/
function At(A){
var R = [];
......@@ -534,7 +416,9 @@ function At(A){
return R;
}
//tested
/*
* Matrix x Vector - any dimensions
*/
function AxV(A,V){
var Vr = [];
......@@ -560,41 +444,108 @@ function AxV(A,V){
return Vr;
}
function align_heading(){
/*
* ui dialog to apply or cancel results
*/
function apply_alignment_dialog(xyh0,xyh1,c,e){
// find selected markers
// pick the first one?
// align?!
console.log("heading");
var d = $("<div>",{id:"aa1_dialog"});
var map_markers = Map.marker._measureMarkers;
var selected_markers = [];
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'));
map_markers.forEach(function(c,i){
if (selected_markers.length<2){
if (c._selected){
selected_markers.push(c);
}
}
d.append(dc);
var b1 = $("<button>").html("apply");
b1.on('click',function(){
apply_alignment(xyh1);
b2.click();
});
if (selected_markers.length<2){
console.log("select 2 markers");
Scene.showMessage("messagewindow","error: select 2 markers","red");
}
var b2 = $("<button>").html("cancel");
b2.on('click',function(){
$("#aa1_dialog").remove();
});
d.append($("<div>").append(b1).append(b2));
console.log(selected_markers);
$("body").append(d);
}
/*
* actual apply function
*/
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
Map.marker.setHeading(xyh[2]);
Map.marker.setBasePoint(new L.LatLng(xyh[0],xyh[1]));
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);
}
x3d_initial_camera_placement();
}
/*
* not used
*/
function align_roll(){
console.log("roll");
}
/*
* not used
*/
function align_tilt(){
console.log("tilt");
......@@ -687,3 +638,16 @@ function test_AxB(){
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 = {
'path' : "1487451413_967079",
'version': "v1",
'experimental': false,
'edit': false,
'edit': true,
'files': {
'x3d':"",
'x3d_background':"",
......@@ -722,6 +722,15 @@ function leaf_drag_marker(){
var p1_ll = Camera._latlng;
var p2_ll = Camera.draggedMarker._latlng;
leaf_update_x3dom_marker(p1_ll,p2_ll,index);
X3DOMObject.displayMarkInfo(index);
}
}
function leaf_update_x3dom_marker(p1_ll,p2_ll,index){
var mark = Data.markers[index];
mark.latitude = p2_ll.lat;
......@@ -739,12 +748,8 @@ function leaf_drag_marker(){
mark.d_map = distance;
X3DOMObject.displayMarkInfo(index);
X3DOMObject.Marker.place(mark.x,mark.y,mark.z,"my-sph-"+index);
}
}
function leaf_translation_v1(p0,p1){
......
......@@ -41,7 +41,7 @@ function convert_color_l2x(color){
/*
* azimuth by geo coords
*/
/*
function getAzimuth2(p1,p2){
//p1 - start point
......@@ -58,7 +58,7 @@ function getAzimuth2(p1,p2){
return azimuth;
}
*/
/*
* azimuth by canvas coords
*/
......
......@@ -64,7 +64,13 @@
<div id='help_wrapper'>
<div id='menu_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 id='info-wrapper'>
<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