Skip to content

I
imagej-elphel
Commit 2b8ae048 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

Before updating IMS interpolation

parent 726f4add
Expand all Show whitespace changes
Inline Side-by-side
Showing with 1453 additions and 99 deletions
src/main/java/com/elphel/imagej/correction/Eyesis_Correction.java
View file @ 2b8ae048
......@@ -411,6 +411,7 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
public static EyesisCorrections EYESIS_CORRECTIONS = null;
public static EyesisCorrections EYESIS_CORRECTIONS_AUX = null;
public static EyesisCorrectionParameters.EquirectangularParameters EQUIRECTANGULAR_PARAMETERS = new EyesisCorrectionParameters.EquirectangularParameters();
public static EventLogger EVENT_LOGGER = null;
public static int CONVOLVE_FFT_SIZE = 128; // FFT size for sliding convolution with kernel
public static int THREADS_MAX = 100; // testing multi-threading, limit maximal number of threads
......@@ -5523,19 +5524,22 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
DEBUG_LEVEL = MASTER_DEBUG_LEVEL;
EYESIS_CORRECTIONS.setDebug(DEBUG_LEVEL);
CLT_PARAMETERS.batch_run = true;
/*
Imx5 imx5 = new Imx5();
byte [] imx5_rslt = imx5.test_imx();
System.out.println("imx5_rslt.length="+imx5_rslt.length);
*/
// EventLogger eventLogger = new EventLogger ("/home/elphel/lwir16-proc/ims/ims_selected/");
String imx_logs = CORRECTION_PARAMETERS.sourceImsDirectory;
String imx_logs = CORRECTION_PARAMETERS.getImsSourceDirectory(); // sourceImsDirectory;
if (EVENT_LOGGER == null) {
if ((imx_logs != null) && !imx_logs.equals("")) {
EventLogger eventLogger = new EventLogger (imx_logs);
EVENT_LOGGER = new EventLogger (imx_logs);
} else {
System.out.println("sourceImsDirectory is not set. Use Setup CLT Batch parameters to define it.");
}
}
if (EVENT_LOGGER != null) {
EVENT_LOGGER.printPimu(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_pimu");
EVENT_LOGGER.printGps(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_gps", 7);
EVENT_LOGGER.printDidIns1(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_ins1");
EVENT_LOGGER.printDidIns2(CORRECTION_PARAMETERS.getImsPrintDirectory()+Prefs.getFileSeparator()+"did_ins2");
} else {
System.out.println("EVENT_LOGGEER is null.");
}
return;
//JTabbedTest
// End of buttons code
......
src/main/java/com/elphel/imagej/ims/Did_gps_pos.java
View file @ 2b8ae048
......@@ -2,6 +2,9 @@ package com.elphel.imagej.ims;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.Properties;
import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;
/** (DID_GPS1_POS, DID_GPS1_UBX_POS, DID_GPS2_POS) GPS position data */
public class Did_gps_pos {
......@@ -94,10 +97,14 @@ public class Did_gps_pos {
}
public Did_gps_pos() {}
public Did_gps_pos(String prefix, Properties properties) {
getProperties(prefix, properties);
}
public Did_gps_pos interpolate(double frac, Did_gps_pos next_did) {
Did_gps_pos new_did = new Did_gps_pos();
double time_ms_this = timeOfWeekMs + Did_strobe_in_time.WEEK_MS * week;
double time_ms_next = timeOfWeekMs + Did_strobe_in_time.WEEK_MS * week;
double time_ms_next = next_did.timeOfWeekMs + Did_strobe_in_time.WEEK_MS * next_did.week;
long time_ms = (long) Did_ins.interpolateDouble(frac, time_ms_this, time_ms_next);
new_did.week = (int) Math.floor(time_ms / Did_strobe_in_time.WEEK_MS);
new_did.timeOfWeekMs = (int) (time_ms - new_did.week * Did_strobe_in_time.WEEK_MS);
......@@ -161,4 +168,45 @@ public class Did_gps_pos {
return s;
}
public void getProperties(String prefix, Properties properties) {
if (properties.getProperty(prefix+"week")!=null) this.week= Integer.parseInt(properties.getProperty(prefix+"week"));
if (properties.getProperty(prefix+"timeOfWeekMs")!=null) this.timeOfWeekMs= Integer.parseInt(properties.getProperty(prefix+"week"));
if (properties.getProperty(prefix+"status")!=null) this.status= Integer.parseInt(properties.getProperty(prefix+"status"));
if (properties.getProperty(prefix+"ecef")!=null) this.ecef= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ecef"),3);
if (properties.getProperty(prefix+"lla")!=null) this.lla= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"lla"),3);
if (properties.getProperty(prefix+"hMSL")!=null) this.hMSL= Float.parseFloat(properties.getProperty(prefix+"hMSL"));
if (properties.getProperty(prefix+"hAcc")!=null) this.hAcc= Float.parseFloat(properties.getProperty(prefix+"hAcc"));
if (properties.getProperty(prefix+"vAcc")!=null) this.vAcc= Float.parseFloat(properties.getProperty(prefix+"vAcc"));
if (properties.getProperty(prefix+"pDop")!=null) this.pDop= Float.parseFloat(properties.getProperty(prefix+"pDop"));
if (properties.getProperty(prefix+"cnoMean")!=null) this.cnoMean= Float.parseFloat(properties.getProperty(prefix+"cnoMean"));
if (properties.getProperty(prefix+"towOffset")!=null) this.towOffset= Double.parseDouble(properties.getProperty(prefix+"towOffset"));
if (properties.getProperty(prefix+"leapS")!=null) this.leapS= Byte.parseByte(properties.getProperty(prefix+"leapS"));
if (properties.getProperty(prefix+"satsUsed")!=null) this.satsUsed= Byte.parseByte(properties.getProperty(prefix+"satsUsed"));
if (properties.getProperty(prefix+"cnoMeanSigma")!=null) this.cnoMeanSigma= Byte.parseByte(properties.getProperty(prefix+"cnoMeanSigma"));
if (properties.getProperty(prefix+"reserved")!=null) this.reserved= Byte.parseByte(properties.getProperty(prefix+"reserved"));
}
public Properties setProperties(String prefix, Properties properties){ // save // USED in lwir
if (properties == null) {
properties = new Properties();
}
properties.setProperty(prefix+"week", this.week+"");
properties.setProperty(prefix+"timeOfWeekMs", this.timeOfWeekMs+"");
properties.setProperty(prefix+"status", this.status+"");
properties.setProperty(prefix+"ecef", IntersceneMatchParameters.doublesToString(this.ecef));
properties.setProperty(prefix+"lla", IntersceneMatchParameters.doublesToString(this.lla));
properties.setProperty(prefix+"hMSL", this.hMSL+"");
properties.setProperty(prefix+"hAcc", this.hAcc+"");
properties.setProperty(prefix+"vAcc", this.vAcc+"");
properties.setProperty(prefix+"pDop", this.pDop+"");
properties.setProperty(prefix+"cnoMean", this.cnoMean+"");
properties.setProperty(prefix+"towOffset", this.towOffset+"");
properties.setProperty(prefix+"leapS", this.leapS+"");
properties.setProperty(prefix+"satsUsed", this.satsUsed+"");
properties.setProperty(prefix+"cnoMeanSigma", this.cnoMeanSigma+"");
properties.setProperty(prefix+"reserved", this.reserved+"");
return properties;
}
}
\ No newline at end of file
src/main/java/com/elphel/imagej/ims/Did_ins.java
View file @ 2b8ae048
package com.elphel.imagej.ims;
import java.util.Properties;
import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;
public abstract class Did_ins <T extends Did_ins <T>>{
final static int WEEK_SECONDS = 7 * 24 * 3600;
/** GPS number of weeks since January 6th, 1980 */
......@@ -42,4 +46,23 @@ public abstract class Did_ins <T extends Did_ins <T>>{
new_did.lla[i] = interpolateDouble(frac, lla[i], next_did.lla[i]) ;
}
}
public void getProperties(String prefix, Properties properties) {
if (properties.getProperty(prefix+"week")!=null) this.week= Integer.parseInt(properties.getProperty(prefix+"week"));
if (properties.getProperty(prefix+"timeOfWeek")!=null) this.timeOfWeek= Double.parseDouble(properties.getProperty(prefix+"timeOfWeek"));
if (properties.getProperty(prefix+"insStatus")!=null) this.insStatus= Integer.parseInt(properties.getProperty(prefix+"insStatus"));
if (properties.getProperty(prefix+"hdwStatus")!=null) this.hdwStatus= Integer.parseInt(properties.getProperty(prefix+"hdwStatus"));
if (properties.getProperty(prefix+"lla")!=null) this.lla= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"lla"),3);
}
public Properties setProperties(String prefix, Properties properties){ // save // USED in lwir
if (properties == null) {
properties = new Properties();
}
properties.setProperty(prefix+"week", this.week+"");
properties.setProperty(prefix+"timeOfWeek", this.timeOfWeek+"");
properties.setProperty(prefix+"insStatus", this.insStatus+"");
properties.setProperty(prefix+"hdwStatus", this.hdwStatus+"");
properties.setProperty(prefix+"lla", IntersceneMatchParameters.doublesToString(this.lla));
return properties;
}
}
\ No newline at end of file
src/main/java/com/elphel/imagej/ims/Did_ins_1.java
View file @ 2b8ae048
......@@ -3,6 +3,9 @@ package com.elphel.imagej.ims;
//import java.io.Serializable;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.Properties;
import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;
//public class Did_ins_1 implements Serializable {
// static final long serialVersionUID = 4;
......@@ -36,6 +39,10 @@ public class Did_ins_1 extends Did_ins <Did_ins_1>{
ned[0]= bb.getFloat(); ned[1] = bb.getFloat(); ned[2]= bb.getFloat();
}
public Did_ins_1() {}
public Did_ins_1(String prefix, Properties properties) {
getProperties(prefix, properties);
}
public Did_ins_1 interpolate(double frac, Did_ins_1 next_did) {
Did_ins_1 new_did = new Did_ins_1();
......@@ -76,5 +83,18 @@ public class Did_ins_1 extends Did_ins <Did_ins_1>{
return s;
}
public void getProperties(String prefix, Properties properties) {
super.getProperties(prefix, properties);
if (properties.getProperty(prefix+"theta")!=null) this.theta= IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"theta"),3);
if (properties.getProperty(prefix+"uvw")!=null) this.uvw= IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"uvw"),3);
if (properties.getProperty(prefix+"ned")!=null) this.ned= IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"ned"),3);
}
public Properties setProperties(String prefix, Properties properties){ // save // USED in lwir
properties = super.setProperties(prefix, properties);
properties.setProperty(prefix+"theta", IntersceneMatchParameters.floatsToString(this.theta));
properties.setProperty(prefix+"uvw", IntersceneMatchParameters.floatsToString(this.uvw));
properties.setProperty(prefix+"ned", IntersceneMatchParameters.floatsToString(this.ned));
return properties;
}
}
\ No newline at end of file
src/main/java/com/elphel/imagej/ims/Did_ins_2.java 0 → 100644
View file @ 2b8ae048
package com.elphel.imagej.ims;
//import java.io.Serializable;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.Properties;
import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;
//public class Did_ins_1 implements Serializable {
// static final long serialVersionUID = 4;
public class Did_ins_2 extends Did_ins <Did_ins_2>{
/** GPS number of weeks since January 6th, 1980 */
// public int week; // uint32_t
/** GPS time of week (since Sunday morning) in seconds */
// public double timeOfWeek;
/** INS status flags (eInsStatusFlags). Copy of DID_SYS_PARAMS.insStatus */
// public int insStatus; //uint32_t
/** Hardware status flags (eHdwStatusFlags). Copy of DID_SYS_PARAMS.hdwStatus */
// public int hdwStatus; // uint32_t
/** Euler angles: roll, pitch, yaw in radians with respect to NED */
public float [] qn2b = new float[4];
/** Velocity U, V, W in meters per second. Convert to NED velocity using "vectorBodyToReference( uvw, theta, vel_ned )". */
public float [] uvw = new float [3];
/** WGS84 latitude, longitude, height above ellipsoid (degrees,degrees,meters) */
// public double [] lla = new double [3];
public Did_ins_2 (ByteBuffer bb) {
bb.order(ByteOrder.LITTLE_ENDIAN);
week= bb.getInt();
timeOfWeek=bb.getDouble();
insStatus= bb.getInt();
hdwStatus= bb.getInt();
qn2b[0]= bb.getFloat(); qn2b[1] = bb.getFloat(); qn2b[2]= bb.getFloat(); qn2b[3]= bb.getFloat();
uvw[0]= bb.getFloat(); uvw[1] = bb.getFloat(); uvw[2]= bb.getFloat();
lla[0]= bb.getDouble(); lla[1] = bb.getDouble(); lla[2]= bb.getDouble();
}
public Did_ins_2() {}
public Did_ins_2(String prefix, Properties properties) {
getProperties(prefix, properties);
}
public Did_ins_2 interpolate(double frac, Did_ins_2 next_did) {
Did_ins_2 new_did = new Did_ins_2();
interpolateBase(frac, next_did, new_did);
for (int i = 0; i < qn2b.length; i++) {
new_did.qn2b[i] = interpolateFloat(frac, qn2b[i], next_did.qn2b[i]) ;
}
for (int i = 0; i < uvw.length; i++) {
new_did.uvw[i] = interpolateFloat(frac, uvw[i], next_did.uvw[i]) ;
}
return new_did;
}
public int pack(ByteBuffer bb) {
int p_start = bb.position();
bb.putInt(week);
bb.putDouble(timeOfWeek);
bb.putInt(insStatus);
bb.putInt(hdwStatus);
bb.putFloat(qn2b[0]); bb.putFloat(qn2b[1]); bb.putFloat(qn2b[2]); bb.putFloat(qn2b[3]);
bb.putFloat(uvw[0]); bb.putFloat(uvw[1]); bb.putFloat(uvw[2]);
bb.putDouble(lla[0]); bb.putDouble(lla[1]); bb.putDouble(lla[2]);
int p_end = bb.position();
return p_end - p_start;
}
public String toString() {
String s = "DID_INS_2\n";
s += String.format("week: %d\n", week);
s += String.format("timeOfWeek: %f\n", timeOfWeek);
s += String.format("insStatus: 0x%x\n", insStatus);
s += String.format("hdwStatus: 0x%x\n", hdwStatus);
s += String.format("qn2b: %f, %f, %f, %f\n", qn2b[0], qn2b[1], qn2b[2], qn2b[3]);
s += String.format("uvw: %f, %f, %f\n", uvw[0], uvw[1], uvw[2]);
s += String.format("lla: %f, %f, %f\n", lla[0], lla[1], lla[2]);
return s;
}
public void getProperties(String prefix, Properties properties) {
super.getProperties(prefix, properties);
if (properties.getProperty(prefix+"qn2b")!=null) this.qn2b= IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"qn2b"),4);
if (properties.getProperty(prefix+"uvw")!=null) this.uvw= IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"uvw"),3);
}
public Properties setProperties(String prefix, Properties properties){ // save // USED in lwir
properties = super.setProperties(prefix, properties);
properties.setProperty(prefix+"qn2b", IntersceneMatchParameters.floatsToString(this.qn2b));
properties.setProperty(prefix+"uvw", IntersceneMatchParameters.floatsToString(this.uvw));
return properties;
}
}
\ No newline at end of file
src/main/java/com/elphel/imagej/ims/Did_pimu.java
View file @ 2b8ae048
......@@ -2,6 +2,9 @@ package com.elphel.imagej.ims;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.Properties;
import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;
public class Did_pimu {
/** Time since boot up in seconds. Convert to GPS time of week by adding gps.towOffset */
......@@ -25,6 +28,10 @@ public class Did_pimu {
}
public Did_pimu() {}
public Did_pimu(String prefix, Properties properties) {
getProperties(prefix, properties);
}
public Did_pimu interpolate(double frac, Did_pimu next_did) {
Did_pimu new_did = new Did_pimu();
new_did.time = Did_ins.interpolateDouble(frac, time, next_did.time);
......@@ -59,4 +66,24 @@ public class Did_pimu {
return s;
}
public void getProperties(String prefix, Properties properties) {
if (properties.getProperty(prefix+"time")!=null) this.time= Double.parseDouble(properties.getProperty(prefix+"time"));
if (properties.getProperty(prefix+"dt")!=null) this.dt= Float.parseFloat(properties.getProperty(prefix+"dt"));
if (properties.getProperty(prefix+"status")!=null) this.status= Integer.parseInt(properties.getProperty(prefix+"status"));
if (properties.getProperty(prefix+"theta")!=null) this.theta= IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"theta"),3);
if (properties.getProperty(prefix+"vel")!=null) this.vel= IntersceneMatchParameters.StringToFloats(properties.getProperty(prefix+"vel"),3);
}
public Properties setProperties(String prefix, Properties properties){ // save // USED in lwir
if (properties == null) {
properties = new Properties();
}
properties.setProperty(prefix+"time", this.time+"");
properties.setProperty(prefix+"dt", this.dt+"");
properties.setProperty(prefix+"status", this.status+"");
properties.setProperty(prefix+"theta", IntersceneMatchParameters.floatsToString(this.theta));
properties.setProperty(prefix+"vel", IntersceneMatchParameters.floatsToString(this.vel));
return properties;
}
}
\ No newline at end of file
src/main/java/com/elphel/imagej/ims/Did_strobe_in_time.java
View file @ 2b8ae048
......@@ -17,7 +17,7 @@ typedef struct PACKED
} strobe_in_time_t;
*/
public class Did_strobe_in_time {
public final static int WEEK_MS = 7 * 24 * 3600 * 1000; // milliseconds in a week
public final static long WEEK_MS = 7 * 24 * 3600 * 1000; // milliseconds in a week
public int week;
/** GPS time of week (since Sunday morning) in milliseconds */
......
src/main/java/com/elphel/imagej/ims/EventLogger.java
View file @ 2b8ae048
......@@ -35,10 +35,10 @@ public class EventLogger {
}
Arrays.sort(logger_files); // increasing start time stamps
testInterpolateDidIns1();
testInterpolateDidIns1();
testInterpolateDidIns1();
// testInterpolateDidIns1();
// testInterpolateDidIns1();
// testInterpolateDidIns1();
/*
// String out_did_gps_path = "/home/elphel/lwir16-proc/office_04/did_gps";
String out_did_gps_path = dir+"../did_gps/did_gps";
out_did_gps_path = Paths.get(out_did_gps_path).normalize().toString();
......@@ -56,8 +56,9 @@ public class EventLogger {
e.printStackTrace();
}
}
*/
// String out_did1_path = "/media/elphel/SSD3-4GB/lwir16-proc/berdich/ims/test_out/did_ins1";
/*
String out_did1_path = dir+"../did_ins1/did_ins1";
out_did1_path = Paths.get(out_did1_path).normalize().toString();
Path op_did1 = Paths.get(out_did1_path);
......@@ -73,9 +74,82 @@ public class EventLogger {
e.printStackTrace();
}
}
*/
System.out.println("Processed "+logger_files.length+" event log files");
}
public void printGps(
String out_did_gps_path,
int gps_mask) { // 7: // +1 - DID_GPS1_POS, +2 - DID_GPS2_POS, +4 - DID_GPS1_UBX_POS
out_did_gps_path = Paths.get(out_did_gps_path).normalize().toString();
Path op_did_gps = Paths.get(out_did_gps_path);
op_did_gps=op_did_gps.normalize();
(new File(op_did_gps.getParent().toString())).mkdirs();
for (int nf = 0; nf < logger_files.length; nf++) {
String out_path= out_did_gps_path+"-"+String.format("%03d", nf)+".out";
System.out.println("Printing all DID_GPS data in "+logger_files[nf].abs_path+" to "+out_path);
try {
logger_files[nf].listGPS(out_path, gps_mask, false);
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
}
public void printDidIns1(String out_did1_path) {
out_did1_path = Paths.get(out_did1_path).normalize().toString();
Path op_did1 = Paths.get(out_did1_path);
op_did1=op_did1.normalize();
(new File(op_did1.getParent().toString())).mkdirs();
for (int nf = 0; nf < logger_files.length; nf++) {
String out_path= out_did1_path+"-"+String.format("%03d", nf)+".out";
System.out.println("Printing all DID_INS_1 data in "+logger_files[nf].abs_path+" to "+out_path);
try {
logger_files[nf].listDidIns1(out_path,false);
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
}
public void printDidIns2(String out_did2_path) {
out_did2_path = Paths.get(out_did2_path).normalize().toString();
Path op_did2 = Paths.get(out_did2_path);
op_did2=op_did2.normalize();
(new File(op_did2.getParent().toString())).mkdirs();
for (int nf = 0; nf < logger_files.length; nf++) {
String out_path= out_did2_path+"-"+String.format("%03d", nf)+".out";
System.out.println("Printing all DID_INS_2 data in "+logger_files[nf].abs_path+" to "+out_path);
try {
logger_files[nf].listDidIns2(out_path,false);
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
}
public void printPimu(String out_pimu_path) {
out_pimu_path = Paths.get(out_pimu_path).normalize().toString();
Path op_pimu = Paths.get(out_pimu_path);
op_pimu=op_pimu.normalize();
(new File(op_pimu.getParent().toString())).mkdirs();
for (int nf = 0; nf < logger_files.length; nf++) {
String out_path= out_pimu_path+"-"+String.format("%03d", nf)+".out";
System.out.println("Printing all DID_INS_1 data in "+logger_files[nf].abs_path+" to "+out_path);
try {
logger_files[nf].listPimu(out_path,false);
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
}
public void testInterpolateDidIns1() {
boolean exit_now = false;
double ts_master = 1694576078.939126;
......@@ -96,7 +170,7 @@ public class EventLogger {
//Did_ins_1
Did_ins_1 interpolateDidIns1(
public Did_ins_1 interpolateDidIns1(
double ts_master,
int debug_level) throws IOException {
byte [][] payload = new byte[2][];
......@@ -118,6 +192,77 @@ public class EventLogger {
frac, // double frac,
did_above); // Did_ins_1 next_did))
}
//Did_ins_2
public Did_ins_2 interpolateDidIns2(
double ts_master,
int debug_level) throws IOException {
byte [][] payload = new byte[2][];
int did = Imx5.DID_INS_2;
double frac = getDidAtTS(
ts_master, // double ts_master,
did, // int did, // DID value (4 for DID_INS_1, 3 for DID_PIMU)
payload, // byte [][] payload,
debug_level); // int debug_level)
if (Double.isNaN(frac)) {
if (debug_level > -1){
System.out.println("interpolateDidIns1(): Failed to extract DID_INS_2 data");
}
return null;
}
Did_ins_2 did_below = new Did_ins_2(ByteBuffer.wrap(payload[0]));
Did_ins_2 did_above = new Did_ins_2(ByteBuffer.wrap(payload[1]));
return did_below.interpolate(
frac, // double frac,
did_above); // Did_ins_1 next_did))
}
public Did_gps_pos interpolateDidGpsPos(
double ts_master,
int did, // Imx5.DID_GPS1_POS, Imx5.DID_GPS2_POS, Imx5.DID_GPS1_UBX_POS
int debug_level) throws IOException {
byte [][] payload = new byte[2][];
double frac = getDidAtTS(
ts_master, // double ts_master,
did, // int did, // DID value (4 for DID_INS_1, 3 for DID_PIMU)
payload, // byte [][] payload,
debug_level); // int debug_level)
if (Double.isNaN(frac)) {
if (debug_level > -1){
System.out.println("interpolateDidIns1(): Failed to extract DID_GPS_POS data");
}
return null;
}
Did_gps_pos did_below = new Did_gps_pos(ByteBuffer.wrap(payload[0]));
Did_gps_pos did_above = new Did_gps_pos(ByteBuffer.wrap(payload[1]));
return did_below.interpolate(
frac, // double frac,
did_above); // Did_gps_pos next_did))
}
public Did_pimu interpolateDidPimu(
double ts_master,
int debug_level) throws IOException {
byte [][] payload = new byte[2][];
int did = Imx5.DID_PIMU;
double frac = getDidAtTS(
ts_master, // double ts_master,
did, // int did, // DID value (4 for DID_INS_1, 3 for DID_PIMU)
payload, // byte [][] payload,
debug_level); // int debug_level)
if (Double.isNaN(frac)) {
if (debug_level > -1){
System.out.println("interpolateDidIns1(): Failed to extract DID_PIMU data");
}
return null;
}
Did_pimu did_below = new Did_pimu(ByteBuffer.wrap(payload[0]));
Did_pimu did_above = new Did_pimu(ByteBuffer.wrap(payload[1]));
return did_below.interpolate(
frac, // double frac,
did_above); // Did_ins_1 next_did))
}
public double getDidAtTS(
double ts_master,
......
src/main/java/com/elphel/imagej/ims/EventLoggerFileInfo.java
View file @ 2b8ae048
......@@ -403,7 +403,7 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
}
}
public void listDid1(String path, boolean keep) throws IOException {
public void listDidIns1(String path, boolean keep) throws IOException {
double max_offset = 1000; // m
open();
float [] start_ned = null;
......@@ -430,7 +430,7 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
for (int nrec = 0; nrec < num_recs; nrec++) {
if (nrec == dbg_nrec) {
System.out.println("listDid1(): nrec="+nrec);
System.out.println("listDidIns1(): nrec="+nrec);
}
int [] full_type = getFullType(bb, nrec);
if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_INS_1)) {
......@@ -487,6 +487,75 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
}
}
public void listDidIns2(String path, boolean keep) throws IOException {
open();
FileWriter writer = null;
if (path != null) {
writer = new FileWriter (path);
}
String header = "record "+
" master TS "+
" local TS "+
"timeOfWeek " +
" insStat "+
" hdwStat "+
" qn2b "+
" uvw "+
" lla \n";
if (writer != null) {
writer.write(header);
} else {
System.out.print(header);
}
int dbg_nrec = -5000;
int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
for (int nrec = 0; nrec < num_recs; nrec++) {
if (nrec == dbg_nrec) {
System.out.println("listDidIns1(): nrec="+nrec);
}
int [] full_type = getFullType(bb, nrec);
if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_INS_2)) {
byte [] payload = getDidPayload(
null, // next_fileinfo, // may be null if payload does not extend beyond single record
nrec);
if (payload == null) {
System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
break;
}
ByteBuffer bb_payload = ByteBuffer.wrap(payload);
bb_payload.order(ByteOrder.LITTLE_ENDIAN);
Did_ins_2 did_ins_2 = new Did_ins_2(bb_payload);
if ((did_ins_2.uvw[0] == 0) && (did_ins_2.uvw[1] == 0) && (did_ins_2.uvw[2] == 0)) {
continue;
}
String line =String.format(
"%6d %17.6f %17.6f %10.3f %08x %08x [%8.4f, %8.4f, %8.4f, %8.4f] "+
"[%8.3f, %8.3f, %8.3f] [%12.7f, %12.7f, %12.7f]\n",
nrec,
getMasterTS(nrec),
getLocalTS(nrec),
did_ins_2.timeOfWeek,
did_ins_2.insStatus,
did_ins_2.hdwStatus,
did_ins_2.qn2b[0], did_ins_2.qn2b[1], did_ins_2.qn2b[2], did_ins_2.qn2b[3], // Quaternion body rotation with respect to NED: W, X, Y, Z
did_ins_2.uvw[0], did_ins_2.uvw[1], did_ins_2.uvw[2], // Velocity U, V, W in meters per second. Convert to NED velocity using "vectorBodyToReference( uvw, theta, vel_ned )".
did_ins_2.lla[0], did_ins_2.lla[1], did_ins_2.lla[2] // WGS84 latitude, longitude, height above ellipsoid (degrees,degrees,meters)
);
if (writer != null) {
writer.write(line);
} else {
System.out.print(line);
}
}
}
if (writer != null) {
writer.close();
}
if (!keep) {
close();
}
}
public void listGPS (String path, int gps_mask, boolean keep) throws IOException {
// +1 - DID_GPS1_POS, +2 - DID_GPS2_POS, +4 - DID_GPS1_UBX_POS
// double max_offset = 1000; // m
......@@ -594,6 +663,76 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
}
public void listPimu(String path, boolean keep) throws IOException {
open();
FileWriter writer = null;
if (path != null) {
writer = new FileWriter (path);
}
String header = "record "+
" master TS "+
" local TS "+
" time since boot " +
" dt "+
" status "+
" omega (rad/s) "+
" a (m/s^2) "+"\n";
if (writer != null) {
writer.write(header);
} else {
System.out.print(header);
}
int dbg_nrec = -5000;
int type = (EventLoggerFileInfo.REC_TYPE_GPS << 4) | EventLoggerFileInfo.REC_SUBTYPE_IMX5; // 0x18 - first subpacket
for (int nrec = 0; nrec < num_recs; nrec++) {
if (nrec == dbg_nrec) {
System.out.println("listPimu(): nrec="+nrec);
}
int [] full_type = getFullType(bb, nrec);
if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_PIMU)) {
byte [] payload = getDidPayload(
null, // next_fileinfo, // may be null if payload does not extend beyond single record
nrec);
if (payload == null) {
System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
break;
}
ByteBuffer bb_payload = ByteBuffer.wrap(payload);
bb_payload.order(ByteOrder.LITTLE_ENDIAN);
Did_pimu did_pimu = new Did_pimu(bb_payload);
if ((did_pimu.vel[0] == 0) && (did_pimu.vel[1] == 0) && (did_pimu.vel[2] == 0)) {
continue; // skip no-movement
}
String line =String.format(
"%6d %17.6f %17.6f %17.6f %8.6f %08x [%9.5f, %9.5f, %9.5f] "+
"[%9.4f, %9.4f, %9.4f]\n",
nrec,
getMasterTS(nrec),
getLocalTS(nrec),
did_pimu.time,
did_pimu.dt,
did_pimu.status,
did_pimu.theta[0]/did_pimu.dt, did_pimu.theta[1]/did_pimu.dt, did_pimu.theta[2]/did_pimu.dt, // Euler angles: roll, pitch, yaw in radians with respect to NED
did_pimu.vel[0]/did_pimu.dt, did_pimu.vel[1]/did_pimu.dt, did_pimu.vel[2]/did_pimu.dt // Velocity U, V, W in meters per second. Convert to NED velocity using "vectorBodyToReference( uvw, theta, vel_ned )".
);
if (writer != null) {
writer.write(line);
} else {
System.out.print(line);
}
}
}
if (writer != null) {
writer.close();
}
if (!keep) {
close();
}
}
byte [] getDidPayload(
EventLoggerFileInfo next_fileinfo, // may be null if payload does not extend beyond single record
int nrec) throws IOException {
......
src/main/java/com/elphel/imagej/ims/Imx5.java
View file @ 2b8ae048
package com.elphel.imagej.ims;
import java.io.ByteArrayOutputStream;
import java.io.IOException;
import java.io.ObjectOutputStream;
//import java.io.ByteArrayOutputStream;
//import java.io.IOException;
//import java.io.ObjectOutputStream;
import java.nio.ByteBuffer;
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;
public class Imx5 {
// defines adapted from Inertial Sense data_sets.h
public static int DID_NULL = 0 ; /** NULL (INVALID) */
public static int DID_DEV_INFO = 1 ; /** (dev_info_t) Device information */
......@@ -119,6 +125,7 @@ public class Imx5 {
/** Count of data ids (including null data id 0) - MUST BE MULTPLE OF 4 and larger than last DID number! */
public static int DID_COUNT = 120; // Used in SDK
public static int DID_COUNT_UINS = 100; // Used in uINS
public static double EARTH_RADIUS = 6356752.0; // meters
/** Maximum number of data ids */
public static int DID_MAX_COUNT = 256;
......@@ -150,44 +157,80 @@ public class Imx5 {
Did_ins_1 did_ins_1_2 = new Did_ins_1(bb);
System.out.println("did_ins_1_2 = "+did_ins_1_2);
return bb.array();
}
// static final RotationConvention ROT_CONV = RotationConvention.FRAME_TRANSFORM;
// RotationConvention.VECTOR_OPERATOR
//RotationOrder.YXZ, ROT_CONV
public static double [] applyQuternionTo(double[]quat, double[] vector, boolean inverse) {
Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
double [] rslt = new double[3];
if (inverse) {
ims_rot.applyInverseTo(vector, rslt);
/*
ByteArrayOutputStream bos = new ByteArrayOutputStream();
ObjectOutputStream out = null;
try {
out = new ObjectOutputStream(bos);
out.writeObject(yourObject);
out.flush();
byte[] yourBytes = bos.toByteArray();
...
} finally {
try {
bos.close();
} catch (IOException ex) {
// ignore close exception
} else {
ims_rot.applyTo(vector, rslt);
}
return rslt;
}
}
Create an object from a byte array:
ByteArrayInputStream bis = new ByteArrayInputStream(yourBytes);
ObjectInput in = null;
try {
in = new ObjectInputStream(bis);
Object o = in.readObject();
...
} finally {
try {
if (in != null) {
in.close();
public static double [] imsToCamRotations(double [] ims_theta, int ord, boolean rev_order, boolean rev_matrix ) {
RotationConvention rc = RotationConvention.FRAME_TRANSFORM;
// RotationOrder ro = zyx ? RotationOrder.ZYX : RotationOrder.XYZ; // XYZ; // ZYX;
RotationOrder ro = RotationOrder.XYZ;
switch (ord) {
case 0: ro = RotationOrder.XYZ; break;
case 1: ro = RotationOrder.XZY; break;
case 2: ro = RotationOrder.YXZ; break;
case 3: ro = RotationOrder.YZX; break;
case 4: ro = RotationOrder.ZXY; break;
case 5: ro = RotationOrder.ZYX; break;
}
} catch (IOException ex) {
// ignore close exception
RotationOrder ro1 = ro; // ? RotationOrder.ZYX : RotationOrder.XYZ; //.XYZ;
Rotation cam_to_ims0 = new Rotation(new double [][] {{0,0,1},{-1,0,0},{0,-1,0}}, 1E-8);
// Rotation cam_to_ims1 = new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2);
Rotation cam_to_ims = rev_matrix? (new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2)):(new Rotation(ro, rc, Math.PI/2, 0, Math.PI/2));
double [] angles1 = cam_to_ims.getAngles(ro1, rc);
Rotation ims_rot = new Rotation(ro, rc, ims_theta[0], ims_theta[1], ims_theta[2]);
//Rotation r1= new Rotation(RotationOrder.YXZ, RC, ro.aux_azimuth, ro.aux_tilt, ro.aux_roll)
Rotation comb_rot = rev_order?ims_rot.applyTo(cam_to_ims): cam_to_ims.applyTo(ims_rot);
double [] angles = comb_rot.getAngles(ro1, rc);
return angles;
}
}
*/
public static double [] imsQToCamRotations(double [] ims_qn2b, int ord, boolean rev_order, boolean rev_matrix ) {
RotationConvention rc = RotationConvention.FRAME_TRANSFORM;
// RotationOrder ro = zyx ? RotationOrder.ZYX : RotationOrder.XYZ; // XYZ; // ZYX;
RotationOrder ro = RotationOrder.XYZ;
switch (ord) {
case 0: ro = RotationOrder.XYZ; break;
case 1: ro = RotationOrder.XZY; break;
case 2: ro = RotationOrder.YXZ; break;
case 3: ro = RotationOrder.YZX; break;
case 4: ro = RotationOrder.ZXY; break;
case 5: ro = RotationOrder.ZYX; break;
}
RotationOrder ro1 = ro; // ? RotationOrder.ZYX : RotationOrder.XYZ; //.XYZ;
Rotation cam_to_ims0 = new Rotation(new double [][] {{0,0,1},{-1,0,0},{0,-1,0}}, 1E-8);
// Rotation cam_to_ims1 = new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2);
Rotation cam_to_ims = rev_matrix? (new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2)):(new Rotation(ro, rc, Math.PI/2, 0, Math.PI/2));
double [] angles1 = cam_to_ims.getAngles(ro1, rc);
// Rotation ims_rot = new Rotation(ro, rc, ims_theta[0], ims_theta[1], ims_theta[2]);
// public Rotation(double q0, double q1, double q2, double q3, boolean needsNormalization) {
Rotation ims_rot = new Rotation(ims_qn2b[0],ims_qn2b[1],ims_qn2b[2],ims_qn2b[3],true);
//Rotation r1= new Rotation(RotationOrder.YXZ, RC, ro.aux_azimuth, ro.aux_tilt, ro.aux_roll)
Rotation comb_rot = rev_order?ims_rot.applyTo(cam_to_ims): cam_to_ims.applyTo(ims_rot);
double [] angles = comb_rot.getAngles(ro1, rc);
return angles;
}
public static double [] nedFromLla (double [] lla, double [] lla_ref) {
double [] ned = new double[] {
EARTH_RADIUS* Math.cos(lla[0] * Math.PI / 180)*(lla[1] - lla_ref[1]) * Math.PI / 180.0,
EARTH_RADIUS* (lla[0]-lla_ref[0]) * Math.PI / 180,
-(lla[2] - lla_ref[2])
};
return ned;
}
}
src/main/java/com/elphel/imagej/tileprocessor/QuadCLT.java
View file @ 2b8ae048
......@@ -37,7 +37,7 @@ import java.nio.channels.Channels;
import java.nio.channels.WritableByteChannel;
import java.util.Arrays;
import java.util.Properties;
import java.util.Random;
//import java.util.Random;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.atomic.DoubleAccumulator;
......@@ -52,13 +52,13 @@ import com.elphel.imagej.correction.EyesisCorrections;
import com.elphel.imagej.gpu.GPUTileProcessor;
import com.elphel.imagej.gpu.GpuQuad;
import com.elphel.imagej.gpu.TpTask;
import com.elphel.imagej.tileprocessor.QuadCLTCPU.SetChannels;
//import com.elphel.imagej.tileprocessor.QuadCLTCPU.SetChannels;
import ij.IJ;
import ij.ImagePlus;
import ij.ImageStack;
import ij.Prefs;
import ij.io.FileSaver;
//import ij.Prefs;
//import ij.io.FileSaver;
public class QuadCLT extends QuadCLTCPU {
int dbg_lev = 1;
private GpuQuad gpuQuad = null; // use updateQuadCLT() to update after switching to a different
......
src/main/java/com/elphel/imagej/tileprocessor/TwoQuadCLT.java
View file @ 2b8ae048
......@@ -2982,7 +2982,7 @@ if (debugLevel > -100) return true; // temporarily !
* @param quadCLT_aux secondary RGB or LWIR QuadCLT instance
* @param clt_parameters CLT parameters
* @param skip_existing do not copy if destination files exist
* @param search_KML search fpr geo data and extract it to KML file if found
* @param search_KML search for geo data and extract it to KML file if found
* @param debugLevel debug level
*/
public static void copyJP4src(
......@@ -3012,6 +3012,7 @@ if (debugLevel > -100) return true; // temporarily !
set_name = set_channels[00].set_name;
}
QuadCLT.SetChannels [] set_channels_main = quadCLT_main.setChannels(set_name,debugLevel); // only for specified image timestamp
QuadCLT.SetChannels [] set_channels_aux = (quadCLT_aux !=null) ? quadCLT_aux.setChannels(set_name,debugLevel) : null;
......@@ -3036,20 +3037,39 @@ if (debugLevel > -100) return true; // temporarily !
true, // smart,
true); //newAllowed, // save
File dir = (new File(jp4_copy_path)); // .getParentFile();
boolean already_copied = false;
if (!dir.exists()){
dir.mkdirs();
System.out.println("Created "+dir);
} else if (skip_existing) {
if (dir.list().length > 0) {
System.out.println("Skipping existing directory "+dir);
return;
already_copied = true;
}
}
if (clt_parameters.imp.ims_use) {
String ims_path = dir.getParent() + Prefs.getFileSeparator()+set_name+quadCLT_main.correctionsParameters.imsSuffix;
double [] saved_offset = quadCLT_main.readImsOffset(
ims_path, // String ims_path,
debugLevel); // int debugLevel)
if ((saved_offset == null) || (saved_offset[0] != clt_parameters.imp.ims_offset) || (saved_offset[1] != clt_parameters.imp.gmt_plus)) {
quadCLT_main.saveIms(
clt_parameters.imp.ims_offset,
clt_parameters.imp.gmt_plus, // double gmt_plus,
ims_path,
debugLevel);
}
}
if (search_KML && (set_channels_main !=null)) { // TODO: make it look in both MAIN and AUX. Look in only the same scene?
quadCLT_main.writeKml(
set_name,
debugLevel ); // also generated with x3d model
}
if (already_copied) {
System.out.println("Skipping existing directory "+dir);
return;
}
for (String fname:path_list) {
if (fname.contains(set_name)) { // only files containing set name // TODO:improve
File file = new File(fname);
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment