UAS Adding NED correction

src/main/java/com/elphel/imagej/cameras/EyesisCorrectionParameters.java

@@ -116,6 +116,8 @@ public class EyesisCorrectionParameters {
     	public String cuasUasLogs =            "";  // json file path containing UAS logs
     	public double cuasUasTimeStamp =       0.0; // timestamp corresponding to the UAS time 0.0
     	public double [] cuasCameraATR =       {0, 0, 0};
+    	public double [] cuasUASHome =         {0, 0, 0};
+    	public boolean cuasSetHome =           true; // if false - just rely on UAS GPS absolute data
     	
 //    	public String sourceImsDirectory=      ""; // source IMS log files
     	public String imsRootDirectory=        ""; // source IMS log files
@@ -306,8 +308,8 @@ public class EyesisCorrectionParameters {
   			cp.cuasUasLogs =            this.cuasUasLogs;
   			cp.cuasUasTimeStamp =       this.cuasUasTimeStamp;
   			cp.cuasCameraATR =          this.cuasCameraATR.clone();
-  			
-  			
+  			cp.cuasUASHome =            this.cuasUASHome.clone();
+  			cp.cuasSetHome =            this.cuasSetHome;
 ///  			cp.sourceImsDirectory=    	this.sourceImsDirectory;
   			cp.imsRootDirectory =       this.imsRootDirectory;      
   			cp.imsDataSubdirectory =    this.imsDataSubdirectory;
@@ -513,7 +515,8 @@ public class EyesisCorrectionParameters {
     		properties.setProperty(prefix+"useCuasSeedDir",      this.useCuasSeedDir+"");
     		properties.setProperty(prefix+"cuasUasTimeStamp",    this.cuasUasTimeStamp+"");
     		properties.setProperty(prefix+"cuasCameraATR",       IntersceneMatchParameters.doublesToString(this.cuasCameraATR));    		
-    		
+    		properties.setProperty(prefix+"cuasUASHome",         IntersceneMatchParameters.doublesToString(this.cuasUASHome));    		
+    		properties.setProperty(prefix+"cuasSetHome",         this.cuasSetHome+"");
     		
 ///    		properties.setProperty(prefix+"sourceImsDirectory",this.sourceImsDirectory);
     		properties.setProperty(prefix+"imsRootDirectory",    this.imsRootDirectory);
@@ -954,6 +957,8 @@ public class EyesisCorrectionParameters {
             gd.addStringField ("UAS log file",                                     this.cuasUasLogs, 80);
 			gd.addNumericField("UAS log start timestamp",                          this.cuasUasTimeStamp,  0);
             gd.addStringField ("Camera ATR in world coordinates",                  IntersceneMatchParameters.doublesToString(this.cuasCameraATR), 80);
+            gd.addStringField ("UAS home position NED relative to the LWIR16",     IntersceneMatchParameters.doublesToString(this.cuasUASHome), 80);
+    		gd.addCheckbox    ("Use specified NED instead of accurate GPS",        cuasSetHome);
     		
     		
     		gd.addStringField ("Sensor calibration directory",                     this.sensorDirectory, 80);
@@ -1110,7 +1115,10 @@ public class EyesisCorrectionParameters {
     		this.cuasSeedDir=            gd.getNextString();
     		this.cuasUasLogs =           gd.getNextString();
     		this.cuasUasTimeStamp =      gd.getNextNumber();
-    		this.cuasCameraATR =         IntersceneMatchParameters.StringToDoubles(gd.getNextString(), cuasCameraATR);		
+    		this.cuasCameraATR =         IntersceneMatchParameters.StringToDoubles(gd.getNextString(), cuasCameraATR);
+    		this.cuasUASHome =           IntersceneMatchParameters.StringToDoubles(gd.getNextString(), cuasUASHome);		
+    		this.cuasSetHome =           gd.getNextBoolean();
+    		
     		this.sensorDirectory=        gd.getNextString(); if (gd.getNextBoolean()) selectSensorDirectory(false, false);
     		this.sharpKernelDirectory=   gd.getNextString(); if (gd.getNextBoolean()) selectSharpKernelDirectory(false, false);
     		this.smoothKernelDirectory=  gd.getNextString(); if (gd.getNextBoolean()) selectSmoothKernelDirectory(false, true);
@@ -1205,6 +1213,10 @@ public class EyesisCorrectionParameters {
             gd.addStringField ("UAS log file",                                     this.cuasUasLogs, 80);
 			gd.addNumericField("UAS log start timestamp",                          this.cuasUasTimeStamp,  0);
             gd.addStringField ("Camera ATR in world coordinates",                  IntersceneMatchParameters.doublesToString(this.cuasCameraATR), 80);
+            gd.addStringField ("UAS home position NED relative to the LWIR16",     IntersceneMatchParameters.doublesToString(this.cuasUASHome), 80);
+    		gd.addCheckbox    ("Use specified NED instead of accurate GPS",        cuasSetHome);
+            
+            
             
     		gd.addStringField ("x3d model version",                                this.x3dModelVersion, 80);    // 10a
     		gd.addStringField ("jp4 source copy subdirectory",                     this.jp4SubDir, 80);          // 10b
@@ -1397,6 +1409,8 @@ public class EyesisCorrectionParameters {
     		this.cuasUasLogs =           gd.getNextString();
     		this.cuasUasTimeStamp =      gd.getNextNumber();
     		this.cuasCameraATR =         IntersceneMatchParameters.StringToDoubles(gd.getNextString(), cuasCameraATR);		
+    		this.cuasUASHome =           IntersceneMatchParameters.StringToDoubles(gd.getNextString(), cuasUASHome);		
+    		this.cuasSetHome =           gd.getNextBoolean();
     		
     		
     		this.x3dModelVersion=        gd.getNextString(); //  10a
@@ -1790,6 +1804,8 @@ public class EyesisCorrectionParameters {
             			this.cuasUasLogs =     dir_string; // dir_path.toString();
             			System.out.println("this.cuasUasLogs=" + this.cuasUasLogs);
             			ArrayList<String> uas_pars = extra_map.get("uasLogs");
+            			cuasUASHome = new double[3];
+            			cuasSetHome = uas_pars.size() > 4;
             			if (uas_pars != null) {
             				for (int n = 0; n < uas_pars.size(); n++) {
             					double d = Double.parseDouble(uas_pars.get(n));
@@ -1798,7 +1814,13 @@ public class EyesisCorrectionParameters {
             					case 1:
             					case 2:
             					case 3:
-            						cuasCameraATR[n-1] = d; 
+            						cuasCameraATR[n-1] = d;
+            						break;
+            					case 4:
+            					case 5:
+            					case 6:
+            						cuasUASHome[n-4] = d;
+            						break;
             					}
             				}
             			}

src/main/java/com/elphel/imagej/ims/UasLogReader.java

@@ -21,14 +21,38 @@ import java.util.HashMap;
 public class UasLogReader {
 	// just for testing
 	public static double [] CAMERA_LLA = {40.36068739259896, -112.02054681495696, 1507.4378478095987};
-
+//	public double [] uas_home_ned = null; // use if non-null; if null - rely on accurate GPS (camera GPS relative to the UAS GPS) 
+	public double [] ned_corr = new double[3]; // add to calculated NED.  
 	UasLogRecord[] rec_arr;
 	double         start_timestamp;
 	double         uas_home_alt = -1.2; // relative to the camera, table + mount height
 	double []      camera_lla = CAMERA_LLA.clone();
-	double []      camera_ATR = new double[3];
+	double []      camera_ATR = new double [] {-2.78, 0.1252, 0}; //  double[3];
 	QuadCLT        parentCLT = null; // for geometic parameters
 
+	
+	public void setUASHomeNed(double [] uas_home_ned){
+		// get NED for the UAS home
+		if (uas_home_ned == null) {
+			ned_corr = new double[] {0,0,0}; // -uas_home_alt};
+		} else {
+			ned_corr = uas_home_ned.clone();
+			/*
+			double [] camera_lla0 = {camera_lla[0], camera_lla[1], 0};
+			UasLogRecord uas_home = this.rec_arr[0];
+			double [] uas_home_lla = {uas_home.home_lat, uas_home.home_long, 0};
+			double [] ned = Imx5.nedFromLla (
+					uas_home_lla,      //  double [] lla,
+					camera_lla0); // double [] lla_ref)
+			ned_corr = new double [3];
+			for (int i = 0; i < 3; i++) {
+				ned_corr[i] = 	uas_home_ned[i] - ned[i];
+			}
+			*/
+		}
+		return;
+	}
+	
 	public void setCameraLLA(double [] camera_lla){
 		if (camera_lla != null) {
 			this.camera_lla = camera_lla.clone();
@@ -125,9 +149,13 @@ public class UasLogReader {
 		UasLogRecord uas_loc = interpolate(timestamp);
 		double [] camera_lla0 = {camera_lla[0], camera_lla[1], -uas_home_alt};
 		double [] uas_lla = {uas_loc.gps_lat, uas_loc.gps_long, uas_loc.gps_alt}; 
-		return Imx5.nedFromLla (
+		double [] ned = Imx5.nedFromLla (
 				uas_lla,      //  double [] lla,
 				camera_lla0); // double [] lla_ref)
+		for (int i = 0; i < 3; i++) {
+			ned[i] += ned_corr[i];
+		}
+		return ned;
 	}
 	
 /*
@@ -220,7 +248,7 @@ public class UasLogReader {
 			return null;
 		}
 		double [] ned = getNED(timestamp);
-		double [] wxyz4 = {-ned[1], -ned[2], -ned[0], 1.0}; // 0.0 for infinity
+		double [] wxyz4 = { ned[1], -ned[2], -ned[0], 1.0}; // 0.0 for infinity
 		double [] atr = getCameraATR();
 		boolean correctDistortions = true;
 		ErsCorrection ec = parentCLT.getErsCorrection();
@@ -238,7 +266,7 @@ public class UasLogReader {
 			) throws JSONException {
 		// just for now
 		UasLogReader uasLogReader = new UasLogReader(filePath, 0, null, parentCLT);
-		double [] camera_atr = {degToRad(-161),degToRad(16),0};
+		double [] camera_atr = new double [] {-2.78, 0.1252, 0}; // {degToRad(-161),degToRad(16),0};
 		double ts = 135.0;
 		
 		uasLogReader.testOrient(camera_atr,ts, parentCLT);

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

@@ -7290,6 +7290,8 @@ public class OpticalFlow {
     	String bkp_cuasUasLogs =            quadCLT_main.correctionsParameters.cuasUasLogs;      // json file path containing UAS logs
     	double bkp_cuasUasTimeStamp =       quadCLT_main.correctionsParameters.cuasUasTimeStamp; // 0.0; // timestamp corresponding to the UAS time 0.0
     	double [] bkp_cuasCameraATR =       quadCLT_main.correctionsParameters.cuasCameraATR;    //{0, 0, 0};
+    	double [] bkp_cuasUASHome =       quadCLT_main.correctionsParameters.cuasUASHome;    //{0, 0, 0};
+		boolean bkp_cuasSetHome = quadCLT_main.correctionsParameters.cuasSetHome;
 		
 		
 		
@@ -7311,6 +7313,8 @@ public class OpticalFlow {
 		quadCLT_main.correctionsParameters.cuasUasLogs =      bkp_cuasUasLogs;   
 		quadCLT_main.correctionsParameters.cuasUasTimeStamp = bkp_cuasUasTimeStamp;
 		quadCLT_main.correctionsParameters.cuasCameraATR =    bkp_cuasCameraATR;
+    	quadCLT_main.correctionsParameters.cuasUASHome =      bkp_cuasUASHome;
+		quadCLT_main.correctionsParameters.cuasSetHome =      bkp_cuasSetHome;
 		
 		System.out.println("buildSeries(): DONE"); //
 //		String top_dir0=quadCLTs[ref_index].getX3dTopDirectory();

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

@@ -8684,6 +8684,7 @@ if (debugLevel > -100) return true; // temporarily !
 			if ((uas_log_path != null) && (uas_log_path.length() > 0)) {
 				uasLogReader = new UasLogReader(uas_log_path, quadCLT_main.correctionsParameters.cuasUasTimeStamp, null, quadCLT_main);
 				uasLogReader.setCameraATR(quadCLT_main.correctionsParameters.cuasCameraATR);
+				uasLogReader.setUASHomeNed(quadCLT_main.correctionsParameters.cuasSetHome? quadCLT_main.correctionsParameters.cuasUASHome : null);
 			}
 			while ((ref_index < 0) || ((ref_index + 1) >= min_num_scenes)) {
 				String model_directory = opticalFlow.buildSeries(