Commit 93c00b54 authored by Andrey Filippov's avatar Andrey Filippov
Browse files

unfinished mod

parent 2cb7df5a

src/main/java/com/elphel/imagej/tileprocessor/Interscene.java

+7 −9
Original line number Diff line number Diff line
@@ -27,8 +27,6 @@ import java.util.ArrayList; 
import java.util.Arrays;

import java.util.concurrent.atomic.AtomicInteger;



import org.apache.commons.math3.geometry.euclidean.threed.Rotation;



import com.elphel.imagej.calibration.CalibrationFileManagement;

import com.elphel.imagej.cameras.CLTParameters;

import com.elphel.imagej.cameras.ColorProcParameters;
@@ -5365,14 +5363,14 @@ public class Interscene { 
					enu,

					false);

		}

		double lambda =            0.1;

		double lambda_scale_good = 0.5;

		double lambda_scale_bad =  8.0;

		double lambda_max =      100;

		double rms_diff =          0.001;

		int    num_iter =         20;

		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;

		double      reg_w = 0.25;

		double      reg_w =        clt_parameters.imp.quat_reg_w; // 0.25;

		double []   quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity

		int         debug_level = 1;

		QuaternionLma quaternionLma = new QuaternionLma();

src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java

+58 −0
Original line number Diff line number Diff line
@@ -435,6 +435,15 @@ public class IntersceneMatchParameters { 
    public  boolean readjust_xy_ims =        true;  // false;

    public  double  reg_weight_xy =          0;     // 10.0; // 1.0; // 10.0; // 0.05; // TODO: find out reasonable values

    

    // Quaternion LMA parameters

    public  double  quat_lambda =            0.1;

    public  double  quat_lambda_scale_good = 0.5;

    public  double  quat_lambda_scale_bad =  8.0;

    public  double  quat_lambda_max =      100;

    public  double  quat_rms_diff =          0.001;

    public  int     quat_num_iter =         20;

    public  double  quat_reg_w =             0.25;

	

	public  boolean stereo_merge =       true;

	public  int     stereo_gap =         32;    // pixels between right and left frames

	public  double  stereo_intereye =    63.5;  // mm
@@ -1241,6 +1250,22 @@ public class IntersceneMatchParameters { 
		gd.addNumericField("LMA regularization weight",                this.reg_weight_xy, 5,7,"",

				"Regularization weight for LMA");

		

		gd.addTab("Quaternion LMA","Parameters fitting rotation to two sets of vectors");

		gd.addNumericField("LMA lambda",                                  this.quat_lambda, 6,8,"",

				"Initial value of the LMA lambda");

		gd.addNumericField("Scale lambda after successful LMA iteration", this.quat_lambda_scale_good, 3,5,"",

				"Scale lambda (reduce) if the new RMSE is lower than the previous one.");

		gd.addNumericField("Scale lambda after failed LMA iteration",     this.quat_lambda_scale_bad, 3,5,"",

				"Scale lambda (increase) if the new RMSE is higher than the previous one.");

		gd.addNumericField("Maximal value of lambda to try",              this.quat_lambda_max, 2,7,"",

				"Fail LMA if the result is still worse than before parameters were updates.");

		gd.addNumericField("Minimal relative RMSE improvement",           this.quat_rms_diff, 5,7,"",

				"Exit LMA iterations if relative RMSE improvement drops below this value.");

		gd.addNumericField("Maximal number of LMA iterations",            this.quat_num_iter, 0,3,"",

				"A hard limit on LMA iterations.");

		gd.addNumericField("Regularization weight",                       this.quat_reg_w, 3,5,"",

				"Regularization weight [0..1) weight of q0^2+q1^2+q1^2+q3^2 -1.");

		

		

		gd.addTab("Stereo/Video","Parameters for stereo video generation");

		gd.addMessage  ("Stereo");
@@ -1673,6 +1698,14 @@ public class IntersceneMatchParameters { 
		this.readjust_xy_ims =          gd.getNextBoolean();

		this.reg_weight_xy =            gd.getNextNumber();

		

		this.quat_lambda =              gd.getNextNumber();

		this.quat_lambda_scale_good =   gd.getNextNumber();

		this.quat_lambda_scale_bad =    gd.getNextNumber();

		this.quat_lambda_max =          gd.getNextNumber();

		this.quat_rms_diff =            gd.getNextNumber();

		this.quat_num_iter =      (int) gd.getNextNumber();

		this.quat_reg_w =               gd.getNextNumber();

		

		if (stereo_views.length > 0) {

			int i =                     gd.getNextChoiceIndex();

			if (i > 0) {
@@ -2105,6 +2138,14 @@ public class IntersceneMatchParameters { 
		properties.setProperty(prefix+"readjust_xy_ims",      this.readjust_xy_ims+"");     // boolean

		properties.setProperty(prefix+"reg_weight_xy",        this.reg_weight_xy+"");       // double

		

		properties.setProperty(prefix+"quat_lambda",          this.quat_lambda+"");         // double

		properties.setProperty(prefix+"quat_lambda_scale_good",this.quat_lambda_scale_good+"");// double

		properties.setProperty(prefix+"quat_lambda_scale_bad",this.quat_lambda_scale_bad+"");// double

		properties.setProperty(prefix+"quat_lambda_max",      this.quat_lambda_max+"");     // double

		properties.setProperty(prefix+"quat_rms_diff",        this.quat_rms_diff+"");       // double

		properties.setProperty(prefix+"quat_num_iter",        this.quat_num_iter+"");       // int

		properties.setProperty(prefix+"quat_reg_w",           this.quat_reg_w+"");          // double

		

		properties.setProperty(prefix+"stereo_merge",         this.stereo_merge+"");        // boolean

		properties.setProperty(prefix+"stereo_gap",           this.stereo_gap+"");          // int

		properties.setProperty(prefix+"stereo_intereye",      this.stereo_intereye+"");     // double
@@ -2498,6 +2539,15 @@ public class IntersceneMatchParameters { 
		if (properties.getProperty(prefix+"readjust_xy_ims")!=null)      this.readjust_xy_ims=Boolean.parseBoolean(properties.getProperty(prefix+"readjust_xy_ims"));

		if (properties.getProperty(prefix+"reg_weight_xy")!=null)        this.reg_weight_xy=Double.parseDouble(properties.getProperty(prefix+"reg_weight_xy"));

		

		if (properties.getProperty(prefix+"quat_lambda")!=null)          this.quat_lambda=Double.parseDouble(properties.getProperty(prefix+"quat_lambda"));

		if (properties.getProperty(prefix+"quat_lambda_scale_good")!=null)this.quat_lambda_scale_good=Double.parseDouble(properties.getProperty(prefix+"quat_lambda_scale_good"));

		if (properties.getProperty(prefix+"quat_lambda_scale_bad")!=null)this.quat_lambda_scale_bad=Double.parseDouble(properties.getProperty(prefix+"quat_lambda_scale_bad"));

		if (properties.getProperty(prefix+"quat_lambda_max")!=null)      this.quat_lambda_max=Double.parseDouble(properties.getProperty(prefix+"quat_lambda_max"));

		if (properties.getProperty(prefix+"quat_rms_diff")!=null)        this.quat_rms_diff=Double.parseDouble(properties.getProperty(prefix+"quat_rms_diff"));

		if (properties.getProperty(prefix+"reg_weight_xy")!=null)        this.reg_weight_xy=Double.parseDouble(properties.getProperty(prefix+"reg_weight_xy"));

		if (properties.getProperty(prefix+"quat_num_iter")!=null)        this.quat_num_iter=Integer.parseInt(properties.getProperty(prefix+"quat_num_iter"));

		if (properties.getProperty(prefix+"quat_reg_w")!=null)           this.quat_reg_w=Double.parseDouble(properties.getProperty(prefix+"quat_reg_w"));

		

		if (properties.getProperty(prefix+"stereo_merge")!=null)         this.stereo_merge=Boolean.parseBoolean(properties.getProperty(prefix+"stereo_merge"));

		if (properties.getProperty(prefix+"stereo_gap")!=null)           this.stereo_gap=Integer.parseInt(properties.getProperty(prefix+"stereo_gap"));

		if (properties.getProperty(prefix+"stereo_intereye")!=null)      this.stereo_intereye=Double.parseDouble(properties.getProperty(prefix+"stereo_intereye"));
@@ -2901,6 +2951,14 @@ public class IntersceneMatchParameters { 
		imp.readjust_xy_ims       = this.readjust_xy_ims;

		imp.reg_weight_xy         = this.reg_weight_xy;

		

		imp.quat_lambda           = this.quat_lambda;

		imp.quat_lambda_scale_good= this.quat_lambda_scale_good;

		imp.quat_lambda_scale_bad = this.quat_lambda_scale_bad;

		imp.quat_lambda_max       = this.quat_lambda_max;

		imp.quat_rms_diff         = this.quat_rms_diff;

		imp.quat_num_iter         = this.quat_num_iter;

		imp.quat_reg_w            = this.quat_reg_w;

		

		imp.stereo_merge          = this.stereo_merge;

		imp.stereo_gap            = this.stereo_gap;

		imp.stereo_intereye       = this.stereo_intereye;

src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java

+32 −0
Original line number Diff line number Diff line
@@ -5416,6 +5416,38 @@ public class OpticalFlow { 
				}				

			}

		}

		// later move to the right place

		if ((quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {

			double [][][] pimu_xyzatr = QuadCLT.integratePIMU(

					clt_parameters, // final CLTParameters clt_parameters,

					quadCLTs,       // final QuadCLT[]     quadCLTs,

					ref_index,      // final int           ref_index,

					null,           // double [][][]       dxyzatr,

					earliest_scene, // final int           early_index,

					last_index      //(quadCLTs.length -1) // int           last_index,

		    		);

			double [][][] xyzatr = new double [quadCLTs.length][][];

			ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();

	    	for (int nscene = earliest_scene; nscene <= last_index; nscene++) {

	        	String ts = quadCLTs[nscene].getImageName();

	        	xyzatr[nscene] = ers_ref.getSceneXYZATR(ts);

	    	}

	    	double [] rms = new double[5];

	    	double [] quat = new double[4];

	    	double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(

	    			clt_parameters, // CLTParameters clt_parameters,

	    			quadCLTs,       // QuadCLT[]     quadCLTs,

	    			xyzatr,         // double [][][] xyzatr,

	    			pimu_xyzatr,    // double [][][] ims_xyzatr,

	    			ref_index,      // int           ref_index,

	    			earliest_scene,    // int           early_index,

	    			last_index,     // int           last_index,

	    			rms,            //double []     rms, // null or double[5];

	    			quat,           // double []     quaternion, // null or double[4]       

	    			0); // int           debugLevel

		}

		

		

		if (run_ly) {

			if (debugLevel > -3) {

				System.out.println("**** Running LY adjustments *****");

src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java

+151 −0
Original line number Diff line number Diff line
@@ -221,6 +221,157 @@ public class QuadCLTCPU { 
    	pimu_offsets = offsets;

    }

    

    // find best rotation between IMU XYZ and camera XYZ

    /**

     * Refine scene poses (now only linear) from currently adjusted poses

     * (adjusted using IMS orientations) and IMS positions (currently

     * only linear are used). Refined scene poses may be used as pull targets

     * with free orientations (and angular velocities for ERS).

     * Result poses are {0,0,0} for the reference frame.

     * 

     * Assuming correct IMU angular velocities and offset linear ones.   

     * 

     * @param clt_parameters configuration parameters

     * @param quadCLTs       scenes sequence (for timestamps)

     * @param xyzatr         current scenes [[x,y,z],[a,t,r]] in reference scene frame

     * @param ims_xyzatr     IMU-derived (integrated) scene position and orientation in reference

     *                       scene frame (only position is currently used)

     * @param ref_index      reference scene index 

     * @param early_index    earliest (lowest) scene index to use

     * @param last_index     last (highest) scene index to use

     * @return quaternion    componets 

     */

    

    public static double [] getRotationFromXYZCameraIms(

    		CLTParameters clt_parameters,

    		QuadCLT[]     quadCLTs,

    		double [][][] xyzatr,

    		double [][][] ims_xyzatr,

    		int           ref_index,

    		int           early_index,

    		int           last_index,

			double []     rms, // null or double[5];

			int           debugLevel

    		) {

    	double [][] vect_y = new double [quadCLTs.length][]; // camera XYZ

    	double [][] vect_x = new double [quadCLTs.length][]; // IMS XYZ

    	for (int nscene = early_index; nscene <= last_index; nscene++) {

    		vect_x[nscene] = ims_xyzatr[nscene][0];

    		vect_y[nscene] = xyzatr[nscene][0];

    	}

		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;

		double      reg_w =        clt_parameters.imp.quat_reg_w; // 0.25;

		double []   quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity

		QuaternionLma quaternionLma = new QuaternionLma();

		quaternionLma.prepareLMA(

				vect_x, // quat_lma_xyz,     // double [][] vect_x,

				vect_y,     // double [][] vect_y,

				null,             // double [][] vect_w, all same weight

				reg_w,            // double      reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1  

				quat0,            // double []   quat0,

				debugLevel);     // 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,

				debugLevel);     // 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;

					}					

				}

			}

			return quaternionLma.getQuaternion();

		}

    }

    

    public static double [][][] rotateImsToCameraXYZ(

    		CLTParameters clt_parameters,

    		QuadCLT[]     quadCLTs,

    		double [][][] xyzatr,

    		double [][][] ims_xyzatr,

    		int           ref_index,

    		int           early_index,

    		int           last_index,

			double []     rms, // null or double[5];

			double []     quaternion, // null or double[4]       

			int           debugLevel

    		) {

    	double [] quat = getRotationFromXYZCameraIms(

    			clt_parameters, // CLTParameters clt_parameters,

    			quadCLTs,       // QuadCLT[]     quadCLTs,

    			xyzatr,         // double [][][] xyzatr,

    			ims_xyzatr,     // double [][][] ims_xyzatr,

    			ref_index,      // int           ref_index,

    			early_index,    // int           early_index,

    			last_index,     // int           last_index,

    			rms,            // double []     rms, // null or double[5];

    			debugLevel);    // int           debugLevel

    	if (quat == null) {

    		return null;

    	}

    	if (quaternion != null) {

    		System.arraycopy(quat, 0, quaternion, 0, 4);

    	}

    	Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled

    	double [][][] rotated_xyzatr = new double [quadCLTs.length][][]; // orientation from camera, xyz from rotated IMS

    	double [] rotated_xyz = new double[3];

//    	double [] rotated_atr = new double[3];

    	for (int nscene = early_index; nscene <= last_index; nscene++) {

    		rot.applyTo(ims_xyzatr[nscene][0], rotated_xyz);

    		Rotation scene_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,

    				xyzatr[nscene][1][0],    xyzatr[nscene][1][1],    xyzatr[nscene][1][2]);

    		Rotation rotation_atr = rot.applyTo(scene_atr);

    		rotated_xyzatr[nscene] = new double [][] {rotated_xyz.clone(),

    			rotation_atr.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};

    	}

        if (debugLevel > -1) {

    		System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");

    		System.out.println(String.format("%3s"+

    				"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+

    				"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+

    				"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s",

    				"N","X","Y","Z","A","T","R",

    				"PIMU-X","PIMU-Y","PIMU-Z","PIMU-A","PIMU-T","PIMU-R",

    				"ROT-X","ROT-Y","ROT-Z","ROT-A","ROT-T","ROT-R"));

        	for (int nscene = early_index; nscene <= last_index; nscene++) {

        		System.out.println(String.format("%3s"+

        				"\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f"+

        				"\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f"+

        				"\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f",

        				nscene,

        				xyzatr[nscene][0][0],xyzatr[nscene][0][1],xyzatr[nscene][0][2],

        				xyzatr[nscene][1][0],xyzatr[nscene][1][1],xyzatr[nscene][1][2],

        				ims_xyzatr[nscene][0][0],ims_xyzatr[nscene][0][1],ims_xyzatr[nscene][0][2],

        				ims_xyzatr[nscene][1][0],ims_xyzatr[nscene][1][1],ims_xyzatr[nscene][1][2],

        				rotated_xyzatr[nscene][0][0],rotated_xyzatr[nscene][0][1],rotated_xyzatr[nscene][0][2],

        				rotated_xyzatr[nscene][1][0],rotated_xyzatr[nscene][1][1],rotated_xyzatr[nscene][1][2]));

        	}    	

        }

    	return rotated_xyzatr;

    }

    

    

    /**

     * Refine scene poses (now only linear) from currently adjusted poses

     * (adjusted using IMS orientations) and IMS velocities (currently