Gauss-Newton algorithm as extension to numbers.js (7df0fde6) · Commits · Elphel / x3domlet

js/numbers/numbers.calculus.extra.js

0 → 100644

+135 −0

Original line number	Diff line number	Diff line
		/**
		* @file numbers.calculus.extra.js
		* @brief Gauss-Newton
		* @copyright Copyright (C) 2017 Elphel Inc.
		* @authors Oleg Dzhimiev <oleg@elphel.com>
		*
		* @license: GPL-3.0
		*
		* @licstart The following is the entire license notice for the
		* JavaScript code in this page.
		*
		* The JavaScript code in this page is free software: you can
		* redistribute it and/or modify it under the terms of the GNU
		* General Public License (GNU GPL) as published by the Free Software
		* Foundation, either version 3 of the License, or (at your option)
		* any later version. The code is distributed WITHOUT ANY WARRANTY;
		* without even the implied warranty of MERCHANTABILITY or FITNESS
		* FOR A PARTICULAR PURPOSE. See the GNU GPL for more details.
		*
		* As additional permission under GNU GPL version 3 section 7, you
		* may distribute non-source (e.g., minimized or compacted) forms of
		* that code without the copy of the GNU GPL normally required by
		* section 4, provided you include this license notice and a URL
		* through which recipients can access the Corresponding Source.
		*
		* @licend The above is the entire license notice
		* for the JavaScript code in this page.
		*/

		/**
		* Gauss-Newton algorithm for minimizing error function which
		* is a sum of squared errors for each measurement
		*
		* @v {Array} vector, initial approximation
		* @n {Number} number of measuments
		* @r {Function} residual function
		* @dr {Array} array of derivative functions
		* @eps {Number} precision
		*
		*/
		numbers.calculus.GaussNewton = function(v,n,r,dr,eps){

		var epsilon = eps \|\| 1e-8
		var limit = 1000

		var stop = false
		var counter = 0
		var v0 = v

		while(!stop){

		counter++

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

		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 J = jacobian(v,n,dr)
		var Jt = numbers.matrix.transpose(J)

		// 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++){
		V.push([r(i,v)])
		}

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

		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;

		for(var i=0;i<n;i++){
		sum += r(i,v)*r(i,v)
		}

		sum = Math.sqrt(sum/n)

		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

		}

		}

js/numbers/numbers.js

0 → 100644

+3161 −0

File added.

Preview size limit exceeded, changes collapsed.

js/numbers/numbers.matrix.extra.js

0 → 100644

+10 −0

Original line number	Diff line number	Diff line
		numbers.matrix.toString = function(a){

		var res = ""

		for(var i=0;i<a.length;i++){
		res += a[i].join(" ")+"\n"
		}

		return res
		}

js/ui_align.js

+22 −356

Original line number	Diff line number	Diff line
		@@ -43,8 +43,8 @@ function align_init(){
		//align_heading();
		//test_markers_set1();
		//if (DEBUG_ALIGN) test_markers_set1();
		//if (DEBUG_ALIGN) test_markers_set2();
		if (DEBUG_ALIGN) test_markers_set3();
		if (DEBUG_ALIGN) test_markers_set2();
		//if (DEBUG_ALIGN) test_markers_set3();
		x3dom_align_GN();
		});

		@@ -123,61 +123,15 @@ function x3dom_align_GN(){
		var x0 = Data.camera.kml.latitude;
		var y0 = Data.camera.kml.longitude;
		var h0 = Data.camera.kml.heading;
		var epsilon = 1e-8;

		//if (h0>180) h0 = h0 - 360;

		//tests
		//test_AxB();
		//test_At();
		//test_AxV();
		//test_Ainv();

		var ε = 0.000000001;

		var iterate = true;
		var counter = 0;
		var result = 0;
		var xyh = [x0,y0,(h0>180)?h0-360:h0];

		while(iterate){

		if (DEBUG_ALIGN){
		// functions values
		for(var i=0;i<Data.markers.length;i++){
		console.log(f1_3d_i(i,xyh[0],xyh[1],xyh[2])+" - "+f2_map_i(i,xyh[0],xyh[1],xyh[2])+" = "+r_i(i,xyh[0],xyh[1],xyh[2]));
		}
		}

		counter++;
		var result = numbers.calculus.GaussNewton(xyh,Data.markers.length,r_i,[dr_dx_i,dr_dy_i,dr_dh_i],epsilon);

		//console.log("Interation: "+counter+" for "+xyh[0]+" "+xyh[1]+" "+xyh[2]);
		xyh_new = GaussNewtonAlgorithm(xyh[0],xyh[1],xyh[2]);
		//if (xyh_new[2]<-180) xyh_new[2] += 360;
		//if (xyh_new[2]> 180) xyh_new[2] -= 360;

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

		if(DEBUG_ALIGN){
		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==1000){
		iterate = false;
		}

		xyh[0] = xyh_new[0];
		xyh[1] = xyh_new[1];
		xyh[2] = xyh_new[2];

		}
		xyh = result.v;
		var s1 = result.error;
		var counter = result.count;

		//calc distance error
		de = distance_error(x0,y0,(h0>180)?h0-360:h0);
		@@ -188,51 +142,14 @@ function x3dom_align_GN(){

		}

		/*
		* calc the next iteration values
		*/
		function GaussNewtonAlgorithm(x,y,h){

		var J = Jacobian(x,y,h);
		var Jt = At(J);
		var JtJ = AxB(Jt,J);
		var JtJi = Ainv(JtJ);
		var JtJixJt = AxB(JtJi,Jt);

		var Vr = [];
		for(var i=0;i<Data.markers.length;i++){
		Vr[i] = r_i(i,x,y,h);
		}

		var d = AxV(JtJixJt,Vr);
		var k = 1;

		return [x-kd[0], y-kd[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,v){
		var base = Data.camera;
		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;
		var vec = new x3dom.fields.SFVec3f(mark.align.x-base.x,0,mark.align.z-base.z);
		var res = Math.atan2(vec.x,-vec.z)*180/Math.PI + v[2];

		if (res> 180) res = res - 360;
		if (res<-180) res = res + 360;
		@@ -243,10 +160,10 @@ function f1_3d_i(i,x,y,h){
		/*
		* heading in degrees from map
		*/
		function f2_map_i(i,x,y,h){
		function f2_map_i(i,v){

		var mark = Data.markers[i];
		var p1_ll = new L.LatLng(x,y);
		var p1_ll = new L.LatLng(v[0],v[1]);
		var p2_ll = new L.LatLng(mark.align.latitude,mark.align.longitude);

		//console.log(p1_ll);
		@@ -275,9 +192,9 @@ function f2_map_i(i,x,y,h){
		/*
		* residuals function
		*/
		function r_i(i,x,y,h){
		var f1 = f1_3d_i(i,x,y,h);
		var f2 = f2_map_i(i,x,y,h);
		function r_i(i,v){
		var f1 = f1_3d_i(i,v);
		var f2 = f2_map_i(i,v);
		//return (f1-f2+360)%360;
		return (f1-f2);
		}
		@@ -285,10 +202,10 @@ function r_i(i,x,y,h){
		/*
		* dr/dx(i)
		*/
		function dr_dx_i(i,x,y,h){
		function dr_dx_i(i,v){

		var mark = Data.markers[i];
		var p1_ll = new L.LatLng(x,y);
		var p1_ll = new L.LatLng(v[0],v[1]);
		var p2_ll = new L.LatLng(mark.align.latitude,mark.align.longitude);

		p1_ll.lat = p1_ll.lat*Math.PI/180;
		@@ -316,10 +233,10 @@ function dr_dx_i(i,x,y,h){
		/*
		* dr/dy(i)
		*/
		function dr_dy_i(i,x,y,h){
		function dr_dy_i(i,v){

		var mark = Data.markers[i];
		var p1_ll = new L.LatLng(x,y);
		var p1_ll = new L.LatLng(v[0],v[1]);
		var p2_ll = new L.LatLng(mark.align.latitude,mark.align.longitude);

		p1_ll.lat = p1_ll.lat*Math.PI/180;
		@@ -347,175 +264,10 @@ function dr_dy_i(i,x,y,h){
		/*
		* dr/dh(i)
		*/
		function dr_dh_i(i,x,y,h){
		function dr_dh_i(i,v){
		return 1;
		}

		/*
		* Jacobi matrix
		*/
		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("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;
		}

		/*
		* Utility functions
		*/

		/*
		* AxB, any dimensions
		*/
		function AxB(A,B){

		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;

		}

		/*
		* Determinant for 3x3 only
		*/
		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();

		}

		/*
		* Inverted matrix for 3x3 only
		*/
		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]
		];
		}

		/*
		* Transposed matrix, any dimensions
		*/
		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;
		}

		/*
		* Matrix x Vector - any dimensions
		*/
		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;
		}

		/*
		* ui dialog to apply or cancel results
		@@ -614,7 +366,7 @@ function distance_error(x,y,h){

		for(var i=0;i<Data.markers.length;i++){
		var angle0 = h;
		var angle1 = f2_map_i(i,x,y,h);
		var angle1 = f2_map_i(i,[x,y,h]);
		var z_map = Math.cos(Math.PI/180(angle0-angle1))Data.markers[i].d_map;
		var z_x3d = -Data.markers[i].align.z;
		sum += 1/z_map-1/z_x3d;
		@@ -647,92 +399,6 @@ function align_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");
		}

		function test_markers_set1(){

		Data.camera.kml.latitude = 40.7233861;

test.html

+4 −0

Original line number	Diff line number	Diff line
		@@ -25,6 +25,10 @@
		<script type='text/javascript' src='js/leaflet/leaflet.camera-view-marker.js'></script>
		<script type='text/javascript' src='js/leaflet/leaflet.camera-view-marker.measure.js'></script>

		<script type='text/javascript' src='js/numbers/numbers.js'></script>
		<script type='text/javascript' src='js/numbers/numbers.matrix.extra.js'></script>
		<script type='text/javascript' src='js/numbers/numbers.calculus.extra.js'></script>

		<!---script type='text/javascript' src='js/x3dom/x3dom-full.debug.js'></script-->
		<script type='text/javascript' src='js/x3dom_init.js'></script>
		<script type='text/javascript' src='js/x3dom_functions.js'></script>

Admin message