dding polinomial approximation 2d (56ca0c83) · Commits · Elphel / motosat

Matrix.php

+23 −0

Original line number	Diff line number	Diff line
		@@ -23,6 +23,11 @@ class Matrix
		return $this->M;
		}

		public function set($i,$j,$v){
		$M[$i][$j] = $v;
		}


		public function getColumnPackedCopy(){
		$rows = sizeof($this->M);
		$cols = sizeof($this->M[0]);
		@@ -82,6 +87,9 @@ class Matrix
		}

		public function plus($B) {
		if ($B instanceof Matrix){
		$B = $B->get();
		}
		$R = $this->M;
		$rows = sizeof($this->M);
		$cols = sizeof($this->M[0]);
		@@ -95,6 +103,21 @@ class Matrix
		}
		return new Matrix ($R, $this->Tol);
		}
		public function plusEquals($B){
		if ($B instanceof Matrix){
		$B = $B->get();
		}
		$rows = sizeof($this->M);
		$cols = sizeof($this->M[0]);
		if (($rows != sizeof($B[0])) \|\| ($cols != sizeof($B[0]))){
		throw new Exception('Dimensions mismatch.');
		}
		for ($i = 0; $i < $rows; $i ++) {
		for ($j = 0; $j < $cols; $j ++) {
		$this->M[$i][$j] += $B[$i][$j];
		}
		}
		}


		public function print($debugFile = null, $decimals = 6)

PolynomialApproximation.php

+250 −1

Original line number	Diff line number	Diff line
		@@ -10,7 +10,7 @@ class PolynomialApproximation
		public function polynomialApproximation1d($data, $N){
		//$my_array = array_fill(0, $size_of_the_array, $some_data);
		if ($this->debugFile === null){
		$this->debugLevel = 1;
		$this->debugLevel = 0;
		}
		$S=array_fill(0,2$N +1,0); // new double [2N+1];
		$SF=array_fill(0,$N+1,0); // new double [N+1];
		@@ -90,6 +90,255 @@ class PolynomialApproximation
		return null;
		}

		public function quadraticApproximation(
		$data,
		$forceLinear = false, // use linear approximation
		$damping = null,
		$thresholdLin = 1.0E-10, // threshold ratio of matrix determinant to norm for linear approximation (det too low - fail)
		$thresholdQuad = 1.0E-15, // threshold ratio of matrix determinant to norm for quadratic approximation (det too low - fail)
		$debugLevel = 1
		){
		if ($this->debugFile === null){
		$this->debugLevel = 0;
		}

		if ((data == null) \|\| (data.length == 0)) {
		return null;
		}
		/* ix, iy - the location of the point with maximal value. We'll approximate the vicinity of that maximum using a
		* second degree polynomial:
		Z(x,y)~=Ax^2+By^2+Cxy+Dx+Ey+F
		by minimizing sum of squared differenceS00between the actual (Z(x,uy)) and approximated values.
		and then find the maximum on the approximated surface. Here iS00the math:

		Z(x,y)~=Ax^2+By^2+Cxy+Dx+Ey+F
		minimizing squared error, using W(x,y) aS00weight function

		error=Sum(W(x,y)((Ax^2+By^2+Cxy+Dx+E*y+F)-Z(x,y))^2)

		error=Sum(W(x,y)(A^2x^4 + 2Ax^2(By^2+Cxy+Dx+Ey+F-Z(x,y)) +(...) )
		0=derror/dA=Sum(W(x,y)(2Ax^4 + 2x^2(By^2+Cxy+Dx+Ey+F-Z(x,y)))
		0=Sum(W(x,y)(Ax^4 + x^2(By^2+Cxy+Dx+Ey+F-Z(x,y)))

		S40=Sum(W(x,y)*x^4), etc

		(1) 0=AS40 + BS22 + CS31 +DS30 +ES21 +FS20 - SZ20

		derror/dB:

		error=Sum(W(x,y)(B^2y^4 + 2By^2(Ax^2+Cxy+Dx+Ey+F-Z(x,y)) +(...) )
		0=derror/dB=Sum(W(x,y)(2By^4 + 2y^2(Ax^2+Cxy+Dx+Ey+F-Z(x,y)))
		0=Sum(W(x,y)(By^4 + y^2(Ax^2+Cxy+Dx+Ey+F-Z(x,y)))

		(2) 0=BS04 + AS22 + CS13 +DS12 +ES03 +FSY2 - SZ02
		(2) 0=AS22 + BS04 + CS13 +DS12 +ES03 +FSY2 - SZ02

		derror/dC:

		error=Sum(W(x,y)(C^2x^2y^2 + 2Cxy(Ax^2+By^2+Dx+E*y+F-Z(x,y)) +(...) )
		0=derror/dC=Sum(W(x,y)(2Cx^2y^2 + 2xy(Ax^2+By^2+Dx+E*y+F-Z(x,y)) )
		0=Sum(W(x,y)(Cx^2y^2 + xy(Ax^2+By^2+Dx+E*y+F-Z(x,y)) )

		(3) 0= AS31 + BS13 + CS22 + DS21 + ES12 + FS11 - SZ11

		derror/dD:

		error=Sum(W(x,y)(D^2x^2 + 2Dx(Ax^2+By^2+Cxy+Ey+F-Z(x,y)) +(...) )
		0=derror/dD=Sum(W(x,y)(2Dx^2 + 2x(Ax^2+By^2+Cxy+Ey+F-Z(x,y)) )
		0=Sum(W(x,y)(Dx^2 + x(Ax^2+By^2+Cxy+Ey+F-Z(x,y)) )

		(4) 0= AS30 + BS12 + CS21 + DS20 + ES11 + FS10 - SZ10

		derror/dE:

		error=Sum(W(x,y)(E^2y^2 + 2Ey(Ax^2+By^2+Cxy+Dx+F-Z(x,y)) +(...) )
		0=derror/dE=Sum(W(x,y)(2Ey^2 + 2y(Ax^2+By^2+Cxy+Dx+F-Z(x,y)) )
		0=Sum(W(x,y)(Ey^2 + y(Ax^2+By^2+Cxy+Dx+F-Z(x,y)) )
		(5) 0= AS21 + BS03 + CS12 + DS11 + ESY2 + FSY - SZ01

		derror/dF:

		error=Sum(W(x,y)(F^2 + 2F(Ax^2+By^2+Cxy+Dx+E*y-Z(x,y)) +(...) )
		0=derror/dF=Sum(W(x,y)(2F + 2(Ax^2+By^2+Cxy+Dx+E*y-Z(x,y)) )
		0=Sum(W(x,y)(F + (Ax^2+By^2+Cxy+Dx+E*y-Z(x,y)) )
		(6) 0= AS20 + BSY2 + CS11 + DS10 + ESY + FS00 - SZ00


		(1) 0= AS40 + BS22 + CS31 + DS30 + ES21 + FS20 - SZ20
		(2) 0= AS22 + BS04 + CS13 + DS12 + ES03 + FS02 - SZ02
		(3) 0= AS31 + BS13 + CS22 + DS21 + ES12 + FS11 - SZ11
		(4) 0= AS30 + BS12 + CS21 + DS20 + ES11 + FS10 - SZ10
		(5) 0= AS21 + BS03 + CS12 + DS11 + ES02 + FS01 - SZ01
		(6) 0= AS20 + BS02 + CS11 + DS10 + ES01 + FS00 - SZ00
		*/
		// Matrix mDampingLin = null;
		// Matrix mDampingQuad = null;
		if ($damping !== null){
		$mDampingLin = new Matrix(Matrix::zeroMatrix(3, 3));
		for ($i = 0; $i < 3; $i++){
		$j = sizeof($damping) + $i - 3;
		if ($j >= 0) $mDampingLin.set($i, $i, $damping[$j]);
		}
		if (!$forceLinear) {
		$mDampingQuad = new Matrix(Matrix::zeroMatrix(6, 6));
		for ($i = 0; $i < 6; $i++){
		$j = sizeof($damping) + $i - 6;
		if ($j >= 0) $mDampingQuad.set($i, $i, $damping[$j]);
		}
		}
		}

		$zDim = sizeof($data[0][1]);

		// double w,z,x,x2,x3,x4,y,y2,y3,y4,wz;
		// int i,j,
		$n = 0;
		$S00 = 0.0;
		$S10=0.0; $S01=0.0;
		$S20=0.0; $S11=0.0; $S02=0.0;
		$S30=0.0; $S21=0.0; $S12=0.0; $S03=0.0;
		$S40=0.0; $S31=0.0; $S22=0.0; $S13=0.0; $S04=0.0;
		$SZ00 = array_fill(0, $zDim, 0.0);
		$SZ01 = array_fill(0, $zDim, 0.0);
		$SZ10 = array_fill(0, $zDim, 0.0);
		$SZ11 = array_fill(0, $zDim, 0.0);
		$SZ02 = array_fill(0, $zDim, 0.0);
		$SZ20 = array_fill(0, $zDim, 0.0);
		$dataLength=sizeof($data);
		for ($i=0; $i < $dataLength; $i++) {
		$w= (sizeof($data[$i]) > 2) ? $data[$i][2][0] : 1.0;
		if ($w > 0) {
		$n++;
		$x=$data[$i][0][0];
		$y=$data[$i][0][1];
		$x2 = $x * $x;
		$y2 = $y * $y;
		$S00 += $w;
		$S10 += $w * $x;
		$S01 += $w * $y;
		$S11 += $w * $x * $y;
		$S20 += $w * $x2;
		$S02 += $w * $y2;
		if (!$forceLinear) {
		$x3 = $x2 * $x;
		$x4 = $x3 * $x;
		$y3 = $y2 * $y;
		$y4 = $y3 * $y;
		$S30 += $w * $x3;
		$S21 += $w * $x2 * $y;
		$S12 += $w * $x * $y2;
		$S03 += $w * $y3;
		$S40 += $w * $x4;
		$S31 += $w * $x3 * $y;
		$S22 += $w * $x2 * $y2;
		$S13 += $w * $x * $y3;
		$S04 += $w * $y4;
		}
		for ($j = 0; $j < $zDim; $j++) {
		$z = $data[$i][1][$j];
		$wz = $w * $z;
		$SZ00[$j] += $wz;
		$SZ10[$j] += $wz * $x;
		$SZ01[$j] += $wz * $y;
		if (!$forceLinear) {
		$SZ20[$j] += $wz * $x2;
		$SZ11[$j] += $wz * $x * $y;
		$SZ02[$j] += $wz * $y2;
		}
		}

		}
		}
		//need to decide if there is enough data for linear and quadratic
		$mAarrayL = array(
		array($S20, $S11, $S10),
		array($S11, $S02, $S01),
		array($S10, $S01, $S00));

		$mLin=new Matrix ($mAarrayL);

		if ($mDampingLin !== null){
		$mLin->plusEquals($mDampingLin);
		}
		// TODO Maybe bypass determinant checks for damped ?
		// if (debugLevel>3) System.out.println(">>> n="+n+" det_lin="+mLin.det()+" norm_lin="+normMatix(mAarrayL));
		$nmL = normMatix($mAarrayL);
		if (($nmL == 0.0) \|\| (abs($mLin.det()) / $nmL < $thresholdLin)){
		// return average value for each channel
		if ($S00 == 0.0) return null; // not even average
		$ABCDEF = Matrix::ZeroMatrix(zDim, 3);
		for ($i = 0; $i < $zDim; $i++) {
		$ABCDEF[$i][0] = 0.0;
		$ABCDEF[$i][1] = 0.0;
		$ABCDEF[$i][2] = $SZ00[$i] / $S00;
		}
		return $ABCDEF;
		}
		$zAarrayL = array_fill(0, 3, 0.0);
		$ABCDEF = array_fill(0,$zDim, null);
		for ($i = 0; $i < $zDim; $i++) {
		$zAarrayL[0]=$SZ10[$i];
		$zAarrayL[1]=$SZ01[$i];
		$zAarrayL[2]=$SZ00[$i];
		$Z = new Matrix ($zAarrayL); // ,3);
		$ABCDEF[$i]= $mLin.solve($Z).getRowPackedCopy();
		}
		if (forceLinear) return ABCDEF;
		// quote try quadratic approximation
		$mAarrayQ = array(
		array($S40, $S22, $S31, $S30, $S21, $S20),
		array($S22, $S04, $S13, $S12, $S03, $S02),
		array($S31, $S13, $S22, $S21, $S12, $S11),
		array($S30, $S12, $S21, $S20, $S11, $S10),
		array($S21, $S03, $S12, $S11, $S02, $S01),
		array($S20, $S02, $S11, $S10, $S01, $S00));
		$mQuad=new Matrix ($mAarrayQ);
		if (isset($mDampingQuad)){
		$mQuad->plusEquals($mDampingQuad);
		}

		// if (debugLevel>3) {
		// System.out.println(" n="+n+" det_quad="+mQuad.det()+" norm_quad="+normMatix(mAarrayQ)+" data.length="+data.length);
		// mQuad.print(10,5);
		// }
		$nmQ = normMatix($mAarrayQ);
		if (($nmQ == 0.0) \|\| (abs($mQuad.det())/normMatix($mAarrayQ) < $thresholdQuad)) {
		// if (debugLevel>0) System.out.println("Using linear approximation, M.det()="+mQuad.det()+
		// " normMatix(mAarrayQ)="+normMatix(mAarrayQ)+
		// ", thresholdQuad="+thresholdQuad+
		// ", nmQ="+nmQ+
		// ", Math.abs(M.det())/normMatix(mAarrayQ)="+(Math.abs(mQuad.det())/normMatix(mAarrayQ))); //did not happen
		return $ABCDEF; // not enough data for the quadratic approximation, return linear
		}
		// double [] zAarrayQ={SZ20,SZ02,SZ11,SZ10,SZ01,SZ00};
		$zAarrayQ = array_fill(0,6,0.0);
		for ($i = 0; $i < $zDim; $i++) {
		$zAarrayQ[0] = $SZ20[$i];
		$zAarrayQ[1] = $SZ02[$i];
		$zAarrayQ[2] = $SZ11[$i];
		$zAarrayQ[3] = $SZ10[$i];
		$zAarrayQ[4] = $SZ01[$i];
		$zAarrayQ[5] = $SZ00[$i];
		$Z = new Matrix ($zAarrayQ); // ,6);
		$ABCDEF[i]= $mQuad.solve($Z).getRowPackedCopy();
		}
		return $ABCDEF;
		}




		// calcualte "volume" made of the matrix row-vectors, placed orthogonally
		// to be compared to determinant
		public function normMatix($a) {
		$norm=1.0;
		for ($i=0; $i<sizeof($a); $i++) {
		$d=0;
		for ($j=0; $j < sizeof($a[$i]); $j++) $d += $a[$i][$j] * $a[$i][$j];
		$norm *= sqrt($d);
		}
		return $norm;
		}


		}

Admin message