Commit 8adc63c0 authored by Oleg Dzhimiev's avatar Oleg Dzhimiev
Browse files

fixed align heading crossing south

parent 113b0bf8

js/align_functions.js

+19 −1
Original line number Original line Diff line number Diff line
@@ -236,17 +236,35 @@ function hll2_w_i(i,v){ 




}

}





function bring_degrees_to_n180_180(a){



  while(a<-180){

    a += 360;

  }

  while(a>180){

    a -= 360;

  }

  return a;



}



/**

/**

 * Functions for position latitude and longitude (heading is fixed)

 * Functions for position latitude and longitude (heading is fixed)

 * hll3_...

 * hll3_...

 */

 */



function hll3_r_i(i,v){

function hll3_r_i(i,v){





  var heading = Data.camera.kml.heading;

  var heading = Data.camera.kml.heading;





  var f1 = hll_f_3d_i(i,[v[0],v[1],heading]);

  var f1 = hll_f_3d_i(i,[v[0],v[1],heading]);

  var f2 = hll_f_map_i(i,[v[0],v[1],heading]);

  var f2 = hll_f_map_i(i,[v[0],v[1],heading]);



  f1 -= heading;

  f2 -= heading;



  f1 = bring_degrees_to_n180_180(f1);

  f2 = bring_degrees_to_n180_180(f2);



  //return (f1-f2+360)%360;

  //return (f1-f2+360)%360;

  return (f1-f2);

  return (f1-f2);

}

}

js/numbers/numbers.calculus.extra.js

+148 −0
Original line number Original line Diff line number Diff line
@@ -102,6 +102,154 @@ numbers.calculus.GaussNewton = function(v,n,r,dr,eps,w){ 
    J = numbers.matrix.multiply(J,Jt)

    J = numbers.matrix.multiply(J,Jt)





    var V = []

    var V = []



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

      V.push([wn(i,v,wsum)*r(i,v)])

    }



    var delta = numbers.matrix.multiply(J,V)



    //console.log("delta: ");

    //console.log(delta);



    var res = []



    for(var i=0;i<v.length;i++){

      res[i] = v[i]-delta[i][0]

    }



    return res



  }



  function sigma(v,n,r){



    var sum = 0

    var wsum = ws(v,n)



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

      sum += wn(i,v,wsum)*r(i,v)*r(i,v)

      //wsum += w(i,v)

    }



    //console.log("sum = "+sum+" wsum = "+wsum);



    //sum = Math.sqrt(sum/wsum)

    sum = Math.sqrt(sum)



    return sum



  }



  function jacobian(v,n,dr){



    var J = []



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



      var row = []

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

        row.push(dr[j](i,v))

      }

      J[i] = row



    }



    return J



  }



  // normalized weight

  function wn(i,v,wsum){



    return w(i,v)/wsum



  }



  // sum of weights for normalization

  function ws(v,n){



    var wsum = 0



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

      wsum += w(i,v)

    }



    return wsum



  }



}



// v   - is a vector of parameters

// n   - number of measurements

// r   - residual function

// dr  - partial derivatives

// eps - epsilon

// w   - weights



numbers.calculus.GaussNewton_nD = function(v,n,r,dr,eps,w){



  var epsilon = eps || 1e-8

  var limit = 1000



  var stop = false

  var counter = 0

  var v0 = v



  if (w===undefined){

    w = function(){

      return 1;

    };

  }



  while(!stop){



    counter++



    var v1 = iterate(v0,n,r,dr)



    //console.log(v1);



    var s0 = sigma(v0,n,r)

    var s1 = sigma(v1,n,r)



    if((Math.abs(s1-s0)<epsilon)||(counter==limit)){

      stop = true

    }



    v0 = v1



  }



  return {

    count: counter,

    error: s1,

    v: v0

  }



  //functions

  function iterate(v,n,r,dr){



    var wsum = ws(v,n)



    var J = jacobian(v,n,dr)

    var Jt = numbers.matrix.transpose(J)



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

      J = numbers.matrix.rowScale(J,i,wn(i,v,wsum))

    }



    // JtJ

    J = numbers.matrix.multiply(Jt,J)



    // (Jt x J)^-1

    J = numbers.matrix.inverse(J)

    // (Jt x J)^-1 x Jt

    J = numbers.matrix.multiply(J,Jt)



    var V = []



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

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

      V.push([wn(i,v,wsum)*r(i,v)])

      V.push([wn(i,v,wsum)*r(i,v)])

    }

    }

js/ui_align.js

+1 −0
Original line number Original line Diff line number Diff line
@@ -401,6 +401,7 @@ function x3dom_align_hll3(){ 
  var xyh = [x0,y0];

  var xyh = [x0,y0];





  var result = numbers.calculus.GaussNewton(xyh,Data.markers.length,hll3_r_i,[hll3_dr_dx_i,hll3_dr_dy_i],epsilon,hll3_w_i);

  var result = numbers.calculus.GaussNewton(xyh,Data.markers.length,hll3_r_i,[hll3_dr_dx_i,hll3_dr_dy_i],epsilon,hll3_w_i);

  //var result = numbers.calculus.GaussNewton_nD(xyh,Data.markers.length,hll3_r_i,[hll3_dr_dx_i,hll3_dr_dy_i],epsilon,hll3_w_i);





  xyh = [result.v[0],result.v[1],h0];

  xyh = [result.v[0],result.v[1],h0];