Skip to content

I
imagej-elphel
Commit 2014ae08 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

Added more DID sane checks - found DID_INS1 with wrong LLA

parent 598a2074
Expand all Show whitespace changes
Inline Side-by-side
Showing with 289 additions and 112 deletions
src/main/java/com/elphel/imagej/ims/Did_gps_pos.java
View file @ 2014ae08
......@@ -101,6 +101,18 @@ public class Did_gps_pos {
getProperties(prefix, properties);
}
public boolean isDidSane() { // add more!
if ( (lla[0] <-90) || (lla[0] > 90) || // latitude
(lla[1] <-90) || (lla[1] > 90) || // longitude
(lla[2] <-10) || (lla[1] > 20000)) { // altitude
System.out.println("isDidSane(): bad lla=["+
lla[0]+", "+lla[1]+", "+lla[2]+"], timeOfWeekMs="+timeOfWeekMs);
return false;
}
return true;
}
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;
......
src/main/java/com/elphel/imagej/ims/Did_ins.java
View file @ 2014ae08
......@@ -91,6 +91,13 @@ public abstract class Did_ins <T extends Did_ins <T>>{
(insStatus ^ eInsStatusFlags_data) & eInsStatusFlags_mask));
return false;
}
if ( (lla[0] <-90) || (lla[0] > 90) || // latitude
(lla[1] <-90) || (lla[1] > 90) || // longitude
(lla[2] <-10) || (lla[1] > 20000)) { // altitude
System.out.println("isDidSane(): bad lla=["+
lla[0]+", "+lla[1]+", "+lla[2]+"], timeOfWeek="+timeOfWeek);
return false;
}
return true;
}
......
src/main/java/com/elphel/imagej/ims/Did_pimu.java
View file @ 2014ae08
......@@ -7,6 +7,14 @@ import java.util.Properties;
import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;
public class Did_pimu {
public static final int IMU_STATUS_IMU_OK_MASK = 0x003F0000; // lower bits may have error
public static final int IMU_STATUS_SATURATION_MASK = 0x0000003F;
public static final int IMU_STATUS_ZEROS = 0xffc0f800;
public static final double IMU_MAX_THETA = 10.0; //rad/s
public static final double IMU_MAX_VEL = 20.0; //m/s/s
public static final double IMU_MIN_PER = 0.008; // s
public static final double IMU_MAX_PER = 1.0; // s
/** Time since boot up in seconds. Convert to GPS time of week by adding gps.towOffset */
public double time;
/** Integral period in seconds for delta theta and delta velocity. This is configured using DID_FLASH_CONFIG.startupNavDtMs. */
......@@ -44,6 +52,39 @@ public class Did_pimu {
return new_did;
}
public boolean isDidSane() {
if (((status ^ IMU_STATUS_IMU_OK_MASK) & IMU_STATUS_IMU_OK_MASK) != 0) {
System.out.println("Did_pimu.isDidSane(): not OK status = "+String.format("0x%08x", status));
return false;
}
if ((status & IMU_STATUS_SATURATION_MASK) != 0) {
System.out.println("Did_pimu.isDidSane(): saturated status = "+String.format("0x%08x", status));
return false;
}
if ((status & IMU_STATUS_ZEROS) != 0) {
System.out.println("Did_pimu.isDidSane(): garbage status = "+String.format("0x%08x", status));
return false;
}
if (dt < IMU_MIN_PER) {
System.out.println("Did_pimu.isDidSane(): insane dt= "+dt+" s < "+IMU_MIN_PER+" s");
return false;
}
if (dt > IMU_MAX_PER) {
System.out.println("Did_pimu.isDidSane(): insane dt= "+dt+" s > "+IMU_MAX_PER+" s");
return false;
}
for (int i = 0; i < 3; i++) {
if (Math.abs(theta[i]) > IMU_MAX_THETA) {
System.out.println("Did_pimu.isDidSane(): insane theta ["+i+"]= "+theta[i]+" rad/s");
return false;
}
if (Math.abs(vel[i]) > IMU_MAX_VEL) {
System.out.println("Did_pimu.isDidSane(): insane vel ["+i+"]= "+vel[i]+" m/s/s");
return false;
}
}
return true;
}
public int pack(ByteBuffer bb) {
int p_start = bb.position();
......
src/main/java/com/elphel/imagej/ims/EventLoggerFileInfo.java
View file @ 2014ae08
......@@ -134,7 +134,12 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
// Timestamp is frame start for images
int [] full_type = getFullType(bb, nrec);
if ((full_type != null) && (full_type[0] == type) && (full_type[1] == did)) {
// is DID sane?
if (isDidSane(nrec, did)) {
break search_opposite; // return nrec;
} else {
System.out.println("getLastBeforeIndex()-1: Skipping insane DID="+did);
}
}
}
}
......@@ -147,7 +152,12 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
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)) {
// is DID sane?
if (isDidSane(nrec, did)) {
return nrec;
} else {
System.out.println("getLastBeforeIndex()-2: Skipping insane DID="+did);
}
}
}
}
......@@ -441,7 +451,40 @@ public class EventLoggerFileInfo implements Comparable<EventLoggerFileInfo> {
return true;
}
public boolean isDidSane(int nrec, int type) {
// int did_ins_type = type_ins_2 ? Imx5.DID_INS_2 : Imx5.DID_INS_1;
byte[] payload;
try {
payload = getDidPayload(
null, // next_fileinfo, // may be null if payload does not extend beyond single record
nrec);
} catch (IOException e) {
// TODO Auto-generated catch block
System.out.println("isDidSane(): failed to read record "+nrec);
e.printStackTrace();
return false;
}
if (payload == null) {
System.out.println("EventLoggerFileInfo(): payload == null, nrec="+nrec);
return false;
}
ByteBuffer bb_payload = ByteBuffer.wrap(payload);
bb_payload.order(ByteOrder.LITTLE_ENDIAN);
switch (type) {
case Imx5.DID_INS_1:
return (new Did_ins_1(bb_payload)).isDidSane();
case Imx5.DID_INS_2:
return (new Did_ins_2(bb_payload)).isDidSane();
case Imx5.DID_GPS1_POS:
case Imx5.DID_GPS2_POS:
case Imx5.DID_GPS1_UBX_POS:
return (new Did_gps_pos(bb_payload)).isDidSane();
case Imx5.DID_PIMU:
return (new Did_pimu(bb_payload)).isDidSane();
}
return true;
}
/**
* Absolute timing calibration with local logging of GPS 1pps events (with IMX-5 it is REC_TYPE_ODO)
......
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ 2014ae08
......@@ -5352,12 +5352,14 @@ public class Interscene {
double [] quat_lma_enu_xyz = (quatCorr != null) ?
Imx5.applyQuaternionTo(quatCorr,cam_xyz_enu,false):
cam_xyz_enu;
// NED without ims_mount_atr correction
sb.append("\t"+cam_xyz1[0]+ "\t"+cam_xyz1[1]+ "\t"+cam_xyz1[2]); //
// NED with ims_mount_atr correction
sb.append("\t"+cam_xyz2[0]+ "\t"+cam_xyz2[1]+ "\t"+cam_xyz2[2]); //
sb.append("\t"+cam_xyz_ned[0]+ "\t"+cam_xyz_ned[1]+ "\t"+cam_xyz_ned[2]); //
sb.append("\t"+cam_xyz_enu[0]+ "\t"+cam_xyz_enu[1]+ "\t"+cam_xyz_enu[2]); //
sb.append("\t"+quat_lma_enu_xyz[0]+ "\t"+quat_lma_enu_xyz[1]+ "\t"+quat_lma_enu_xyz[2]); //
sb.append("\t"+cam_xyz_enu[0]+ "\t"+cam_xyz_enu[1]+ "\t"+cam_xyz_enu[2]); // WITH ims_mount_atr, NO quatCorr
sb.append("\t"+quat_lma_enu_xyz[0]+ "\t"+quat_lma_enu_xyz[1]+ "\t"+quat_lma_enu_xyz[2]); // WITH ims_mount_atr, WITH quatCorr
double [] scene_abs_atr = Imx5.quatToCamAtr(cam_quat2);
......
src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java
View file @ 2014ae08
......@@ -98,6 +98,13 @@ public class IntersceneMatchParameters {
ims_mount_atr[1] * Math.PI/180,
ims_mount_atr[2] * Math.PI/180};
}
public void setImsMountATR( double [] atr) {
ims_mount_atr[0] = atr[0] * 180/Math.PI;
ims_mount_atr[1] = atr[1] * 180/Math.PI;
ims_mount_atr[2] = atr[2] * 180/Math.PI;
}
public boolean center_reference = false;
public boolean overlap_sequences = false; // overlap sequences: scan down from the previous center
public boolean reset_photometric = true; // reset photometric calibration - once for each new series
......@@ -106,7 +113,7 @@ public class IntersceneMatchParameters {
public boolean run_ly = false; // will return just after LY adjustments, skipping all output generation
public int min_num_orient = 2; // make from parameters, should be >= 1
public int min_num_interscene = 2; // make from parameters, should be >= 1
public boolean adjust_imu_orient = false;// adjust IMU misalignment to the camera
public boolean calc_quat_corr = true; // calculate camera orientation correction from predicted by IMS
public boolean apply_quat_corr = true; // apply camera orientation correction from predicted by IMS
public boolean use_quat_corr = true; // Use orientation correction everywhere if available
......@@ -631,6 +638,8 @@ public class IntersceneMatchParameters {
"Minimal number of fitting scenes cycles, should be >=1. First cycle includes spiral search for the first scene");
gd.addNumericField("Minimal number of interscene accumulations", this.min_num_interscene, 0,3,"",
"Minimal required number of re-calculations of the interscene-accumulated DSI.");
gd.addCheckbox ("Adjust IMU orientation", this.adjust_imu_orient,
"Adjust IMU misalignment to the camera.");
gd.addCheckbox ("Calculate IMS orientation correction", this.calc_quat_corr,
"Calculate camera orientation correction from predicted by IMS .");
gd.addCheckbox ("Apply IMS orientation correction ", this.apply_quat_corr,
......@@ -1415,6 +1424,7 @@ public class IntersceneMatchParameters {
this.run_ly = gd.getNextBoolean();
this.min_num_orient = (int) gd.getNextNumber(); if (min_num_orient < 1) min_num_orient = 1;
this.min_num_interscene = (int) gd.getNextNumber(); if (min_num_interscene < 1) min_num_interscene = 1;
this.adjust_imu_orient = gd.getNextBoolean();
this.calc_quat_corr = gd.getNextBoolean();
this.apply_quat_corr = gd.getNextBoolean();
this.use_quat_corr = gd.getNextBoolean();
......@@ -1845,6 +1855,7 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"run_ly", this.run_ly + ""); // boolean
properties.setProperty(prefix+"min_num_orient", this.min_num_orient+""); // int
properties.setProperty(prefix+"min_num_interscene", this.min_num_interscene+""); // int
properties.setProperty(prefix+"adjust_imu_orient", this.adjust_imu_orient+""); // boolean
properties.setProperty(prefix+"calc_quat_corr", this.calc_quat_corr+""); // boolean
properties.setProperty(prefix+"apply_quat_corr", this.apply_quat_corr+""); // boolean
properties.setProperty(prefix+"use_quat_corr", this.use_quat_corr+""); // boolean
......@@ -2240,6 +2251,7 @@ public class IntersceneMatchParameters {
if (properties.getProperty(prefix+"run_ly")!=null) this.run_ly=Boolean.parseBoolean(properties.getProperty(prefix+"run_ly"));
if (properties.getProperty(prefix+"min_num_orient")!=null) this.min_num_orient=Integer.parseInt(properties.getProperty(prefix+"min_num_orient"));
if (properties.getProperty(prefix+"min_num_interscene")!=null) this.min_num_interscene=Integer.parseInt(properties.getProperty(prefix+"min_num_interscene"));
if (properties.getProperty(prefix+"adjust_imu_orient")!=null) this.adjust_imu_orient=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_imu_orient"));
if (properties.getProperty(prefix+"calc_quat_corr")!=null) this.calc_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"calc_quat_corr"));
if (properties.getProperty(prefix+"apply_quat_corr")!=null) this.apply_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"apply_quat_corr"));
if (properties.getProperty(prefix+"use_quat_corr")!=null) this.use_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"use_quat_corr"));
......@@ -2662,6 +2674,7 @@ public class IntersceneMatchParameters {
imp.run_ly = this.run_ly;
imp.min_num_orient = this.min_num_orient;
imp.min_num_interscene = this.min_num_interscene;
imp.adjust_imu_orient = this.adjust_imu_orient;
imp.calc_quat_corr = this.calc_quat_corr;
imp.apply_quat_corr = this.apply_quat_corr;
imp.use_quat_corr = this.use_quat_corr;
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 2014ae08
......@@ -4658,6 +4658,7 @@ public class OpticalFlow {
save_mapped_mono_color[1] || gen_avi_mono_color[1] || show_mono_color[1]} ;
// skip completely if no color or mono, tiff or video
boolean adjust_imu_orient = clt_parameters.imp.adjust_imu_orient; // calculate camera orientation correction from predicted by IMS
boolean calc_quat_corr = clt_parameters.imp.calc_quat_corr; // calculate camera orientation correction from predicted by IMS
// apply_quat_corr used inside getGroundIns() - used when generating output
// boolean apply_quat_corr = clt_parameters.imp.apply_quat_corr; // apply camera orientation correction from predicted by IMS
......@@ -5420,7 +5421,7 @@ public class OpticalFlow {
}
}
// later move to the right place
if ((quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {
if (adjust_imu_orient) { // (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,
......@@ -5438,7 +5439,7 @@ public class OpticalFlow {
double [] rms = new double[5];
double [] quat = new double[4];
int quat_lma_mode = 4; // 3; // 2; // 1;
int debug_lev = 3;
int debug_lev = debugLevel; // 3;
double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
double translation_weight = 1.0 / (avg_z + 1.0);
......@@ -5456,7 +5457,33 @@ public class OpticalFlow {
rms, //double [] rms, // null or double[5];
quat, // double [] quaternion, // null or double[4]
debug_lev); // int debugLevel
if (rotated_xyzatr != null) {
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
if (debugLevel > -3) {
System.out.println("Applying correction ot the IMS mount orientation:");
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3];
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
System.out.println("ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]");
System.out.println("ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]");
}
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
ims_mount_atr, // double [] ims_atr,
quat); // double [] corr_q)
clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
if (debugLevel > -3) {
double [] new_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] degrees = new double[3];
for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI;
System.out.println("New ATR(rad)=["+new_atr[0]+", "+new_atr[1]+", "+new_atr[2]+"]");
System.out.println("New ATR(deg)=["+degrees[0]+", "+degrees[1]+", "+degrees[2]+"]");
System.out.println ("*** Need to save the main configuration file ***");
}
} else {
System.out.println ("*** Failed to calculate IMS mount correction! ***");
}
}
if (run_ly) {
if (debugLevel > -3) {
......
src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
View file @ 2014ae08
......@@ -1210,13 +1210,31 @@ public class QuaternionLma {
String pfx,
double [] data) {
// if ((mode == 1) || ((mode == 2) && (data.length >= x_vector.length))) { // different data size data[3*nscene+...]
if ((mode == 1) || (data.length >= x_vector.length)) { // different data size data[3*nscene+...]
if (mode == 0 ) {
System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s",
"N",pfx+"X",pfx+"Y",pfx+"Z"));
for (int nscene = 0; nscene < N; nscene++) {
System.out.println(String.format("%3d"+
"\t%9.5f\t%9.5f\t%9.5f",
nscene,
data[samples*nscene + 0],data[samples*nscene + 1],data[samples*nscene + 2]));
}
System.out.println();
} else if ((mode == 1) || (data.length >= x_vector.length)) { // different data size data[3*nscene+...]
System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+ //x,y,z, q0,q1,q2,q3,a,t,r
"\t%9s\t%9s\t%9s",
"N",pfx+"X",pfx+"Y",pfx+"Z",pfx+"q0",pfx+"q1",pfx+"q2",pfx+"q3",
pfx+"A",pfx+"T",pfx+"R"));
for (int nscene = 0; nscene < N; nscene++) {
if ( (data[7*nscene + 3]*data[7*nscene + 3] +
data[7*nscene + 4]*data[7*nscene + 4] +
data[7*nscene + 5]*data[7*nscene + 5]+
data[7*nscene + 6]*data[7*nscene + 6]) < 0.001) {
System.out.println(String.format("%3d\t-\t-\t-\t-\t-\t-\t-\t-\t-\t-", nscene));
continue;
}
Rotation rot = new Rotation(data[7*nscene + 3],data[7*nscene + 4],data[7*nscene + 5],data[7*nscene + 6], false);
double [] angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
System.out.println(String.format("%3d"+
......
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