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

js/ui.css

@@ -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;
+}

js/ui_align.js

@@ -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  }}
+  ];
+
+}

js/ui_init.js

@@ -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){

js/util_functions.js

@@ -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
  */

test.html

@@ -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>