Commit a675f808 authored by Andrey Filippov's avatar Andrey Filippov
Browse files

debugging inconsistent pose fitting

parent e2a311fc

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

+172 −540

File changed.

Preview size limit exceeded, changes collapsed.

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

+0 −19
Original line number Diff line number Diff line
@@ -68,12 +68,6 @@ public class IntersceneMatchParameters { 
	public double [] pimu_gyr_offs = {-0.000360, -0.001173, -0.001418}; 

	public double [] pimu_acc_offs = {0, 0, 0}; // camera x,y,z, (raw_ins_vxyz/corrected_vxyz -1)  

	

	

	@Deprecated

	public  double  [] ims_scale_xyz =   {1.0,1.0,1.0}; // {1.1, 1.1, 1.1};

	@Deprecated

	public  double  [] ims_scale_atr =   {1.0,1.0,1.0}; // {1.1, 1.1, 1.1};

		

	public  boolean fmg_initial_en =     true; // enable IMS-based FPN mitigation for initial orientation

	public  boolean fmg_reorient_en =    true; // enable IMS-based FPN mitigation for readjustmnet orientation

	public  double  fmg_distance =       10.0; // try to find other reference scene not closer than this pixels
@@ -1385,11 +1379,6 @@ min_str_neib_fpn 0.35 
		gd.addStringField ("IMU accelerators gain",  IntersceneMatchParameters.doublesToString(pimu_acc_offs), 80,

				"IMU accelerators gain difference from 1. Negative values when IMU-reported velocities are smaller than actual");

		

		gd.addStringField ("scale XYZ from IMS", IntersceneMatchParameters.doublesToString(ims_scale_xyz), 80,

				"Scales for X, Y, and Z velocities when converting from IMS data, such as '1.1 1.1 1.1'.");

		gd.addStringField ("scale ATR from IMS", IntersceneMatchParameters.doublesToString(ims_scale_atr), 80,

				"Scales for Azimuth, Tilt, and Roll velocities when converting from IMS data, such as '1.1 1.1 1.1' (not needed).");

		

		gd.addMessage  ("IMS-based FPN mitigation for interscene matching");

		gd.addCheckbox ("FPN mitigation for initial orientation",   this.fmg_initial_en,

				"Enable IMS-based FPN mitigation for initial orientation.");
@@ -3396,8 +3385,6 @@ min_str_neib_fpn 0.35 
		this.ims_mount_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);

		this.pimu_gyr_offs = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);

		this.pimu_acc_offs = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);

		this.ims_scale_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);

		this.ims_scale_atr = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);

		this.fmg_initial_en =                 gd.getNextBoolean();

		this.fmg_reorient_en =                gd.getNextBoolean();

		this.fmg_distance =                   gd.getNextNumber();
@@ -4582,8 +4569,6 @@ min_str_neib_fpn 0.35 
		properties.setProperty(prefix+"ims_mount_xyz",                 IntersceneMatchParameters.doublesToString(this.ims_mount_xyz));

		properties.setProperty(prefix+"pimu_gyr_offs",                 IntersceneMatchParameters.doublesToString(this.pimu_gyr_offs));

		properties.setProperty(prefix+"pimu_acc_offs",                 IntersceneMatchParameters.doublesToString(this.pimu_acc_offs));

		properties.setProperty(prefix+"ims_scale_xyz",                 IntersceneMatchParameters.doublesToString(this.ims_scale_xyz));

		properties.setProperty(prefix+"ims_scale_atr",                 IntersceneMatchParameters.doublesToString(this.ims_scale_atr));		

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

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

		properties.setProperty(prefix+"fmg_distance",                  this.fmg_distance+"");                  // double
@@ -5710,8 +5695,6 @@ min_str_neib_fpn 0.35 
		if (properties.getProperty(prefix+"ims_mount_xyz")!=null) 	   this.ims_mount_xyz= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_mount_xyz"),3);

		if (properties.getProperty(prefix+"pimu_gyr_offs")!=null) 	   this.pimu_gyr_offs= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_gyr_offs"),3);

		if (properties.getProperty(prefix+"pimu_acc_offs")!=null) 	   this.pimu_acc_offs= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_acc_offs"),3);

		if (properties.getProperty(prefix+"ims_scale_xyz")!=null) 	   this.ims_scale_xyz= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_scale_xyz"),3);

		if (properties.getProperty(prefix+"ims_scale_atr")!=null) 	   this.ims_scale_atr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_scale_atr"),3);

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

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

		if (properties.getProperty(prefix+"fmg_distance")!=null)       this.fmg_distance=Double.parseDouble(properties.getProperty(prefix+"fmg_distance"));
@@ -6870,8 +6853,6 @@ min_str_neib_fpn 0.35 
 		imp.ims_mount_xyz                 = this.ims_mount_xyz.clone();

 		imp.pimu_gyr_offs                 = this.pimu_gyr_offs.clone();

 		imp.pimu_acc_offs                 = this.pimu_acc_offs.clone();

 		imp.ims_scale_xyz                 = this.ims_scale_xyz.clone();

 		imp.ims_scale_atr                 = this.ims_scale_atr.clone();

 		imp.fmg_initial_en                = this.fmg_initial_en;

 		imp.fmg_reorient_en               = this.fmg_reorient_en;

 		imp.fmg_distance                  = this.fmg_distance;

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

+40 −858

File changed.

Preview size limit exceeded, changes collapsed.

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

+1 −1
Original line number Diff line number Diff line
@@ -6138,7 +6138,7 @@ if (debugLevel < -100) { 
			

			return quadCLT;

		}

		public QuadCLT spawnNoModelQuadCLT(

		public QuadCLT spawnNoModelQuadCLT( // copies quat_corr from this

				String              set_name,

				CLTParameters       clt_parameters,

				ColorProcParameters colorProcParameters, //

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

+9 −25
Original line number Diff line number Diff line
@@ -224,9 +224,6 @@ public class QuadCLTCPU { 
    public String      ims_last_path =    null;

    public double []   quat_corr =        null; // correction for IMS camera frame to actual camera frame (for reference frames)

    public double []   enu_corr_metric =  null; // GNSS correction - add metrix offset to GNSS of the reference scene

@Deprecated

    public double [][] pimu_offsets =     new double[2][3]; // linear and angular velocities offsets to DID_PIMU outputs (subtract from IMU data)

//    public boolean     quat_corr_active = false; // correction for IMS camera frame to actual camera frame (for reference frames)

   

    // latest (or any? first also?) scene in a sequence has a timestamp of the reference scene  

    public String      timestamp_reference = null;
@@ -1665,11 +1662,11 @@ public class QuadCLTCPU { 
    			prev_scene = nscene;

    		}

    	}

    	if (debugLevel>0) {

    	if (debugLevel>0) { // null pointer at com.elphel.imagej.tileprocessor.QuadCLTCPU.integratePIMU(QuadCLTCPU.java:1676)

    		System.out.println(String.format(

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

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

        	for (int nscene = 0; nscene < quadCLTs.length; nscene++) if (quadCLTs[nscene] != null) {

        	for (int nscene = 0; nscene < quadCLTs.length; nscene++) if ((quadCLTs[nscene] != null) &&  (xyzatr[nscene] != null)){

           		System.out.println(String.format(

        				"%3d\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f",

        				nscene,
@@ -1724,12 +1721,12 @@ public class QuadCLTCPU { 
    	for (int nscene = 0; nscene < quadCLTs.length; nscene++) if (quadCLTs[nscene] != null) {

    		QuadCLT scene = quadCLTs[nscene];

    		Did_ins_2 d2 = scene.did_ins_2;

    		double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla); // East-North-Up

    		double [] cam_quat_enu =Imx5.quaternionImsToCam(

    		double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla); // East-North-Up May have inacurate altitude 

    		double [] cam_quat_enu =Imx5.quaternionImsToCam( //per scene rotation from enu (from lla) to camera 

    				d2.getQEnu(),

    				ims_mount_atr,

    				ims_ortho);

    		if (quat_corr != null) {

    		if (quat_corr != null) { // with correction if exists

    			cam_quat_enu=Imx5.applyQuaternionToQuaternion(quat_corr, cam_quat_enu, false);

    		}			

    		double [] cam_xyz_enu = Imx5.applyQuaternionTo(
@@ -3198,7 +3195,7 @@ public class QuadCLTCPU { 
    	}

    	return geometryCorrection.getNumSensors();

    }

    public QuadCLTCPU(

    public QuadCLTCPU( // copies quat_corr

    		QuadCLTCPU  qParent,

    		String      name

    		){
@@ -6878,12 +6875,6 @@ public class QuadCLTCPU { 
		if (this.enu_corr_metric != null) {

			properties.setProperty(prefix+"enu_corr_metric",  IntersceneMatchParameters.doublesToString(this.enu_corr_metric));		

		}

		

		if (this.pimu_offsets != null) {

			properties.setProperty(prefix+"pimu_offsets_lin",  IntersceneMatchParameters.doublesToString(this.pimu_offsets[0]));

			properties.setProperty(prefix+"pimu_offsets_ang",  IntersceneMatchParameters.doublesToString(this.pimu_offsets[1]));

		}

		

		return properties;

	}










@@ -7065,13 +7056,6 @@ public class QuadCLTCPU {







		if (properties.getProperty(prefix+"enu_corr_metric")!=null) {









	        this.enu_corr_metric= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"enu_corr_metric"),3);









		}









		if (properties.getProperty(prefix+"pimu_offsets_lin")!=null) {









	        this.pimu_offsets[0]= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_offsets_lin"),3);









		}









		if (properties.getProperty(prefix+"pimu_offsets_ang")!=null) {









	        this.pimu_offsets[1]= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"pimu_offsets_ang"),3);









		}

















	









	}