Commit 77381e24 authored by Andrey Filippov's avatar Andrey Filippov
Browse files

Unfinished mods

parent 2014ae08
Loading
Loading
Loading
Loading
+13 −0
Original line number Diff line number Diff line
@@ -9,6 +9,7 @@ import java.nio.ByteOrder;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;

import com.elphel.imagej.tileprocessor.ErsCorrection;

@@ -207,6 +208,18 @@ public class Imx5 {
				rslt_rot.getQ3()};
	}

	/**
	 * Get up (for IMS) vector relative to the camera reference frame
	 * @param ims_atr mount to camera correction
	 * @return {x,y,z} of the IMS-up relative to the camera frame (reference scene)
	 */
	public static double [] getUpAxis(
			double [] ims_atr ) {// -> mount_to_cam
		Rotation mount_to_cam = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
				ims_atr[0],    ims_atr[1],    ims_atr[2]);
		return mount_to_cam.applyInverseTo(new Vector3D(0, 0, 1)).toArray();
	}
	
	
	public static double [] quaternionImsToCam(
			double [] quat,      // ims_to_ned
+145 −22
Original line number Diff line number Diff line
@@ -263,7 +263,9 @@ public class Interscene {
					ref_index,//            ref_indx,
					cent_index, // earliest_scene, // int            earliest_scene,
					ego_path, // String         path,
					ego_comment); // String         comment);
					ego_comment, // String         comment);
					debugLevel); // int            debugLevel);

			if (debugLevel> -3) {
				System.out.println("Egomotion table saved to "+ego_path);
			}
@@ -335,7 +337,8 @@ public class Interscene {
					cent_index,//            ref_indx,
					earliest_scene, // int            earliest_scene,
					ego_path, // String         path,
					ego_comment); // String         comment);
					ego_comment, // String         comment);
					debugLevel); // int debugLevel)
			if (debugLevel> -3) {
				System.out.println("Egomotion table saved to "+ego_path);
			}
@@ -370,7 +373,8 @@ public class Interscene {
					cent_index,//            ref_indx,
					earliest_scene, // int            earliest_scene,
					ego_path, // String         path,
					ego_comment); // String         comment);
					ego_comment, // String         comment);
					debugLevel); // int debugLevel)
			if (debugLevel> -3) {
				System.out.println("Egomotion table saved to "+ego_path);
			}
@@ -2043,7 +2047,8 @@ public class Interscene {
        			earliest_scene,  // int            start_scene,
        			last_scene,       // int            end1_scene,
        			scenes_xyzatr,   // double [][][]  scenes_xyzatr,
        			half_run_range); // double         half_run_range
        			half_run_range, // double         half_run_range
					debugLevel); // int            debugLevel);
        	break;
        default: // do nothing - already read
        }
@@ -2084,7 +2089,8 @@ public class Interscene {
            		ref_index,      // int           ref_index,
            		earliest_scene, // int           early_index,
            		last_scene);    // int           last_index)
        } else if (readjust_xy_ims && (reg_weight_xy > 0.0)) {
//        } else if (readjust_xy_ims && (reg_weight_xy > 0.0)) {
        } else if (readjust_xy_ims) {
			double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
					clt_parameters, // final CLTParameters clt_parameters,
					quadCLTs,       // final QuadCLT[]     quadCLTs,
@@ -2095,7 +2101,7 @@ public class Interscene {
		    		);
	    	double [] rms = new double[5];
	    	double [] quat = new double[4];
	    	int quat_lma_mode = 2; // 1; // 2;
	    	int quat_lma_mode = QuaternionLma.MODE_COMBO_LOCAL; // 2; // 1; // 2;
	    	int debug_lev = debugLevel; // 3;
//	        double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters		
	    	double  translation_weight =   1.0 / (avg_z + 1.0);
@@ -5155,7 +5161,8 @@ public class Interscene {
			int            ref_index,
			int            earliest_scene,
			String         path,
			String         comment) {
			String         comment,
			int            debugLevel) {
		double [] ims_ortho =     clt_parameters.imp.ims_ortho;
		double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
		QuadCLT ref_scene = quadCLTs[ref_index];
@@ -5170,8 +5177,18 @@ public class Interscene {
				quadCLTs,       // QuadCLT []     quadCLTs,
				ref_index,      // int            ref_index,
				earliest_scene, // int            earliest_scene,
				rms);           // double []      rms // null or double[2];
	    		
				rms,           // double []      rms // null or double[2];
				debugLevel); // int            debugLevel
		if (debugLevel > -3) {
        	Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
			System.out.println("Applying correction ot the IMS to world orientation (rotating around IMS vertical):");
			double []  corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
			double []  corr_degrees = new double[3];
			for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
			System.out.println("quatCorr=["+quatCorr[0]+", "+quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]");
			System.out.println("ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]");
			System.out.println("ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]");
		}
		
		for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
			QuadCLT scene = quadCLTs[nscene];
@@ -5196,6 +5213,7 @@ public class Interscene {
				"\tINS->X\tINS->Y\tINS->Z"+
				"\tabs_A_ned\tabs_T_ned\tabs_R_ned\trel_A_ned\trel_T_ned\trel_R_ned"+
				"\tabs_A_enu\tabs_T_enu\tabs_R_enu\trel_A_enu\trel_T_enu\trel_R_enu"+
				"\traw_A_enu\traw_T_enu\traw_R_enu"+
				"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
		
		String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
@@ -5216,7 +5234,8 @@ public class Interscene {
					earliest_scene,    // int            start_scene,
					quadCLTs.length-1, // int            end1_scene,
					scenes_xyzatr,     // double [][][]  scenes_xyzatr, // 5.0
					clt_parameters.ofp.lpf_series); // half_run_range); // double         half_run_range
					clt_parameters.ofp.lpf_series, // half_run_range); // double         half_run_range
					debugLevel); // int            debugLevel);
		}
		
				
@@ -5324,7 +5343,7 @@ public class Interscene {
			double [] ned = Imx5.nedFromLla (d2.lla, d2_ref.lla);
			double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
			double [] ims_xyz = Imx5.applyQuaternionTo(double_qn2b, ned, false);
			
//"\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"			
			sb.append("\t"+ned[0]+  "\t"+ned[1]+  "\t"+ned[2]);             // global axes
			sb.append("\t"+ims_xyz[0]+  "\t"+ims_xyz[1]+  "\t"+ims_xyz[2]); // imu axes
			
@@ -5335,6 +5354,9 @@ public class Interscene {
			double [] cam_quat_enu =Imx5.quaternionImsToCam(d2.getQEnu(),
					ims_mount_atr, // new double[] {0, 0.13, 0},
					ims_ortho);
			double [] cam_quat_enu_raw =Imx5.quaternionImsToCam(d2.getQEnu(),
					new double[3], // new double[] {0, 0.13, 0},
					ims_ortho);
			
			double [] cam_xyz1 = Imx5.applyQuaternionTo(cam_quat1, ned, false);
			double [] cam_xyz2 = Imx5.applyQuaternionTo(cam_quat2, ned, false);
@@ -5352,14 +5374,16 @@ public class Interscene {
			double [] quat_lma_enu_xyz = (quatCorr != null) ?
					Imx5.applyQuaternionTo(quatCorr,cam_xyz_enu,false):
						cam_xyz_enu;
			// NED without ims_mount_atr correction
//"\tcam_X1\tcam_Y1\tcam_Z1\tcam_X2\tcam_Y2\tcam_Z2"			
			// NED without ims_mount_atr correction \tcam_X1\tcam_Y1\tcam_Z1
			sb.append("\t"+cam_xyz1[0]+  "\t"+cam_xyz1[1]+  "\t"+cam_xyz1[2]); // 
			// NED with ims_mount_atr correction
			// NED with ims_mount_atr correction \tcam_X2\tcam_Y2\tcam_Z2
			sb.append("\t"+cam_xyz2[0]+  "\t"+cam_xyz2[1]+  "\t"+cam_xyz2[2]); //

			sb.append("\t"+cam_xyz_ned[0]+      "\t"+cam_xyz_ned[1]+      "\t"+cam_xyz_ned[2]); // 
			sb.append("\t"+cam_xyz_enu[0]+      "\t"+cam_xyz_enu[1]+      "\t"+cam_xyz_enu[2]); // WITH ims_mount_atr, NO quatCorr
			sb.append("\t"+quat_lma_enu_xyz[0]+ "\t"+quat_lma_enu_xyz[1]+ "\t"+quat_lma_enu_xyz[2]); // WITH ims_mount_atr, WITH quatCorr
//"\tned->X\tned->Y\tned->Z\tenu->X\tenu->Y\tenu->Z"
			sb.append("\t"+cam_xyz_ned[0]+      "\t"+cam_xyz_ned[1]+      "\t"+cam_xyz_ned[2]); // \tned->X\tned->Y\tned->Z
			sb.append("\t"+cam_xyz_enu[0]+      "\t"+cam_xyz_enu[1]+      "\t"+cam_xyz_enu[2]); // WITH ims_mount_atr, NO quatCorr \tenu->X\tenu->Y\tenu->Z
//"\tINS->X\tINS->Y\tINS->Z"			
			sb.append("\t"+quat_lma_enu_xyz[0]+ "\t"+quat_lma_enu_xyz[1]+ "\t"+quat_lma_enu_xyz[2]); // WITH ims_mount_atr, WITH quatCorr "\tINS->X\tINS->Y\tINS->Z"
			
			double [] scene_abs_atr = Imx5.quatToCamAtr(cam_quat2);
			
@@ -5370,6 +5394,7 @@ public class Interscene {
			
			double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
			double [][] ims_scene_xyzatr_enu = {ZERO3, scene_abs_atr_enu};
			double [] scene_raw_atr_enu = Imx5.quatToCamAtr(cam_quat_enu_raw); // not corrected by ims_mount_atr

			double [] scene_rel_atr_enu=ErsCorrection.combineXYZATR(
					ims_scene_xyzatr_enu,
@@ -5380,6 +5405,8 @@ public class Interscene {
//          "\tabs_A_enu\tabs_T_enu\tabs_R_enu\trel_A_enu\trel_T_enu\trel_R_enu"			
			sb.append("\t"+scene_abs_atr_enu[0]+  "\t"+scene_abs_atr_enu[1]+  "\t"+scene_abs_atr_enu[2]); // 
			sb.append("\t"+scene_rel_atr_enu[0]+  "\t"+scene_rel_atr_enu[1]+  "\t"+scene_rel_atr_enu[2]); //
//			"\traw_A_enu\traw_T_enu\traw_R_enu"+
			sb.append("\t"+scene_raw_atr_enu[0]+  "\t"+scene_raw_atr_enu[1]+  "\t"+scene_raw_atr_enu[2]); //
//			"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv"
			sb.append("\t"+uvw_dir[0]+  "\t"+uvw_dir[1]+  "\t"+uvw_dir[2]); // wrong
			sb.append("\t"+uvw_inv[0]+  "\t"+uvw_inv[1]+  "\t"+uvw_inv[2]); // correct
@@ -5412,9 +5439,20 @@ public class Interscene {
			sb.append("\n");
		}
		// test QuaternionLMA here
		
		
		
		// Add another data
		double [] new_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
		double [] degrees = new double[3];
		for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI;
		sb.append("New ATR mount (rad):[\t"+new_atr[0]+"\t"+new_atr[1]+"\t"+new_atr[2]+"]\n");
		sb.append("New ATR mount (deg):[\t"+degrees[0]+"\t"+degrees[1]+"\t"+degrees[2]+"]\n");
//		double [] quatCorr
    	Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
		double []  corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
		double []  corr_degrees = new double[3];
		for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
		sb.append("quatCorr=[\t"+quatCorr[0]+"\t"+quatCorr[1]+"\t"+quatCorr[2]+"\t"+quatCorr[3]+"]\n");
		sb.append("ATR(rad)=[\t"+corr_angles[0]+"\t"+corr_angles[1]+"\t"+corr_angles[2]+"]\n");
		sb.append("ATR(deg)=[\t"+corr_degrees[0]+"\t"+corr_degrees[1]+"\t"+corr_degrees[2]+"]\n");
		
		if (path!=null) {
			String footer=(comment != null) ? ("Comment: "+comment): "";
@@ -5425,8 +5463,9 @@ public class Interscene {
			new TextWindow("Sharpness History", header, sb.toString(), 1000,900);
		}
    }
 
    public static double [] getQuaternionCorrection(
    @Deprecated
    // adjusts by all 3 axis rotation 
    public static double [] getQuaternionCorrection_Old(
			CLTParameters  clt_parameters,
			QuadCLT []     quadCLTs,
			int            ref_index,
@@ -5504,6 +5543,90 @@ public class Interscene {
		}
    }

    
    public static double [] getQuaternionCorrection(
			CLTParameters  clt_parameters,
			QuadCLT []     quadCLTs,
			int            ref_index,
			int            earliest_scene,
			double []      rms, // null or double[2];
			int            debugLevel
    		) {
		double []     ims_ortho =     clt_parameters.imp.ims_ortho;
		double []     ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
		QuadCLT       ref_scene = quadCLTs[ref_index];
		ErsCorrection ers_reference = ref_scene.getErsCorrection();
		double [][]   quat_lma_xyz =     new double [quadCLTs.length][];
		double [][]   quat_lma_enu_xyz = new double [quadCLTs.length][];
		Did_ins_2     d2_ref = quadCLTs[ref_index].did_ins_2;
		for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
			QuadCLT scene = quadCLTs[nscene];
			if (nscene == ref_index) {
				quat_lma_xyz[nscene] =    new double[3];
			} else {
				String ts = scene.getImageName();
				quat_lma_xyz[nscene] = ers_reference.getSceneXYZ(ts);
			}
			Did_ins_2   d2 = scene.did_ins_2;
			double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
			quat_lma_enu_xyz[nscene] = Imx5.applyQuaternionTo(
					Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[]  quat_enu,
							ims_mount_atr,
							ims_ortho),
					enu,
					false);
		}
		double [] up_axis = Imx5.getUpAxis(
				ims_mount_atr); // double [] ims_atr )
		
		double  lambda =            clt_parameters.imp.quat_lambda; // 0.1;
		double  lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
		double  lambda_scale_bad =  clt_parameters.imp.quat_lambda_scale_bad; // 8.0;
		double  lambda_max =        clt_parameters.imp.quat_lambda_max; // 100;
		double  rms_diff =          clt_parameters.imp.quat_rms_diff; // 0.001;
		int     num_iter =          clt_parameters.imp.quat_num_iter; // 20;
		boolean last_run =         false;
		int     debug_level = debugLevel;
		QuaternionLma quaternionLma = new QuaternionLma();
		quaternionLma.prepareCompassLMA(
				quat_lma_enu_xyz, // quat_lma_xyz,     // double [][] vect_x,
				quat_lma_xyz,     // double [][] vect_y,
				null,             // double [][] vect_w, all same weight
				up_axis,            // double []   vector_up, // Up in the IMS axes nearest to the camera Z (rotated by - ims_mount_atr)
				debug_level);     // int   debug_level)
		int lma_result = quaternionLma.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
				lambda,           // double lambda,           // 0.1
				lambda_scale_good,// double lambda_scale_good,// 0.5
				lambda_scale_bad, // double lambda_scale_bad, // 8.0
				lambda_max,       // double lambda_max,       // 100
				rms_diff,         // double rms_diff,         // 0.001
				num_iter,         // int    num_iter,         // 20
				last_run,         // boolean last_run,
				debug_level);     // int    debug_level)
		if (lma_result < 0) {
			return null;
		} else {
			if (rms != null) { // null or double[2];
				double [] last_rms = quaternionLma.getLastRms();
				rms[0] = last_rms[0];
				rms[1] = last_rms[1];
				if (rms.length >= 4) {
					double [] initial_rms = quaternionLma.getInitialRms();
					rms[2] = initial_rms[0];
					rms[3] = initial_rms[1];
					if (rms.length >= 5) {
						rms[4] = lma_result;
					}					
				}
			}
			if (debugLevel > -3) {
				System.out.println("getQuaternionCorrection(): Rotated around IMS-vertical by "+quaternionLma.getQuaternion()[0]+" rad");
				System.out.println("getQuaternionCorrection(): Rotated around IMS-vertical by "+quaternionLma.getQuaternion()[0]*180/Math.PI+" degrees");
			}
			return quaternionLma.getAxisQuat();
		}
    }
    
    static double [] test_xyz_ned(
    		double [] ned,
			double[]  quat_ned,
+31 −9
Original line number Diff line number Diff line
@@ -5032,7 +5032,9 @@ public class OpticalFlow {
							ref_index,//            ref_indx,
							earliest_scene, // int            earliest_scene,
							ego_path, // String         path,
							ego_comment); // String         comment);
							ego_comment, // String         comment);
							debugLevel); // int            debugLevel);
							
					if (debugLevel> -3) {
						System.out.println("Egomotion table saved to "+ego_path);
					}
@@ -5386,7 +5388,8 @@ public class OpticalFlow {
							ref_index,//            ref_indx,
							earliest_scene, // int            earliest_scene,
							ego_path, // String         path,
							ego_comment); // String         comment);
							ego_comment, // String         comment);
							debugLevel); // int debugLevel)
					if (debugLevel> -3) {
						System.out.println("Egomotion table saved to "+ego_path);
					}
@@ -5438,7 +5441,7 @@ public class OpticalFlow {
	    	}
	    	double [] rms = new double[5];
	    	double [] quat = new double[4];
	    	int quat_lma_mode = 4; // 3; //  2; // 1;
	    	int quat_lma_mode = 03; // 4; // 3; //  2; // 1;
	    	int debug_lev = debugLevel; // 3;
	        double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters		
	    	double  translation_weight =   1.0 / (avg_z + 1.0);
@@ -5583,7 +5586,8 @@ public class OpticalFlow {
					quadCLTs,       // QuadCLT []     quadCLTs,
					ref_index,      // int            ref_index,
					earliest_scene, // int            earliest_scene,
					quat_rms);           // double []      rms // null or double[2];
					quat_rms,           // double []      rms // null or double[2];
					debugLevel); // int            debugLevel
			if (quatCorr != null) {
				int num_iter = (int) quat_rms[4];
				if (debugLevel> -3) {
@@ -5596,6 +5600,16 @@ public class OpticalFlow {
				quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
						null, // String path,             // full name with extension or w/o path to use x3d directory
						debugLevel+1);
				if (debugLevel > -3) {
		        	Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
					System.out.println("Applying correction ot the IMS to world orientation (rotating around IMS vertical):");
					double []  corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
					double []  corr_degrees = new double[3];
					for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
					System.out.println("quatCorr=["+quatCorr[0]+", "+quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]");
					System.out.println("ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]");
					System.out.println("ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]");
				}
			} else {
				if (debugLevel> -3) {
					System.out.println("Failed to perform attitude correction with QuaternionLma.");
@@ -5616,7 +5630,9 @@ public class OpticalFlow {
					ref_index,//            ref_indx,
					earliest_scene, // int            earliest_scene,
					ego_path, // String         path,
					ego_comment); // String         comment);
					ego_comment, // String         comment);
					debugLevel); // int debugLevel)
			if (debugLevel> -3) {
				System.out.println("Egomotion table saved to "+ego_path);
			}
@@ -5628,7 +5644,8 @@ public class OpticalFlow {
		    			ref_index,//            ref_indx,
						earliest_scene, // int            earliest_scene,
		    			ego_path, // String         path,
		    			ego_comment); // String         comment);
		    			ego_comment, // String         comment);
						debugLevel); // int debugLevel)
		    }
		}
		boolean test_ground = false; // true;
@@ -8256,7 +8273,8 @@ public class OpticalFlow {
			int            start_scene,
			int            end_scene,
			double [][][]  scenes_xyzatr, // <=0 use +/-1 or +0 if other are not available
			double         half_run_range
			double         half_run_range,
			int            debugLevel
			){
		double [][][] ers_xyzatr = new double [scenes_xyzatr.length][][];
		if (half_run_range <=0 ) {
@@ -8314,9 +8332,13 @@ public class OpticalFlow {
						for (int m = 0; m < sy.length; m++) {
							for (int d = 0; d < sy[m].length; d++) {
								if (scenes_xyzatr[ns]== null ) {
									if (debugLevel > -1) {
										System.out.println("getVelocitiesFromScenes():scenes_xyzatr["+ns+"]== null");
									}
								}else if (scenes_xyzatr[ns][m]== null ) {
									if (debugLevel > -1) {
										System.out.println("getVelocitiesFromScenes():scenes_xyzatr["+ns+"]["+m+"] == null");
									}
								} else {
									double y = scenes_xyzatr[ns][m][d];
									sy [m][d] += w *  y;
+2 −1
Original line number Diff line number Diff line
@@ -256,6 +256,7 @@ public class QuadCLTCPU {
			double []     rms, // null or double[5];
			int           debugLevel
    		) {
    	final boolean use3 = (quat_lma_mode == 3); // true;
		double lambda =            clt_parameters.imp.quat_lambda; // 0.1;
		double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
		double lambda_scale_bad =  clt_parameters.imp.quat_lambda_scale_bad; // 8.0;
@@ -264,7 +265,7 @@ public class QuadCLTCPU {
		int    num_iter =          clt_parameters.imp.quat_num_iter; // 20;
		boolean last_run =         false;
		double      reg_w =        clt_parameters.imp.quat_reg_w; // 0.25;
		double []   quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity
		double []   quat0 = use3? new double[3] : new double [] {1.0, 0.0, 0.0, 0.0}; // identity
		QuaternionLma quaternionLma = new QuaternionLma();
		if ((quat_lma_mode == 2) || (quat_lma_mode == 4)) {
			double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR
+311 −26

File changed.

Preview size limit exceeded, changes collapsed.