Commit 726f4add authored by Andrey Filippov's avatar Andrey Filippov

IMS interpolation

parent e83cb48d
......@@ -14,14 +14,14 @@ public abstract class Did_ins <T extends Did_ins <T>>{
public double [] lla = new double [3];
static int interpolateInt(double frac, int v_this, int v_next) {
return (int) Math.round(frac * v_this + (1.0 - frac) * v_next);
return (int) Math.round(frac * v_next + (1.0 - frac) * v_this);
}
static float interpolateFloat(double frac, float v_this, float v_next) {
return (float) (frac * v_this + (1.0 - frac) * v_next);
return (float) (frac * v_next + (1.0 - frac) * v_this);
}
static double interpolateDouble(double frac, double v_this, double v_next) {
return frac * v_this + (1.0 - frac) * v_next;
return frac * v_next + (1.0 - frac) * v_this;
}
//https://www.swtestacademy.com/return-subclass-instance-java-generics/
......
......@@ -3,6 +3,8 @@ package com.elphel.imagej.ims;
import java.io.File;
import java.io.IOException;
import java.nio.ByteBuffer;
import java.nio.file.Path;
import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.stream.Collectors;
......@@ -33,39 +35,66 @@ public class EventLogger {
}
Arrays.sort(logger_files); // increasing start time stamps
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();
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();
int gps_mask = 7; // +1 - DID_GPS1_POS, +2 - DID_GPS2_POS, +4 - DID_GPS1_UBX_POS
for (int nf = 0; nf < logger_files.length; nf++) {
System.out.println("Printing all DID_INS_1 data in "+logger_files[nf].abs_path);
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_did_gps_path+"-"+String.format("%03d", nf)+".out", gps_mask, false);
logger_files[nf].listGPS(out_path, gps_mask, false);
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
String out_did1_path = "/media/elphel/SSD3-4GB/lwir16-proc/berdich/ims/test_out/did_ins1";
// 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);
op_did1=op_did1.normalize();
(new File(op_did1.getParent().toString())).mkdirs();
for (int nf = 0; nf < logger_files.length; nf++) {
System.out.println("Printing all DID_INS_1 data in "+logger_files[nf].abs_path);
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].listDid1(out_did1_path+"-"+String.format("%03d", nf)+".out",false);
logger_files[nf].listDid1(out_path,false);
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
System.out.println("Processed "+logger_files.length+" event log files");
}
public void testInterpolate() {
public void testInterpolateDidIns1() {
boolean exit_now = false;
double ts_master = 1694576078.939126;
while (!exit_now) {
Did_ins_1 d1=null;
try {
d1=interpolateDidIns1(
ts_master,
0);
} catch (IOException e) {
// TODO Auto-generated catch block
e.printStackTrace();
} // debug_level) throws IOException {
System.out.println("ts_master="+ts_master);
}
}
//Did_ins_1
Did_ins_1 interpolateDidIns1(
double ts_master,
......
......@@ -120,13 +120,29 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
// initial approximation
int nrec0 = first_last_index[0] + (int) Math.round((first_last_index[1]-first_last_index[0]) *
(ts_master - first_last_ts[0])/ (first_last_ts[1]-first_last_ts[0]));
int drec = after ? 1: -1;
for (int nrec = nrec0; (nrec > first_last_index[0]) && (nrec < first_last_index[1]); nrec += drec) {
// find any type in the opposite direction
int drec = after ? -1: 1;
int nrec = nrec0;
search_opposite: {
for (; (nrec > first_last_index[0]) && (nrec < first_last_index[1]); nrec += drec) {
double ts_master_indx = getMasterTS(bb, nrec);
if (after?(ts_master_indx <= ts_master):(ts_master_indx > ts_master)) {
break search_opposite; // return nrec;
}
}
System.out.println("getLastBeforeIndex(): could not find oppposite, ts_master="+ts_master+", after="+after);
return first_last_index[after ? 1 : 0]; // could not find
}
drec = after ? 1: -1;
for (; (nrec > first_last_index[0]) && (nrec < first_last_index[1]); nrec += drec) {
double ts_master_indx = getMasterTS(bb, nrec);
if (after?(ts_master_indx > ts_master) :(ts_master_indx <= ts_master)) {
int [] full_type = getFullType(bb, nrec);
if ((full_type != null) && (full_type[0] == type) && (full_type[1] == did)) {
return nrec;
}
}
}
return first_last_index[after ? 1 : 0];
}
......@@ -211,7 +227,7 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
if ((full_type[0] >> 4) == REC_TYPE_IMG) {
double ts_local = getLocalTS(bb, nrec);
double ts_master = getMasterTSImg(bb, nrec);
if (debugLevel > -1) { // -1
if (debugLevel > 0) { // -1
System.out.println (
String.format("%6d %6d %10.6f",nimg, nrec, ts_master));
}
......@@ -357,7 +373,7 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
sx2 += ts_local * ts_local;
sy += ts_frac;
sxy += ts_local * ts_frac;
if (debugLevel > -1) {
if (debugLevel > 0) {
System.out.println (
String.format("%6d %10.6f %10.6f", nrec, ts_local, ts_frac));
}
......@@ -410,9 +426,12 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
} 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("listDid1(): nrec="+nrec);
}
int [] full_type = getFullType(bb, nrec);
if ((full_type != null) && (full_type[0] == type) && (full_type[1] == Imx5.DID_INS_1)) {
byte [] payload = getDidPayload(
......
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