LensDistortionParameters.java 152 KB
Newer Older
1
package com.elphel.imagej.calibration;
2
import java.util.Arrays;
3 4
import java.util.Properties;

5 6 7
import com.elphel.imagej.cameras.EyesisSubCameraParameters;
import com.elphel.imagej.common.WindowTools;

8
import Jama.Matrix;
9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24
import ij.IJ;
import ij.gui.GenericDialog;

//import EyesisCameraParameters;
//import Distortions.EyesisSubCameraParameters;
//import LensDistortionParameters;
//import PatternParameters;
//import DistortionCalibrationData.GridImageParameters;
//import DistortionCalibrationData.GridImageSet;

	public class LensDistortionParameters {
		/*
		 * Hugin Rsrc=a*Rdest^4+b*Rdest^3+c*Rdest^2+d*Rdest; d=1-(a+b+c)
		 */
		// lens parameters (add more later)

25 26 27
		final int numInputs= 53; //27; // with A8...// 24;   // parameters in subcamera+...
	    final int numOutputs=42; //16; // with A8...//13;  // parameters in a single camera
//		final
28
//		boolean cummulativeCorrection=true; // r_xy, r_od for higher terms are relative to lower ones
29 30
	    public int defaultLensDistortionModel=200;
	    public int lensDistortionModel=defaultLensDistortionModel;
31
	    public int lensDistortionModelType=0; // set from lensDistortionModel
32
		boolean cummulativeCorrection=false; //true; // r_xy, r_od for higher terms are relative to lower ones
33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53
		public double focalLength=4.5;
		public double pixelSize=  2.2; //um
		public double distortionRadius=  2.8512; // mm - half width of the sensor
		public double distortionA8=0.0; // r^8 (normalized to focal length or to sensor half width?)
		public double distortionA7=0.0; // r^7 (normalized to focal length or to sensor half width?)
		public double distortionA6=0.0; // r^6 (normalized to focal length or to sensor half width?)
		public double distortionA5=0.0; // r^5 (normalized to focal length or to sensor half width?)
		public double distortionA=0.0; // r^4 (normalized to focal length or to sensor half width?)
		public double distortionB=0.0; // r^3
		public double distortionC=0.0; // r^2
		// orientation/position parameters
		public double yaw=0.0;   // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - clockwise from top
		public double pitch=0.0; // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - up
		public double roll=0.0;  // angle in degrees rotation around camera optical axis (perpendicular to pattern if yaw==0, pitch==0), positive - clockwise
		public double x0=0;      // lens axis from pattern center, mm (to the right)
		public double y0=0;      // lens axis from pattern center, mm (down)
		public double z0=0;      // lens axis from pattern center, mm (away from the camera)
		public double distance=2360; // distance from the lens input pupil to the pattern plane along the camera axis, mm
		public double px0=1296.0;           // center of the lens on the sensor, pixels
		public double py0=968.0;           // center of the lens on the sensor, pixels
		public boolean flipVertical; // acquired image is mirrored vertically (mirror used)
54

55
		// new non-radial parameters
56
		final private double [][] r_xy_dflt={{0.0,0.0},{0.0,0.0},{0.0,0.0},{0.0,0.0},{0.0,0.0},{0.0,0.0}}; // only 6, as for the first term delta x, delta y ==0
57
		final private double [][] r_od_dflt=   {{0.0,0.0},{0.0,0.0},{0.0,0.0},{0.0,0.0},{0.0,0.0},{0.0,0.0},{0.0,0.0}}; // ortho
58 59

		public double [][] r_xy=null; // only 6, as for the first term delta x, delta y ==0
60
		public double [][] r_od=null; // ortho
61

62
		public double [][] r_xyod=null; //{x0,y0,ortho, diagonal}
63 64

		// total number of new parameters = 6*2+7+7=26
65

66 67 68 69 70 71 72
		public int debugLevel=1; // was 2
/*
 Modifying to accommodate for eccentricity of different terms (2 parameters per term) and elliptical shape (another 2 terms). When all are
 zeroes, the old parameters are in effect:
		Rdist/R=A8*R^7+A7*R^6+A6*R^5+A5*R^4+A*R^3+B*R^2+C*R+(1-A6-A7-A6-A5-A-B-C)");
		Rdist/R=A8*R6^7+A7*R5^6+A6*R4^5+A5*R3^4+A*R2^3+B*R1^2+C*R0+(1-A6-A7-A6-A5-A-B-C)");
		R[i] depends on px,py and r_xy[i][], r_o[i] (positive - "landscape", negative - "portrait"), r_d (positive - along y=x, negative - along y=-x)
73

74 75
		R[i] = r0[i]*(1+  (r_od[i][0]*(y[i]**2-x[i]**2)+ 2*r_od[i][1]*x[i]*y[i])/r0[i]**2;
		r0[i]=sqrt(x[i]**2+y[2]**2)
76
		x[i]=pixel_x-px0-((i>0)?r_xy[i-1][0]:0)
77
		y[i]=pixel_y-py0-((i>0)?r_xy[i-1][1]:0)
78 79
*/

80 81 82
// intermediate values
		public double phi, theta,psi,cPH,sPH,cTH,sTH,cPS,sPS;
		public double [][] rotMatrix=new double[3][3]; // includes mirroring for Y (target coordinates y- down, camera - y  up)
83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98
		public void setDistortionModelParameters(){
			if (lensDistortionModel<0){
				System.out.println("BUG:setDistortionModelParameters() - lensDistortionModel<0");
				lensDistortionModel=defaultLensDistortionModel;
			}
			this.cummulativeCorrection= (lensDistortionModel==101);
			if (lensDistortionModel<100){
				lensDistortionModelType=0;
			} else if (lensDistortionModel<200){
				lensDistortionModelType=1;
			} else if (lensDistortionModel<300){
				lensDistortionModelType=2;
			} else {
				lensDistortionModelType=0;
			}
		}
99 100
	    public LensDistortionParameters(
	    		boolean isTripod,
101
	    		boolean cartesian,
102 103
	    		double  pixelSize,
	    		double  distortionRadius,
104 105 106 107 108 109 110
	            double [][] interParameterDerivatives, //partial derivative matrix from subcamera-camera-goniometer to single camera (12x21) if null - just values, no derivatives
	    		double [] parVect,
	    		boolean [] mask, // calculate only selected derivatives (all parVect values are still
	    		int debugLevel
	    		){
	    	this.debugLevel=debugLevel;
	    	lensCalcInterParamers( // changed name to move calcInterParamers method from enclosing class
Andrey Filippov's avatar
Andrey Filippov committed
111
	    			this,
112
		    		isTripod,
113
		    		cartesian,
114 115
		    		pixelSize,
		    		distortionRadius,
116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140
		            interParameterDerivatives, //partial derivative matrix from subcamera-camera-goniometer to single camera (12x21) if null - just values, no derivatives
		    		parVect,
		    		mask // calculate only selected derivatives (all parVect values are still
		    		);
	    }

		public LensDistortionParameters(
				double focalLength,
				double pixelSize,  //um
				double distortionRadius, // mm
				double distortionA8, // r^8
				double distortionA7, // r^7
				double distortionA6, // r^6
				double distortionA5, // r^5
				double distortionA, // r^4
				double distortionB, // r^3
				double distortionC, // r^2
				// orientation/position parameters
				double yaw,   // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - clockwise from top
				double pitch, // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - up
				double roll,  // angle in degrees rotation around camera optical axis (perpendicular to pattern if yaw==0, pitch==0), positive - clockwise
				double x0,      // lens axis from pattern center, mm (to the right)
				double y0,      // lens axis from pattern center, mm (down)
				double z0,      // lens axis from pattern center, mm (away from the camera)
				double distance, // distance from the lens input pupil to the pattern plane along the camera axis, mm
141 142 143
				double px0,           // center of the lens on the sensor, pixels
				double py0,           // center of the lens on the sensor, pixels
				boolean flipVertical, // acquired image is mirrored vertically (mirror used)
144
				int lensDistortionModel,
145
				double [][] r_xy,
146
				double [][] r_od
147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167
		){
			setLensDistortionParameters(
			focalLength,
			pixelSize,
			distortionRadius,
			distortionA8,
			distortionA7,
			distortionA6,
			distortionA5,
			distortionA,
			distortionB,
			distortionC,
			yaw,
			pitch,
			roll,
			x0,
			y0,
			z0,
			distance,
			px0,
			py0,
168
			flipVertical,
169
			lensDistortionModel,
170
			r_xy,
171
			r_od);
172
		}
173

174
		public LensDistortionParameters(){
175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195
			setLensDistortionParameters(
			4.5,    // focalLength,
			2.2,    // pixelSize,
			2.8512, // distortionRadius,
			0.0,    // distortionA8,
			0.0,    // distortionA7,
			0.0,    // distortionA6,
			0.0,    // distortionA5,
			0.0,    // distortionA,
			0.0,    // distortionB,
			0.0,    // distortionC,
			0.0,    // yaw,
			0.0,    // pitch,
			0.0,    // roll,
			0.0,    // x0,
			0.0,    // y0,
			0.0,    // z0,
			2360.0, // distance,
			1296,   // px0,
			698,    // py0,
			true,   // flipVertical,
196
			-1,     // lensDistortionModel
197
			null,   // r_xy,
198 199
			null   // r_od,
			);
200
		}
201
		@Override
202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222
		public LensDistortionParameters clone() {
			return new LensDistortionParameters(
					this.focalLength,
					this.pixelSize,
					this.distortionRadius,
					this.distortionA8,
					this.distortionA7,
					this.distortionA6,
					this.distortionA5,
					this.distortionA,
					this.distortionB,
					this.distortionC,
					this.yaw,
					this.pitch,
					this.roll,
					this.x0,
					this.y0,
					this.z0,
					this.distance,
					this.px0,
					this.py0,
223
					this.flipVertical,
224
					this.lensDistortionModel,
225 226
					this.r_xy,
					this.r_od
227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249
			);
		}
		public void setLensDistortionParameters(
				double focalLength,
				double pixelSize,  //um
				double distortionRadius, // mm
				double distortionA8, // r^7
				double distortionA7, // r^6
				double distortionA6, // r^5
				double distortionA5, // r^4
				double distortionA, // r^4
				double distortionB, // r^3
				double distortionC, // r^2
				// orientation/position parameters
				double yaw,   // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - clockwise from top
				double pitch, // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - up
				double roll,  // angle in degrees rotation around camera optical axis (perpendicular to pattern if yaw==0, pitch==0), positive - clockwise
				double x0,      // lens axis from pattern center, mm (to the right)
				double y0,      // lens axis from pattern center, mm (down)
				double z0,      // lens axis from pattern center, mm (away)
				double distance, // distance from the lens input pupil to the pattern plane along the camera axis, mm
				double px0,           // center of the lens on the sensor, pixels
				double py0,           // center of the lens on the sensor, pixels
250
				boolean flipVertical, // acquired image is mirrored vertically (mirror used)
251
				int lensDistortionModel,
252
				double [][] r_xy,   // per polynomial term center x,y correction only 6, as for the first term delta x, delta y ==0
253
				double [][] r_od   // per polynomial term orthogonal+diagonal elongation
254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274
		){
			this.focalLength=focalLength;
			this.pixelSize=pixelSize;
			this.distortionRadius=distortionRadius;
			this.distortionA8=distortionA8;
			this.distortionA7=distortionA7;
			this.distortionA6=distortionA6;
			this.distortionA5=distortionA5;
			this.distortionA=distortionA;
			this.distortionB=distortionB;
			this.distortionC=distortionC;
			this.yaw=yaw;
			this.pitch=pitch;
			this.roll=roll;
			this.x0=x0;
			this.y0=y0;
			this.z0=z0;
			this.distance=distance;
			this.px0=px0;
			this.py0=py0;
			this.flipVertical=flipVertical;
275
			this.lensDistortionModel=(lensDistortionModel>=0)?lensDistortionModel:defaultLensDistortionModel;
276 277 278 279 280 281
			if (r_xy==null) r_xy=r_xy_dflt;
			if (r_od==null) r_od=r_od_dflt;
			this.r_xy=new double [r_xy.length][2];
			for (int i=0;i<r_xy.length;i++)this.r_xy[i]=r_xy[i].clone();
			this.r_od=new double [r_od.length][2];
			for (int i=0;i<r_od.length;i++)this.r_od[i]=r_od[i].clone();
282 283
			recalcCommons();
		}
284

285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306
		public void setIntrincicFromSubcamera(EyesisSubCameraParameters pars){
			setLensDistortionParameters(
					pars.focalLength,
					pars.pixelSize,  //um
					pars.distortionRadius, // mm
					pars.distortionA8, // r^7
					pars.distortionA7, // r^6
					pars.distortionA6, // r^5
					pars.distortionA5, // r^4
					pars.distortionA,  // r^4
					pars.distortionB,  // r^3
					pars.distortionC,  // r^2
					// orientation/position parameters
					this.yaw,          // (keep) angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - clockwise from top
					this.pitch,        // (keep) angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - up
					this.roll,         // (keep) angle in degrees rotation around camera optical axis (perpendicular to pattern if yaw==0, pitch==0), positive - clockwise
					this.x0,           // (keep) lens axis from pattern center, mm (to the right)
					this.y0,           // (keep) lens axis from pattern center, mm (down)
					this.z0,           // (keep) lens axis from pattern center, mm (away)
					this.distance,     // (keep) distance from the lens input pupil to the pattern plane along the camera axis, mm
					pars.px0,          //        center of the lens on the sensor, pixels
					pars.py0,          // center of the lens on the sensor, pixels
307
					this.flipVertical, // (keep)  acquired image is mirrored vertically (mirror used)
308
					pars.lensDistortionModel,
309 310
					pars.r_xy,         // do not exist yet!
					pars.r_od          // do not exist yet!
311 312
			);
		}
313

314 315
		public void setLensDistortionParameters(LensDistortionParameters ldp
		){
316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336
			setLensDistortionParameters(
					ldp.focalLength,
					ldp.pixelSize,
					ldp.distortionRadius,
					ldp.distortionA8,
					ldp.distortionA7,
					ldp.distortionA6,
					ldp.distortionA5,
					ldp.distortionA,
					ldp.distortionB,
					ldp.distortionC,
					ldp.yaw,
					ldp.pitch,
					ldp.roll,
					ldp.x0,
					ldp.y0,
					ldp.z0,
					ldp.distance,
					ldp.px0,
					ldp.py0,
					ldp.flipVertical,
337
					ldp.lensDistortionModel,
338
					ldp.r_xy,
339
					ldp.r_od);
340
		}
341 342 343 344 345 346




// TODO: Fix for non-radial !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
		// just for debugging
347 348 349 350
		public void setLensDistortionParameters(LensDistortionParameters ldp,
				int index, // parameter to add delta, 1..13->14->17
				double delta
		){
351
/*
352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371
			this.focalLength=ldp.focalLength+((index==7)?delta:0);
			this.pixelSize=ldp.pixelSize;
			this.distortionRadius=ldp.distortionRadius;
			this.distortionA8=ldp.distortionA8+((index==9)?delta:0);
			this.distortionA7=ldp.distortionA7+((index==10)?delta:0);
			this.distortionA6=ldp.distortionA6+((index==11)?delta:0);
			this.distortionA5=ldp.distortionA5+((index==12)?delta:0);
			this.distortionA=ldp.distortionA+((index==13)?delta:0);
			this.distortionB=ldp.distortionB+((index==14)?delta:0);
			this.distortionC=ldp.distortionC+((index==15)?delta:0);
			this.yaw=ldp.yaw+((index==1)?delta:0);
			this.pitch=ldp.pitch+((index==2)?delta:0);
			this.roll=ldp.roll+((index==3)?delta:0);
			this.x0=ldp.x0+((index==4)?delta:0);
			this.y0=ldp.y0+((index==5)?delta:0);
			this.z0=ldp.z0+((index==6)?delta:0);
			this.distance=ldp.distance+((index==8)?delta:0);
			this.px0=ldp.px0+((index==16)?delta:0);
			this.py0=ldp.py0+((index==17)?delta:0);
			this.flipVertical=ldp.flipVertical;
372
*/
373 374 375 376
			setLensDistortionParameters(ldp);
			final int index_r_xy=18;
			final int index_r_od=30;
			final int index_end=44;
377

378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402
			switch (index){
			case 1: this.yaw+=delta; break;
			case 2: this.pitch+=delta; break; //=ldp.pitch+((index==2)?delta:0);
			case 3: this.roll+=delta; break; // =ldp.roll+((index==3)?delta:0);
			case 4: this.x0+=delta; break; // =ldp.x0+((index==4)?delta:0);
			case 5: this.y0+=delta; break; // =ldp.y0+((index==5)?delta:0);
			case 6: this.z0+=delta; break; // =ldp.z0+((index==6)?delta:0);
			case 7: this.focalLength+=delta; break; // =ldp.focalLength+((index==7)?delta:0);
			case 8: this.distance+=delta; break; // =ldp.distance+((index==8)?delta:0);
			case 9: this.distortionA8+=delta; break; // =ldp.distortionA8+((index==9)?delta:0);
			case 10: this.distortionA7+=delta; break; // =ldp.distortionA7+((index==10)?delta:0);
			case 11: this.distortionA6+=delta; break; // =ldp.distortionA6+((index==11)?delta:0);
			case 12: this.distortionA5+=delta; break; // =ldp.distortionA5+((index==12)?delta:0);
			case 13: this.distortionA+=delta; break; // =ldp.distortionA+((index==13)?delta:0);
			case 14: this.distortionB+=delta; break; // =ldp.distortionB+((index==14)?delta:0);
			case 15: this.distortionC+=delta; break; // =ldp.distortionC+((index==15)?delta:0);
			case 16: this.px0+=delta; break; // =ldp.px0+((index==16)?delta:0);
			case 17: this.py0+=delta; break; // =ldp.py0+((index==17)?delta:0);
			default:
				if ((index>=index_r_xy) && (index<index_r_od)){
					this.r_xy[(index-index_r_xy)/2][(index-index_r_xy)%2]+=delta;
				} else if ((index>=index_r_od) && (index<index_end)){
					this.r_od[(index-index_r_od)/2][(index-index_r_od)%2]+=delta;
				}
			}
403 404
			recalcCommons();
		}
405 406

// recalculate common (point-invariant) intermediate values (cos, sin, rotation matrix)
407 408 409

		public void recalcCommons(){
//        	this.cummulativeCorrection=false; // just debugging
410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433
//		public double phi, theta,psi,cPH,sPH,cTH,sTH,cPS,sPS;
//    		public double [][] rotMatrix=new double[3][3];
        	this.phi=  this.yaw*Math.PI/180;
        	this.theta=this.pitch*Math.PI/180;
        	this.psi=  this.roll*Math.PI/180;
        	this.sPH=Math.sin(this.phi);
        	this.cPH=Math.cos(this.phi);
        	this.sTH=Math.sin(this.theta);
        	this.cTH=Math.cos(this.theta);
        	this.sPS=Math.sin(this.psi);
        	this.cPS=Math.cos(this.psi);
/*
| Xe |   |   0  |   | cPS*cPH+sPS*sTH*sPH   -sPS*cTH  -cPS*sPH+sPS*sTH*cPH |   | Xp |
| Ye | = |   0  | + | sPS*cPH-cPS*sTH*sPH    cPS*cTH  -sPS*sPH-cPS*sTH*cPH | * |-Yp |
| Ze |   | dist |   | cTH*sPH                    sTH       cTH*cPH         |   | Zp |

| PX | =(1000*f)/(Ze*Psz) * |  Xe | + | PX0 |
| PY | =                    | -Ye |   | PY0 |


 Xe =   (cPS*cPH+sPS*sTH*sPH)*Xp   +sPS*cTH*Yp  +(-cPS*sPH+sPS*sTH*cPH)*Zp
 Ye =   (sPS*cPH-cPS*sTH*sPH)*Xp   -cPS*cTH*Yp  +(-sPS*sPH-cPS*sTH*cPH)*Zp
 Ze =   (cTH*sPH)*Xp               -sTH*Yp      +( cTH*cPH)* Zp + dist

434 435 436

theta==0, psi==0:
 Xe =   (cPH)*Xp
437 438 439 440
 Ye =   Yp
 Ze =   cPH* Zp + dist

(4) PXmmc =f/(cPH* Zp + dist)*  (cPH)*Xp  // mm, left from the lens axis intersection with the sensor
441

442
dPXmmc/dphi=
443

444 445 446 447 448 449 450 451 452 453 454 455 456 457
 */
        	this.rotMatrix[0][0]= cPS*cPH+sPS*sTH*sPH;
        	this.rotMatrix[0][1]= sPS*cTH;
           	this.rotMatrix[0][2]=-cPS*sPH+sPS*sTH*cPH;
           	this.rotMatrix[1][0]= sPS*cPH-cPS*sTH*sPH;
           	this.rotMatrix[1][1]=-cPS*cTH;
           	this.rotMatrix[1][2]=-sPS*sPH-cPS*sTH*cPH;
           	this.rotMatrix[2][0]= cTH*sPH;
           	this.rotMatrix[2][1]=-sTH;
           	this.rotMatrix[2][2]= cTH*cPH;
           	if (this.debugLevel>2){
           		System.out.println("recalcCommons():this.rotMatrix:");
        		(new Matrix(this.rotMatrix)).print(10, 5);
           	}
458 459 460 461 462 463
           	//
    		this.r_xyod=new double [this.r_od.length][4]; //{x0,y0,ortho, diagonal}
    		this.r_xyod[0][0]=0.0; // this.px0; //
    		this.r_xyod[0][1]=0.0; // this.py0;
    		this.r_xyod[0][2]=this.r_od[0][0];
    		this.r_xyod[0][3]=this.r_od[0][1];
464
    		setDistortionModelParameters();
465 466 467 468 469 470 471 472 473 474 475 476 477 478 479
    		if (cummulativeCorrection){
    			for (int i=1;i<this.r_xyod.length;i++){
    	    		this.r_xyod[i][0]=this.r_xyod[i-1][0]+this.r_xy[i-1][0];
    	    		this.r_xyod[i][1]=this.r_xyod[i-1][1]+this.r_xy[i-1][1];
    	    		this.r_xyod[i][2]=this.r_xyod[i-1][2]+this.r_od[i][0];
    	    		this.r_xyod[i][3]=this.r_xyod[i-1][3]+this.r_od[i][1];
    			}
    		} else {
    			for (int i=1;i<this.r_xyod.length;i++){
    	    		this.r_xyod[i][0]=this.r_xy[i-1][0]; // this.px0+this.r_xy[i-1][0];
    	    		this.r_xyod[i][1]=this.r_xy[i-1][1]; // this.py0+this.r_xy[i-1][1];
    	    		this.r_xyod[i][2]=this.r_od[i][0];
    	    		this.r_xyod[i][3]=this.r_od[i][1];
    			}
    		}
480
        }
481

482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498
        private String [][] descriptions=
        {
        		{"distance",    "Distance from the intersection of the lens axis with z=0 target plane to the camera lens entrance pupil", "mm", "e"},
        		{"x0",          "Lens axis from pattern center, (to the right)", "mm","e"},
        		{"y0",          "Lens axis from pattern center, (down)", "mm","e"},
        		{"yaw",         "Angle from perpendicular to the pattern, 0 - towards wall, positive - clockwise from top", "degrees","e"},
        		{"pitch",       "Angle from perpendicular to the pattern, 0 - towards wall, positive - up", "degrees","e"},
        		{"roll",        "Angle around camera optical axis (perpendicular to pattern if yaw==0, pitch==0), positive - clockwise", "degrees","e"},
        		{"focalLength", "Lens focal length", "mm","i"},
        		{"px0",         "horizontal (left to right) pixel number of the lens axis intersection with the sensor", "pix","i"},
        		{"py0",         "vertical (up to down) pixel number of the lens axis intersection with the sensor", "pix","i"},
        		{"distortionA8","Lens distortion coefficient for r^8 (r0=half sensor width)", "r0","i"},
        		{"distortionA7","Lens distortion coefficient for r^7 (r0=half sensor width)", "r0","i"},
        		{"distortionA6","Lens distortion coefficient for r^6 (r0=half sensor width)", "r0","i"},
        		{"distortionA5","Lens distortion coefficient for r^5 (r0=half sensor width)", "r0","i"},
        		{"distortionA", "Lens distortion coefficient for r^4 (r0=half sensor width)", "r0","i"},
        		{"distortionB", "Lens distortion coefficient for r^3 (r0=half sensor width)", "r0","i"},
499 500 501 502 503 504 505 506
        		{"distortionC", "Lens distortion coefficient for r^2 (r0=half sensor width)", "r0","i"},
        		{"elong_C_o",   "Orthogonal elongation for r^2", "rel","i"},
        		{"elong_C_d",   "Diagonal   elongation for r^2", "rel","i"},

        		{"eccen_B_x",   "Distortion center shift X for r^3", "rel","i"},
        		{"eccen_B_y",   "Distortion center shift Y for r^3", "rel","i"},
        		{"elong_B_o",   "Orthogonal elongation for r^3", "rel","i"},
        		{"elong_B_d",   "Diagonal   elongation for r^3", "rel","i"},
507

508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531
        		{"eccen_A_x",   "Distortion center shift X for r^4", "rel","i"},
        		{"eccen_A_y",   "Distortion center shift Y for r^4", "rel","i"},
        		{"elong_A_o",   "Orthogonal elongation for r^4", "rel","i"},
        		{"elong_A_d",   "Diagonal   elongation for r^4", "rel","i"},

        		{"eccen_A5_x",  "Distortion center shift X for r^5", "rel","i"},
        		{"eccen_A5_y",  "Distortion center shift Y for r^5", "rel","i"},
        		{"elong_A5_o",  "Orthogonal elongation for r^5", "rel","i"},
        		{"elong_A5_d",  "Diagonal   elongation for r^5", "rel","i"},

        		{"eccen_A6_x",  "Distortion center shift X for r^6", "rel","i"},
        		{"eccen_A6_y",  "Distortion center shift Y for r^6", "rel","i"},
        		{"elong_A6_o",  "Orthogonal elongation for r^6", "rel","i"},
        		{"elong_A6_d",  "Diagonal   elongation for r^6", "rel","i"},

        		{"eccen_A7_x",  "Distortion center shift X for r^7", "rel","i"},
        		{"eccen_A7_y",  "Distortion center shift Y for r^7", "rel","i"},
        		{"elong_A7_o",  "Orthogonal elongation for r^7", "rel","i"},
        		{"elong_A7_d",  "Diagonal   elongation for r^7", "rel","i"},

        		{"eccen_A8_x",  "Distortion center shift X for r^8", "rel","i"},
        		{"eccen_A8_y",  "Distortion center shift Y for r^8", "rel","i"},
        		{"elong_A8_o",  "Orthogonal elongation for r^8", "rel","i"},
        		{"elong_A8_d",  "Diagonal   elongation for r^8", "rel","i"}
532 533 534 535 536 537 538 539
        };
        private int numberIntExtrisic(String type){
        	int num=0;
        	for (int i=0;i<this.descriptions.length;i++) if (type.indexOf(descriptions[i][3].charAt(0))>=0) num++;
        	return num;
        }
        /**
         * Verifies that the camera is looking towards the target
540
         * @return true if looking to the target, false - if away
541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561
         */
        public boolean isTargetVisible(boolean verbose){
        	if (verbose) System.out.println("isTargetVisible(): this.distance="+this.distance+", this.yaw="+this.yaw+", this.pitch="+this.pitch);
        	if (this.distance <=0.0) return false;
        	if (Math.cos(this.yaw*Math.PI/180)<0.0) return false;
        	if (Math.cos(this.pitch*Math.PI/180)<0.0) return false;
        	return true;
        }

        public double [] getExtrinsicVector(){
        	double [] extVector = {
        			this.distance,
        			this.x0,
        			this.y0,
        			this.yaw,
        			this.pitch,
        			this.roll
        			};
        	return extVector;
        }
        public double [] getIntrinsicVector(){
562
        	double [] intVector = {
563 564 565 566 567 568 569 570 571
        			this.focalLength,
        			this.px0,
        			this.py0,
        			this.distortionA8,
        			this.distortionA7,
        			this.distortionA6,
        			this.distortionA5,
        			this.distortionA,
        			this.distortionB,
572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604
        			this.distortionC,
        			this.r_od[0][0],
        			this.r_od[0][1],

        			this.r_xy[0][0],
        			this.r_xy[0][1],
        			this.r_od[1][0],
        			this.r_od[1][1],

        			this.r_xy[1][0],
        			this.r_xy[1][1],
        			this.r_od[2][0],
        			this.r_od[2][1],

        			this.r_xy[2][0],
        			this.r_xy[2][1],
        			this.r_od[3][0],
        			this.r_od[3][1],

        			this.r_xy[3][0],
        			this.r_xy[3][1],
        			this.r_od[4][0],
        			this.r_od[4][1],

        			this.r_xy[4][0],
        			this.r_xy[4][1],
        			this.r_od[5][0],
        			this.r_od[5][1],

        			this.r_xy[5][0],
        			this.r_xy[5][1],
        			this.r_od[6][0],
        			this.r_od[6][1]
605
        	};
606
        	return intVector;
607 608 609 610
        }
        public double [] getAllVector(){
        	double [] extVector=getExtrinsicVector();
        	double [] intVector=getIntrinsicVector();
611 612
        	double [] allVector = new double[extVector.length+intVector.length];
        	int index=0;
613 614 615 616 617 618 619
        	for (int i=0;i<extVector.length;i++) allVector[index++]= extVector[i];
        	for (int i=0;i<intVector.length;i++) allVector[index++]= intVector[i];
        	return allVector;
        }
        public void setAllVector(double [] vector){
        	if (vector.length!=(getExtrinsicVector().length+getIntrinsicVector().length)){
        		String msg="Parameter vector should have exactly"+(getExtrinsicVector().length+getIntrinsicVector().length)+" elements";
620
        		IJ.showMessage("Error",msg);
621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638
        		throw new IllegalArgumentException (msg);
        	}
        	this.distance=    vector[ 0];
        	this.x0=          vector[ 1];
        	this.y0=          vector[ 2];
        	this.yaw=         vector[ 3];
        	this.pitch=       vector[ 4];
        	this.roll=        vector[ 5];
        	this.focalLength= vector[ 6];
        	this.px0=         vector[ 7];
        	this.py0=         vector[ 8];
        	this.distortionA8=vector[ 9];
        	this.distortionA7=vector[10];
        	this.distortionA6=vector[11];
        	this.distortionA5=vector[12];
        	this.distortionA= vector[13];
        	this.distortionB= vector[14];
        	this.distortionC= vector[15];
639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657
        	int index=16;
        	for (int i=0;i<this.r_od.length;i++){
        		if (i>0) {
            		this.r_xy[i-1][0]=vector[index++];
            		this.r_xy[i-1][1]=vector[index++];
        		}
        		this.r_od[i][0]=vector[index++];
        		this.r_od[i][1]=vector[index++];
        	}
        	/*
        	for (int i=0;i<this.r_xy.length;i++){
        		this.r_xy[i][0]=vector[index++];
        		this.r_xy[i][1]=vector[index++];
        	}
        	for (int i=0;i<this.r_od.length;i++){
        		this.r_od[i][0]=vector[index++];
        		this.r_od[i][1]=vector[index++];
        	}
        	*/
658 659
        	recalcCommons();
        }
660

661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677
        public String [] getExtrinsicNames()       {return getDescriptionStrings("e", 0);}
        public String [] getExtrinsicDescriptions(){return getDescriptionStrings("e", 1);}
        public String [] getExtrinsicUnits()       {return getDescriptionStrings("e", 2);}
        public String [] getIntrinsicNames()       {return getDescriptionStrings("i", 0);}
        public String [] getIntrinsicDescriptions(){return getDescriptionStrings("i", 1);}
        public String [] getIntrinsicUnits()       {return getDescriptionStrings("i", 2);}
        public String [] getAllNames()             {return getDescriptionStrings("ei", 0);}
        public String [] getAllDescriptions()      {return getDescriptionStrings("ei", 1);}
        public String [] getAllUnits()             {return getDescriptionStrings("ei", 2);}
        public String [] getAllFlags()             {return getDescriptionStrings("ei", 3);}

        private String [] getDescriptionStrings(String type, int var){
        	String [] s=new String [numberIntExtrisic(type)];
        	int num=0;
        	for (int i=0;i<this.descriptions.length;i++) if (type.indexOf(descriptions[i][3].charAt(0))>=0) s[num++]=this.descriptions[i][var];
        	return s;
        }
678 679


680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696
/*
 * Calculate pixel value of projection from the pattern point [xp,yp,zp] using current distortion/position parameters
(1) Xe =   (cPS*cPH+sPS*sTH*sPH)*(Xp-X0)   +sPS*cTH*(Yp-Y0)  +(-cPS*sPH+sPS*sTH*cPH)*(Zp-Z0)
(2) Ye =   (sPS*cPH-cPS*sTH*sPH)*(Xp-X0)   -cPS*cTH*(Yp-Y0)  +(-sPS*sPH-cPS*sTH*cPH)*(Zp-Z0)
(3) Ze =   (cTH*sPH)*(Xp-X0)               -sTH*(Yp-Y0)      +( cTH*cPH)* (Zp-Z0) + dist

(4) PXmmc =f/Ze*  Xe  // mm, left from the lens axis intersection with the sensor
(5) PYmmc =f/Ze*  Ye  // mm, up from the lens axis intersection with the sensor

(6) r=sqrt(PXmmc^2+PYmmc^2) // distance from the image point to the lens axis intersection with the sensor (pinhole model)
(7) kD=(Da*(r/r0)^3+Db*(r/r0)^2+Dc*(r/r0)^1+(1-Da-Db-Dc)) correction to the actual distance from the image point to the lens axis due to distortion
(8) xDist = kD *  PXmmc // horisontal distance (mm) from the lens axis on the sensor to the image point, mm (positive - right)
(9) yDist = kD *  PYmmc // vertical distance (mm) from the lens axis on the sensor to the image point, mm (positive - up)


(10) PX = 1000/Psz*( xDist) + PX0 // horizontal pixel of the image (positive - right)
(11) PY = 1000/Psz*(-yDist) + PY0 // vertical pixel of the image (positive - down)
697

698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747
 */
        public double [] patternToPixels(
        		double xp, // target point horizontal, positive - right,  mm
        		double yp, // target point vertical,   positive - down,  mm
        		double zp // target point horizontal, positive - away from camera,  mm
        		){
        	return calcPartialDerivatives(xp,yp,zp,false)[0];
        }
/*
0		public double x0=0;      // lens axis from pattern center, mm (to the right)
1		public double y0=0;      // lens axis from pattern center, mm (down)
2		public double distance=2360; // distance from the lens input pupil to the pattern plane along the camera axis, mm
3		public double yaw=0.0;   // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - clockwise from top
4		public double pitch=0.0; // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - up
5		public double roll=0.0;  // angle in degrees rotation around camera optical axis (perpendicular to pattern if yaw==0, pitch==0), positive - clockwise
6		public double focalLength=4.5;
7		public double px0=1296.0;           // center of the lens on the sensor, pixels
8		public double py0=968.0;           // center of the lens on the sensor, pixels
 		public double distortionRadius=  2.8512; // mm - half width of the sensor
9		public double distortionA8=0.0; // r^8 (normalized to focal length or to sensor half width?)
10		public double distortionA7=0.0; // r^7 (normalized to focal length or to sensor half width?)
11		public double distortionA6=0.0; // r^6 (normalized to focal length or to sensor half width?)
12		public double distortionA5=0.0; // r^5 (normalized to focal length or to sensor half width?)
13		public double distortionA=0.0; // r^4 (normalized to focal length or to sensor half width?)
14		public double distortionB=0.0; // r^3
15		public double distortionC=0.0; // r^2

 */
        /**
         *  extract needed ones, and reorder partial derivatives to match calcInterParameters
         *  @param srcDerivatives - values and 15 derivatives for px, py
         */
        public double [][] reorderPartialDerivatives (double [][] srcDerivatives){
        	int [] order={
        			 4, // 0		public double x0=0;      // lens axis from pattern center, mm (to the right)
        			 5, // 1		public double y0=0;      // lens axis from pattern center, mm (down)
        			 8, // 2		public double distance=2360; // distance from the lens input pupil to the pattern plane along the camera axis, mm
        			 1, // 3		public double yaw=0.0;   // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - clockwise from top
        			 2, // 4		public double pitch=0.0; // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - up
        			 3, // 5		public double roll=0.0;  // angle in degrees rotation around camera optical axis (perpendicular to pattern if yaw==0, pitch==0), positive - clockwise
        			 7, // 6		public double focalLength=4.5;
        			16, // 7		public double px0=1296.0;           // center of the lens on the sensor, pixels
        			17, // 8		public double py0=968.0;           // center of the lens on the sensor, pixels
        			    // 	public double distortionRadius=  2.8512; // mm - half width of the sensor
       			     9, // 9		public double distortionA5=0.0; // r^5 (normalized to focal length or to sensor half width?)
    			    10, // 10		public double distortionA5=0.0; // r^5 (normalized to focal length or to sensor half width?)
    			    11, // 11		public double distortionA5=0.0; // r^5 (normalized to focal length or to sensor half width?)
        			12, // 12		public double distortionA5=0.0; // r^5 (normalized to focal length or to sensor half width?)
        			13, // 13		public double distortionA=0.0; // r^4 (normalized to focal length or to sensor half width?)
        			14, // 14		public double distortionB=0.0; // r^3
748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774
        			15, // 15		public double distortionC=0.0; // r^2
        			18, // 16		Orthogonal elongation for r^2"
        			19, // 17		Diagonal   elongation for r^2"
        			20, // 18		Distortion center shift X for r^3"
        			21, // 19		Distortion center shift Y for r^3"
        			22, // 20		Orthogonal elongation for r^3"
        			23, // 21		Diagonal   elongation for r^3"
        			24, // 22		Distortion center shift X for r^4"
        			25, // 23		Distortion center shift Y for r^4"
        			26, // 24		Orthogonal elongation for r^4"
        			27, // 25		Diagonal   elongation for r^4"
        			28, // 26		Distortion center shift X for r^5"
        			29, // 27		Distortion center shift Y for r^5"
        			30, // 28		Orthogonal elongation for r^5"
        			31, // 29		Diagonal   elongation for r^5"
        			32, // 30		Distortion center shift X for r^6"
        			33, // 31		Distortion center shift Y for r^6"
        			34, // 32		Orthogonal elongation for r^6"
        			35, // 33		Diagonal   elongation for r^6"
        			36, // 34		Distortion center shift X for r^7"
        			37, // 35		Distortion center shift Y for r^7"
        			38, // 36		Orthogonal elongation for r^6"
        			39, // 37		Diagonal   elongation for r^6"
        			40, // 38		Distortion center shift X for r^8"
        			41, // 29		Distortion center shift Y for r^8"
        			42, // 40		Orthogonal elongation for r^8"
        			43  // 41		Diagonal   elongation for r^8"
775 776 777 778 779 780 781 782
        	};
        	double [][] result = new double [order.length][2];
        	for (int i=0; i<order.length;i++){
        		result[i][0]=srcDerivatives[order[i]][0];
        		result[i][1]=srcDerivatives[order[i]][1];
        	}
        	return result;
        }
783 784
/*
 * result = {{srcDerivatives[4][0],srcDerivatives[4][1]}, - X0
785 786 787
 *           {srcDerivatives[5][0],srcDerivatives[5][1]}  - Y0
 *           {srcDerivatives[8][0],srcDerivatives[8][1]}  - dist
 *           ...
788
 */
789 790


791
/**
792
 * Reorder to match the sequence of names - seems to be different :-(
793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812
 * @param srcDerivatives  values and 15 derivatives for px, py
 * @return
 */
        public double [][] reorderPartialDerivativesAsNames (double [][] srcDerivatives){
        	int [] order={
        			 8, // 2		public double distance=2360; // distance from the lens input pupil to the pattern plane along the camera axis, mm
        			 4, // 0		public double x0=0;      // lens axis from pattern center, mm (to the right)
        			 5, // 1		public double y0=0;      // lens axis from pattern center, mm (down)
        			 1, // 3		public double yaw=0.0;   // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - clockwise from top
        			 2, // 4		public double pitch=0.0; // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - up
        			 3, // 5		public double roll=0.0;  // angle in degrees rotation around camera optical axis (perpendicular to pattern if yaw==0, pitch==0), positive - clockwise
        			 7, // 6		public double focalLength=4.5;
        			16, // 7		public double px0=1296.0;           // center of the lens on the sensor, pixels
        			17, // 8		public double py0=968.0;           // center of the lens on the sensor, pixels
        			 9, // 9		public double distortionA8=0.0; // r^5 (normalized to focal length or to sensor half width?)
        			10, // 10		public double distortionA7=0.0; // r^5 (normalized to focal length or to sensor half width?)
        			11, // 11		public double distortionA6=0.0; // r^5 (normalized to focal length or to sensor half width?)
        			12, // 12		public double distortionA5=0.0; // r^5 (normalized to focal length or to sensor half width?)
        			13, // 13		public double distortionA=0.0; // r^4 (normalized to focal length or to sensor half width?)
        			14, // 14		public double distortionB=0.0; // r^3
813 814 815
        			15, // 15		public double distortionC=0.0; // r^2
        			18, // 16		Orthogonal elongation for r^2"
        			19, // 17		Diagonal   elongation for r^2"
816

817 818 819 820
        			20, // 18		Distortion center shift X for r^3"
        			21, // 19		Distortion center shift Y for r^3"
        			22, // 20		Orthogonal elongation for r^3"
        			23, // 21		Diagonal   elongation for r^3"
821

822 823 824 825
        			24, // 22		Distortion center shift X for r^4"
        			25, // 23		Distortion center shift Y for r^4"
        			26, // 24		Orthogonal elongation for r^4"
        			27, // 25		Diagonal   elongation for r^4"
826

827 828 829 830
        			28, // 26		Distortion center shift X for r^5"
        			29, // 27		Distortion center shift Y for r^5"
        			30, // 28		Orthogonal elongation for r^5"
        			31, // 29		Diagonal   elongation for r^5"
831

832 833 834 835
        			32, // 30		Distortion center shift X for r^6"
        			33, // 31		Distortion center shift Y for r^6"
        			34, // 32		Orthogonal elongation for r^6"
        			35, // 33		Diagonal   elongation for r^6"
836

837 838 839 840 841 842 843 844 845
        			36, // 34		Distortion center shift X for r^7"
        			37, // 35		Distortion center shift Y for r^7"
        			38, // 36		Orthogonal elongation for r^6"
        			39, // 37		Diagonal   elongation for r^6"

        			40, // 38		Distortion center shift X for r^8"
        			41, // 29		Distortion center shift Y for r^8"
        			42, // 40		Orthogonal elongation for r^8"
        			43  // 41		Diagonal   elongation for r^8"
846

847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863
        	};
        	double [][] result = new double [order.length][2];
        	for (int i=0; i<order.length;i++){
        		result[i][0]=srcDerivatives[order[i]][0];
        		result[i][1]=srcDerivatives[order[i]][1];
        	}
        	return result;
        }
/**
 * Calculate lens center location in target coordinates
 * @return lens center coordinates
 */
        public double [] getLensCenterCoordinates(){
        	double [] p={
        			 this.x0-this.distance*this.rotMatrix[2][0],
        			-this.y0-this.distance*this.rotMatrix[2][1], // this.y0 - up?
        			 this.z0-this.distance*this.rotMatrix[2][2]};
864
/*
865 866 867 868 869 870
        	Matrix MR=new Matrix(this.rotMatrix);
        	Matrix MRT=MR.transpose();
        	Matrix E=MR.times(MRT);
        	System.out.println("===MR==");  	MR.print(8, 5);System.out.println("");
        	System.out.println("===MRT==");  	MRT.print(8, 5);System.out.println("");
        	System.out.println("===E===");  	E.print(8, 5);System.out.println("");
871

872 873
        	System.out.println("x0="+IJ.d2s(this.x0,1)+" y0="+IJ.d2s(this.y0,1)+" z0="+IJ.d2s(this.z0,1)+" this.distance="+IJ.d2s(this.distance,1));
        	System.out.println("phi="+IJ.d2s(this.phi,4)+" theta="+IJ.d2s(this.theta,4)+" psi="+IJ.d2s(this.psi,4));
874

875 876 877
        	System.out.println("|"+IJ.d2s(this.rotMatrix[0][0],4)+" "+IJ.d2s(this.rotMatrix[0][1],4)+" "+IJ.d2s(this.rotMatrix[0][2],4)+"|");
        	System.out.println("|"+IJ.d2s(this.rotMatrix[1][0],4)+" "+IJ.d2s(this.rotMatrix[1][1],4)+" "+IJ.d2s(this.rotMatrix[1][2],4)+"|");
        	System.out.println("|"+IJ.d2s(this.rotMatrix[2][0],4)+" "+IJ.d2s(this.rotMatrix[2][1],4)+" "+IJ.d2s(this.rotMatrix[2][2],4)+"|");
878
*/
879 880 881 882 883 884 885 886 887 888 889 890 891 892 893 894 895 896 897 898 899 900 901 902 903
        	return p;
        }

/*
 * Calculate pixel value and partial derivatives for different parameters
 * of projection from the pattern point [xp,yp,zp] using current distortion/position parameters
 * [0][0] - pixel x - NaN if looking away
 * [1][ 0] - pixel y- NaN if looking away
 * [*][ 1] - pixel x[or y] partial derivative for phi    (yaw)
 * [*][ 2] - pixel x[or y] partial derivative for theta  (pitch)
 * [*][ 3] - pixel x[or y] partial derivative for psi    (roll)
 * [*][ 4] - pixel x[or y] partial derivative for X0 (intersection of the lens axis with zp==Z0 plane of the target)
 * [*][ 5] - pixel x[or y] partial derivative for Y0 (intersection of the lens axis with zp==Z0 plane of the target)
 * [*][ 6] - pixel x[or y] partial derivative for Z0 (intersection of the lens axis with zp==Z0 plane of the target) - not used
 * [*][ 7] - pixel x[or y] partial derivative for f  (focal length)
 * [*][ 8] - pixel x[or y] partial derivative for dist (distance from [X0,Y0,Z0] to the lens entrance pupil
 * [*][ 9] - pixel x[or y] partial derivative for Da8 (distortion coefficient for r^8)
 * [*][10] - pixel x[or y] partial derivative for Da7 (distortion coefficient for r^7)
 * [*][11] - pixel x[or y] partial derivative for Da6 (distortion coefficient for r^6)
 * [*][12] - pixel x[or y] partial derivative for Da5 (distortion coefficient for r^5)
 * [*][13] - pixel x[or y] partial derivative for Da (distortion coefficient for r^4)
 * [*][14] - pixel x[or y] partial derivative for Db (distortion coefficient for r^3)
 * [*][15] - pixel x[or y] partial derivative for Dc (distortion coefficient for r^2)
 * [*][16] - pixel x[or y] partial derivative for PX0 (lens axis on the sensor, horizontal, right,in pixels)
 * [*][17] - pixel x[or y] partial derivative for PY0 (lens axis on the sensor, vertical, down, in pixels)
904 905 906 907 908 909 910 911 912 913 914 915 916 917 918 919 920 921 922 923 924 925 926 927 928 929
 * [*][18] - Orthogonal elongation for r^2"
 * [*][19] - Diagonal   elongation for r^2"
 * [*][20] - Distortion center shift X for r^3"
 * [*][21] - Distortion center shift Y for r^3"
 * [*][22] - Orthogonal elongation for r^3"
 * [*][23] - Diagonal   elongation for r^3"
 * [*][24] - Distortion center shift X for r^4"
 * [*][25] - Distortion center shift Y for r^4"
 * [*][26] - Orthogonal elongation for r^4"
 * [*][27] - Diagonal   elongation for r^4"
 * [*][28] - Distortion center shift X for r^5"
 * [*][29] - Distortion center shift Y for r^5"
 * [*][30] - Orthogonal elongation for r^5"
 * [*][31] - Diagonal   elongation for r^5"
 * [*][32] - Distortion center shift X for r^6"
 * [*][33] - Distortion center shift Y for r^6"
 * [*][34] - Orthogonal elongation for r^6"
 * [*][35] - Diagonal   elongation for r^6"
 * [*][36] - Distortion center shift X for r^7"
 * [*][37] - Distortion center shift Y for r^7"
 * [*][38] - Orthogonal elongation for r^7"
 * [*][39] - Diagonal   elongation for r^7"
 * [*][40] - Distortion center shift X for r^8"
 * [*][41] - Distortion center shift Y for r^8"
 * [*][42] - Orthogonal elongation for r^8"
 * [*][43] - Diagonal   elongation for r^8"
930 931 932 933
 */
        /*
         * TODO: minimaize calculations for individual {xp,yp,zp}
         */
934 935 936 937 938 939 940 941 942 943 944 945 946 947 948
        public double ipow(double a, int b){
        	switch (b) {
        	case 1: return a;
        	case 2: return a*a;
        	case 3: return a*a*a;
        	case 0: return 1.0;
        	default:
        		if (b<0) {
        			return 1.0/ipow(a,-b);
        		} else {
        			int b1=b>>1;
        		    return ipow(a,b1)*ipow(a,b-b1);
        		}
        	}
        }
949 950 951 952 953 954 955
        public double [][] calcPartialDerivatives(
        		double xp, // target point horizontal, positive - right,  mm
        		double yp, // target point vertical,   positive - down,  mm
        		double zp, // target point horizontal, positive - away from camera,  mm
        		boolean calculateAll){ // calculate derivatives, false - values only
        	double maxRelativeRadius=2.0;
        	return calcPartialDerivatives(xp,yp,zp,maxRelativeRadius,calculateAll);
956
        }
957 958 959 960
        public double [][] calcPartialDerivatives(
        		double xp, // target point horizontal, positive - right,  mm
        		double yp, // target point vertical,   positive - down,  mm
        		double zp, // target point horizontal, positive - away from camera,  mm
961
            	double maxRelativeRadius, // make configurable?
962 963 964 965 966 967 968 969 970 971 972
        		boolean calculateAll){ // calculate derivatives, false - values only
        	switch (this.lensDistortionModelType){
        	case 0:
        	case 1:
        		return calcPartialDerivatives_type1(xp,yp,zp,maxRelativeRadius,calculateAll);
        	case 2:
        		return calcPartialDerivatives_type2(xp,yp,zp,maxRelativeRadius,calculateAll);
        	default:
        		return calcPartialDerivatives_type1(xp,yp,zp,maxRelativeRadius,calculateAll);
        	}
        }
973 974


975
        public double [][] calcPartialDerivatives_type1(
976 977 978
        		double xp, // target point horizontal, positive - right,  mm
        		double yp, // target point vertical,   positive - down,  mm
        		double zp, // target point horizontal, positive - away from camera,  mm
979
        		double maxRelativeRadius,
980
        		boolean calculateAll){ // calculate derivatives, false - values only
981
//        	this.cummulativeCorrection=false; // just debugging
982

983 984 985 986 987 988
        	// TODO - add reduced calculations for less terms?
//        	final int numDerivatives=44; // 18+6*2+7*2; // 18  for radial and 26 more for non-radial
        	final int numRadialDerivatives=18;
        	final int numNonRadialDerivatives=26;
        	final int distortionCIndex=15; // a8->9, a7->10, a6->11, a5->12, a->13, b->14, c->15
        	double partDeriv[][] = new double [calculateAll?(numRadialDerivatives+numNonRadialDerivatives):1][2];
989
//        	double [] XYZ= {xp-this.x0, yp-this.y0, zp-this.z0};
990
        	double [] XYZ= {xp-this.x0, yp+this.y0, zp-this.z0}; // Y - "down" (as in images), not up
991 992 993 994 995
        	double [] XeYeZe={
        			this.rotMatrix[0][0]*XYZ[0] + this.rotMatrix[0][1]*XYZ[1] + this.rotMatrix[0][2]*XYZ[2],
        			this.rotMatrix[1][0]*XYZ[0] + this.rotMatrix[1][1]*XYZ[1] + this.rotMatrix[1][2]*XYZ[2],
        			this.rotMatrix[2][0]*XYZ[0] + this.rotMatrix[2][1]*XYZ[1] + this.rotMatrix[2][2]*XYZ[2]+this.distance
        	};
996
        	// PXYmmc - pinhole (non-distorted) projection coordinates. metric (in mm)
997
        	double [] PXYmmc={this.focalLength/XeYeZe[2]*XeYeZe[0],this.focalLength/XeYeZe[2]*XeYeZe[1]};
998 999
        	// now each term has individual radius
//        	double [] rr=new double [r_xyod.length];
1000
        	// Geometric - get to pinhole coordinates on the sensor
1001 1002 1003 1004 1005 1006 1007 1008 1009 1010 1011
            double [][] dXeYeZe=null; //[14];
            double [][] dPXYmmc=null;

    		/*
   		 Modifying to accommodate for eccentricity of different terms (2 parameters per term) and elliptical shape (another 2 terms). When all are
   		 zeroes, the old parameters are in effect:
   				Rdist/R=A8*R^7+A7*R^6+A6*R^5+A5*R^4+A*R^3+B*R^2+C*R+(1-A6-A7-A6-A5-A-B-C)");
   				Rdist/R=A8*R6^7+A7*R5^6+A6*R4^5+A5*R3^4+A*R2^3+B*R1^2+C*R0+(1-A6-A7-A6-A5-A-B-C)");
   				R[i] depends on px,py and r_xy[i][], r_o[i] (positive - "landscape", negative - "portrait"), r_d (positive - along y=x, negative - along y=-x)
   				R[i] = r0[i]*(1+  (r_od[i][0]*(y[i]**2-x[i]**2)+ 2*r_od[i][1]*x[i]*y[i])/r0[i]**2;
   				r0[i]=sqrt(x[i]**2+y[2]**2)
1012
   				x[i]=pixel_x-px0-((i>0)?r_xy[i-1][0]:0)
1013
   				y[i]=pixel_y-py0-((i>0)?r_xy[i-1][1]:0)
1014
   		*/
1015 1016 1017 1018
        	double [] a={this.distortionC,this.distortionB,this.distortionA,this.distortionA5,this.distortionA6,this.distortionA7,this.distortionA8};
        	double deltaX=0.0,deltaY=0.0; // difference between distorted and non-distorted as a fraction of this.distortionRadius
        	double xmmc=PXYmmc[0]/this.distortionRadius;
        	double ymmc=PXYmmc[1]/this.distortionRadius;
1019
        	double dDeltaX_dxmmc=0.0, dDeltaX_dymmc=0.0, dDeltaY_dxmmc=0.0, dDeltaY_dymmc=0;
1020 1021
        	if (calculateAll){
            	for (double [] r:partDeriv) Arrays.fill(r, 0.0); // will fill the [0] (value) too, should be done before calculation of these 2 values
1022
            	// Geometric - get to pinhole coordinates on the sensor
1023
                dXeYeZe=new double[9][3]; //[14];
1024
             // Geometric - get to pinhole coordinates on the sensor
1025 1026 1027
                dXeYeZe[0][0]=XeYeZe[0];
                dXeYeZe[0][1]=XeYeZe[1];
                dXeYeZe[0][2]=XeYeZe[2];
1028
    // /dphi
1029 1030 1031 1032 1033 1034 1035
                dXeYeZe[1][0]=(-cPS*sPH + sPS*sTH*cPH)*XYZ[0]                    +(-cPS*cPH-sPS*sTH*sPH)*XYZ[2];
                dXeYeZe[1][1]=(-sPS*sPH-cPS*sTH*cPH)  *XYZ[0]                    +(-sPS*cPH+cPS*sTH*sPH)*XYZ[2];
                dXeYeZe[1][2]=(cTH*cPH)*XYZ[0]                                     -cTH*sPH* XYZ[2];
    // /dtheta
                dXeYeZe[2][0]=(sPS*cTH*sPH)*XYZ[0]              -sPS*sTH*XYZ[1]  +(sPS*cTH*cPH)*XYZ[2];
                dXeYeZe[2][1]=(-cPS*cTH*sPH)*XYZ[0]             +cPS*sTH*XYZ[1]  +(-cPS*cTH*cPH)*XYZ[2];
                dXeYeZe[2][2]=(-sTH*sPH)*XYZ[0]                 -cTH*XYZ[1]      -sTH*cPH* XYZ[2];
1036
    // /dpsi
1037 1038 1039 1040 1041 1042 1043 1044 1045 1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056 1057 1058 1059 1060 1061 1062 1063 1064
                dXeYeZe[3][0]=(-sPS*cPH+cPS*sTH*sPH)*XYZ[0]     +cPS*cTH*XYZ[1]  +(sPS*sPH+cPS*sTH*cPH)*XYZ[2];
                dXeYeZe[3][1]=(cPS*cPH+ sPS*sTH*sPH)*XYZ[0]     +sPS*cTH*XYZ[1]  +(-cPS*sPH+sPS*sTH*cPH)*XYZ[2];
                dXeYeZe[3][2]=0.0;
    // /dX0
//              dXeYeZe[4][0]=-cPS*cPH+sPS*sTH*sPH; // bad?
                dXeYeZe[4][0]=-cPS*cPH-sPS*sTH*sPH; // bad?
                dXeYeZe[4][1]=-sPS*cPH+cPS*sTH*sPH;
                dXeYeZe[4][2]=-cTH*sPH;
    // /dY0
//                dXeYeZe[5][0]=-sPS*cTH;
//                dXeYeZe[5][1]= cPS*cTH;
//                dXeYeZe[5][2]= sTH;
                dXeYeZe[5][0]=+sPS*cTH;
                dXeYeZe[5][1]=-cPS*cTH;
                dXeYeZe[5][2]=-sTH;
    // /dZ0
//              dXeYeZe[6][0]= cPS*sPH+sPS*sTH*cPH; //bad?
                dXeYeZe[6][0]= cPS*sPH-sPS*sTH*cPH; //bad?
                dXeYeZe[6][1]= sPS*sPH+cPS*sTH*cPH;
                dXeYeZe[6][2]=-cTH*cPH;
    // /df
                dXeYeZe[7][0]=0.0;
                dXeYeZe[7][1]=0.0;
                dXeYeZe[7][2]=0.0;
    // /ddist
                dXeYeZe[8][0]=0.0;
                dXeYeZe[8][1]=0.0;
                dXeYeZe[8][2]=1.0;
1065 1066


1067
                dPXYmmc=new double[9][2]; //[14];
1068
             // TODO: move up
1069 1070 1071 1072
//              double [][] dPXYmmc=new double[9][2]; //[14];
              dPXYmmc[0][0]=PXYmmc[0]; // is it needed? probably not used
              dPXYmmc[0][1]=PXYmmc[1];
  //(4) PXmmc =f/Ze*  Xe  // mm, left from the lens axis intersection with the sensor
1073

1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107
  //dPXmmc/dphi   = f/Ze * dXe/dphi - f*Xe/Ze^2 * dZe/dphi
              dPXYmmc[1][0]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[1][0]-dXeYeZe[0][0]/dXeYeZe[0][2]*dXeYeZe[1][2]);
  //dPXmmc/dtheta = f/Ze * dXe/dtheta - f*Xe/Ze^2 * dZe/dtheta
              dPXYmmc[2][0]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[2][0]-dXeYeZe[0][0]/dXeYeZe[0][2]*dXeYeZe[2][2]);
  //dPXmmc/dpsi   = f/Ze * dXe/dpsi - f*Xe/Ze^2 * dZe/dpsi
              dPXYmmc[3][0]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[3][0]-dXeYeZe[0][0]/dXeYeZe[0][2]*dXeYeZe[3][2]);
  //dPXmmc/dX0    = f/Ze * dXe/dX0 - f*Xe/Ze^2 * dZe/dX0
              dPXYmmc[4][0]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[4][0]-dXeYeZe[0][0]/dXeYeZe[0][2]*dXeYeZe[4][2]);
  //dPXmmc/dY0    = f/Ze * dXe/dY0 - f*Xe/Ze^2 * dZe/dY0
              dPXYmmc[5][0]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[5][0]-dXeYeZe[0][0]/dXeYeZe[0][2]*dXeYeZe[5][2]);
  //dPXmmc/dZ0    = f/Ze * dXe/dZ0 - f*Xe/Ze^2 * dZe/dZ0
              dPXYmmc[6][0]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[6][0]-dXeYeZe[0][0]/dXeYeZe[0][2]*dXeYeZe[6][2]); //bad?
  //dPXmmc/df     = Xe/Ze
              dPXYmmc[7][0]=dXeYeZe[0][0]/dXeYeZe[0][2];
  //dPXmmc/ddist =  - f*Xe/Ze^2
              dPXYmmc[8][0]=-this.focalLength*dXeYeZe[0][0]/(dXeYeZe[0][2]*dXeYeZe[0][2]);

  //(5) PYmmc =f/Ze*  Ye  // mm, up from the lens axis intersection with the sensor
  //dPYmmc/dphi   = f/Ze * dYe/dphi - f*Ye/Ze^2 * dZe/dphi
              dPXYmmc[1][1]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[1][1]-dXeYeZe[0][1]/dXeYeZe[0][2]*dXeYeZe[1][2]);
  //dPYmmc/dtheta = f/Ze * dYe/dtheta - f*Ye/Ze^2 * dZe/dtheta
              dPXYmmc[2][1]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[2][1]-dXeYeZe[0][1]/dXeYeZe[0][2]*dXeYeZe[2][2]);
  //dPYmmc/dpsi   = f/Ze * dYe/dpsi - f*Ye/Ze^2 * dZe/dpsi
              dPXYmmc[3][1]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[3][1]-dXeYeZe[0][1]/dXeYeZe[0][2]*dXeYeZe[3][2]);
  //dPYmmc/dX0    = f/Ze * dYe/dX0 - f*Ye/Ze^2 * dZe/dX0
              dPXYmmc[4][1]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[4][1]-dXeYeZe[0][1]/dXeYeZe[0][2]*dXeYeZe[4][2]);
  //dPYmmc/dY0    = f/Ze * dYe/dY0 - f*Ye/Ze^2 * dZe/dY0
              dPXYmmc[5][1]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[5][1]-dXeYeZe[0][1]/dXeYeZe[0][2]*dXeYeZe[5][2]);
  //dPYmmc/dZ0    = f/Ze * dYe/dZ0 - f*Ye/Ze^2 * dZe/dZ0
              dPXYmmc[6][1]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[6][1]-dXeYeZe[0][1]/dXeYeZe[0][2]*dXeYeZe[6][2]); // good?
  //dPYmmc/df     = Ye/Ze
              dPXYmmc[7][1]=dXeYeZe[0][1]/dXeYeZe[0][2];
  //dPYmmc/ddist =  - f*Ye/Ze^2
              dPXYmmc[8][1]=-this.focalLength*dXeYeZe[0][1]/(dXeYeZe[0][2]*dXeYeZe[0][2]);
1108

1109 1110 1111 1112 1113 1114
        	}
        	// conversion coefficient from relative (to distortionRadius) to pixels
        	// negate for y!
        	double rel_to_pix=this.distortionRadius*1000.0/this.pixelSize;
        	//TODO:  seems that rr[i] can be just a single running variable, not an array
        	for (int i=0;i<r_xyod.length;i++){
1115

1116 1117 1118 1119 1120 1121 1122 1123 1124 1125 1126 1127 1128 1129 1130 1131 1132 1133 1134 1135 1136 1137 1138
        		double x=xmmc-r_xyod[i][0]; // relative X-shift of this term center
        		double y=ymmc-r_xyod[i][1]; // relative X-shift of this term center
        		double x2=x*x;   // relative squared X-shift from this term center
        		double y2=y*y;   // relative squared Y-shift from this term center
        		double r2=x2+y2; // relative squared distance from this term center
        		// effective distance from this term center corrected for elongation
        		double rr=Math.sqrt(r2+ r_xyod[i][2]*(y2-x2)+ 2.0*r_xyod[i][3]*x*y );
                if (rr<0.00000001*this.distortionRadius) rr=0.00000001*this.distortionRadius; // avoid 1/0.0 -// Added 3 zeros (old comment)

        		double rr_pow_i=ipow(rr,i);
//        		double ki=a[i]*(Math.pow(rr,i+1)-1.0);
//        		double ki=a[i]*(ipow(rr,i+1)-1.0);
        		double ki=a[i]*(rr_pow_i*rr-1.0); // scaling of distorted distance from this term's center (does not include pinhole center shift itself)
        		deltaX+=x*ki; // relative distorted distance from the center
        		deltaY+=y*ki;
//        		if ((debugLevel>2) && ((r_xyod[i][0]!=0.0) || (r_xyod[i][0]!=0.0))){
//        			System.out.println ("i="+i+" r_xyod[i][0]="+r_xyod[i][0]+" r_xyod[i][1]="+r_xyod[i][1]+" a[0]="+a[0]+" a[1]="+a[1]+" a[2]="+a[2]+" a[3]="+a[3]);
//        		}
        		if (calculateAll) {
        			double csi=rel_to_pix*a[i]*(i+1)*rr_pow_i;

//        		double dx2_dxmmc=2*x, dy2_dymmc=2*y, dr2_dxmmc=2*x, dr2_dymmc=2*y;
// double drr_dxmmc=0.5/rr*(dr2_dxmmc+ r_xyod[i][2]*(-dx2_dxmmc)+2*r_xyod[i][3]*y)=0.5/rr*(2*x - 2*x*r_xyod[i][2]+2*r_xyod[i][3]*y)=
1139
//                  =  (x*(1.0-r_xyod[i][2])+y*r_xyod[i][3])/rr
1140 1141
// double drr_dymmc=  (y*(1.0+r_xyod[i][2])+x*r_xyod[i][3])/rr
// double ki=a[i]*(rr^(i+1)-1.0);
1142 1143 1144
// double dki_dxmmc=a[i]*(i+1)*rr^i*(x*(1.0-r_xyod[i][2])+y*r_xyod[i][3])/rr
// double dki_dymmc=a[i]*(i+1)*rr^i*(y*(1.0+r_xyod[i][2])+x*r_xyod[i][3])/rr

1145 1146
// double dki_dxmmc=a[i]*(i+1)*rr_pow_i*(x*(1.0-r_xyod[i][2])+y*r_xyod[i][3])/rr
// double dki_dymmc=a[i]*(i+1)*rr_pow_i*(y*(1.0+r_xyod[i][2])+x*r_xyod[i][3])/rr
1147 1148

        			double ai_iplus1_rr_pow_i= a[i]*(i+1)*rr_pow_i;
1149 1150
        			double dki_dxmmc=          ai_iplus1_rr_pow_i*(x*(1.0-r_xyod[i][2])+y*r_xyod[i][3])/rr;
        			double dki_dymmc=          ai_iplus1_rr_pow_i*(y*(1.0+r_xyod[i][2])+x*r_xyod[i][3])/rr;
1151

1152 1153 1154 1155 1156 1157 1158 1159 1160 1161 1162 1163 1164 1165 1166 1167 1168 1169 1170 1171 1172
                    // the following 4 shifts are "extra", on top of non-distorted (pinhole) - pinhole should be added to the per-term sum
        			double dDeltaXi_dxmmc = ki+x*dki_dxmmc; // here dDelta*_d*mmc are relative (both I/O are fractions of distortionRadius)
        			double dDeltaXi_dymmc =    x*dki_dymmc;
        			double dDeltaYi_dxmmc =    y*dki_dxmmc;
        			double dDeltaYi_dymmc = ki+y*dki_dymmc;
        			dDeltaX_dxmmc += dDeltaXi_dxmmc; // here dDelta*_d*mmc are relative (both I/O are fractions of distortionRadius)
        			dDeltaX_dymmc += dDeltaXi_dymmc;
        			dDeltaY_dxmmc += dDeltaYi_dxmmc;
        			dDeltaY_dymmc += dDeltaYi_dymmc;
// dependence on eccentricity
// dDeltaX_d_r_xyod0 =  dDeltaXi_dxmmc * dxmmc_d_r_xyod0 = -dDeltaXi_dxmmc
// dDeltaY_d_r_xyod0 =  dDeltaYi_dxmmc * dxmmc_d_r_xyod0 = -dDeltaYi_dxmmc
// dDeltaX_d_r_xyod1 =  dDeltaXi_dymmc * dymmc_d_r_xyod1 = -dDeltaXi_dymmc
// dDeltaY_d_r_xyod1 =  dDeltaYi_dymmc * dymmc_d_r_xyod1 = -dDeltaYi_dymmc
        			int index=numRadialDerivatives-2+4*i;
        			if (i>0){ // eccentricity is not applicabe to the first (C) term
        				partDeriv[index  ][0]= -rel_to_pix*(dDeltaXi_dxmmc-0.0); // dPx_dr_xyod0
        				partDeriv[index  ][1]=  rel_to_pix* dDeltaYi_dxmmc; // dPy_dr_xyod0
        				partDeriv[index+1][0]= -rel_to_pix* dDeltaXi_dymmc; // dPx_dr_xyod1
        				partDeriv[index+1][1]=  rel_to_pix*(dDeltaYi_dymmc-0.0); // dPy_dr_xyod1
        			}
1173

1174 1175 1176 1177 1178 1179 1180 1181 1182
        			// d/dai
        			// ki=a[i]*(rr_pow_i*rr-1.0);
        			// double dki_dai=(rr_pow_i*rr-1.0);
        			// double dpx_dai= rel_to_pix*x*(dki_dai)=rel_to_pix*x*(rr_pow_i*rr-1.0)
        			// double dpy_dai=-rel_to_pix*y*(dki_dai)=rel_to_pix*y*(rr_pow_i*rr-1.0)
        			//        	final int distortionCIndex=15; // a8->9, a7->10, a6->11, a5->12, a->13, b->14, c->15
        			index= distortionCIndex-i; // reverse order, // a8->9, a7->10, a6->11, a5->12, a->13, b->14, c->15
    				partDeriv[index][0]= rel_to_pix*x*(rr_pow_i*rr-1.0); // OK
    				partDeriv[index][1]=-rel_to_pix*y*(rr_pow_i*rr-1.0); // OK
1183

1184 1185 1186 1187 1188 1189 1190 1191 1192 1193 1194 1195 1196 1197 1198 1199 1200 1201 1202 1203 1204 1205 1206 1207 1208 1209 1210 1211 1212 1213 1214 1215 1216
        			// d/dr_xyod[0] (x shift of the center
        			// rr=Math.sqrt(r2+ r_xyod[i][2]*(y2-x2)+ 2.0*r_xyod[i][3]*x*y );
        			// ki=a[i]*(rr_pow_i*rr-1.0);
        			// dx_dr_xyod0=-1.0
        			// dy_dr_xyod1=-1.0
        			// dx2_dr_xyod0=-2*x
        			// dy2_dr_xyod1=-2*y
        			// dr2_dr_xyod0=-2*x
        			// dr2_dr_xyod1=-2*y
        			// dxy_dr_xyod0=-y
        			// dxy_dr_xyod1=-x
        			// dri_dr_xyod[0]=0.5*(dr2_dr_xyod0 -r_xyod[i][2]*dx2_dr_xyod0 + 2.0*r_xyod[i][3]*dxy_dr_xyod0)/rr=
        			//                0.5*(-2*x -r_xyod[i][2]*(-2*x) + 2.0*r_xyod[i][3]*(-y))/rr=
        			//                (-x +r_xyod[i][2]*x -r_xyod[i][3]*y)/rr=
        			//                (x*(r_xyod[i][2]-1.0) -r_xyod[i][3]*y)/rr
//        			double dri_dr_xyod0 = (x*(r_xyod[i][2]-1.0) -r_xyod[i][3]*y)/rr;
        			// dri_dr_xyod[1]=0.5*(dr2_dr_xyod1 -r_xyod[i][2]*dx2_dr_xyod1 + 2.0*r_xyod[i][3]*dxy_dr_xyod1)/rr=
        			//                0.5*(-2*y +r_xyod[i][2]*(-2*y) + 2.0*r_xyod[i][3]*(-x))/rr=
        			//                (-y -r_xyod[i][2]*y -r_xyod[i][3]*x)/rr=
        			//                (-y*(r_xyod[i][2]+1.0) -r_xyod[i][3]*x)/rr
//        			double dri_dr_xyod1 = (-y*(r_xyod[i][2]+1.0) -r_xyod[i][3]*x)/rr;
        			// dri_dr_xyod[2]=0.5*(y2-x2)/rr
        			double dri_dr_xyod2 = 0.5*(y2-x2)/rr;
        			// dri_dr_xyod[3]=(y*x)/rr
        			double dri_dr_xyod3 = (y*x)/rr;
        			//--------------------
        			// double ki=a[i]*(rr^(i+1)-1.0);
        			// double ki=a[i]*(rr_pow_i*rr-1.0);
        			// deltaX+=x*ki;
        			// deltaY+=y*ki;
        			// dki_dri=a[i]*(i+1)*rr^i = a[i]*(i+1)*rr_pow_i
        			// dDeltaX_dri=x*a[i]*(i+1)*rr_pow_i
        			// dDeltaY_dri=y*a[i]*(i+1)*rr_pow_i
1217
        			// dPx_dri= rel_to_pix*x*a[i]*(i+1)*rr_pow_i =  csi*x
1218 1219 1220 1221 1222 1223 1224 1225 1226 1227 1228 1229 1230 1231 1232 1233 1234 1235 1236 1237 1238 1239
        			// dPy_dri=-rel_to_pix*y*a[i]*(i+1)*rr_pow_i = -csi*y
        			// dPx_dr_xyod0= dPx_dri*dri_dr_xyod0= csi*x*dri_dr_xyod0;
        			// dPx_dr_xyod1= dPx_dri*dri_dr_xyod1= csi*x*dri_dr_xyod1;
        			// dPx_dr_xyod2= dPx_dri*dri_dr_xyod2= csi*x*dri_dr_xyod2;
        			// dPx_dr_xyod3= dPx_dri*dri_dr_xyod3= csi*x*dri_dr_xyod3;
        			// dPy_dr_xyod0= dPy_dri*dri_dr_xyod0=-csi*y*dri_dr_xyod0;
        			// dPy_dr_xyod1= dPy_dri*dri_dr_xyod1=-csi*y*dri_dr_xyod1;
        			// dPy_dr_xyod2= dPy_dri*dri_dr_xyod2=-csi*y*dri_dr_xyod2;
        			// dPy_dr_xyod3= dPy_dri*dri_dr_xyod3=-csi*y*dri_dr_xyod3;
        			index=numRadialDerivatives-2+4*i;
        			/*
        			if (i>0){ // eccentricity is not applicabe to the first (C) term
        				partDeriv[index  ][0]= csi*x*dri_dr_xyod0; // dPx_dr_xyod0
        				partDeriv[index  ][1]=-csi*y*dri_dr_xyod0; // dPy_dr_xyod0
        				partDeriv[index+1][0]= csi*x*dri_dr_xyod1; // dPx_dr_xyod1
        				partDeriv[index+1][1]=-csi*y*dri_dr_xyod1; // dPy_dr_xyod1
        			}
        			*/
    				partDeriv[index+2][0]= csi*x*dri_dr_xyod2; // dPx_dr_xyod2
    				partDeriv[index+2][1]=-csi*y*dri_dr_xyod2; // dPy_dr_xyod2
    				partDeriv[index+3][0]= csi*x*dri_dr_xyod3; // dPx_dr_xyod3
    				partDeriv[index+3][1]=-csi*y*dri_dr_xyod3; // dPy_dr_xyod3
1240 1241

        		}
1242 1243 1244 1245
        	}

        	double [] xyDist={PXYmmc[0]+this.distortionRadius*deltaX,PXYmmc[1]+this.distortionRadius*deltaY};
        	// convert to sensor pixels coordinates
1246 1247
        	partDeriv[0][0]=  1000.0/this.pixelSize*xyDist[0] + this.px0;
        	partDeriv[0][1]= -1000.0/this.pixelSize*xyDist[1] + this.py0;
1248

1249
        	if (!calculateAll) { // TODO: how to deal with it when calculating Jacobian???
1250
        		// TODO: Looking away from the target, trying only with no dervatives. Do the same for derivatives too?
1251
            	if ((XeYeZe[2]<0.0) || ((xmmc*xmmc+ymmc*ymmc)>maxRelativeRadius*maxRelativeRadius)){ // non-distorted too far from the axis
1252 1253 1254 1255 1256
            		partDeriv[0][0]=Double.NaN;
            		partDeriv[0][1]=Double.NaN;
            	}
        		return partDeriv;
        	}
1257 1258 1259 1260 1261 1262 1263 1264 1265
        	// correct parameter derivatives to cumulative version
        	if (this.cummulativeCorrection){
            	for (int i=r_xyod.length-2;i>=0;i--){
        			int index=numRadialDerivatives-2+4*i;
        			if (i>0){ // eccentricity is not applicabe to the first (C) term
        				partDeriv[index  ][0]+=partDeriv[index+4][0]; // oob=36
        				partDeriv[index  ][1]+=partDeriv[index+4][1];
        				partDeriv[index+1][0]+=partDeriv[index+5][0];
        				partDeriv[index+1][1]+=partDeriv[index+5][1];
1266
        			}
1267 1268 1269 1270 1271 1272
    				partDeriv[index+2][0]+=partDeriv[index+6][0];
    				partDeriv[index+2][1]+=partDeriv[index+6][1];
    				partDeriv[index+3][0]+=partDeriv[index+7][0];
    				partDeriv[index+3][1]+=partDeriv[index+7][1];
            	}
        	}
1273

1274
// convert    dDelta*_d*mmc from relative/relative to pix/mm (invert pixel Y direction)
1275
// added 1.0 to account for non-distorted (pinhole) shift
1276 1277 1278 1279 1280 1281 1282 1283 1284 1285
        	double dPx_dPinholeX= (1.0+dDeltaX_dxmmc)*1000.0/this.pixelSize;
        	double dPx_dPinholeY=       dDeltaX_dymmc*1000.0/this.pixelSize;
        	double dPy_dPinholeX=      -dDeltaY_dxmmc*1000.0/this.pixelSize;
        	double dPy_dPinholeY=-(1.0+dDeltaY_dymmc)*1000.0/this.pixelSize;
        	if (this.debugLevel>2){
        		System.out.println(" deltaX="+deltaX+" deltaY="+deltaY+" xyDist[0]="+xyDist[0]+" xyDist[1]="+xyDist[1]);
        		System.out.println(" PXYmmc[0]="+PXYmmc[0]+" PXYmmc[1]="+PXYmmc[1]+" xmmc="+xmmc+" ymmc"+ymmc);
        		System.out.println(" dDeltaX_dxmmc="+dDeltaX_dxmmc+" dDeltaX_dymmc="+dDeltaX_dymmc+" dDeltaY_dxmmc="+dDeltaY_dxmmc+" dDeltaY_dymmc"+dDeltaY_dymmc);
        		System.out.println(" dPx_dPinholeX="+dPx_dPinholeX+" dPx_dPinholeY="+dPx_dPinholeY+" dPy_dPinholeX="+dPy_dPinholeX+" dPy_dPinholeY"+dPy_dPinholeY);
        	}
1286 1287

            double K=Math.PI/180; // multiply all derivatives my angles
1288
// dPX/dphi   =  1000/Psz* dxDist/dphi
1289 1290
        	partDeriv[ 1][0]=  K*(dPx_dPinholeX*dPXYmmc[1][0]+dPx_dPinholeY*dPXYmmc[1][1]);
        	partDeriv[ 1][1]=  K*(dPy_dPinholeX*dPXYmmc[1][0]+dPy_dPinholeY*dPXYmmc[1][1]);
1291

1292
// dPX/dtheta =  1000/Psz* dxDist/dtheta
1293 1294
        	partDeriv[ 2][0]=  K*(dPx_dPinholeX*dPXYmmc[2][0]+dPx_dPinholeY*dPXYmmc[2][1]);
        	partDeriv[ 2][1]=  K*(dPy_dPinholeX*dPXYmmc[2][0]+dPy_dPinholeY*dPXYmmc[2][1]);
1295
// dPX/dpsi   =  1000/Psz* dxDist/dpsi
1296 1297
        	partDeriv[ 3][0]=  K*(dPx_dPinholeX*dPXYmmc[3][0]+dPx_dPinholeY*dPXYmmc[3][1]);
        	partDeriv[ 3][1]=  K*(dPy_dPinholeX*dPXYmmc[3][0]+dPy_dPinholeY*dPXYmmc[3][1]);
1298
// dPX/dX0    =  1000/Psz* dxDist/dX0
1299 1300
        	partDeriv[ 4][0]=  dPx_dPinholeX*dPXYmmc[4][0]+dPx_dPinholeY*dPXYmmc[4][1];
        	partDeriv[ 4][1]=  dPy_dPinholeX*dPXYmmc[4][0]+dPy_dPinholeY*dPXYmmc[4][1];
1301
// dPX/dY0    =  1000/Psz* dxDist/dY0
1302 1303
        	partDeriv[ 5][0]=  dPx_dPinholeX*dPXYmmc[5][0]+dPx_dPinholeY*dPXYmmc[5][1];
        	partDeriv[ 5][1]=  dPy_dPinholeX*dPXYmmc[5][0]+dPy_dPinholeY*dPXYmmc[5][1];
1304
// dPX/dZ0    =  1000/Psz* dxDist/dZ0
1305 1306
        	partDeriv[ 6][0]=  dPx_dPinholeX*dPXYmmc[6][0]+dPx_dPinholeY*dPXYmmc[6][1];
        	partDeriv[ 6][1]=  dPy_dPinholeX*dPXYmmc[6][0]+dPy_dPinholeY*dPXYmmc[6][1];
1307
// dPX/df     =  1000/Psz* dxDist/df
1308 1309
        	partDeriv[ 7][0]=  dPx_dPinholeX*dPXYmmc[7][0]+dPx_dPinholeY*dPXYmmc[7][1];
        	partDeriv[ 7][1]=  dPy_dPinholeX*dPXYmmc[7][0]+dPy_dPinholeY*dPXYmmc[7][1];
1310
// dPX/ddist  =  1000/Psz* dxDist/ddist
1311 1312
        	partDeriv[ 8][0]=  dPx_dPinholeX*dPXYmmc[8][0]+dPx_dPinholeY*dPXYmmc[8][1];
        	partDeriv[ 8][1]=  dPy_dPinholeX*dPXYmmc[8][0]+dPy_dPinholeY*dPXYmmc[8][1];
1313

1314 1315 1316 1317 1318 1319 1320 1321 1322 1323
// dPX/dPX0   =  1
// dPY/dPX0   =  0
        	partDeriv[16][0]=  1.0;
        	partDeriv[16][1]=  0.0;
// dPX/dPY0   =  0
// dPY/dPY0   =  1
        	partDeriv[17][0]=  0.0;
        	partDeriv[17][1]=  1.0;
            return partDeriv;
        }
1324

1325
        public double [][] calcPartialDerivatives_type2(
1326 1327 1328
        		double xp, // target point horizontal, positive - right,  mm
        		double yp, // target point vertical,   positive - down,  mm
        		double zp, // target point horizontal, positive - away from camera,  mm
1329
        		double maxRelativeRadius,
1330 1331
        		boolean calculateAll){ // calculate derivatives, false - values only
//        	this.cummulativeCorrection=false; // just debugging
1332

1333 1334 1335 1336 1337 1338 1339 1340 1341 1342 1343 1344 1345 1346 1347 1348 1349
        	// TODO - add reduced calculations for less terms?
//        	final int numDerivatives=44; // 18+6*2+7*2; // 18  for radial and 26 more for non-radial
        	final int numRadialDerivatives=18;
        	final int numNonRadialDerivatives=26;
        	final int distortionCIndex=15; // a8->9, a7->10, a6->11, a5->12, a->13, b->14, c->15
        	double partDeriv[][] = new double [calculateAll?(numRadialDerivatives+numNonRadialDerivatives):1][2];
//        	double [] XYZ= {xp-this.x0, yp-this.y0, zp-this.z0};
        	double [] XYZ= {xp-this.x0, yp+this.y0, zp-this.z0}; // Y - "down" (as in images), not up
        	double [] XeYeZe={
        			this.rotMatrix[0][0]*XYZ[0] + this.rotMatrix[0][1]*XYZ[1] + this.rotMatrix[0][2]*XYZ[2],
        			this.rotMatrix[1][0]*XYZ[0] + this.rotMatrix[1][1]*XYZ[1] + this.rotMatrix[1][2]*XYZ[2],
        			this.rotMatrix[2][0]*XYZ[0] + this.rotMatrix[2][1]*XYZ[1] + this.rotMatrix[2][2]*XYZ[2]+this.distance
        	};
        	// PXYmmc - pinhole (non-distorted) projection coordinates. metric (in mm)
        	double [] PXYmmc={this.focalLength/XeYeZe[2]*XeYeZe[0],this.focalLength/XeYeZe[2]*XeYeZe[1]};
        	// now each term has individual radius
//        	double [] rr=new double [r_xyod.length];
1350
        	// Geometric - get to pinhole coordinates on the sensor
1351 1352 1353
            double [][] dXeYeZe=null; //[14];
            double [][] dPXYmmc=null;

Andrey Filippov's avatar
Andrey Filippov committed
1354
/*
1355 1356 1357 1358 1359 1360 1361
   		 Modifying to accommodate for eccentricity of different terms (2 parameters per term) and elliptical shape (another 2 terms). When all are
   		 zeroes, the old parameters are in effect:
   				Rdist/R=A8*R^7+A7*R^6+A6*R^5+A5*R^4+A*R^3+B*R^2+C*R+(1-A6-A7-A6-A5-A-B-C)");
   				Rdist/R=A8*R6^7+A7*R5^6+A6*R4^5+A5*R3^4+A*R2^3+B*R1^2+C*R0+(1-A6-A7-A6-A5-A-B-C)");
   				R[i] depends on px,py and r_xy[i][], r_o[i] (positive - "landscape", negative - "portrait"), r_d (positive - along y=x, negative - along y=-x)
   				R[i] = r0[i]*(1+  (r_od[i][0]*(y[i]**2-x[i]**2)+ 2*r_od[i][1]*x[i]*y[i])/r0[i]**2;
   				r0[i]=sqrt(x[i]**2+y[2]**2)
1362
   				x[i]=pixel_x-px0-((i>0)?r_xy[i-1][0]:0)
1363
   				y[i]=pixel_y-py0-((i>0)?r_xy[i-1][1]:0)
1364
   		*/
1365 1366 1367 1368
        	double [] a={this.distortionC,this.distortionB,this.distortionA,this.distortionA5,this.distortionA6,this.distortionA7,this.distortionA8};
        	double deltaX=0.0,deltaY=0.0; // difference between distorted and non-distorted as a fraction of this.distortionRadius
        	double x=PXYmmc[0]/this.distortionRadius; // was xmmc
        	double y=PXYmmc[1]/this.distortionRadius; // was ymmc
1369
        	double dDeltaX_dx=0.0, dDeltaX_dy=0.0, dDeltaY_dx=0.0, dDeltaY_dy=0;
1370 1371
        	if (calculateAll){
            	for (double [] r:partDeriv) Arrays.fill(r, 0.0); // will fill the [0] (value) too, should be done before calculation of these 2 values
1372
            	// Geometric - get to pinhole coordinates on the sensor
1373
                dXeYeZe=new double[9][3]; //[14];
1374
             // Geometric - get to pinhole coordinates on the sensor
1375 1376 1377
                dXeYeZe[0][0]=XeYeZe[0];
                dXeYeZe[0][1]=XeYeZe[1];
                dXeYeZe[0][2]=XeYeZe[2];
1378
    // /dphi
1379 1380 1381 1382 1383 1384 1385
                dXeYeZe[1][0]=(-cPS*sPH + sPS*sTH*cPH)*XYZ[0]                    +(-cPS*cPH-sPS*sTH*sPH)*XYZ[2];
                dXeYeZe[1][1]=(-sPS*sPH-cPS*sTH*cPH)  *XYZ[0]                    +(-sPS*cPH+cPS*sTH*sPH)*XYZ[2];
                dXeYeZe[1][2]=(cTH*cPH)*XYZ[0]                                     -cTH*sPH* XYZ[2];
    // /dtheta
                dXeYeZe[2][0]=(sPS*cTH*sPH)*XYZ[0]              -sPS*sTH*XYZ[1]  +(sPS*cTH*cPH)*XYZ[2];
                dXeYeZe[2][1]=(-cPS*cTH*sPH)*XYZ[0]             +cPS*sTH*XYZ[1]  +(-cPS*cTH*cPH)*XYZ[2];
                dXeYeZe[2][2]=(-sTH*sPH)*XYZ[0]                 -cTH*XYZ[1]      -sTH*cPH* XYZ[2];
1386
    // /dpsi
1387 1388 1389 1390 1391 1392 1393 1394 1395 1396 1397 1398 1399 1400 1401 1402 1403 1404 1405 1406 1407 1408 1409 1410 1411 1412 1413 1414
                dXeYeZe[3][0]=(-sPS*cPH+cPS*sTH*sPH)*XYZ[0]     +cPS*cTH*XYZ[1]  +(sPS*sPH+cPS*sTH*cPH)*XYZ[2];
                dXeYeZe[3][1]=(cPS*cPH+ sPS*sTH*sPH)*XYZ[0]     +sPS*cTH*XYZ[1]  +(-cPS*sPH+sPS*sTH*cPH)*XYZ[2];
                dXeYeZe[3][2]=0.0;
    // /dX0
//              dXeYeZe[4][0]=-cPS*cPH+sPS*sTH*sPH; // bad?
                dXeYeZe[4][0]=-cPS*cPH-sPS*sTH*sPH; // bad?
                dXeYeZe[4][1]=-sPS*cPH+cPS*sTH*sPH;
                dXeYeZe[4][2]=-cTH*sPH;
    // /dY0
//                dXeYeZe[5][0]=-sPS*cTH;
//                dXeYeZe[5][1]= cPS*cTH;
//                dXeYeZe[5][2]= sTH;
                dXeYeZe[5][0]=+sPS*cTH;
                dXeYeZe[5][1]=-cPS*cTH;
                dXeYeZe[5][2]=-sTH;
    // /dZ0
//              dXeYeZe[6][0]= cPS*sPH+sPS*sTH*cPH; //bad?
                dXeYeZe[6][0]= cPS*sPH-sPS*sTH*cPH; //bad?
                dXeYeZe[6][1]= sPS*sPH+cPS*sTH*cPH;
                dXeYeZe[6][2]=-cTH*cPH;
    // /df
                dXeYeZe[7][0]=0.0;
                dXeYeZe[7][1]=0.0;
                dXeYeZe[7][2]=0.0;
    // /ddist
                dXeYeZe[8][0]=0.0;
                dXeYeZe[8][1]=0.0;
                dXeYeZe[8][2]=1.0;
1415 1416


1417
                dPXYmmc=new double[9][2]; //[14];
1418
             // TODO: move up
1419 1420 1421 1422
//              double [][] dPXYmmc=new double[9][2]; //[14];
              dPXYmmc[0][0]=PXYmmc[0]; // is it needed? probably not used
              dPXYmmc[0][1]=PXYmmc[1];
  //(4) PXmmc =f/Ze*  Xe  // mm, left from the lens axis intersection with the sensor
1423

1424 1425 1426 1427 1428 1429 1430 1431 1432 1433 1434 1435 1436 1437 1438 1439 1440 1441 1442 1443 1444 1445 1446 1447 1448 1449 1450 1451 1452 1453 1454 1455 1456 1457
  //dPXmmc/dphi   = f/Ze * dXe/dphi - f*Xe/Ze^2 * dZe/dphi
              dPXYmmc[1][0]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[1][0]-dXeYeZe[0][0]/dXeYeZe[0][2]*dXeYeZe[1][2]);
  //dPXmmc/dtheta = f/Ze * dXe/dtheta - f*Xe/Ze^2 * dZe/dtheta
              dPXYmmc[2][0]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[2][0]-dXeYeZe[0][0]/dXeYeZe[0][2]*dXeYeZe[2][2]);
  //dPXmmc/dpsi   = f/Ze * dXe/dpsi - f*Xe/Ze^2 * dZe/dpsi
              dPXYmmc[3][0]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[3][0]-dXeYeZe[0][0]/dXeYeZe[0][2]*dXeYeZe[3][2]);
  //dPXmmc/dX0    = f/Ze * dXe/dX0 - f*Xe/Ze^2 * dZe/dX0
              dPXYmmc[4][0]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[4][0]-dXeYeZe[0][0]/dXeYeZe[0][2]*dXeYeZe[4][2]);
  //dPXmmc/dY0    = f/Ze * dXe/dY0 - f*Xe/Ze^2 * dZe/dY0
              dPXYmmc[5][0]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[5][0]-dXeYeZe[0][0]/dXeYeZe[0][2]*dXeYeZe[5][2]);
  //dPXmmc/dZ0    = f/Ze * dXe/dZ0 - f*Xe/Ze^2 * dZe/dZ0
              dPXYmmc[6][0]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[6][0]-dXeYeZe[0][0]/dXeYeZe[0][2]*dXeYeZe[6][2]); //bad?
  //dPXmmc/df     = Xe/Ze
              dPXYmmc[7][0]=dXeYeZe[0][0]/dXeYeZe[0][2];
  //dPXmmc/ddist =  - f*Xe/Ze^2
              dPXYmmc[8][0]=-this.focalLength*dXeYeZe[0][0]/(dXeYeZe[0][2]*dXeYeZe[0][2]);

  //(5) PYmmc =f/Ze*  Ye  // mm, up from the lens axis intersection with the sensor
  //dPYmmc/dphi   = f/Ze * dYe/dphi - f*Ye/Ze^2 * dZe/dphi
              dPXYmmc[1][1]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[1][1]-dXeYeZe[0][1]/dXeYeZe[0][2]*dXeYeZe[1][2]);
  //dPYmmc/dtheta = f/Ze * dYe/dtheta - f*Ye/Ze^2 * dZe/dtheta
              dPXYmmc[2][1]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[2][1]-dXeYeZe[0][1]/dXeYeZe[0][2]*dXeYeZe[2][2]);
  //dPYmmc/dpsi   = f/Ze * dYe/dpsi - f*Ye/Ze^2 * dZe/dpsi
              dPXYmmc[3][1]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[3][1]-dXeYeZe[0][1]/dXeYeZe[0][2]*dXeYeZe[3][2]);
  //dPYmmc/dX0    = f/Ze * dYe/dX0 - f*Ye/Ze^2 * dZe/dX0
              dPXYmmc[4][1]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[4][1]-dXeYeZe[0][1]/dXeYeZe[0][2]*dXeYeZe[4][2]);
  //dPYmmc/dY0    = f/Ze * dYe/dY0 - f*Ye/Ze^2 * dZe/dY0
              dPXYmmc[5][1]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[5][1]-dXeYeZe[0][1]/dXeYeZe[0][2]*dXeYeZe[5][2]);
  //dPYmmc/dZ0    = f/Ze * dYe/dZ0 - f*Ye/Ze^2 * dZe/dZ0
              dPXYmmc[6][1]=this.focalLength/dXeYeZe[0][2]*(dXeYeZe[6][1]-dXeYeZe[0][1]/dXeYeZe[0][2]*dXeYeZe[6][2]); // good?
  //dPYmmc/df     = Ye/Ze
              dPXYmmc[7][1]=dXeYeZe[0][1]/dXeYeZe[0][2];
  //dPYmmc/ddist =  - f*Ye/Ze^2
              dPXYmmc[8][1]=-this.focalLength*dXeYeZe[0][1]/(dXeYeZe[0][2]*dXeYeZe[0][2]);
1458

1459 1460 1461 1462 1463 1464 1465 1466 1467 1468 1469 1470 1471 1472 1473 1474 1475 1476 1477 1478 1479 1480 1481 1482 1483 1484 1485 1486 1487 1488 1489 1490 1491 1492 1493 1494
        	}
        	// conversion coefficient from relative (to distortionRadius) to pixels
        	// negate for y!
        	double rel_to_pix=this.distortionRadius*1000.0/this.pixelSize;
    		double x2=x*x;   // relative squared X-shift from this term center
    		double y2=y*y;   // relative squared Y-shift from this term center
    		double r2=x2+y2; // relative squared distance from this term center
    		double xy=x*y;
    		double rr=Math.sqrt(r2);
        	//TODO:  seems that rr[i] can be just a single running variable, not an array
        	for (int i=0;i<r_xyod.length;i++){
        		// effective distance from this term center corrected for elongation
//        		double rr=Math.sqrt(r2+ r_xyod[i][2]*(y2-x2)+ 2.0*r_xyod[i][3]*x*y );
                if (rr<0.00000001*this.distortionRadius) rr=0.00000001*this.distortionRadius; // avoid 1/0.0 -// Added 3 zeros (old comment)
        		double rr_pow_i_minus_1,rr_pow_i,rr_pow_i_plus_1;
        		if (i==0){
        			rr_pow_i_minus_1=1.0/rr;
        			rr_pow_i=1.0;
        		}else{
        			rr_pow_i_minus_1=ipow(rr,i-1);
        			rr_pow_i=rr_pow_i_minus_1*rr;
        		}
        		rr_pow_i_plus_1=rr_pow_i*rr;
        		double ki1= r_xyod[i][0]*x+r_xyod[i][1]*y;
        		double ki2= r_xyod[i][2]*(y2-x2)+2.0*r_xyod[i][3]*xy;
        		double ki=a[i]*(rr_pow_i_plus_1-1.0)+ki1*rr_pow_i+ki2*rr_pow_i_minus_1; // scaling of distorted distance from this term's center (does not include pinhole center shift itself)
//        		double ki=a[i]*(rr_pow_i*rr-1.0); // scaling of distorted distance from this term's center (does not include pinhole center shift itself)
        		deltaX+=x*ki; // relative distorted distance from the center
        		deltaY+=y*ki;
//        		if ((debugLevel>2) && ((r_xyod[i][0]!=0.0) || (r_xyod[i][0]!=0.0))){
//        			System.out.println ("i="+i+" r_xyod[i][0]="+r_xyod[i][0]+" r_xyod[i][1]="+r_xyod[i][1]+" a[0]="+a[0]+" a[1]="+a[1]+" a[2]="+a[2]+" a[3]="+a[3]);
//        		}
        		if (calculateAll) {
//        			double csi=rel_to_pix*a[i]*(i+1)*rr_pow_i;
// drr_dx=x/rr, drr_dy=y/r
//                  ki=a[i]*(rr^(i+1-1.0) +ki1*rr^i+ki2*rr^(i-1)
Andrey Filippov's avatar
Andrey Filippov committed
1495 1496 1497 1498 1499 1500 1501 1502
//        			dki_dx=a[i]*(i+1)*(rr^i)*x/rr+ (dki1_dx*rr^i+ ki1*i* rr^(i-1)*x/r) + (dki2_dx+           ki2*(i-1)*rr^(i-2)*x/r)= // (WAS)
//        			dki_dx=a[i]*(i+1)*(rr^i)+      (dki1_dx*rr^i+ ki1*i* rr^(i-1)*x/r) + (dki2_dx*rr^(i-1) + ki2*(i-1)*rr^(i-2)*x/r)= // (CORRECTED)
//        			(a[i]*(i+1)*(rr^i)/rr)*x +                       // OK
//        			(axi*r^(i-1) + (axi*x+ayi*y)/r*i)) * x  +        // (WAS)
//        			(axi*r^i     + (axi*x+ayi*y)*i*rr^(i-2) * x) +   // (CORRECTED)
//        			2*(adi*y - aoi*x)*rr^(i-1)+                      // OK
//        			(aoi*(y^2-x^2)+2*adi*x*y)*(i-1)/r^3 * x          // (WAS)
//        			(aoi*(y^2-x^2)+2*adi*x*y)*(i-1)*r^(i-3) * x      // (CORRECTED)
1503
//        			dki_dy=
Andrey Filippov's avatar
Andrey Filippov committed
1504 1505 1506 1507 1508 1509
//        			(a[i]*(i+1)*(rr^i)/rr)*y +                       // OK
//        			(ayi*r^(i-1)*y + (axi*x+ayi*y)/r*i)) * y +       // (WAS)
//        			(ayi*r^i       + (axi*x+ayi*y)*i*rr^(i-2) * x) + // (CORRECTED)
//        			2*(adi*x + aoi*y)*rr^(i-1)+                      // OK
//        			(aoi*(y^2-x^2)+2*adi*x*y)*(i-1)/r^3 * y          // (WAS)
//        			(aoi*(y^2-x^2)+2*adi*x*y)*(i-1)*r^(i-3) * x      // (CORRECTED)
1510
// Below may speed up by branching for i==0, i==1 - starting with commonn code
Andrey Filippov's avatar
Andrey Filippov committed
1511 1512 1513
        			double ai_iplus1_rr_pow_i_minus_1= a[i]*(i+1)*rr_pow_i_minus_1;
        			double rr_pow_i_minus_2=rr_pow_i_minus_1/rr;
//        			double ki1_div_r_mul_i=i*ki1/rr;
1514
        			double ki1_r_pow_im2_mul_i=i*ki1*rr_pow_i_minus_2;       // (axi*x+ayi*y)*i*rr^(i-2)
Andrey Filippov's avatar
Andrey Filippov committed
1515 1516 1517 1518 1519
//        			double ki2_div_r3_mul_im1=(i-1)*ki2/(rr*rr*rr);
        			double ki2_r_pow_im3_mul_im1=(i-1)*ki2*rr_pow_i_minus_2/rr; // (aoi*(y^2-x^2)+2*adi*x*y)* (i-1)*r^(i-3)
//        			double dki_dx= x*(ai_iplus1_rr_pow_i_minus_1 +
//        					r_xyod[i][0]*rr_pow_i_minus_1 + ki1_div_r_mul_i+ki2_div_r3_mul_im1)+
//        					2*(r_xyod[i][3]*y - r_xyod[i][2]*x)/rr_pow_i_minus_1;
1520
        			double dki_dx= x*(ai_iplus1_rr_pow_i_minus_1 +
1521
        					ki1_r_pow_im2_mul_i + ki2_r_pow_im3_mul_im1)+ // r_xyod[i][0]*rr_pow_i_minus_1 +
1522
        					r_xyod[i][0]*rr_pow_i + 2*(r_xyod[i][3]*y - r_xyod[i][2]*x)*rr_pow_i_minus_1;
Andrey Filippov's avatar
Andrey Filippov committed
1523 1524 1525
//        			double dki_dy= y*(ai_iplus1_rr_pow_i_minus_1 +
//        					r_xyod[i][1]*rr_pow_i_minus_1 + ki1_div_r_mul_i + ki2_div_r3_mul_im1)+
//        					2*(r_xyod[i][3]*x + r_xyod[i][2]*y)/rr_pow_i_minus_1;
1526
        			double dki_dy= y*(ai_iplus1_rr_pow_i_minus_1 +
Andrey Filippov's avatar
Andrey Filippov committed
1527
        					ki1_r_pow_im2_mul_i + ki2_r_pow_im3_mul_im1)+ // r_xyod[i][1]*rr_pow_i_minus_1 +
1528
        					r_xyod[i][1]*rr_pow_i + 2*(r_xyod[i][3]*x + r_xyod[i][2]*y)*rr_pow_i_minus_1;
1529
//        			double ai_iplus1_rr_pow_i= a[i]*(i+1)*rr_pow_i;
1530 1531 1532 1533 1534 1535 1536 1537 1538 1539 1540
//        			double dki_dxmmc=          ai_iplus1_rr_pow_i*(x*(1.0-r_xyod[i][2])+y*r_xyod[i][3])/rr;
//        			double dki_dymmc=          ai_iplus1_rr_pow_i*(y*(1.0+r_xyod[i][2])+x*r_xyod[i][3])/rr;
                    // the following 4 shifts are "extra", on top of non-distorted (pinhole) - pinhole should be added to the per-term sum
        			double dDeltaXi_dx = ki+x*dki_dx; // mmc; // here dDelta*_d*mmc are relative (both I/O are fractions of distortionRadius)
        			double dDeltaXi_dy =    x*dki_dy; // mmc;
        			double dDeltaYi_dx =    y*dki_dx; // mmc;
        			double dDeltaYi_dy = ki+y*dki_dy; // mmc;
        			dDeltaX_dx += dDeltaXi_dx; // here dDelta*_d*mmc are relative (both I/O are fractions of distortionRadius)
        			dDeltaX_dy += dDeltaXi_dy;
        			dDeltaY_dx += dDeltaYi_dx;
        			dDeltaY_dy += dDeltaYi_dy;
1541
//dpx_dai, dpi_dai - same as before
1542 1543 1544 1545 1546 1547 1548 1549 1550 1551 1552 1553 1554 1555 1556 1557 1558 1559 1560 1561 1562 1563 1564
        			int index= distortionCIndex-i; // reverse order, // a8->9, a7->10, a6->11, a5->12, a->13, b->14, c->15
    				partDeriv[index][0]= rel_to_pix*x*(rr_pow_i_plus_1-1.0); // OK
    				partDeriv[index][1]=-rel_to_pix*y*(rr_pow_i_plus_1-1.0); // OK
// dpx_daxi,dpy_daxi,dpx_dayi,dpy_dayi
// dpx_daxi= x^2*r^i;
// dpy_daxi=-x*y*r^i;
// dpx_dayi= x*y*r^i;
// dpy_dayi=-y^2*r^i;
        			index=numRadialDerivatives-2+4*i;
        			double rel_to_pix_mul_rr_pow_i=rel_to_pix*rr_pow_i;
        			if (i>0){ // eccentricity is not applicabe to the first (C) term
        				partDeriv[index  ][0]=  rel_to_pix_mul_rr_pow_i*x2;
        				partDeriv[index  ][1]= -rel_to_pix_mul_rr_pow_i*xy;
        				partDeriv[index+1][0]=  rel_to_pix_mul_rr_pow_i*xy;
        				partDeriv[index+1][1]= -rel_to_pix_mul_rr_pow_i*y2;
        			}
// dpx_daoi,dpy_daoi,dpx_dadi,dpy_dadi
// dpx_daoi= x*(y^2-x^2)*r^(i-1);
// dpY_daoi=-y*(y^2-x^2)*r^(i-1);
// dpx_daDi= 2*x*xy*r^(i-1);
// dpy_daDi=-2*y*xy*r^(i-1);
    				double rel_to_pix_mul_y2_minus_x2_mul_rr_pow_i_minus_1=rel_to_pix* (y2-x2)*rr_pow_i_minus_1;
    				double rel_to_pix_mul_2xy_mul_rr_pow_i_minus_1=rel_to_pix*2.0*xy*rr_pow_i_minus_1;
1565

1566 1567 1568 1569 1570
        			index=numRadialDerivatives-2+4*i;
    				partDeriv[index+2][0]= x*rel_to_pix_mul_y2_minus_x2_mul_rr_pow_i_minus_1;
    				partDeriv[index+2][1]=-y*rel_to_pix_mul_y2_minus_x2_mul_rr_pow_i_minus_1;
    				partDeriv[index+3][0]= x*rel_to_pix_mul_2xy_mul_rr_pow_i_minus_1;
    				partDeriv[index+3][1]=-y*rel_to_pix_mul_2xy_mul_rr_pow_i_minus_1;
1571
        		}
1572 1573 1574 1575 1576 1577
        	}

        	double [] xyDist={PXYmmc[0]+this.distortionRadius*deltaX,PXYmmc[1]+this.distortionRadius*deltaY};
        	// convert to sensor pixels coordinates
        	partDeriv[0][0]=  1000.0/this.pixelSize*xyDist[0] + this.px0;
        	partDeriv[0][1]= -1000.0/this.pixelSize*xyDist[1] + this.py0;
1578

1579 1580 1581 1582 1583 1584 1585 1586 1587 1588 1589 1590 1591 1592 1593 1594 1595
        	if (!calculateAll) { // TODO: how to deal with it when calculating Jacobian???
        		// TODO: Looking away from the target, trying only with no dervatives. Do the same for derivatives too?
            	if ((XeYeZe[2]<0.0) || (r2>maxRelativeRadius*maxRelativeRadius)){ // non-distorted too far from the axis
            		partDeriv[0][0]=Double.NaN;
            		partDeriv[0][1]=Double.NaN;
            	}
        		return partDeriv;
        	}
        	// correct parameter derivatives to cumulative version
        	if (this.cummulativeCorrection){
            	for (int i=r_xyod.length-2;i>=0;i--){
        			int index=numRadialDerivatives-2+4*i;
        			if (i>0){ // eccentricity is not applicabe to the first (C) term
        				partDeriv[index  ][0]+=partDeriv[index+4][0]; // oob=36
        				partDeriv[index  ][1]+=partDeriv[index+4][1];
        				partDeriv[index+1][0]+=partDeriv[index+5][0];
        				partDeriv[index+1][1]+=partDeriv[index+5][1];
1596
        			}
1597 1598 1599 1600 1601 1602
    				partDeriv[index+2][0]+=partDeriv[index+6][0];
    				partDeriv[index+2][1]+=partDeriv[index+6][1];
    				partDeriv[index+3][0]+=partDeriv[index+7][0];
    				partDeriv[index+3][1]+=partDeriv[index+7][1];
            	}
        	}
1603

1604
// convert    dDelta*_d* from relative/relative to pix/mm (invert pixel Y direction)
1605
// added 1.0 to account for non-distorted (pinhole) shift
1606 1607 1608 1609 1610 1611 1612 1613 1614 1615
        	double dPx_dPinholeX= (1.0+dDeltaX_dx)*1000.0/this.pixelSize;
        	double dPx_dPinholeY=       dDeltaX_dy*1000.0/this.pixelSize;
        	double dPy_dPinholeX=      -dDeltaY_dx*1000.0/this.pixelSize;
        	double dPy_dPinholeY=-(1.0+dDeltaY_dy)*1000.0/this.pixelSize;
        	if (this.debugLevel>2){
        		System.out.println(" deltaX="+deltaX+" deltaY="+deltaY+" xyDist[0]="+xyDist[0]+" xyDist[1]="+xyDist[1]);
        		System.out.println(" PXYmmc[0]="+PXYmmc[0]+" PXYmmc[1]="+PXYmmc[1]+" x="+x+" y"+y);
        		System.out.println(" dDeltaX_dxmmc="+dDeltaX_dx+" dDeltaX_dymmc="+dDeltaX_dy+" dDeltaY_dxmmc="+dDeltaY_dx+" dDeltaY_dymmc"+dDeltaY_dy);
        		System.out.println(" dPx_dPinholeX="+dPx_dPinholeX+" dPx_dPinholeY="+dPx_dPinholeY+" dPy_dPinholeX="+dPy_dPinholeX+" dPy_dPinholeY"+dPy_dPinholeY);
        	}
1616 1617

            double K=Math.PI/180; // multiply all derivatives my angles
1618 1619 1620
// dPX/dphi   =  1000/Psz* dxDist/dphi
        	partDeriv[ 1][0]=  K*(dPx_dPinholeX*dPXYmmc[1][0]+dPx_dPinholeY*dPXYmmc[1][1]);
        	partDeriv[ 1][1]=  K*(dPy_dPinholeX*dPXYmmc[1][0]+dPy_dPinholeY*dPXYmmc[1][1]);
1621

1622 1623 1624 1625 1626 1627 1628 1629 1630 1631 1632 1633 1634 1635 1636 1637 1638 1639 1640 1641 1642
// dPX/dtheta =  1000/Psz* dxDist/dtheta
        	partDeriv[ 2][0]=  K*(dPx_dPinholeX*dPXYmmc[2][0]+dPx_dPinholeY*dPXYmmc[2][1]);
        	partDeriv[ 2][1]=  K*(dPy_dPinholeX*dPXYmmc[2][0]+dPy_dPinholeY*dPXYmmc[2][1]);
// dPX/dpsi   =  1000/Psz* dxDist/dpsi
        	partDeriv[ 3][0]=  K*(dPx_dPinholeX*dPXYmmc[3][0]+dPx_dPinholeY*dPXYmmc[3][1]);
        	partDeriv[ 3][1]=  K*(dPy_dPinholeX*dPXYmmc[3][0]+dPy_dPinholeY*dPXYmmc[3][1]);
// dPX/dX0    =  1000/Psz* dxDist/dX0
        	partDeriv[ 4][0]=  dPx_dPinholeX*dPXYmmc[4][0]+dPx_dPinholeY*dPXYmmc[4][1];
        	partDeriv[ 4][1]=  dPy_dPinholeX*dPXYmmc[4][0]+dPy_dPinholeY*dPXYmmc[4][1];
// dPX/dY0    =  1000/Psz* dxDist/dY0
        	partDeriv[ 5][0]=  dPx_dPinholeX*dPXYmmc[5][0]+dPx_dPinholeY*dPXYmmc[5][1];
        	partDeriv[ 5][1]=  dPy_dPinholeX*dPXYmmc[5][0]+dPy_dPinholeY*dPXYmmc[5][1];
// dPX/dZ0    =  1000/Psz* dxDist/dZ0
        	partDeriv[ 6][0]=  dPx_dPinholeX*dPXYmmc[6][0]+dPx_dPinholeY*dPXYmmc[6][1];
        	partDeriv[ 6][1]=  dPy_dPinholeX*dPXYmmc[6][0]+dPy_dPinholeY*dPXYmmc[6][1];
// dPX/df     =  1000/Psz* dxDist/df
        	partDeriv[ 7][0]=  dPx_dPinholeX*dPXYmmc[7][0]+dPx_dPinholeY*dPXYmmc[7][1];
        	partDeriv[ 7][1]=  dPy_dPinholeX*dPXYmmc[7][0]+dPy_dPinholeY*dPXYmmc[7][1];
// dPX/ddist  =  1000/Psz* dxDist/ddist
        	partDeriv[ 8][0]=  dPx_dPinholeX*dPXYmmc[8][0]+dPx_dPinholeY*dPXYmmc[8][1];
        	partDeriv[ 8][1]=  dPy_dPinholeX*dPXYmmc[8][0]+dPy_dPinholeY*dPXYmmc[8][1];
1643

1644 1645 1646 1647 1648 1649 1650 1651 1652 1653
// dPX/dPX0   =  1
// dPY/dPX0   =  0
        	partDeriv[16][0]=  1.0;
        	partDeriv[16][1]=  0.0;
// dPX/dPY0   =  0
// dPY/dPY0   =  1
        	partDeriv[17][0]=  0.0;
        	partDeriv[17][1]=  1.0;
            return partDeriv;
        }
1654 1655 1656



1657 1658 1659 1660 1661 1662 1663 1664 1665 1666 1667 1668 1669 1670 1671 1672 1673 1674 1675 1676 1677
        public void setProperties(String prefix,Properties properties){
			properties.setProperty(prefix+"focalLength",this.focalLength+"");
			properties.setProperty(prefix+"pixelSize",this.pixelSize+"");
			properties.setProperty(prefix+"distortionRadius",this.distortionRadius+"");
			properties.setProperty(prefix+"distortionA8",this.distortionA8+"");
			properties.setProperty(prefix+"distortionA7",this.distortionA7+"");
			properties.setProperty(prefix+"distortionA6",this.distortionA6+"");
			properties.setProperty(prefix+"distortionA5",this.distortionA5+"");
			properties.setProperty(prefix+"distortionA",this.distortionA+"");
			properties.setProperty(prefix+"distortionB",this.distortionB+"");
			properties.setProperty(prefix+"distortionC",this.distortionC+"");
			properties.setProperty(prefix+"yaw",this.yaw+"");
			properties.setProperty(prefix+"pitch",this.pitch+"");
			properties.setProperty(prefix+"roll",this.roll+"");
			properties.setProperty(prefix+"x0",this.x0+"");
			properties.setProperty(prefix+"y0",this.y0+"");
			properties.setProperty(prefix+"z0",this.z0+"");
			properties.setProperty(prefix+"distance",this.distance+"");
			properties.setProperty(prefix+"px0",this.px0+"");
			properties.setProperty(prefix+"py0",this.py0+"");
			properties.setProperty(prefix+"flipVertical",this.flipVertical+"");
1678 1679 1680 1681 1682 1683 1684 1685
			for (int i=0;i<this.r_xy.length;i++){
				properties.setProperty(prefix+"r_xy_"+i+"_x",this.r_xy[i][0]+"");
				properties.setProperty(prefix+"r_xy_"+i+"_y",this.r_xy[i][1]+"");
			}
			for (int i=0;i<this.r_od.length;i++){
				properties.setProperty(prefix+"r_od_"+i+"_o",this.r_od[i][0]+"");
				properties.setProperty(prefix+"r_od_"+i+"_d",this.r_od[i][1]+"");
			}
1686
		}
1687 1688 1689 1690 1691 1692 1693 1694
    	public void setDefaultNonRadial(){
			r_od=new double [r_od_dflt.length][2];
			for (int i=0;i<r_od.length;i++) r_od[i]=r_od_dflt[i].clone();
			r_xy=new double [r_xy_dflt.length][2];
			for (int i=0;i<r_xy.length;i++) r_xy[i]=r_xy_dflt[i].clone();
    	}

    	public void getProperties(String prefix,Properties properties){
1695 1696 1697 1698 1699 1700 1701 1702 1703 1704 1705 1706 1707 1708 1709 1710 1711 1712 1713 1714 1715 1716 1717 1718 1719 1720 1721 1722 1723 1724 1725 1726 1727 1728 1729 1730 1731 1732 1733 1734
			if (properties.getProperty(prefix+"focalLength")!=null)
				this.focalLength=Double.parseDouble(properties.getProperty(prefix+"focalLength"));
			if (properties.getProperty(prefix+"pixelSize")!=null)
				this.pixelSize=Double.parseDouble(properties.getProperty(prefix+"pixelSize"));
			if (properties.getProperty(prefix+"distortionRadius")!=null)
				this.distortionRadius=Double.parseDouble(properties.getProperty(prefix+"distortionRadius"));
			if (properties.getProperty(prefix+"distortionA8")!=null)
				this.distortionA8=Double.parseDouble(properties.getProperty(prefix+"distortionA8"));
			if (properties.getProperty(prefix+"distortionA7")!=null)
				this.distortionA7=Double.parseDouble(properties.getProperty(prefix+"distortionA7"));
			if (properties.getProperty(prefix+"distortionA6")!=null)
				this.distortionA6=Double.parseDouble(properties.getProperty(prefix+"distortionA6"));
			if (properties.getProperty(prefix+"distortionA5")!=null)
				this.distortionA5=Double.parseDouble(properties.getProperty(prefix+"distortionA5"));
			if (properties.getProperty(prefix+"distortionA")!=null)
				this.distortionA=Double.parseDouble(properties.getProperty(prefix+"distortionA"));
			if (properties.getProperty(prefix+"distortionB")!=null)
				this.distortionB=Double.parseDouble(properties.getProperty(prefix+"distortionB"));
			if (properties.getProperty(prefix+"distortionC")!=null)
				this.distortionC=Double.parseDouble(properties.getProperty(prefix+"distortionC"));
			if (properties.getProperty(prefix+"yaw")!=null)
				this.yaw=Double.parseDouble(properties.getProperty(prefix+"yaw"));
			if (properties.getProperty(prefix+"pitch")!=null)
				this.pitch=Double.parseDouble(properties.getProperty(prefix+"pitch"));
			if (properties.getProperty(prefix+"roll")!=null)
				this.roll=Double.parseDouble(properties.getProperty(prefix+"roll"));
			if (properties.getProperty(prefix+"x0")!=null)
				this.x0=Double.parseDouble(properties.getProperty(prefix+"x0"));
			if (properties.getProperty(prefix+"y0")!=null)
				this.y0=Double.parseDouble(properties.getProperty(prefix+"y0"));
			if (properties.getProperty(prefix+"z0")!=null)
				this.z0=Double.parseDouble(properties.getProperty(prefix+"z0"));
			if (properties.getProperty(prefix+"distance")!=null)
				this.distance=Double.parseDouble(properties.getProperty(prefix+"distance"));
			if (properties.getProperty(prefix+"px0")!=null)
				this.px0=Double.parseDouble(properties.getProperty(prefix+"px0"));
			if (properties.getProperty(prefix+"py0")!=null)
				this.py0=Double.parseDouble(properties.getProperty(prefix+"py0"));
			if (properties.getProperty(prefix+"flipVertical")!=null)
				this.flipVertical=Boolean.parseBoolean(properties.getProperty(prefix+"flipVertical"));
1735

1736 1737 1738 1739 1740 1741 1742 1743 1744
			setDefaultNonRadial();
			for (int i=0;i<this.r_xy.length;i++){
				if (properties.getProperty(prefix+"r_xy_"+i+"_x")!=null) this.r_xy[i][0]=Double.parseDouble(properties.getProperty(prefix+"r_xy_"+i+"_x"));
				if (properties.getProperty(prefix+"r_xy_"+i+"_y")!=null) this.r_xy[i][1]=Double.parseDouble(properties.getProperty(prefix+"r_xy_"+i+"_y"));
			}
			for (int i=0;i<this.r_od.length;i++){
				if (properties.getProperty(prefix+"r_od_"+i+"_o")!=null) this.r_od[i][0]=Double.parseDouble(properties.getProperty(prefix+"r_od_"+i+"_o"));
				if (properties.getProperty(prefix+"r_od_"+i+"_d")!=null) this.r_od[i][1]=Double.parseDouble(properties.getProperty(prefix+"r_od_"+i+"_d"));
			}
1745 1746 1747 1748 1749 1750 1751 1752 1753 1754 1755 1756 1757 1758 1759 1760 1761 1762 1763 1764 1765 1766 1767 1768
			recalcCommons();
		}
		public boolean showDialog() {
			GenericDialog gd = new GenericDialog("Lens distortion, location and orientation");
			gd.addNumericField("Lens focal length",               this.focalLength, 3,6,"mm");
			gd.addNumericField("Sensor pixel period",             this.pixelSize, 3,6,"um");
			gd.addNumericField("Distortion radius (halw width)",  this.distortionRadius, 5,8,"mm");
			gd.addNumericField("Distortion A8(r^5)",              this.distortionA8, 6,8,"");
			gd.addNumericField("Distortion A7(r^5)",              this.distortionA7, 6,8,"");
			gd.addNumericField("Distortion A6(r^5)",              this.distortionA6, 6,8,"");
			gd.addNumericField("Distortion A5(r^5)",              this.distortionA5, 6,8,"");
			gd.addNumericField("Distortion A (r^4)",              this.distortionA, 6,8,"");
			gd.addNumericField("Distortion B (r^3)",              this.distortionB, 6,8,"");
			gd.addNumericField("Distortion C (r^2)",              this.distortionC, 6,8,"");
			gd.addNumericField("Lens axis from perpendicular to the pattern, positive - clockwise (from top)", this.yaw, 2,6,"degrees");
			gd.addNumericField("Lens axis from perpendicular to the pattern, positive - up",                   this.pitch, 2,6,"degrees");
			gd.addNumericField("Rotation around lens axis, positive - clockwise (looking to pattern)",         this.roll, 2,6,"degrees");
			gd.addNumericField("Lens axis from the pattern center, (to the right)",                            this.x0, 1,6,"mm");
			gd.addNumericField("Lens axis from the pattern center, (down)",                                    this.y0, 1,6,"mm");
			gd.addNumericField("Lens axis from the pattern center, (away from camera, normally 0.0)",          this.z0, 1,6,"mm");
			gd.addNumericField("Distance from the lens input pupil to the pattern plane along the camera axis",this.distance, 1,6,"mm");
			gd.addNumericField("Lens axis on the sensor (horizontal, from left edge)",                         this.px0, 1,6,"pixels");
			gd.addNumericField("Lens axis on the sensor (vertical, from top edge)",                            this.py0, 1,6,"pixels");
			gd.addCheckbox    ("Camera looks through the mirror",                                              this.flipVertical);
1769 1770 1771 1772 1773 1774 1775 1776 1777 1778 1779 1780 1781 1782 1783 1784 1785 1786 1787 1788 1789 1790 1791 1792 1793 1794 1795 1796 1797 1798 1799 1800 1801 1802 1803
			gd.addMessage("=== non-radial model parameters ===");
			gd.addMessage("For r^2 (Distortion C):");
			gd.addNumericField("Orthogonal elongation for r^2",   100*this.r_od[0][0], 3,7,"%");
			gd.addNumericField("Diagonal elongation for r^2",     100*this.r_od[0][1], 3,7,"%");
			gd.addMessage("For r^3 (Distortion B):");
			gd.addNumericField("Distortion center shift X for r^3", 100*this.r_xy[0][0], 1,6,"%");
			gd.addNumericField("Distortion center shift Y for r^3", 100*this.r_xy[0][1], 1,6,"%");
			gd.addNumericField("Orthogonal elongation for r^3",  100*this.r_od[1][0], 3,7,"%");
			gd.addNumericField("Diagonal elongation for r^3",    100*this.r_od[1][1], 3,7,"%");
			gd.addMessage("For r^4 (Distortion A):");
			gd.addNumericField("Distortion center shift X for r^4", 100*this.r_xy[1][0], 1,6,"%");
			gd.addNumericField("Distortion center shift Y for r^4", 100*this.r_xy[1][1], 1,6,"%");
			gd.addNumericField("Orthogonal elongation for r^4",  100*this.r_od[2][0], 3,7,"%");
			gd.addNumericField("Diagonal elongation for r^4",    100*this.r_od[2][1], 3,7,"%");
			gd.addMessage("For r^5 (Distortion A5):");
			gd.addNumericField("Distortion center shift X for r^5", 100*this.r_xy[2][0], 1,6,"%");
			gd.addNumericField("Distortion center shift Y for r^5", 100*this.r_xy[2][1], 1,6,"%");
			gd.addNumericField("Orthogonal elongation for r^5",  100*this.r_od[3][0], 3,7,"%");
			gd.addNumericField("Diagonal elongation for r^5",    100*this.r_od[3][1], 3,7,"%");
			gd.addMessage("For r^6 (Distortion A6:");
			gd.addNumericField("Distortion center shift X for r^6", 100*this.r_xy[3][0], 1,6,"%");
			gd.addNumericField("Distortion center shift Y for r^6", 100*this.r_xy[3][1], 1,6,"%");
			gd.addNumericField("Orthogonal elongation for r^6",  100*this.r_od[4][0], 3,7,"%");
			gd.addNumericField("Diagonal elongation for r^6",    100*this.r_od[4][1], 3,7,"%");
			gd.addMessage("For r^7 (Distortion A7):");
			gd.addNumericField("Distortion center shift X for r^7", 100*this.r_xy[4][0], 1,6,"%");
			gd.addNumericField("Distortion center shift Y for r^7", 100*this.r_xy[4][1], 1,6,"%");
			gd.addNumericField("Orthogonal elongation for r^7",  100*this.r_od[5][0], 3,7,"%");
			gd.addNumericField("Diagonal elongation for r^7",    100*this.r_od[5][1], 3,7,"%");
			gd.addMessage("For r^8 (Distortion A8):");
			gd.addNumericField("Distortion center shift X for r^8", 100*this.r_xy[5][0], 1,6,"%");
			gd.addNumericField("Distortion center shift Y for r^8", 100*this.r_xy[5][1], 1,6,"%");
			gd.addNumericField("Orthogonal elongation for r^8",  100*this.r_od[6][0], 3,7,"%");
			gd.addNumericField("Diagonal elongation for r^8",    100*this.r_od[6][1], 3,7,"%");
    		WindowTools.addScrollBars(gd);
1804 1805 1806 1807 1808 1809 1810 1811 1812 1813 1814 1815 1816 1817 1818 1819 1820 1821
			gd.showDialog();
			if (gd.wasCanceled()) return false;
			this.focalLength=     gd.getNextNumber();
			this.pixelSize=       gd.getNextNumber();
			this.distortionRadius=gd.getNextNumber();
			this.distortionA8=    gd.getNextNumber();
			this.distortionA7=    gd.getNextNumber();
			this.distortionA6=    gd.getNextNumber();
			this.distortionA5=    gd.getNextNumber();
			this.distortionA=     gd.getNextNumber();
			this.distortionB=     gd.getNextNumber();
			this.distortionC=     gd.getNextNumber();
			this.yaw=             gd.getNextNumber();
			this.pitch=           gd.getNextNumber();
			this.roll=            gd.getNextNumber();
			this.x0=              gd.getNextNumber();
			this.y0=              gd.getNextNumber();
			this.z0=              gd.getNextNumber();
1822
			this.distance=        gd.getNextNumber();
1823 1824
			this.px0=             gd.getNextNumber();
			this.py0=             gd.getNextNumber();
1825
			this.flipVertical=    gd.getNextBoolean();
1826

1827 1828 1829 1830 1831 1832 1833 1834 1835 1836 1837 1838 1839 1840 1841 1842 1843 1844 1845 1846 1847 1848 1849 1850 1851 1852
			this.r_od[0][0]= 0.01*gd.getNextNumber();
			this.r_od[0][1]= 0.01*gd.getNextNumber();
			this.r_xy[0][0]= 0.01*gd.getNextNumber();
			this.r_xy[0][1]= 0.01*gd.getNextNumber();
			this.r_od[1][0]= 0.01*gd.getNextNumber();
			this.r_od[1][1]= 0.01*gd.getNextNumber();
			this.r_xy[1][0]= 0.01*gd.getNextNumber();
			this.r_xy[1][1]= 0.01*gd.getNextNumber();
			this.r_od[2][0]= 0.01*gd.getNextNumber();
			this.r_od[2][1]= 0.01*gd.getNextNumber();
			this.r_xy[2][0]= 0.01*gd.getNextNumber();
			this.r_xy[2][1]= 0.01*gd.getNextNumber();
			this.r_od[3][0]= 0.01*gd.getNextNumber();
			this.r_od[3][1]= 0.01*gd.getNextNumber();
			this.r_xy[3][0]= 0.01*gd.getNextNumber();
			this.r_xy[3][1]= 0.01*gd.getNextNumber();
			this.r_od[4][0]= 0.01*gd.getNextNumber();
			this.r_od[4][1]= 0.01*gd.getNextNumber();
			this.r_xy[4][0]= 0.01*gd.getNextNumber();
			this.r_xy[4][1]= 0.01*gd.getNextNumber();
			this.r_od[5][0]= 0.01*gd.getNextNumber();
			this.r_od[5][1]= 0.01*gd.getNextNumber();
			this.r_xy[5][0]= 0.01*gd.getNextNumber();
			this.r_xy[5][1]= 0.01*gd.getNextNumber();
			this.r_od[6][0]= 0.01*gd.getNextNumber();
			this.r_od[6][1]= 0.01*gd.getNextNumber();
1853 1854 1855 1856
			return true;
		}
	    /**
	     * Calculate/set  this.lensDistortionParameters and this.interParameterDerivatives
1857
     * UPDATE - Modifies lensDistortionParameters, not "this" for multi-threaded
1858 1859 1860 1861 1862
	     * @param parVect 21-element vector for eyesis sub-camera, including common and individual parameters
	     * @param mask -mask - which partial derivatives are needed to be calculated (others will be null)
	     * @param calculateDerivatives calculate array of partial derivatives, if false - just the values
	     */
	    public void lensCalcInterParamers(
Andrey Filippov's avatar
Andrey Filippov committed
1863
	    		LensDistortionParameters lensDistortionParameters,
1864
	    		boolean isTripod,
1865
	    		boolean cartesian,
1866 1867
	    		double  pixelSize,
	    		double  distortionRadius,
1868 1869 1870 1871
	            double [][] interParameterDerivatives, //partial derivative matrix from subcamera-camera-goniometer to single camera (12x21) if null - just values, no derivatives
	    		double [] parVect,
	    		boolean [] mask // calculate only selected derivatives (all parVect values are still
	    		){
1872
	    	boolean calculateDerivatives=(interParameterDerivatives!=null);  // calculate this.interParameterDerivatives -derivatives array (false - just this.values)
1873
	    	 // change meaning of goniometerHorizontal (tripod vertical) and goniometerAxial (tripod horizontal)
1874

1875 1876 1877 1878 1879 1880 1881 1882
	    	// Alternative variables for cartesian/cylindrical modes
	    	double azimuth_cyl=  cartesian? Double.NaN:  parVect[0];
	    	double right_cart=   cartesian? parVect[0] : Double.NaN;
	    	double radius_cyl=   cartesian? Double.NaN:  parVect[1];
	    	double forward_cart= cartesian? parVect[1]:  Double.NaN;
	    	double height=       parVect[2];
	    	double phi_cyl=      cartesian? Double.NaN:  parVect[3];
	    	double heading_cart= cartesian? parVect[3]:  Double.NaN;
1883 1884 1885 1886 1887 1888 1889 1890 1891 1892 1893 1894 1895
	    	double theta=  parVect[4];
	    	double psi=    parVect[5];
	    	double goniometerHorizontal=parVect[6];
	    	double goniometerAxial=parVect[7];
	    	double interAxisDistance=parVect[8];
	    	double interAxisAngle=parVect[9];
	    	double horAxisErrPhi=parVect[10];
	    	double horAxisErrPsi=parVect[11];
	    	double entrancePupilForward=parVect[12];
	    	double centerAboveHorizontal=parVect[13];
	    	double GXYZ0=parVect[14];
	    	double GXYZ1=parVect[15];
	    	double GXYZ2=parVect[16];
1896

1897 1898 1899 1900
	    	double cPS=   Math.cos(psi*Math.PI/180); // subCam.psi
	    	double sPS=   Math.sin(psi*Math.PI/180); // subCam.psi
	    	double cTH=   Math.cos(theta*Math.PI/180); // subCam.theta
	    	double sTH=   Math.sin(theta*Math.PI/180); // subCam.theta
1901

1902 1903 1904 1905 1906 1907
	    	double cAZP=  Math.cos((cartesian?heading_cart:(azimuth_cyl+phi_cyl))*Math.PI/180); //subCam.azimuth+subCam.phi
	    	double sAZP=  Math.sin((cartesian?heading_cart:(azimuth_cyl+phi_cyl))*Math.PI/180); //subCam.azimuth+subCam.phi

	    	// renaming the following 2 to be replaced for cartesian coordinates
	    	double cAZ_cyl=   Math.cos(azimuth_cyl*Math.PI/180); //subCam.azimuth
	    	double sAZ_cyl=   Math.sin(azimuth_cyl*Math.PI/180); //subCam.azimuth
1908

1909 1910 1911 1912 1913 1914 1915 1916 1917 1918
	    	double cGA=   Math.cos(goniometerAxial*Math.PI/180); //eyesisCameraParameters.goniometerAxial
	    	double sGA=   Math.sin(goniometerAxial*Math.PI/180); //eyesisCameraParameters.goniometerAxial
	    	double cGH=   Math.cos(goniometerHorizontal*Math.PI/180); //eyesisCameraParameters.goniometerHorizontal
	    	double sGH=   Math.sin(goniometerHorizontal*Math.PI/180); //eyesisCameraParameters.goniometerHorizontal
	    	double cGIAA= Math.cos(interAxisAngle*Math.PI/180); // eyesisCameraParameters.interAxisAngle
	    	double sGIAA= Math.sin(interAxisAngle*Math.PI/180); //eyesisCameraParameters.interAxisAngle
	    	double cHAEPH=Math.cos(horAxisErrPhi*Math.PI/180); //eyesisCameraParameters.horAxisErrPhi
	    	double sHAEPH=Math.sin(horAxisErrPhi*Math.PI/180); //eyesisCameraParameters.horAxisErrPhi
	    	double cHAEPS=Math.cos(horAxisErrPsi*Math.PI/180); //eyesisCameraParameters.horAxisErrPsi
	    	double sHAEPS=Math.sin(horAxisErrPsi*Math.PI/180); //eyesisCameraParameters.horAxisErrPsi\
1919

1920 1921 1922 1923 1924 1925 1926 1927
	/*
	 0) Translate by distance to entrance pupil (lens center)
	    	| Xc0 |   | 0                     |   |Xc|
	    	| Yc0 | = | 0                     | + |Yc|
	    	| Zc0 |   | entrancePupilForward  |   |Zc|
	    	*/
	    	double [][] aT0={{0.0},{0.0},{entrancePupilForward}};
	    	Matrix T0=new Matrix(aT0);
1928

1929 1930 1931 1932 1933 1934 1935 1936 1937
	/*
	Converting from the sub-camera coordinates to the target coordinates
	1) rotate by -psi around CZ: Vc1= R1*Vc
	| Xc1 |   | cos(psi)  sin(psi)    0         |   |Xc0|
	| Yc1 | = |-sin(psi)  cos(psi)    0         | * |Yc0|
	| Zc1 |   |    0         0        1         |   |Zc0|
	*/
	    	double [][] aR1={{cPS,sPS,0.0},{-sPS,cPS,0.0},{0.0,0.0,1.0}};
	    	Matrix R1=new Matrix(aR1);
1938
	/*
1939 1940 1941 1942 1943 1944 1945
	2) rotate by - theta around C1X:Vc2= R2*Vc1
	| Xc2 |   |    1         0         0        |   |Xc1|
	| Yc2 | = |    0    cos(theta)   sin(theta) | * |Yc1|
	| Zc2 |   |    0   -sin(theta)   cos(theta) |   |Zc1|
	*/
	    	double [][] aR2={{1.0,0.0,0.0},{0.0,cTH,sTH},{0.0,-sTH,cTH}};
	    	Matrix R2=new Matrix(aR2);
1946
	/*
1947 1948 1949 1950 1951 1952 1953
	3) rotate by -(azimuth+phi) around C2Y:Vc3= R3*Vc2
	| Xc3 |   | cos(azimuth+phi)    0   sin(azimuth+phi)   |   |Xc2|
	| Yc3 | = |     0               1         0            | * |Yc2|
	| Zc3 |   | -sin(azimuth+phi)   0   cos(azimuth+phi)   |   |Zc2|
	*/
	    	double [][] aR3={{cAZP,0.0,sAZP},{0.0,1.0,0.0},{-sAZP,0.0,cAZP}};
	    	Matrix R3=new Matrix(aR3);
1954
	/*
1955 1956 1957 1958 1959
	4) Now axes are aligned, just translate to get to eyesis coordinates: Vey= T1+Vc3
	| Xey |   |      r * sin (azimuth)       |   |Xc3|
	| Yey | = | height+centerAboveHorizontal | + |Yc3|
	| Zey |   |      r * cos (azimuth)       |   |Zc3|
	*/
1960 1961
	    	double [][] aT1_cyl= {{radius_cyl*sAZ_cyl},{(height+centerAboveHorizontal)},{radius_cyl*cAZ_cyl}}; // {{subCam.radius*sAZ},{subCam.height},{subCam.radius*cAZ}};
	    	double [][] aT1_cart={{right_cart},        {(height+centerAboveHorizontal)},{forward_cart}}; // {{subCam.radius*sAZ},{subCam.height},{subCam.radius*cAZ}};
1962 1963

	    	double [][] aT1= cartesian ? aT1_cart : aT1_cyl;
1964
	    	Matrix T1=new Matrix(aT1);
1965

1966 1967 1968 1969 1970 1971 1972
	/**
	5) rotate around moving goniometer axis, by (-goniometerAxial) - same as around EY: Vgm1=R4*Vey
	| Xgm1 |   |    1           0                     0              |   |Xey|
	| Ygm1 | = |    0   cos(goniometerAxial)  sin(goniometerAxial)   | * |Yey|
	| Zgm1 |   |    0   -sin(goniometerAxial) cos(goniometerAxial)   |   |Zey|
	*/
	    	double [][] aR4_tripod=    {{1.0,0.0,0.0},{0.0,cGA,sGA},{0.0,-sGA,cGA}};
1973
	/*
1974 1975 1976 1977 1978 1979 1980
	5) rotate around moving goniometer axis, by (-goniometerAxial) - same as around EY: Vgm1=R4*Vey
	| Xgm1 |   | cos(goniometerAxial)    0   sin(goniometerAxial)   |   |Xey|
	| Ygm1 | = |        0                1             0            | * |Yey|
	| Zgm1 |   |-sin(goniometerAxial)    0   cos(goniometerAxial)   |   |Zey|
	*/
			double [][] aR4_goniometer={{cGA,0.0,sGA},{0.0,1.0,0.0},{-sGA,0.0,cGA}};
			Matrix R4=new Matrix(isTripod?aR4_tripod:aR4_goniometer);
1981
	/*
1982 1983 1984 1985 1986 1987 1988
	6) move to the goniometer horizontal axis:Vgm2=T2+Vgm1
	| Xgm2 |   |                 0  |   |Xgm1|
	| Ygm2 | = |                 0  | + |Ygm1|
	| Zgm2 |   | interAxisDistance  |   |Zgm1|
	*/
	    	double [][] aT2={{0.0},{0.0},{interAxisDistance}}; //eyesisCameraParameters.interAxisDistance
	    	Matrix T2=new Matrix(aT2);
1989
	/*
1990 1991 1992 1993 1994 1995 1996 1997 1998 1999 2000 2001 2002 2003
	7) rotate around Zgm2 by -interAxisAngle, so Xgm3 axis is the same as horizontal goniometer axis: Vgm3=R5*Vgm2
	| Xgm3 |   | cos(interAxisAngle)  sin(interAxisAngle)    0         |   |Xgm2|
	| Ygm3 | = |-sin(interAxisAngle)  cos(interAxisAngle)    0         | * |Ygm2|
	| Zgm3 |   |    0                         0              1         |   |Zgm2|
	*/
	    	double [][] aR5={{cGIAA,sGIAA,0.0},{-sGIAA,cGIAA,0.0},{0.0,0.0,1.0}};
	    	Matrix R5=new Matrix(aR5);
	/**
	8)  rotate around goniometer horizontal axis (Xgm3) by -goniometerHorizontal: Vgm4=R6*Vgm3
	| Xgm4 |   |    cos(goniometerHorizontal)  0   sin(goniometerHorizontal) |   |Xgm3|
	| Ygm4 | = |               0               1               0             | * |Ygm3|
	| Zgm4 |   |   -sin(goniometerHorizontal)  0   cos(goniometerHorizontal) |   |Zgm3|
	*/
	    	double [][] aR6_tripod=    {{cGH,0.0,sGH},{0.0,1.0,0.0},{-sGH,0.0,cGH}};
2004
	/*
2005 2006 2007 2008 2009 2010 2011
	8)  rotate around goniometer horizontal axis (Xgm3) by -goniometerHorizontal: Vgm4=R6*Vgm3
	| Xgm4 |   |    1                 0                           0            |   |Xgm3|
	| Ygm4 | = |    0    cos(goniometerHorizontal)   sin(goniometerHorizontal) | * |Ygm3|
	| Zgm4 |   |    0   -sin(goniometerHorizontal)   cos(goniometerHorizontal) |   |Zgm3|
	*/
	    	double [][] aR6_goniometer={{1.0,0.0,0.0},{0.0,cGH,sGH},{0.0,-sGH,cGH}};
			Matrix R6=new Matrix(isTripod?aR6_tripod:aR6_goniometer);
2012
	/*
2013 2014 2015 2016 2017 2018 2019
	9) Correct roll error of the goniometer horizontal axis - rotate by -horAxisErrPsi around Zgm4: Vgm5=R7*Vgm4
	| Xgm5 |   | cos(horAxisErrPsi)  sin(horAxisErrPsi)    0         |   |Xgm4|
	| Ygm5 | = |-sin(horAxisErrPsi)  cos(horAxisErrPsi)    0         | * |Ygm4|
	| Zgm5 |   |         0                   0             1         |   |Zgm4|
	*/
	    	double [][] aR7={{cHAEPS,sHAEPS,0.0},{-sHAEPS,cHAEPS,0.0},{0.0,0.0,1.0}};
	    	Matrix R7=new Matrix(aR7);
2020
	/*
2021
	10) Correct azimuth error of the goniometer hoirizontal axis - rotate by -horAxisErrPhi around Ygm5: Vgm6=R8*Vgm5
2022

2023 2024 2025 2026 2027 2028 2029 2030 2031 2032 2033 2034
	| Xgm6 |   | cos(horAxisErrPhi)      0   sin(horAxisErrPhi)   |   |Xgm5|
	| Ygm6 | = |        0                1             0          | * |Ygm5|
	| Zgm6 |   |-sin(horAxisErrPhi)      0   cos(horAxisErrPhi)   |   |Zgm5|
	For Tripod - rotate around X-axis (like theta)
	| Xgm6 |   |   1           0                       0      |   |Xgm5|
	| Ygm6 | = |   0  cos(horAxisErrPhi)  sin(horAxisErrPhi)  | * |Ygm5|
	| Zgm6 |   |   0 -sin(horAxisErrPhi)  cos(horAxisErrPhi)  |   |Zgm5|

	*/
	    	double [][] aR8_tripod=    {{1.0,   0.0,   0.0},{0.0,cHAEPH,sHAEPH},{0.0,  -sHAEPH,cHAEPH}};
	    	double [][] aR8_goniometer={{cHAEPH,0.0,sHAEPH},{0.0,   1.0,   0.0},{-sHAEPH,  0.0,cHAEPH}};
	    	Matrix R8=new Matrix(isTripod?aR8_tripod:aR8_goniometer);
2035
	/*
2036 2037 2038 2039 2040 2041
	11) translate to the target zero point: Vt=  T3+Vgm6
	| Xt |   | GXYZ[0]  |   |Xgm6|
	| Yt | = |-GXYZ[1]  | + |Ygm6| // Y - up positive
	| Zt |   |-GXYZ[2]  |   |Zgm6| // Z - away positive
	    	 */
//	    	double [][] aT3={{parVect[12]},{-parVect[13]},{-parVect[14]}};//{{eyesisCameraParameters.GXYZ[0]},{eyesisCameraParameters.GXYZ[1]},{eyesisCameraParameters.GXYZ[2]}};
2042

2043 2044 2045 2046 2047 2048 2049 2050 2051 2052 2053 2054 2055 2056 2057 2058 2059 2060 2061 2062
	    	double [][] aT3={{GXYZ0},{-GXYZ1},{-GXYZ2}};//{{eyesisCameraParameters.GXYZ[0]},{eyesisCameraParameters.GXYZ[1]},{eyesisCameraParameters.GXYZ[2]}};
//	    	double [][] aT3={{parVect[12]},{ parVect[13]},{-parVect[14]}};//{{eyesisCameraParameters.GXYZ[0]},{eyesisCameraParameters.GXYZ[1]},{eyesisCameraParameters.GXYZ[2]}};
	    	Matrix T3=new Matrix(aT3);

	// MA=R8*R7*R6*R5*R4*R3*R2*R1;
	// MB=T3+(R8*R7*R6*R5*(T2+R4*T1)); - old
	// MB=T3+(R8*R7*R6*R5*(T2+R4*(T1+R3*R2*R1*T0)));
	        Matrix MA=R8.times(R7.times(R6.times(R5.times(R4.times(R3.times(R2.times(R1)))))));
//	        Matrix MB=T3.plus(R8.times(R7.times(R6.times(R5.times(T2.plus(R4.times(T1)))))));
	        Matrix MB=T3.plus(R8.times(R7.times(R6.times(R5.times(T2.plus(R4.times(T1.plus(R3.times(R2.times(R1.times(T0)))) )))))));
			if (this.debugLevel>2) {
				System.out.println("MA:");
				MA.print(10, 5);
				System.out.println("MB:");
				MB.print(10, 5);
				System.out.println("T3:");
				T3.print(10, 5);
//				System.out.println("interParameterDerivatives[0]="+sprintfArray(interParameterDerivatives[0]));
			}
	        double [] extrinsicParams=parametersFromMAMB(MA,MB); // all after 6 are 0;
2063 2064 2065 2066

	        lensDistortionParameters.pixelSize =        pixelSize;
	        lensDistortionParameters.distortionRadius = distortionRadius;

2067 2068 2069 2070 2071 2072 2073 2074 2075 2076 2077 2078 2079 2080 2081 2082 2083 2084
	        lensDistortionParameters.distance=extrinsicParams[2];
	        lensDistortionParameters.x0=extrinsicParams[0];
	        lensDistortionParameters.y0=extrinsicParams[1];
	        lensDistortionParameters.z0=0.0; // used here
	        lensDistortionParameters.pitch=extrinsicParams[4];
	        lensDistortionParameters.yaw=  extrinsicParams[3];
	        lensDistortionParameters.roll= extrinsicParams[5];

			lensDistortionParameters.focalLength=parVect[17]; //subCam.focalLength;
			lensDistortionParameters.px0=parVect[18]; //subCam.px0;
			lensDistortionParameters.py0=parVect[19]; //subCam.py0;
			lensDistortionParameters.distortionA8=parVect[20]; //subCam.distortion5;
			lensDistortionParameters.distortionA7=parVect[21]; //subCam.distortion5;
			lensDistortionParameters.distortionA6=parVect[22]; //subCam.distortion5;
			lensDistortionParameters.distortionA5=parVect[23]; //subCam.distortion5;
			lensDistortionParameters.distortionA=parVect[24]; //subCam.distortionA;
			lensDistortionParameters.distortionB=parVect[25]; //subCam.distortionB;
			lensDistortionParameters.distortionC=parVect[26]; //subCam.distortionC;
2085

2086 2087
			lensDistortionParameters.r_xy=new double [6][2];
			lensDistortionParameters.r_od=new double [7][2];
2088

2089 2090 2091 2092 2093 2094 2095 2096 2097
			int index=27;
			for (int i=0;i<lensDistortionParameters.r_od.length;i++){
				if (i>0){
					lensDistortionParameters.r_xy[i-1][0]=parVect[index++];
					lensDistortionParameters.r_xy[i-1][1]=parVect[index++];
				}
				lensDistortionParameters.r_od[i][0]=parVect[index++];
				lensDistortionParameters.r_od[i][1]=parVect[index++];
			}
2098 2099 2100 2101

			lensDistortionParameters.recalcCommons(); // uncluding lensDistortionParameters.r_xyod


2102 2103 2104 2105 2106 2107 2108 2109 2110 2111 2112 2113 2114 2115 2116 2117 2118 2119 2120
	        if (this.debugLevel>2){
	        	System.out.println("lensDistortionParameters.recalcCommons()");
	        	System.out.println("lensDistortionParameters.distance="+IJ.d2s(lensDistortionParameters.distance, 3));
	        	System.out.println("lensDistortionParameters.x0="+      IJ.d2s(lensDistortionParameters.x0, 3));
	        	System.out.println("lensDistortionParameters.y0="+      IJ.d2s(lensDistortionParameters.y0, 3));
	        	System.out.println("lensDistortionParameters.z0="+      IJ.d2s(lensDistortionParameters.z0, 3));
	        	System.out.println("lensDistortionParameters.pitch="+   IJ.d2s(lensDistortionParameters.pitch, 3));
	        	System.out.println("lensDistortionParameters.yaw="+IJ.d2s(lensDistortionParameters.yaw, 3));
	        	System.out.println("lensDistortionParameters.roll="+IJ.d2s(lensDistortionParameters.roll, 3));
	        	System.out.println("lensDistortionParameters.focalLength="+IJ.d2s(lensDistortionParameters.focalLength, 3));
	        	System.out.println("lensDistortionParameters.px0="+IJ.d2s(lensDistortionParameters.px0, 3));
	        	System.out.println("lensDistortionParameters.py0="+IJ.d2s(lensDistortionParameters.py0, 3));
	        	System.out.println("lensDistortionParameters.distortionA8="+IJ.d2s(lensDistortionParameters.distortionA8, 5));
	        	System.out.println("lensDistortionParameters.distortionA7="+IJ.d2s(lensDistortionParameters.distortionA7, 5));
	        	System.out.println("lensDistortionParameters.distortionA6="+IJ.d2s(lensDistortionParameters.distortionA6, 5));
	        	System.out.println("lensDistortionParameters.distortionA5="+IJ.d2s(lensDistortionParameters.distortionA5, 5));
	        	System.out.println("lensDistortionParameters.distortionA="+IJ.d2s(lensDistortionParameters.distortionA, 5));
	        	System.out.println("lensDistortionParameters.distortionB="+IJ.d2s(lensDistortionParameters.distortionB, 5));
	        	System.out.println("lensDistortionParameters.distortionC="+IJ.d2s(lensDistortionParameters.distortionC, 5));
2121 2122 2123 2124 2125 2126 2127 2128 2129
	        	for (int i=0;i<lensDistortionParameters.r_xy.length;i++){
	        		System.out.println("lensDistortionParameters.r_xy["+i+"][0]"+IJ.d2s(lensDistortionParameters.r_xy[i][0], 5));
	        		System.out.println("lensDistortionParameters.r_xy["+i+"][1]"+IJ.d2s(lensDistortionParameters.r_xy[i][1], 5));

	        	}
	        	for (int i=0;i<lensDistortionParameters.r_od.length;i++){
	        		System.out.println("lensDistortionParameters.r_od["+i+"][0]"+IJ.d2s(lensDistortionParameters.r_od[i][0], 5));
	        		System.out.println("lensDistortionParameters.r_od["+i+"][1]"+IJ.d2s(lensDistortionParameters.r_od[i][1], 5));
	        	}
2130
	        }
2131
	        if (!calculateDerivatives) return;
2132 2133 2134 2135 2136 2137 2138 2139 2140
	/* Calculate all derivativs as a matrix.
	 *  Input parameters (columns):
	0    	public double azimuth; // azimuth of the lens entrance pupil center, degrees, clockwise looking from top
	1    	public double radius;  // mm, distance from the rotation axis
	2   	public double height;       // mm, up (was downwards?) - from the origin point
	3   	public double phi;     // degrees, optical axis from azimuth/r vector, clockwise
	4   	public double theta;   // degrees, optical axis from the eyesis horizon, positive - up
	5   	public double psi;     // degrees, rotation (of the sensor) around the optical axis. Positive if camera is rotated clockwise looking to the target
	6   	public double goniometerHorizontal; // goniometer rotation around "horizontal" axis (tilting from the target - positive)
2141
	7   	public double goniometerAxial; // goniometer rotation around Eyesis axis (clockwise in plan - positive
2142 2143 2144 2145 2146 2147
	8   	public double interAxisDistance; // distance in mm between two goniometer axes
	9    	public double interAxisAngle;    // angle in degrees between two goniometer axes minus 90. negative if "vertical" axis is rotated
	10   	public double horAxisErrPhi;   // angle in degrees "horizontal" goniometer axis is rotated around target Y axis from target X axis (CW)
	11   	public double horAxisErrPsi;   // angle in degrees "horizontal" goniometer axis is rotated around moving X axis (up)
	Two new parameters
	12    	public double entrancePupilForward; // common to all lenses - distance from the sensor to the lens entrance pupil
2148 2149
	13    	public double centerAboveHorizontal; // camera center distance along camera axis above the closest point to horizontal rotation axis (adds to
	14(12)  x	public double [] GXYZ=null; // coordinates (in mm) of the goniometer horizontal axis closest to the moving one in target system
2150 2151 2152 2153 2154 2155 2156 2157 2158 2159 2160
	15(13)  y
	16(14)  z
	17(15)	public double focalLength=4.5;
	18(16)	public double px0=1296.0;          // center of the lens on the sensor, pixels
	19(17)		public double py0=968.0;           // center of the lens on the sensor, pixels
	20(18)		public double distortionA8=0.0; // r^8 (normalized to focal length or to sensor half width?)
	21(19)		public double distortionA7=0.0; // r^7 (normalized to focal length or to sensor half width?)
	22(20)		public double distortionA6=0.0; // r^6 (normalized to focal length or to sensor half width?)
	23(21)		public double distortionA5=0.0; // r^5 (normalized to focal length or to sensor half width?)
	24(22)		public double distortionA=0.0; // r^4 (normalized to focal length or to sensor half width?)
	25(23)		public double distortionB=0.0; // r^3
Andrey Filippov's avatar
Andrey Filippov committed
2161
	26(24)		public double distortionC=0.0; // r^2
2162 2163 2164 2165 2166 2167 2168 2169 2170 2171 2172 2173 2174 2175 2176 2177 2178 2179 2180 2181 2182 2183 2184 2185 2186 2187
    27          Distortion center shift X for r^3"
    28          Distortion center shift Y for r^3"
    29          Distortion center shift X for r^4"
    30          Distortion center shift Y for r^4"
    31          Distortion center shift X for r^5"
    32          Distortion center shift Y for r^5"
    33          Distortion center shift X for r^6"
    34          Distortion center shift Y for r^6"
    35          Distortion center shift X for r^7"
    36          Distortion center shift Y for r^7"
    37          Distortion center shift X for r^8"
    38          Distortion center shift Y for r^8"
    39          Orthogonal elongation for r^2"
    40          Diagonal   elongation for r^2"
    41          Orthogonal elongation for r^3"
    42          Diagonal   elongation for r^3"
    43          Orthogonal elongation for r^4"
    44          Diagonal   elongation for r^4"
    45          Orthogonal elongation for r^5"
    46          Diagonal   elongation for r^5"
    47          Orthogonal elongation for r^6"
    48          Diagonal   elongation for r^6"
    49          Orthogonal elongation for r^7"
    50          Diagonal   elongation for r^7"
    51          Orthogonal elongation for r^8"
    52          Diagonal   elongation for r^8"
2188 2189 2190 2191 2192 2193 2194 2195 2196 2197 2198 2199

	 * Output parameters (rows):
	0		public double x0=0;      // lens axis from pattern center, mm (to the right)
	1		public double y0=0;      // lens axis from pattern center, mm (down)
	2		public double distance=2360; // distance from the lens input pupil to the pattern plane along the camera axis, mm
	3		public double yaw=0.0;   // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - clockwise from top
	4		public double pitch=0.0; // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - up
	5		public double roll=0.0;  // angle in degrees rotation around camera optical axis (perpendicular to pattern if yaw==0, pitch==0), positive - clockwise
	6		public double focalLength=4.5;
	7		public double px0=1296.0;           // center of the lens on the sensor, pixels
	8		public double py0=968.0;           // center of the lens on the sensor, pixels
	 		public double distortionRadius=  2.8512; // mm - half width of the sensor
Andrey Filippov's avatar
Andrey Filippov committed
2200 2201 2202
	9		public double distortionA8=0.0; // r^8 (normalized to focal length or to sensor half width?)
	10		public double distortionA7=0.0; // r^7 (normalized to focal length or to sensor half width?)
	11		public double distortionA6=0.0; // r^6 (normalized to focal length or to sensor half width?)
2203 2204 2205 2206
	12		public double distortionA5=0.0; // r^5 (normalized to focal length or to sensor half width?)
	13		public double distortionA=0.0; // r^4 (normalized to focal length or to sensor half width?)
	14		public double distortionB=0.0; // r^3
	15		public double distortionC=0.0; // r^2
2207 2208 2209 2210 2211 2212 2213 2214 2215 2216 2217 2218 2219 2220 2221 2222 2223 2224 2225 2226 2227 2228 2229 2230 2231 2232 2233
    16          Distortion center shift X for r^3"
    17          Distortion center shift Y for r^3"
    18          Distortion center shift X for r^4"
    19          Distortion center shift Y for r^4"
    20          Distortion center shift X for r^5"
    21          Distortion center shift Y for r^5"
    22          Distortion center shift X for r^6"
    23          Distortion center shift Y for r^6"
    24          Distortion center shift X for r^7"
    25          Distortion center shift Y for r^7"
    26          Distortion center shift X for r^8"
    27          Distortion center shift Y for r^8"
    28          Orthogonal elongation for r^2"
    29          Diagonal   elongation for r^2"
    30          Orthogonal elongation for r^3"
    31          Diagonal   elongation for r^3"
    32          Orthogonal elongation for r^4"
    33          Diagonal   elongation for r^4"
    34          Orthogonal elongation for r^5"
    35          Diagonal   elongation for r^5"
    36          Orthogonal elongation for r^6"
    37          Diagonal   elongation for r^6"
    38          Orthogonal elongation for r^7"
    39          Diagonal   elongation for r^7"
    40          Orthogonal elongation for r^8"
    41          Diagonal   elongation for r^8"
 */
2234
//	        interParameterDerivatives=new double[getNumInputs()][]; //partial derivative matrix from subcamera-camera-goniometer to single camera (12x21)
2235 2236 2237

	//calculate dMA_azimuth
	//calculate dMB_azimuth
2238 2239 2240 2241 2242 2243 2244 2245 2246 2247 2248 2249 2250 2251 2252 2253 2254 2255 2256 2257
	/*
	// MA=R8*R7*R6*R5*R4*R3*R2*R1;
	// MB=T3+(R8*R7*R6*R5*(T2+R4*T1)); - old
	// MB=T3+(R8*R7*R6*R5*(T2+R4*(T1+R3*R2*R1*T0)));
	        Matrix MA=R8.times(R7.times(R6.times(R5.times(R4.times(R3.times(R2.times(R1)))))));
	        Matrix MB=T3.plus(R8.times(R7.times(R6.times(R5.times(T2.plus(R4.times(T1)))))));  old
	        Matrix MB=T3.plus(R8.times(R7.times(R6.times(R5.times(T2.plus(R4.times(T1.plus(R3.times(R2.times(R1.times(T0)))) )))))));
	3) rotate by -(azimuth+phi) around C2Y:Vc3= R3*Vc2
	| Xc3 |   | cos(azimuth+phi)    0   sin(azimuth+phi)   |   |Xc2|
	| Yc3 | = |     0               1         0            | * |Yc2|
	| Zc3 |   | -sin(azimuth+phi)   0   cos(azimuth+phi)   |   |Zc2|
	    	double [][] aR3={{cAZP,0.0,sAZP},{0.0,1.0,0.0},{-sAZP,0.0,cAZP}};
	    	Matrix R3=new Matrix(aR3);
	4) Now axes are aligned, just translate to get to eyesis coordinates: Vey= T1+Vc3
	| Xey |   |  r * sin (azimuth)  |   |Xc3|
	| Yey | = |             height  | + |Yc3|
	| Zey |   |  r * cos (azimuth)  |   |Zc3|
	    	double [][] aT1={{subCam.radius*sAZ},{subCam.height},{subCam.radius*cAZ}};
	    	Matrix T1=new Matrix(aT1);

2258
	        Matrix
2259 2260 2261 2262 2263 2264 2265 2266 2267 2268 2269 2270 2271 2272
	// Make a function MA, MB - >parameters (column) - reuse it above and for each    interParameterDerivatives row
	Matrix dMA_azimuth=R8*R7*R6*R5*R4*dR3_azimuth*R2*R1;
	Matrix dMB_azimuth=T3+(R8*R7*R6*R5*(R4*dT1_azimuth));

	Use extrinsicParams=parametersFromMAMB(dMA_azimuth,dMB_azimuth);
		       		if (this.debugLevel>3) {
		    			System.out.println("calculateFxAndJacobian->calcPartialDerivatives("+IJ.d2s(targetXYZ[fullIndex][0],2)+","+
		    					IJ.d2s(targetXYZ[fullIndex][1],2)+","+
		    					IJ.d2s(targetXYZ[fullIndex][2],2)+" ("+calcJacobian+") -> "+
		    					IJ.d2s(derivatives14[0][0],2)+"/"+IJ.d2s(derivatives14[0][1],2));
		    		}

	Which parameters affect which matrices
	                                               R1 | R2 | R3 | R4 | R5 | R6 | R7 | R8 || T0 | T1 | T2 | T3 |
2273 2274 2275 2276
	0    	public double azimuth_cyl;          //    |    | +  |    |    |    |    |    ||    | +  |    |    |
	1    	public double radius_cyl;           //    |    |    |    |    |    |    |    ||    | +  |    |    |
	0    	public double right_cart;           //    |    |    |    |    |    |    |    ||    | +  |    |    |
	1    	public double forward_cart;         //    |    |    |    |    |    |    |    ||    | +  |    |    |
2277
	2   	public double height;               //    |    |    |    |    |    |    |    ||    | +  |    |    |
2278 2279
	3   	public double phi_cyl;              //    |    | +  |    |    |    |    |    ||    |    |    |    |
	3   	public double head_cart;            //    |    | +  |    |    |    |    |    ||    |    |    |    |
2280 2281 2282 2283 2284 2285 2286 2287 2288 2289 2290 2291 2292 2293 2294 2295 2296 2297 2298 2299 2300
	4   	public double theta;                //    | +  |    |    |    |    |    |    ||    |    |    |    |
	5   	public double psi;                  // +  |    |    |    |    |    |    |    ||    |    |    |    |
	6   	public double goniometerHorizontal; //    |    |    |    |    | +  |    |    ||    |    |    |    |
	7   	public double goniometerAxial;      //    |    |    | +  |    |    |    |    ||    |    |    |    |
	8   	public double interAxisDistance;    //    |    |    |    |    |    |    |    ||    |    | +  |    |
	9    	public double interAxisAngle;       //    |    |    |    | +  |    |    |    ||    |    |    |    |
	10   	public double horAxisErrPhi;        //    |    |    |    |    |    |    | +  ||    |    |    |    |
	11   	public double horAxisErrPsi;        //    |    |    |    |    |    | +  |    ||    |    |    |    |
	12      public double entrancePupilForward  //    |    |    |    |    |    |    |    || +  |    |    |    |
	13      public double centerAboveHorizontal //    |    |    |    |    |    |    |    ||    | +  |    |    |
	14  x	public double [] GXYZ=null;         //    |    |    |    |    |    |    |    ||    |    |    | +  |
	15  y                                       //    |    |    |    |    |    |    |    ||    |    |    | +  |
	16  z                                       //    |    |    |    |    |    |    |    ||    |    |    | +  |

	 */
	// Below can be optimized with common intermediate results
	   		if (this.debugLevel>2) {
	   			for (int i=0;i<parVect.length;i++){
	   				System.out.println("calcInterParamers(): parVect["+i+"]="+parVect[i]);
	   			}
	   		}
2301
	//0    	public double right_cart; // right displacement of the lens entrance pupil center, mm
2302
	        if (mask[0]) {
2303 2304 2305 2306 2307 2308 2309 2310 2311 2312 2313 2314 2315 2316 2317 2318 2319 2320 2321 2322 2323 2324 2325 2326 2327 2328 2329 2330 2331 2332 2333 2334
	        	if (cartesian) {
	        		double [][] adT1_right_cart={{1.0},{0.0},{0.0}};
	        		Matrix dT1_right_cart=new Matrix(adT1_right_cart);
		        	Matrix dMA_right_cart=new Matrix(3,3,0.0); // zero
	        		Matrix dMB_right_cart=R8.times(R7.times(R6.times(R5.times(R4.times(dT1_right_cart)))));
		        	interParameterDerivatives[0]=d_parametersFromMAMB(dMA_right_cart,dMB_right_cart,MA,MB,false);
		    		if (this.debugLevel>2) {
		    			System.out.println("dMA_right_cart:");
		    			dMA_right_cart.print(10, 5);
		    			System.out.println("dMB_right_cart:");
		    			dMB_right_cart.print(10, 5);
		    			System.out.println("interParameterDerivatives[0]="+sprintfArray(interParameterDerivatives[0]));
		    		}
	        	} else {
    //0    	public double azimuth; // azimuth of the lens entrance pupil center, degrees, clockwise looking from top
	        		double [][] adR3_azimuth_cyl={{-sAZP,0.0,cAZP},{0.0,0.0,0.0},{-cAZP,0.0,-sAZP}};
	        		Matrix dR3_azimuth_cyl=new Matrix(adR3_azimuth_cyl);
	        		double [][] adT1_azimuth_cyl={{radius_cyl*cAZ_cyl},{0.0},{-radius_cyl*sAZ_cyl}}; //{{subCam.radius*cAZ},{subCam.height},{-subCam.radius*sAZ}}
	        		Matrix dT1_azimuth_cyl=new Matrix(adT1_azimuth_cyl);

	        		Matrix dMA_azimuth_cyl=R8.times(R7.times(R6.times(R5.times(R4.times(dR3_azimuth_cyl.times(R2.times(R1)))))));
	        		Matrix dMB0_azimuth_cyl=R8.times(R7.times(R6.times(R5.times(R4.times(dT1_azimuth_cyl)))));
	        		Matrix dMB_azimuth_cyl=dMB0_azimuth_cyl.plus(dMA_azimuth_cyl.times(T0)); // new term
	        		interParameterDerivatives[0]=d_parametersFromMAMB(dMA_azimuth_cyl,dMB_azimuth_cyl,MA,MB,true); // all after 6 are 0;
	        		if (this.debugLevel>2) {
	        			System.out.println("dMA_azimuth_cyl:");
	        			dMA_azimuth_cyl.print(10, 5);
	        			System.out.println("dMB_azimuth_cyl:");
	        			dMB_azimuth_cyl.print(10, 5);
	        			System.out.println("interParameterDerivatives[0]="+sprintfArray(interParameterDerivatives[0]));
	        		}
	        	}
2335 2336 2337
	        } else interParameterDerivatives[0]=null;
	//1    	public double radius;  // mm, distance from the rotation axis
	        if (mask[1]) {
2338 2339 2340 2341 2342 2343 2344 2345 2346 2347 2348 2349 2350 2351 2352 2353 2354 2355 2356 2357 2358 2359 2360 2361 2362 2363 2364
	        	if (cartesian) {
	        		double [][] adT1_forward_cart={{0.0},{0.0},{1.0}}; //{{subCam.radius*sAZ},{0.0},{subCam.radius*cAZ}}
	        		Matrix dT1_forward_cart=new Matrix(adT1_forward_cart);
	        		Matrix dMA_forward_cart=new Matrix(3,3,0.0);
	        		Matrix dMB_forward_cart=R8.times(R7.times(R6.times(R5.times(R4.times(dT1_forward_cart)))));
	        		interParameterDerivatives[1]=d_parametersFromMAMB(dMA_forward_cart,dMB_forward_cart,MA,MB,false); // all after 6 are 0;
	        		if (this.debugLevel>2) {
	        			System.out.println("dMA_forward_cart:");
	        			dMA_forward_cart.print(10, 5);
	        			System.out.println("dMB_forward_cart:");
	        			dMB_forward_cart.print(10, 5);
	        			System.out.println("interParameterDerivatives[1]="+sprintfArray(interParameterDerivatives[1]));
	        		}
	        	} else {
	        		double [][] adT1_radius_cyl={{sAZ_cyl},{0.0},{cAZ_cyl}}; //{{subCam.radius*sAZ},{0.0},{subCam.radius*cAZ}}
	        		Matrix dT1_radius_cyl=new Matrix(adT1_radius_cyl);
	        		Matrix dMA_radius_cyl=new Matrix(3,3,0.0);
	        		Matrix dMB_radius_cyl=R8.times(R7.times(R6.times(R5.times(R4.times(dT1_radius_cyl)))));
	        		interParameterDerivatives[1]=d_parametersFromMAMB(dMA_radius_cyl,dMB_radius_cyl,MA,MB,false); // all after 6 are 0;
	        		if (this.debugLevel>2) {
	        			System.out.println("dMA_radius_cyl:");
	        			dMA_radius_cyl.print(10, 5);
	        			System.out.println("dMB_radius_cyl:");
	        			dMB_radius_cyl.print(10, 5);
	        			System.out.println("interParameterDerivatives[1]="+sprintfArray(interParameterDerivatives[1]));
	        		}
	        	}
2365
	        } else interParameterDerivatives[1]=null;
2366 2367 2368 2369 2370 2371 2372 2373 2374 2375 2376 2377 2378 2379
	//2   	public double height;       // mm, downwards - from the origin point
	        if (mask[2]) {
	        	double [][] adT1_height={{0.0},{1.0},{0.0}};
	        	Matrix dT1_height=new Matrix(adT1_height);
	        	Matrix dMA_height=new Matrix(3,3,0.0);
	        	Matrix dMB_height=R8.times(R7.times(R6.times(R5.times(R4.times(dT1_height)))));
	        	interParameterDerivatives[2]=d_parametersFromMAMB(dMA_height,dMB_height,MA,MB,false); // all after 6 are 0;
	    		if (this.debugLevel>2) {
	    			System.out.println("dMA_height:");
	    			dMA_height.print(10, 5);
	    			System.out.println("dMB_height:");
	    			dMB_height.print(10, 5);
	    			System.out.println("interParameterDerivatives[2]="+sprintfArray(interParameterDerivatives[2]));
	    		}
2380
	        } else interParameterDerivatives[2]=null;
2381
	//3   	public double phi;     // degrees, optical axis from azimuth/r vector, clockwise
2382
	        if (mask[3]) { // here the same for cartesian
2383 2384 2385 2386 2387 2388 2389 2390 2391 2392 2393 2394 2395
	        	double [][] adR3_phi={{-sAZP,0.0,cAZP},{0.0,0.0,0.0},{-cAZP,0.0,-sAZP}}; // same as adR3_azimuth
	        	Matrix dR3_phi=new Matrix(adR3_phi); // same as dR3_azimuth
	        	Matrix dMA_phi=R8.times(R7.times(R6.times(R5.times(R4.times(dR3_phi.times(R2.times(R1))))))); //same as dMA_azimuth
//	        	Matrix dMB_phi=new Matrix(3,1,0.0);
	        	Matrix dMB_phi=dMA_phi.times(T0); // new term
	        	interParameterDerivatives[3]=d_parametersFromMAMB(dMA_phi,dMB_phi,MA,MB,true); // all after 6 are 0;
	    		if (this.debugLevel>2) {
	    			System.out.println("dMA_phi:");
	    			dMA_phi.print(10, 5);
	    			System.out.println("dMB_phi:");
	    			dMB_phi.print(10, 5);
	    			System.out.println("interParameterDerivatives[3]="+sprintfArray(interParameterDerivatives[3]));
	    		}
2396
	        } else interParameterDerivatives[3]=null;
2397 2398 2399 2400 2401 2402 2403 2404 2405 2406 2407 2408 2409 2410 2411
	//4   	public double theta;   // degrees, optical axis from the eyesis horizon, positive - up
	        if (mask[4]) {
	        	double [][] adR2_theta={{0.0,0.0,0.0},{0.0,-sTH,cTH},{0.0,-cTH,-sTH}};
	        	Matrix dR2_theta=new Matrix(adR2_theta);
	        	Matrix dMA_theta=R8.times(R7.times(R6.times(R5.times(R4.times(R3.times(dR2_theta.times(R1)))))));
//	        	Matrix dMB_theta=new Matrix(3,1,0.0);
	        	Matrix dMB_theta=dMA_theta.times(T0); // new term
	        	interParameterDerivatives[4]=d_parametersFromMAMB(dMA_theta,dMB_theta,MA,MB,true); // all after 6 are 0;
	    		if (this.debugLevel>2) {
	    			System.out.println("dMA_theta:");
	    			dMA_theta.print(10, 5);
	    			System.out.println("dMB_theta:");
	    			dMB_theta.print(10, 5);
	    			System.out.println("interParameterDerivatives[4]="+sprintfArray(interParameterDerivatives[4]));
	    		}
2412
	        } else interParameterDerivatives[4]=null;
2413 2414 2415 2416 2417 2418 2419 2420 2421
	//5   	public double psi;     // degrees, rotation (of the sensor) around the optical axis. Positive if camera is rotated clockwise looking to the target
	        if (mask[5]) {
	        	double [][] adR1_psi={{-sPS,cPS,0.0},{-cPS,-sPS,0.0},{0.0,0.0,0.0}};
	        	Matrix dR1_psi=new Matrix(adR1_psi);
	        	Matrix dMA_psi=R8.times(R7.times(R6.times(R5.times(R4.times(R3.times(R2.times(dR1_psi)))))));
//	        	Matrix dMB_psi=new Matrix(3,1,0.0);
	        	Matrix dMB_psi=dMA_psi.times(T0); // new term
	        	interParameterDerivatives[5]=d_parametersFromMAMB(dMA_psi,dMB_psi,MA,MB,true); // all after 6 are 0;
	    		if (this.debugLevel>2) {
2422
	/*
2423 2424 2425 2426 2427 2428 2429 2430 2431 2432 2433 2434 2435 2436
	    			System.out.print("R1:");
	    			R1.print(10, 5);
	    			System.out.print("dR1_psi:");
	    			dR1_psi.print(10, 5);
	    			Matrix R82_psi=R8.times(R7.times(R6.times(R5.times(R4.times(R3.times(R2))))));
	    			System.out.print("R82_psi:");
	    			R82_psi.print(10, 5);
	    			*/
	    			System.out.print("dMA_psi:");
	    			dMA_psi.print(10, 5);
	    			System.out.print("dMB_psi:");
	    			dMB_psi.print(10, 5);
	    			System.out.println("interParameterDerivatives[5]="+sprintfArray(interParameterDerivatives[5]));
	    		}
2437
	        } else interParameterDerivatives[5]=null;
2438 2439 2440 2441 2442 2443 2444 2445 2446 2447 2448 2449 2450 2451 2452 2453
	//6   	public double goniometerHorizontal; // goniometer rotation around "horizontal" axis (tilting from the target - positive)
	        if (mask[6]) {
	/* define for isTripod */
	        	double [][] adR6_goniometerHorizontal_tripod=    {{-sGH,0.0,cGH},{0.0, 0.0,0.0},{-cGH, 0.0,-sGH}};
	        	double [][] adR6_goniometerHorizontal_goniometer={{ 0.0,0.0,0.0},{0.0,-sGH,cGH},{ 0.0,-cGH,-sGH}};
	        	Matrix dR6_goniometerHorizontal=new Matrix(isTripod?adR6_goniometerHorizontal_tripod:adR6_goniometerHorizontal_goniometer);
	        	Matrix dMA_goniometerHorizontal=R8.times(R7.times(dR6_goniometerHorizontal.times(R5.times(R4.times(R3.times(R2.times(R1)))))));
	        	Matrix dMB_goniometerHorizontal=R8.times(R7.times(dR6_goniometerHorizontal.times(R5.times(T2.plus(R4.times(T1))))));
	        	interParameterDerivatives[6]=d_parametersFromMAMB(dMA_goniometerHorizontal,dMB_goniometerHorizontal,MA,MB,true); // all after 6 are 0;
	    		if (this.debugLevel>2) {
	    			System.out.println("dMA_goniometerHorizontal:");
	    			dMA_goniometerHorizontal.print(10, 5);
	    			System.out.println("dMB_goniometerHorizontal:");
	    			dMB_goniometerHorizontal.print(10, 5);
	    			System.out.println("interParameterDerivatives[6]="+sprintfArray(interParameterDerivatives[6]));
	    		}
2454 2455
	        } else interParameterDerivatives[6]=null;
	//7   	public double goniometerAxial; // goniometer rotation around Eyesis axis (clockwise in plan - positive
2456 2457 2458 2459 2460 2461 2462 2463 2464 2465 2466 2467 2468 2469 2470
	        if (mask[7]) {
	// define for isTripod
	        	double [][] adR4_goniometerAxial_tripod=    {{ 0.0,0.0,0.0},{0.0,-sGA,cGA},{ 0.0,-cGA,-sGA}};
	        	double [][] adR4_goniometerAxial_goniometer={{-sGA,0.0,cGA},{0.0, 0.0,0.0},{-cGA, 0.0,-sGA}};
	        	Matrix dR4_goniometerAxial=new Matrix(isTripod?adR4_goniometerAxial_tripod:adR4_goniometerAxial_goniometer);
	        	Matrix dMA_goniometerAxial=R8.times(R7.times(R6.times(R5.times(dR4_goniometerAxial.times(R3.times(R2.times(R1)))))));
	        	Matrix dMB_goniometerAxial=R8.times(R7.times(R6.times(R5.times(dR4_goniometerAxial.times(T1)))));
	        	interParameterDerivatives[7]=d_parametersFromMAMB(dMA_goniometerAxial,dMB_goniometerAxial,MA,MB,true); // all after 6 are 0;
	    		if (this.debugLevel>2) {
	    			System.out.println("dMA_goniometerAxial:");
	    			dMA_goniometerAxial.print(10, 5);
	    			System.out.println("dMB_goniometerAxial:");
	    			dMB_goniometerAxial.print(10, 5);
	    			System.out.println("interParameterDerivatives[7]="+sprintfArray(interParameterDerivatives[7]));
	    		}
2471
	        } else interParameterDerivatives[7]=null;
2472 2473 2474 2475 2476 2477 2478 2479 2480 2481 2482 2483 2484 2485
	//8   	public double interAxisDistance; // distance in mm between two goniometer axes
	        if (mask[8]) {
	        	double [][] adT2_interAxisDistance={{0.0},{0.0},{1.0}};
	        	Matrix dT2_interAxisDistance=new Matrix(adT2_interAxisDistance);
	        	Matrix dMA_interAxisDistance=new Matrix(3,3,0.0);
	        	Matrix dMB_interAxisDistance=R8.times(R7.times(R6.times(R5.times(dT2_interAxisDistance))));
	        	interParameterDerivatives[8]=d_parametersFromMAMB(dMA_interAxisDistance,dMB_interAxisDistance,MA,MB,false); // all after 6 are 0;
	    		if (this.debugLevel>2) {
	    			System.out.println("dMA_interAxisDistance:");
	    			dMA_interAxisDistance.print(10, 5);
	    			System.out.println("dMB_interAxisDistance:");
	    			dMB_interAxisDistance.print(10, 5);
	    			System.out.println("interParameterDerivatives[8]="+sprintfArray(interParameterDerivatives[8]));
	    		}
2486
	        } else interParameterDerivatives[8]=null;
2487 2488 2489 2490 2491
	//9    	public double interAxisAngle;    // angle in degrees between two goniometer axes minus 90. negative if "vertical" axis is rotated
	        if (mask[9]) {
	        	double [][] adR5_interAxisAngle={{-sGIAA,cGIAA,0.0},{-cGIAA,-sGIAA,0.0},{0.0,0.0,0.0}};
	        	Matrix dR5_interAxisAngle=new Matrix(adR5_interAxisAngle);
	        	Matrix dMA_interAxisAngle=R8.times(R7.times(R6.times(dR5_interAxisAngle.times(R4.times(R3.times(R2.times(R1)))))));
2492
	        	Matrix dMB_interAxisAngle=R8.times(R7.times(R6.times(dR5_interAxisAngle.times(T2.plus(R4.times(T1))))));
2493 2494 2495 2496 2497 2498 2499 2500
	        	interParameterDerivatives[9]=d_parametersFromMAMB(dMA_interAxisAngle,dMB_interAxisAngle,MA,MB,true); // all after 6 are 0;
	    		if (this.debugLevel>2) {
	    			System.out.println("dMA_interAxisAngle:");
	    			dMA_interAxisAngle.print(10, 5);
	    			System.out.println("dMB_interAxisAngle:");
	    			dMB_interAxisAngle.print(10, 5);
	    			System.out.println("interParameterDerivatives[9]="+sprintfArray(interParameterDerivatives[9]));
	    		}
2501
	        } else interParameterDerivatives[9]=null;
2502
	//10   	public double horAxisErrPhi;   // angle in degrees "horizontal" goniometer axis is rotated around target Y axis from target X axis (CW)
2503 2504

	// change sHAEPH to rotate like theta /1 0 0 /0 c s/ 0 -s c/ (derivative -/0 0 0 /0 -s c/ 0 -c -s/
2505 2506 2507 2508 2509 2510 2511 2512 2513 2514 2515 2516 2517 2518
	        if (mask[10]) {
	        	double [][] adR8_horAxisErrPhi_tripod=    {{0.0,   0.0,    0.0},{0.0,-sHAEPH,cHAEPH},{    0.0,-cHAEPH,-sHAEPH}};
	        	double [][] adR8_horAxisErrPhi_goniometer={{-sHAEPH,0.0,cHAEPH},{0.0,    0.0,   0.0},{-cHAEPH,    0.0,-sHAEPH}};
	        	Matrix dR8_horAxisErrPhi=new Matrix(isTripod?adR8_horAxisErrPhi_tripod:adR8_horAxisErrPhi_goniometer);
	        	Matrix dMA_horAxisErrPhi=dR8_horAxisErrPhi.times(R7.times(R6.times(R5.times(R4.times(R3.times(R2.times(R1)))))));
	        	Matrix dMB_horAxisErrPhi=dR8_horAxisErrPhi.times(R7.times(R6.times(R5.times(T2.plus(R4.times(T1))))));
	        	interParameterDerivatives[10]=d_parametersFromMAMB(dMA_horAxisErrPhi,dMB_horAxisErrPhi,MA,MB,true); // all after 6 are 0;
	    		if (this.debugLevel>2) {
	    			System.out.println("dMA_horAxisErrPhi:");
	    			dMA_horAxisErrPhi.print(10, 5);
	    			System.out.println("dMB_horAxisErrPhi:");
	    			dMB_horAxisErrPhi.print(10, 5);
	    			System.out.println("interParameterDerivatives[10]="+sprintfArray(interParameterDerivatives[10]));
	    		}
2519
	        } else interParameterDerivatives[10]=null;
2520 2521 2522 2523 2524 2525 2526 2527 2528 2529 2530 2531 2532 2533
	//11   	public double horAxisErrPsi;   // angle in degrees "horizontal" goniometer axis is rotated around moving X axis (up)
	        if (mask[11]) {
	        	double [][] adR7_horAxisErrPsi={{-sHAEPS,cHAEPS,0.0},{-cHAEPS,-sHAEPS,0.0},{0.0,0.0,0.0}};
	        	Matrix dR7_horAxisErrPsi=new Matrix(adR7_horAxisErrPsi);
	        	Matrix dMA_horAxisErrPsi=R8.times(dR7_horAxisErrPsi.times(R6.times(R5.times(R4.times(R3.times(R2.times(R1)))))));
	        	Matrix dMB_horAxisErrPsi=R8.times(dR7_horAxisErrPsi.times(R6.times(R5.times(T2.plus(R4.times(T1))))));
	        	interParameterDerivatives[11]=d_parametersFromMAMB(dMA_horAxisErrPsi,dMB_horAxisErrPsi,MA,MB,true); // all after 6 are 0;
	    		if (this.debugLevel>2) {
	    			System.out.println("dMA_horAxisErrPsi:");
	    			dMA_horAxisErrPsi.print(10, 5);
	    			System.out.println("dMB_horAxisErrPsi:");
	    			dMB_horAxisErrPsi.print(10, 5);
	    			System.out.println("interParameterDerivatives[11]="+sprintfArray(interParameterDerivatives[11]));
	    		}
2534
	        } else interParameterDerivatives[11]=null;
2535 2536 2537 2538 2539 2540 2541 2542 2543 2544 2545 2546 2547 2548 2549 2550 2551
//	                                          // R1 | R2 | R3 | R4 | R5 | R6 | R7 | R8 || T0 | T1 | T2 | T3 |
	//12   	public double entrancePupilForward  //    |    |    |    |    |    |    |    || +  |    |    |    |
	//13   	public double centerAboveHorizontal //    |    |    |    |    |    |    |    ||    | +  |    |    |

	        if (mask[12]) {
	        	double [][] adT0_entrancePupilForward={{0.0},{0.0},{1.0}};
	        	Matrix dT0_entrancePupilForward=new Matrix(adT0_entrancePupilForward);
	        	Matrix dMA_entrancePupilForward=new Matrix(3,3,0.0);
	        	Matrix dMB_entrancePupilForward=MA.times(dT0_entrancePupilForward);
	        	interParameterDerivatives[12]=d_parametersFromMAMB(dMA_entrancePupilForward,dMB_entrancePupilForward,MA,MB,false); // all after 6 are 0;
	    		if (this.debugLevel>2) {
	    			System.out.println("dMA_centerAboveHorizontal:");
	    			dMA_entrancePupilForward.print(10, 5);
	    			System.out.println("dMB_centerAboveHorizontal:");
	    			dMB_entrancePupilForward.print(10, 5);
	    			System.out.println("interParameterDerivatives[12]="+sprintfArray(interParameterDerivatives[12]));
	    		}
2552 2553
	        } else interParameterDerivatives[12]=null;

2554 2555 2556 2557 2558 2559 2560 2561 2562 2563 2564 2565 2566
	        if (mask[13]) {
	        	double [][] adT1_centerAboveHorizontal={{0.0},{1.0},{0.0}};
	        	Matrix dT1_centerAboveHorizontal=new Matrix(adT1_centerAboveHorizontal);
	        	Matrix dMA_centerAboveHorizontal=new Matrix(3,3,0.0);
	        	Matrix dMB_centerAboveHorizontal=R8.times(R7.times(R6.times(R5.times(R4.times(dT1_centerAboveHorizontal)))));
	        	interParameterDerivatives[13]=d_parametersFromMAMB(dMA_centerAboveHorizontal,dMB_centerAboveHorizontal,MA,MB,false); // all after 6 are 0;
	    		if (this.debugLevel>2) {
	    			System.out.println("dMA_centerAboveHorizontal:");
	    			dMA_centerAboveHorizontal.print(10, 5);
	    			System.out.println("dMB_centerAboveHorizontal:");
	    			dMB_centerAboveHorizontal.print(10, 5);
	    			System.out.println("interParameterDerivatives[13]="+sprintfArray(interParameterDerivatives[13]));
	    		}
2567 2568 2569
	        } else interParameterDerivatives[13]=null;


2570 2571 2572 2573 2574 2575 2576 2577 2578 2579 2580 2581 2582
	//14  x	public double [] GXYZ=null; // coordinates (in mm) of the goniometer horizontal axis closest to the moving one in target system
	        if (mask[14]) {
	        	double [][] adT3_GXYZ0={{1.0},{0.0},{0.0}};
	        	Matrix dMA_GXYZ0=new Matrix(3,3,0.0);
	        	Matrix dMB_GXYZ0=new Matrix(adT3_GXYZ0);
	        	interParameterDerivatives[14]=d_parametersFromMAMB(dMA_GXYZ0,dMB_GXYZ0,MA,MB,false); // all after 6 are 0;
	        	if (this.debugLevel>2) {
	        		System.out.println("dMA_GXYZ0:");
	        		dMA_GXYZ0.print(10, 5);
	        		System.out.println("dMA_GXYZ0:");
	        		dMB_GXYZ0.print(10, 5);
	        		System.out.println("interParameterDerivatives[14]="+sprintfArray(interParameterDerivatives[14]));
	        	}
2583
	        } else interParameterDerivatives[14]=null;
2584 2585 2586 2587 2588 2589 2590 2591 2592 2593 2594 2595 2596 2597 2598
	//13  y
	        if (mask[15]) {
//	    	double [][] adT3_GXYZ1={{0.0},{1.0},{0.0}};
	        	double [][] adT3_GXYZ1={{0.0},{-1.0},{0.0}}; // up - positive
//	        	double [][] adT3_GXYZ1={{0.0},{ 1.0},{0.0}}; // up - positive
	        	Matrix dMA_GXYZ1=new Matrix(3,3,0.0);
	        	Matrix dMB_GXYZ1=new Matrix(adT3_GXYZ1);
	        	interParameterDerivatives[15]=d_parametersFromMAMB(dMA_GXYZ1,dMB_GXYZ1,MA,MB,false); // all after 6 are 0;
	        	if (this.debugLevel>2) {
	        		System.out.println("dMA_GXYZ1:");
	        		dMA_GXYZ1.print(10, 5);
	        		System.out.println("dMB_GXYZ1:");
	        		dMB_GXYZ1.print(10, 5);
	        		System.out.println("interParameterDerivatives[15]="+sprintfArray(interParameterDerivatives[15]));
	        	}
2599
	        } else interParameterDerivatives[15]=null;
2600 2601 2602 2603 2604 2605 2606 2607 2608 2609 2610 2611 2612 2613
	//14  z
	        if (mask[16]) {
//	    	double [][] adT3_GXYZ2={{0.0},{0.0},{1.0}};
	        	double [][] adT3_GXYZ2={{0.0},{0.0},{-1.0}}; // away - positive
	        	Matrix dMA_GXYZ2=new Matrix(3,3,0.0);
	        	Matrix dMB_GXYZ2=new Matrix(adT3_GXYZ2);
	        	interParameterDerivatives[16]=d_parametersFromMAMB(dMA_GXYZ2,dMB_GXYZ2,MA,MB,false); // all after 6 are 0;
	        	if (this.debugLevel>2) {
	        		System.out.println("dMA_GXYZ2:");
	        		dMA_GXYZ2.print(10, 5);
	        		System.out.println("dMB_GXYZ2:");
	        		dMB_GXYZ2.print(10, 5);
	        		System.out.println("interParameterDerivatives[16]="+sprintfArray(interParameterDerivatives[16]));
	        	}
2614
	        } else interParameterDerivatives[16]=null;
2615 2616 2617 2618 2619 2620 2621 2622 2623
	// now fill the rest, unchanged parameters
	/*
	    int numInputs=22;   //was 21  parameters in subcamera+...
	    int numOutputs=13;  //was 12  parameters in a single camera

	 */
	//17 (15)		public double focalLength=4.5;
	//18 (16)		public double px0=1296.0;          // center of the lens on the sensor, pixels
	//19 (17)		public double py0=968.0;           // center of the lens on the sensor, pixels
Andrey Filippov's avatar
Andrey Filippov committed
2624 2625 2626
	//20 (18)		public double distortionA8=0.0; // r^8 (normalized to focal length or to sensor half width?)
	//21 (19)		public double distortionA7=0.0; // r^7 (normalized to focal length or to sensor half width?)
	//22 (20)		public double distortionA6=0.0; // r^6 (normalized to focal length or to sensor half width?)
2627 2628 2629 2630
	//23 (21)		public double distortionA5=0.0; // r^5 (normalized to focal length or to sensor half width?)
	//24 (22)		public double distortionA=0.0; // r^4 (normalized to focal length or to sensor half width?)
	//25 (23)		public double distortionB=0.0; // r^3
	//26 (24)		public double distortionC=0.0; // r^2
2631
	//27..52 - non-radial parameters
2632 2633

//	        for (int inpPar=15; inpPar<getNumInputs(); inpPar++){
2634
	        for (int inpPar=17; inpPar<getNumInputs(); inpPar++){ // OK with A8. Should be OK with non-radial parameters ((27..52) also
2635 2636 2637 2638 2639 2640 2641 2642 2643 2644 2645
	        	if (mask[inpPar]){
	        		interParameterDerivatives[inpPar]=new double[getNumOutputs()];
	        		for (int outPar=0; outPar<getNumOutputs(); outPar++){
	        			interParameterDerivatives[inpPar][outPar]=((getNumOutputs()-outPar)==(getNumInputs()-inpPar))?1.0:0.0;
	            		if (this.debugLevel>2) {
	            			System.out.println("interParameterDerivatives["+inpPar+"]["+outPar+"]="+interParameterDerivatives[inpPar][outPar]);
	            		}
	        		}
	        	} else interParameterDerivatives[inpPar]=null;
	        }
	    }
Andrey Filippov's avatar
Andrey Filippov committed
2646

2647

2648 2649 2650 2651 2652 2653 2654 2655 2656 2657 2658 2659 2660 2661 2662 2663 2664 2665 2666 2667 2668 2669 2670 2671 2672 2673 2674 2675 2676 2677 2678 2679 2680 2681 2682 2683 2684 2685 2686 2687 2688 2689 2690 2691 2692 2693 2694 2695 2696 2697 2698 2699 2700 2701 2702
	     public double [] parametersFromMAMB(Matrix MA, Matrix MB){
	    	double [] result= new double [getNumOutputs()]; // only first 6 are used
	    	for (int i=6; i<getNumOutputs();i++) result[i]=0.0;
	/*
	0		public double x0=0;      // lens axis from pattern center, mm (to the right)
	1		public double y0=0;      // lens axis from pattern center, mm (down)
	2		public double distance=2360; // distance from the lens input pupil to the pattern plane along the camera axis, mm
	3		public double yaw=0.0;   // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - clockwise from top
	4		public double pitch=0.0; // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - up
	5		public double roll=0.0;  // angle in degrees rotation around camera optical axis (perpendicular to pattern if yaw==0, pitch==0), positive - clockwise

	        Vt=MB+MA*Vc
	        calculate X0,Y0 (Z0==0), dist, phi, theta, psi from MB and MA
	        | MA00 MA01 MA02 |
	        | MA10 MA11 MA12 |
	        | MA20 MA21 MA22 |
	        ===================
	        | MB0 |
	        | MB1 |
	        | MB2 |
	        Take point Pc on a sub-camera axis and on the target Zt=0 plane: [0,0,dist]
	        MA*Pc+MB= dist*[MA02,MA12,MA22]+MB =[Tx0,Ty0,0]
	        MA02*dist+MB0=Tx0
	        MA12*dist+MB1=Ty0
	        MA22*dist+MB2=0
	        dist=-MB2/MA22;
	        Tx0=MA02*(-MB2/MA22)+MB0
	        Ty0=MA12*(-MB2/MA22)+MB1
	        Tx0=MB0-MB2*MA02/MA22
	        Ty0=MB1-MB2*MA12/MA22
	        */
	    	result[2]=-MB.get(2,0)/MA.get(2,2); // distance
	    	result[0]= MB.get(0,0)-MB.get(2,0)*MA.get(0,2)/MA.get(2,2); // x0
	    	result[1]= MB.get(1,0)-MB.get(2,0)*MA.get(1,2)/MA.get(2,2); // y0
	        /*
	        // now find phi, theta, psi
	        MA - rotation from camera to target, transp(MA) - rotation from target to camera - same as rot(phi, theta,psi)
	        MT=transp(MA),
	        */
	        /*
	        MA[1,2]= sin(theta)
	        MA[0,2]= cos(theta)*sin(phi)
	        MA[2,2]= cos(theta)*cos(phi)
	        MA[1,0]=-cos(theta)*sin(psi)
	        MA[1,1]= cos(theta)*cos(psi)
	        theta=arcsin(MA[1,2])
	        phi=  atan2(MA[0,2],MA[2,2])
	        psi= -atan2(MA[1,0],MA[1,1])
	         */
	        result[4]=180.0/Math.PI* Math.asin(MA.get(1, 2));  //pitch
	        result[3]=  180.0/Math.PI* Math.atan2(MA.get(0,2),MA.get(2, 2));//yaw
	        result[5]=-180.0/Math.PI* Math.atan2(MA.get(1, 0),MA.get(1, 1));//roll
	    	return result;
	    }
	    /**
2703
	     *
2704 2705 2706 2707 2708 2709 2710 2711
	     * @param d_MA differential of the rotational matrix MA by some parameter
	     * @param d_MB differential of the translational matrix MB by some parameter
	     * @param MA - rotation matrix
	     * @param MB - translation matrix
	     * @param isAngle - when true, the partial derivative is for angles, d_MA, d_MB should be divided by 180/pi
	     * @return differentials of the {x0,y0,dist,phi,theta,psi} by that parameter
	     */
	    public double [] d_parametersFromMAMB(Matrix d_MA, Matrix d_MB, Matrix MA, Matrix MB, boolean isAngle){
2712 2713 2714
	    	double [] result= new double [getNumOutputs()]; // only first 6 are used, rest are 0
	    	Arrays.fill(result,6, result.length, 0.0);
//	    	for (int i=6; i<getNumOutputs();i++) result[i]=0.0;
2715 2716 2717 2718 2719 2720 2721 2722 2723 2724 2725 2726 2727 2728 2729 2730 2731 2732 2733 2734 2735 2736 2737 2738 2739 2740 2741 2742 2743 2744 2745 2746 2747 2748 2749 2750 2751 2752 2753 2754 2755 2756 2757 2758 2759 2760 2761 2762 2763 2764 2765 2766 2767 2768 2769 2770 2771 2772 2773 2774 2775 2776 2777 2778 2779 2780 2781 2782 2783 2784 2785 2786 2787 2788 2789 2790 2791 2792 2793 2794 2795 2796 2797 2798 2799 2800 2801 2802 2803 2804 2805 2806 2807 2808 2809
	/*
	0		public double x0=0;      // lens axis from pattern center, mm (to the right)
	1		public double y0=0;      // lens axis from pattern center, mm (down)
	2		public double distance=2360; // distance from the lens input pupil to the pattern plane along the camera axis, mm
	3		public double yaw=0.0;   // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - clockwise from top
	4		public double pitch=0.0; // angle in degrees from perpendicular to the pattern, 0 - towards wall, positive - up
	5		public double roll=0.0;  // angle in degrees rotation around camera optical axis (perpendicular to pattern if yaw==0, pitch==0), positive - clockwise

	        Vt=MB+MA*Vc
	        calculate X0,Y0 (Z0==0), dist, phi, theta, psi from MB and MA
	        | MA00 MA01 MA02 |
	        | MA10 MA11 MA12 |
	        | MA20 MA21 MA22 |
	        ===================
	        | MB0 |
	        | MB1 |
	        | MB2 |
	        Take point Pc on a sub-camera axis and on the target Zt=0 plane: [0,0,dist]
	        MA*Pc+MB= dist*[MA02,MA12,MA22]+MB =[Tx0,Ty0,0]
	        MA02*dist+MB0=Tx0
	        MA12*dist+MB1=Ty0
	        MA22*dist+MB2=0
	        dist=-MB2/MA22;
	        Tx0=MB0-MB2*MA02/MA22
	        Ty0=MB1-MB2*MA12/MA22
	        */
//	    	result[2]=-MB.get(2,0)/MA.get(2,2); // distance
	/*
	        d_dist=-(d_MB2/MA22 - d_MA22*MB2/(MA22^2) ;
	 */
	    	double K=isAngle?(Math.PI/180):1.0;
	    	result[2]=K*(-d_MB.get(2,0)/MA.get(2,2)+
	    	d_MA.get(2,2)*MB.get(2,0)/(MA.get(2,2)*MA.get(2,2))); // d_distance
	    	/*
	        Tx0=MB0-MB2*MA02/MA22
	        d_Tx0=d_MB0 -  d_MB2*MA02/MA22 -d_MA02*MB2/MA22 +d_MA22*MB2*MA02/(MA22^2)
	    	 */
	    	result[0]= K*(d_MB.get(0,0) -
	    	d_MB.get(2,0)*MA.get(0,2)/MA.get(2,2) -
	    	d_MA.get(0,2)*MB.get(2,0)/MA.get(2,2) +
	    	d_MA.get(2,2)*MB.get(2,0)*MA.get(0,2)/(MA.get(2,2)*MA.get(2,2))); // x0

	    	/*
	        Ty0=MB0-MB2*MA02/MA22
	        d_Ty0=d_MB1 -  d_MB2*MA12/MA22 -d_MA12*MB2/MA22 +d_MA22*MB2*MA12/(MA22^2)
	    	 */
	    	result[1]= K*(d_MB.get(1,0) -
	    	d_MB.get(2,0)*MA.get(1,2)/MA.get(2,2) -
	    	d_MA.get(1,2)*MB.get(2,0)/MA.get(2,2) +
	    	d_MA.get(2,2)*MB.get(2,0)*MA.get(1,2)/(MA.get(2,2)*MA.get(2,2))); // y0
	        /*
	        // now find phi, theta, psi
	        MA - rotation from camera to target, transp(MA) - rotation from target to camera - same as rot(phi, theta,psi)
	        MT=transp(MA),
	        */
	        /*
	        MA[1,2]= sin(theta)
	        MA[0,2]= cos(theta)*sin(phi)
	        MA[2,2]= cos(theta)*cos(phi)
	        MA[1,0]=-cos(theta)*sin(psi)
	        MA[1,1]= cos(theta)*cos(psi)
	        theta=arcsin(MA[1,2])
	        phi=  atan2(MA[0,2],MA[2,2])
	        psi= -atan2(MA[1,0],MA[1,1])
	        arcsin(x)'= 1/sqrt(1-x^2)
	        arccos(x)'=-1/sqrt(1-x^2)
	        arctan(x)'= 1/sqrt(1+x^2)

	         */
	/*
	        theta=arcsin(MA12)
	        d_theta=d_MA12/sqrt(1-MA12^2)
	 */
	        result[4]=K*d_MA.get(1, 2)*180.0/Math.PI/Math.sqrt(1.0-MA.get(1, 2)*MA.get(1, 2));  //pitch
	/*
	        phi=  atan2(MA02,MA22)
	        d_phi=(d_MA02*MA22-d_MA22*MA02) / (MA22^2+MA02^2)
	 */

	        result[3]=  K*180.0/Math.PI*(d_MA.get(0,2)*MA.get(2,2) - d_MA.get(2,2)*MA.get(0,2)) /
	          ((MA.get(2,2)*MA.get(2,2)) + (MA.get(0,2)*MA.get(0,2)));//yaw
	/*
	        psi=  -atan2(MA10,MA11)
	        d_psi=-(d_MA10*MA11-d_MA11*MA10) / (MA10^2+MA11^2)
	*/
	        result[5]=  -K* 180.0/Math.PI*(d_MA.get(1,0)*MA.get(1,1) - d_MA.get(1,1)*MA.get(1,0)) /
	          ((MA.get(1,1)*MA.get(1,1)) + (MA.get(1,0)*MA.get(1,0)));//roll
	    	return result;
	    }

	    public String sprintfArray(double []arr){
	    	String result="";
	    	for (int i=0;i<arr.length;i++) result+= ((i>0)?", ":"")+arr[i];
	    	return "["+result+"]";
	    }
2810

2811 2812 2813
		public int getNumInputs(){return numInputs;}
		public int getNumOutputs(){return numOutputs;}
	}