Commit d693f718 authored by Andrey Filippov's avatar Andrey Filippov

added East-North-Up that better matches camera orientation. Tested

parent 4d5b60b2
...@@ -27,6 +27,16 @@ public class Did_ins_1 extends Did_ins <Did_ins_1>{ ...@@ -27,6 +27,16 @@ public class Did_ins_1 extends Did_ins <Did_ins_1>{
/** North, east and down (meters) offset from reference latitude, longitude, and altitude to current latitude, longitude, and altitude */ /** North, east and down (meters) offset from reference latitude, longitude, and altitude to current latitude, longitude, and altitude */
public float [] ned = new float [3]; public float [] ned = new float [3];
public double [] getTheta() {
return new double [] {theta[0], theta[1], theta[2]};
}
public double [] getUvw() {
return new double [] {uvw[0], uvw[1], uvw[2]};
}
public double [] getNed() {
return new double [] {ned[0], ned[1], ned[2]};
}
public Did_ins_1 (ByteBuffer bb) { public Did_ins_1 (ByteBuffer bb) {
bb.order(ByteOrder.LITTLE_ENDIAN); bb.order(ByteOrder.LITTLE_ENDIAN);
week= bb.getInt(); week= bb.getInt();
......
...@@ -5,6 +5,8 @@ import java.nio.ByteBuffer; ...@@ -5,6 +5,8 @@ import java.nio.ByteBuffer;
import java.nio.ByteOrder; import java.nio.ByteOrder;
import java.util.Properties; import java.util.Properties;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import com.elphel.imagej.tileprocessor.IntersceneMatchParameters; import com.elphel.imagej.tileprocessor.IntersceneMatchParameters;
//public class Did_ins_1 implements Serializable { //public class Did_ins_1 implements Serializable {
...@@ -39,7 +41,25 @@ public class Did_ins_2 extends Did_ins <Did_ins_2>{ ...@@ -39,7 +41,25 @@ public class Did_ins_2 extends Did_ins <Did_ins_2>{
public Did_ins_2(String prefix, Properties properties) { public Did_ins_2(String prefix, Properties properties) {
getProperties(prefix, properties); getProperties(prefix, properties);
} }
public double [] getQn2b() {
return new double [] {qn2b[0], qn2b[1], qn2b[2], qn2b[3]};
}
public double [] getQEnu () {
Rotation rot_enu_ned = new Rotation (0, Math.sqrt(0.5), Math.sqrt(0.5), 0, true);
Rotation quat_rot = new Rotation(qn2b[0],qn2b[1],qn2b[2],qn2b[3],true);
Rotation quat_enu_rot = quat_rot.applyTo(rot_enu_ned);
return new double[] {
quat_enu_rot.getQ0(),
quat_enu_rot.getQ1(),
quat_enu_rot.getQ2(),
quat_enu_rot.getQ3()};
}
public double [] getUvw() {
return new double [] {uvw[0], uvw[1], uvw[2]};
}
public Did_ins_2 interpolate(double frac, Did_ins_2 next_did) { public Did_ins_2 interpolate(double frac, Did_ins_2 next_did) {
Did_ins_2 new_did = new Did_ins_2(); Did_ins_2 new_did = new Did_ins_2();
......
...@@ -163,6 +163,18 @@ public class Imx5 { ...@@ -163,6 +163,18 @@ public class Imx5 {
// static final RotationConvention ROT_CONV = RotationConvention.FRAME_TRANSFORM; // static final RotationConvention ROT_CONV = RotationConvention.FRAME_TRANSFORM;
// RotationConvention.VECTOR_OPERATOR // RotationConvention.VECTOR_OPERATOR
//RotationOrder.YXZ, ROT_CONV //RotationOrder.YXZ, ROT_CONV
// move to did?
public static double [] quatEnu (double [] quat_ned) {
Rotation rot_enu_ned = new Rotation (0, Math.sqrt(0.5), Math.sqrt(0.5), 0, true);
Rotation quat_rot = new Rotation(quat_ned[0],quat_ned[1],quat_ned[2],quat_ned[3],true);
Rotation quat_enu_rot = quat_rot.applyTo(rot_enu_ned);
return new double[] {
quat_enu_rot.getQ0(),
quat_enu_rot.getQ1(),
quat_enu_rot.getQ2(),
quat_enu_rot.getQ3()};
}
public static double [] applyQuternionTo(double[]quat, double[] vector, boolean inverse) { public static double [] applyQuternionTo(double[]quat, double[] vector, boolean inverse) {
Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true); Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
double [] rslt = new double[3]; double [] rslt = new double[3];
...@@ -202,7 +214,23 @@ public class Imx5 { ...@@ -202,7 +214,23 @@ public class Imx5 {
return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()}; return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
} }
public static double [] quaternionImsToCam(
double[]quat,
double [] ims_atr,
double [] quat_ort) {
Rotation ims_to_mount_ortho = new Rotation(quat_ort[0],quat_ort[1],quat_ort[2],quat_ort[3],true);
Rotation ims_to_ned = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
Rotation mount_to_cam = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
ims_atr[0], ims_atr[1], ims_atr[2]);
Rotation mount_to_ned = ims_to_mount_ortho.applyTo(ims_to_ned);
Rotation cam_quat = mount_to_cam.applyTo(mount_to_ned);
return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
}
public static double [] quatToCamAtr(double[]quat) {
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
return rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
}
public static double [] imsToCamRotations(double [] ims_theta, int ord, boolean rev_order, boolean rev_matrix ) { public static double [] imsToCamRotations(double [] ims_theta, int ord, boolean rev_order, boolean rev_matrix ) {
RotationConvention rc = RotationConvention.FRAME_TRANSFORM; RotationConvention rc = RotationConvention.FRAME_TRANSFORM;
...@@ -264,4 +292,14 @@ public class Imx5 { ...@@ -264,4 +292,14 @@ public class Imx5 {
return ned; return ned;
} }
public static double [] enuFromLla (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;
}
} }
...@@ -37,8 +37,17 @@ public class IntersceneMatchParameters { ...@@ -37,8 +37,17 @@ public class IntersceneMatchParameters {
public boolean ims_use = true; // use IMS data public boolean ims_use = true; // use IMS data
public boolean ims_rebuild = false; // recalculate INS data (after lag change) public boolean ims_rebuild = false; // recalculate INS data (after lag change)
public double ims_offset = 0.0; // IMS data lead relative to images (seconds) public double ims_offset = 0.0; // IMS data lead relative to images (seconds)
public double gmt_plus = 3.0; // (hrs) filename timestamp is this lower than in IMU log public double gmt_plus = 3.0; // (hrs) filename timestamp is this lower than in IMU log
public double [] ims_ortho = {0.5, -0.5, 0.5, -0.5}; // approximate (90-deg) IMS to camera
public double [] ims_mount_atr = {0, 0, 0}; // IMS mount fine correction (A,T,R around camera axes)
public double [] ims_mount_xyz = {0, 0, 0}; // IMS center in camera coordinates
public double [] getImsMountATR() {
return new double [] {
ims_mount_atr[0] * Math.PI/180,
ims_mount_atr[1] * Math.PI/180,
ims_mount_atr[2] * Math.PI/180};
}
public boolean force_ref_dsi = false; // true; public boolean force_ref_dsi = false; // true;
public boolean force_orientations = false; public boolean force_orientations = false;
public boolean run_ly = false; // will return just after LY adjustments, skipping all output generation public boolean run_ly = false; // will return just after LY adjustments, skipping all output generation
...@@ -217,6 +226,9 @@ public class IntersceneMatchParameters { ...@@ -217,6 +226,9 @@ public class IntersceneMatchParameters {
public double disp_boost_amount = 2.0; public double disp_boost_amount = 2.0;
public double scale_combo_strength = 0.4; // reduce strength when it comes from combo, not DSI-MAIN public double scale_combo_strength = 0.4; // reduce strength when it comes from combo, not DSI-MAIN
public boolean save_debug_images = true; // Save blue sky debug images in model directory public boolean save_debug_images = true; // Save blue sky debug images in model directory
/// IMS intergration
// Some "AGC" to adjust how much to discard // Some "AGC" to adjust how much to discard
public int margin = 1; // do not use tiles if their centers are closer to the image edge public int margin = 1; // do not use tiles if their centers are closer to the image edge
...@@ -402,17 +414,26 @@ public class IntersceneMatchParameters { ...@@ -402,17 +414,26 @@ public class IntersceneMatchParameters {
public void dialogQuestions(GenericJTabbedDialog gd) { public void dialogQuestions(GenericJTabbedDialog gd) {
// gd.addMessage ("Scene parameters selection"); // gd.addMessage ("Scene parameters selection");
// gd.addTab ("Inter-Match", "Parameters for full-resolution (no decimation/macrotiles) scene matching"); // gd.addTab ("Inter-Match", "Parameters for full-resolution (no decimation/macrotiles) scene matching");
gd.addTab ("Scene Series", "Processing series of scenes and multi-series sets"); gd.addTab ("IMS", "IMS Integration for interscene matching");
gd.addMessage ("Build series options");
gd.addCheckbox ("Use IMS data", this.ims_use, gd.addCheckbox ("Use IMS data", this.ims_use,
"Use INS data for egomotion."); "Use IMS data for egomotion.");
gd.addCheckbox ("Recalculate IMS", this.ims_rebuild, gd.addCheckbox ("Recalculate IMS", this.ims_rebuild,
"Recalculate scene INS data."); "Recalculate scene INS data.");
gd.addNumericField("IMS offset", this.ims_offset, 5,8,"s", gd.addNumericField("IMS offset", this.ims_offset, 5,8,"s",
"Positive if the IMS data has lower lag than images."); "Positive if the IMS data has lower lag than images.");
gd.addNumericField("GMT plus", this.gmt_plus, 5,8,"hrs", gd.addNumericField("GMT plus", this.gmt_plus, 5,8,"hrs",
"Timestamp filename is lower by this hours than in the IMU log."); "Timestamp filename is lower by this hours than in the IMU log.");
gd.addStringField ("IMS ortho mount quaterion", IntersceneMatchParameters.doublesToString(ims_ortho), 80,
"Approximate (90-deg) IMS to camera {0.5, -0.5, 0.5, -0.5}.");
gd.addStringField ("IMS mount ATR correction (deg)", IntersceneMatchParameters.doublesToString(ims_mount_atr), 80,
"IMS mount fine correction (A,T,R around camera axes).");
gd.addStringField ("IMS mount XYZ correction (m)", IntersceneMatchParameters.doublesToString(ims_mount_xyz), 80,
"MS center (X,Y,Z m) in camera coordinates.");
gd.addTab ("Scene Series", "Processing series of scenes and multi-series sets");
gd.addMessage ("Build series options");
gd.addCheckbox ("Force reference scene DSI calculation", this.force_ref_dsi, gd.addCheckbox ("Force reference scene DSI calculation", this.force_ref_dsi,
"Calculate reference scene DSI even if the file exists."); "Calculate reference scene DSI even if the file exists.");
gd.addCheckbox ("Force egomotion calculation", this.force_orientations, gd.addCheckbox ("Force egomotion calculation", this.force_orientations,
...@@ -1072,6 +1093,9 @@ public class IntersceneMatchParameters { ...@@ -1072,6 +1093,9 @@ public class IntersceneMatchParameters {
this.ims_rebuild = gd.getNextBoolean(); this.ims_rebuild = gd.getNextBoolean();
this.ims_offset = gd.getNextNumber(); this.ims_offset = gd.getNextNumber();
this.gmt_plus = gd.getNextNumber(); this.gmt_plus = gd.getNextNumber();
this.ims_ortho = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 4);
this.ims_mount_atr = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.ims_mount_xyz = IntersceneMatchParameters. StringToDoubles(gd.getNextString(), 3);
this.force_ref_dsi = gd.getNextBoolean(); this.force_ref_dsi = gd.getNextBoolean();
this.force_orientations = gd.getNextBoolean(); this.force_orientations = gd.getNextBoolean();
this.run_ly = gd.getNextBoolean(); this.run_ly = gd.getNextBoolean();
...@@ -1420,12 +1444,12 @@ public class IntersceneMatchParameters { ...@@ -1420,12 +1444,12 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"ims_rebuild", this.ims_rebuild + ""); // boolean properties.setProperty(prefix+"ims_rebuild", this.ims_rebuild + ""); // boolean
properties.setProperty(prefix+"ims_offset", this.ims_offset+""); // double properties.setProperty(prefix+"ims_offset", this.ims_offset+""); // double
properties.setProperty(prefix+"gmt_plus", this.gmt_plus+""); // double properties.setProperty(prefix+"gmt_plus", this.gmt_plus+""); // double
properties.setProperty(prefix+"ims_ortho", IntersceneMatchParameters.doublesToString(this.ims_ortho));
properties.setProperty(prefix+"ims_mount_atr", IntersceneMatchParameters.doublesToString(this.ims_mount_atr));
properties.setProperty(prefix+"ims_mount_xyz", IntersceneMatchParameters.doublesToString(this.ims_mount_xyz));
properties.setProperty(prefix+"force_ref_dsi", this.force_ref_dsi + ""); // boolean properties.setProperty(prefix+"force_ref_dsi", this.force_ref_dsi + ""); // boolean
properties.setProperty(prefix+"force_orientations", this.force_orientations + ""); // boolean properties.setProperty(prefix+"force_orientations", this.force_orientations + ""); // boolean
properties.setProperty(prefix+"run_ly", this.run_ly + ""); // boolean properties.setProperty(prefix+"run_ly", this.run_ly + ""); // boolean
// properties.setProperty(prefix+"readjust_orient", this.readjust_orient + ""); // boolean
// properties.setProperty(prefix+"force_interscene", this.force_interscene + ""); // boolean
properties.setProperty(prefix+"min_num_orient", this.min_num_orient+""); // int 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+"min_num_interscene", this.min_num_interscene+""); // int
properties.setProperty(prefix+"generate_egomotion", this.generate_egomotion+""); // boolean properties.setProperty(prefix+"generate_egomotion", this.generate_egomotion+""); // boolean
...@@ -1732,6 +1756,12 @@ public class IntersceneMatchParameters { ...@@ -1732,6 +1756,12 @@ public class IntersceneMatchParameters {
if (properties.getProperty(prefix+"ims_offset")!=null) this.ims_offset=Double.parseDouble(properties.getProperty(prefix+"ims_offset")); if (properties.getProperty(prefix+"ims_offset")!=null) this.ims_offset=Double.parseDouble(properties.getProperty(prefix+"ims_offset"));
if (properties.getProperty(prefix+"gmt_plus")!=null) this.gmt_plus=Double.parseDouble(properties.getProperty(prefix+"gmt_plus")); if (properties.getProperty(prefix+"gmt_plus")!=null) this.gmt_plus=Double.parseDouble(properties.getProperty(prefix+"gmt_plus"));
if (properties.getProperty(prefix+"ims_ortho")!=null) this.ims_ortho= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_ortho"),4);
if (properties.getProperty(prefix+"ims_mount_atr")!=null) this.ims_mount_atr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"ims_mount_atr"),3);
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+"force_ref_dsi")!=null) this.force_ref_dsi=Boolean.parseBoolean(properties.getProperty(prefix+"force_ref_dsi")); if (properties.getProperty(prefix+"force_ref_dsi")!=null) this.force_ref_dsi=Boolean.parseBoolean(properties.getProperty(prefix+"force_ref_dsi"));
if (properties.getProperty(prefix+"force_orientations")!=null) this.force_orientations=Boolean.parseBoolean(properties.getProperty(prefix+"force_orientations")); if (properties.getProperty(prefix+"force_orientations")!=null) this.force_orientations=Boolean.parseBoolean(properties.getProperty(prefix+"force_orientations"));
if (properties.getProperty(prefix+"run_ly")!=null) this.run_ly=Boolean.parseBoolean(properties.getProperty(prefix+"run_ly")); if (properties.getProperty(prefix+"run_ly")!=null) this.run_ly=Boolean.parseBoolean(properties.getProperty(prefix+"run_ly"));
...@@ -2068,6 +2098,10 @@ public class IntersceneMatchParameters { ...@@ -2068,6 +2098,10 @@ public class IntersceneMatchParameters {
imp.ims_offset = this.ims_offset; imp.ims_offset = this.ims_offset;
imp.gmt_plus = this.gmt_plus; imp.gmt_plus = this.gmt_plus;
imp.ims_ortho = this.ims_ortho.clone();
imp.ims_mount_atr = this.ims_mount_atr.clone();
imp.ims_mount_xyz = this.ims_mount_xyz.clone();
imp.force_ref_dsi = this.force_ref_dsi; imp.force_ref_dsi = this.force_ref_dsi;
imp.force_orientations = this.force_orientations; imp.force_orientations = this.force_orientations;
imp.run_ly = this.run_ly; imp.run_ly = this.run_ly;
......
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