Commit 40ab280c authored by Oleg Dzhimiev's avatar Oleg Dzhimiev
Browse files

Gauss-Newton algorithm to align heading and lat,lng

parent 05a68386

js/ui_align.js

+530 −10
Original line number Original line Diff line number Diff line
@@ -14,6 +14,20 @@ function align_init(){ 




}

}





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

  ];



}



function x3dom_align_0(){

function x3dom_align_0(){





  //get all points

  //get all points
@@ -22,33 +36,452 @@ function x3dom_align_0(){ 
  //console.log("Markers");

  //console.log("Markers");

  //console.log(Data.markers);

  //console.log(Data.markers);





  var base = Data.camera;

  //test_markers_set1();



  //var base = Data.camera;

  // base.x

  // base.x

  // base.y

  // base.y

  // base.z

  // base.z

  // base.latitude

  // base.latitude

  // base.longitude

  // base.longitude





  //initial?

  var x0 = Data.camera.kml.latitude;

  var y0 = Data.camera.kml.longitude;

  var h0 = Data.camera.kml.heading;



  //tests

  //test_AxB();

  //test_At();

  //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++){

  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.0000000001;



  var iterate = true;

  var counter = 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++;



    //console.log("Interation: "+counter+" for "+xyh[0]+" "+xyh[1]+" "+xyh[2]);

    xyh_new = GaussNewtonAlgorithm(xyh[0],xyh[1],xyh[2]);



    //console.log(xyh_new);



    var s0 = sigma(xyh[0],xyh[1],xyh[2]);

    var s1 = sigma(xyh_new[0],xyh_new[1],xyh_new[2]);



    //if ((s1>s0)||((s0-s1)<ε)){

    if (Math.abs(s0-s1)<ε){

      iterate = false;

    }



    //console.log("Errors: "+(xyh_new[0]-xyh[0])+" "+(xyh_new[1]-xyh[1])+" "+(xyh_new[2]-xyh[2]));

    //console.log("Iteration "+counter+" result: "+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){

      iterate = false;

    }



    xyh[0] = xyh_new[0];

    xyh[1] = xyh_new[1];

    xyh[2] = xyh_new[2];



  }



}



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



  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]];



}



function f1_3d_i(i,x,y,h){

  var base = Data.camera;

  var mark = Data.markers[i];

  var mark = Data.markers[i];

  var v = new x3dom.fields.SFVec3f(mark.align.x-base.x,0,mark.align.z-base.z);

  var res = Math.atan2(v.x,-v.z)*180/Math.PI + h;

  return res;

}



function f2_map_i(i,x,y,h){





    // map azimuth

  var mark = Data.markers[i];

    var p1_ll = new L.LatLng(base.latitude,base.longitude);

  var p1_ll = new L.LatLng(x,y);

  var p2_ll = new L.LatLng(mark.align.latitude,mark.align.longitude);

  var p2_ll = new L.LatLng(mark.align.latitude,mark.align.longitude);





    var azimuth_map = getAzimuth(p1_ll,p2_ll);

  //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 da = x3dom_getDistAngle(mark.align.x,mark.align.y,mark.align.z);

  var res = 180/Math.PI*Math.atan2(dy,dx);

    //var azimuth_3d = da[1];





    var v = new x3dom.fields.SFVec3f(mark.align.x-base.x,mark.align.y-base.y,mark.align.z-base.z);

  return res;

    var azimuth_3d = Math.atan2(v.x,-v.z)*180/Math.PI;





    console.log(azimuth_map+" - "+azimuth_3d+" = "+(azimuth_map-azimuth_3d));

}



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;



}





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;

}



function dr_dx_i(i,x,y,h){



  var mark = Data.markers[i];

  var p1_ll = new L.LatLng(x,y);

  var p2_ll = new L.LatLng(mark.align.latitude,mark.align.longitude);



  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);



  var dydx = 0;

  var dxdx = (-Math.sin(p1_ll.lat)*Math.sin(p2_ll.lat)-Math.cos(p1_ll.lat)*Math.cos(p2_ll.lat)*Math.cos(dlon))*Math.PI/180;



  var Arg = dy/dx;



  var res = -180/Math.PI*1/(1+Math.pow(Arg,2))*(-dy*dxdx)/Math.pow(dx,2);



  return res;



}



function dr_dy_i(i,x,y,h){



  var mark = Data.markers[i];

  var p1_ll = new L.LatLng(x,y);

  var p2_ll = new L.LatLng(mark.align.latitude,mark.align.longitude);



  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);



  var dydy = Math.cos(p2_ll.lat)*Math.cos(dlon)*(-1)*Math.PI/180;

  var dxdy = -Math.sin(p1_ll.lat)*Math.cos(p2_ll.lat)*(-1)*Math.sin(dlon)*(-1)*Math.PI/180;



  var Arg = dy/dx;



  var res = -180/Math.PI*1/(1+Math.pow(Arg,2))*(dydy*dx-dy*dxdy)/Math.pow(dx,2);



  return res;



}



function Jacobian(x,y,h){



  var J = [];



  var base = Data.camera;



  for(var i=0;i<Data.markers.length;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)];

    /*

    e0 = 0.000000001;

    e1 = 0.000000001;

    e2 = 0.00001;



    dri_dx_cal = dr_dx_i(i,x,y,h);

    dri_dy_cal = dr_dy_i(i,x,y,h);

    dri_dh_cal = dr_dh_i(i,x,y,h);



    dri_dx_num = (r_i(i,x+e0,y   ,h   )-r_i(i,x-e0,y   ,h   ))/e0/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;



    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));

    */

  }



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



  //A.length == B[0].length

  //A[0].length == B.length



  var m1 = A.length;

  var n1 = A[0].length;



  var m2 = B.length;

  var n2 = B[0].length;



  if(n1!=m2){

    console.log("M=AxB: cannot multiply matrices A_"+m1+"_"+n1+" x B_"+m2+"_"+n2);

    return [];

  }



  var R = [];



  for(var i=0;i<m1;i++){

    R[i] = [];

    for(var j=0;j<n2;j++){

      R[i][j] = 0;

      for(var k=0;k<n1;k++){

          R[i][j] += A[i][k]*B[k][j];

      }

    }

  }



  return R;



}



//tested

function Adet(A){



  var m = A.length;

  var n = A[0].length;



  if ((m!=3)||(n!=3)){

    console.log("Matrix inverting works only for 3x3 dimension");

  }



  var M = new x3dom.fields.SFMatrix4f(

        A[0][0],A[0][1],A[0][2],0,

        A[1][0],A[1][1],A[1][2],0,

        A[2][0],A[2][1],A[2][2],0,

        0,0,0,1

    );



  return M.det();



}



//tested

function Ainv(A){



  var m = A.length;

  var n = A[0].length;



  if ((m!=3)||(n!=3)){

    console.log("Matrix inverting works only for 3x3 dimension");

  }



  var M = new x3dom.fields.SFMatrix4f(

        A[0][0],A[0][1],A[0][2],0,

        A[1][0],A[1][1],A[1][2],0,

        A[2][0],A[2][1],A[2][2],0,

        0,0,0,1

    );

  var R = M.inverse();



  return [

    [R._00,R._01,R._02],

    [R._10,R._11,R._12],

    [R._20,R._21,R._22]

  ];

}



//tested

function At(A){

  var R = [];



  for(var i=0;i<A[0].length;i++){

    R[i] = [];

    for(var j=0;j<A.length;j++){

      R[i][j] = A[j][i];

    }

  }



  return R;

}



//tested

function AxV(A,V){



  var Vr = [];



  var m1 = A.length;

  var n1 = A[0].length;



  var m2 = V.length;

  var n2 = 1;



  if (n1!=m2){

    console.log("Matrix or vector dimension errors, too bad");

    return [];

  }



  for(var i=0;i<m1;i++){

    Vr[i] = 0;

    for(var j=0;j<m2;j++){

        Vr[i] += A[i][j]*V[j];

    }

  }



  return Vr;

}

}





function align_heading(){

function align_heading(){
@@ -91,3 +524,90 @@ function align_tilt(){ 
    console.log("tilt");

    console.log("tilt");





}

}



function test_Ainv(){



  var A = [

    [0,0,1],

    [1,0,0],

    [0,1,0]

  ];



  console.log(A);



  console.log(Ainv(A));



}



function test_AxV(){



  var A = [

    [1,2,3,4],

    [5,6,7,8],

    [9,10,11,12]

  ];



  var V1 = [

    13,

    14,

    15,

    16

  ];



  var V2 = [

    13,

    14,

    15

  ];



  console.log(AxV(A,V1));

  console.log(AxV(A,V2));



}



function test_At(){



  console.log("testing At: begin");



  var A = [

    [1,2,3,4],

    [5,6,7,8],

    [9,10,11,12]

  ];



  console.log(A);

  console.log(At(A));



  console.log("testing At: end");

}



function test_AxB(){



  console.log("testing AxB: begin");



  var A = [

    [1,2,3,4],

    [5,6,7,8],

    [9,10,11,12]

  ];



  var B = [

    [13,14,15],

    [16,17,18],

    [19,20,21],

    [22,23,24],

  ];



  var C = [

    [2,2],

    [1,1]

  ];



  //test1: 3x4 x 4x3 = 3x3

  console.log(AxB(A,B));

  //test2: fail test case

  console.log(AxB(A,C));



  console.log("testing AxB: end");

}