Commit 76f96ef8 authored by Andrey Filippov's avatar Andrey Filippov

working snapshot (working on IMU-to-camera rotation calibration)

parent 77381e24
...@@ -249,7 +249,7 @@ public class Imx5 { ...@@ -249,7 +249,7 @@ public class Imx5 {
* Adjsut IMS orientation relative to the camera (or opposite?) by a correction quaternion * Adjsut IMS orientation relative to the camera (or opposite?) by a correction quaternion
* @param ims_atr original ATR of the IMS mount * @param ims_atr original ATR of the IMS mount
* @param corr_q correction quaternion * @param corr_q correction quaternion
* @return corrected ATR of teh IMU mount * @return corrected ATR of the IMU mount
*/ */
public static double [] adjustMountAtrByQuat( public static double [] adjustMountAtrByQuat(
double [] ims_atr, double [] ims_atr,
......
...@@ -5181,7 +5181,7 @@ public class Interscene { ...@@ -5181,7 +5181,7 @@ public class Interscene {
debugLevel); // int debugLevel debugLevel); // int debugLevel
if (debugLevel > -3) { if (debugLevel > -3) {
Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
System.out.println("Applying correction ot the IMS to world orientation (rotating around IMS vertical):"); System.out.println("Applying correction to the IMS to world orientation (rotating around IMS vertical):");
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV); double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3]; double [] corr_degrees = new double[3];
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI; for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
...@@ -5476,20 +5476,20 @@ public class Interscene { ...@@ -5476,20 +5476,20 @@ public class Interscene {
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
QuadCLT ref_scene = quadCLTs[ref_index]; QuadCLT ref_scene = quadCLTs[ref_index];
ErsCorrection ers_reference = ref_scene.getErsCorrection(); ErsCorrection ers_reference = ref_scene.getErsCorrection();
double [][] quat_lma_xyz = new double [quadCLTs.length][3]; double [][][] quat_lma_xyzatr = new double [quadCLTs.length][2][3];
double [][] quat_lma_enu_xyz = new double [quadCLTs.length][3]; double [][][] quat_lma_enu_xyzatr = new double [quadCLTs.length][2][3];
Did_ins_2 d2_ref = quadCLTs[ref_index].did_ins_2; Did_ins_2 d2_ref = quadCLTs[ref_index].did_ins_2;
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) { for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
QuadCLT scene = quadCLTs[nscene]; QuadCLT scene = quadCLTs[nscene];
if (nscene == ref_index) { if (nscene == ref_index) {
quat_lma_xyz[nscene] = new double[3]; quat_lma_xyzatr[nscene][0] = new double[3];
} else { } else {
String ts = scene.getImageName(); String ts = scene.getImageName();
quat_lma_xyz[nscene] = ers_reference.getSceneXYZ(ts); quat_lma_xyzatr[nscene][0] = ers_reference.getSceneXYZ(ts);
} }
Did_ins_2 d2 = scene.did_ins_2; Did_ins_2 d2 = scene.did_ins_2;
double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla); double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
quat_lma_enu_xyz[nscene] = Imx5.applyQuaternionTo( quat_lma_enu_xyzatr[nscene][0] = Imx5.applyQuaternionTo(
Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu, Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
ims_mount_atr, ims_mount_atr,
ims_ortho), ims_ortho),
...@@ -5508,8 +5508,8 @@ public class Interscene { ...@@ -5508,8 +5508,8 @@ public class Interscene {
int debug_level = 1; int debug_level = 1;
QuaternionLma quaternionLma = new QuaternionLma(); QuaternionLma quaternionLma = new QuaternionLma();
quaternionLma.prepareLMA( quaternionLma.prepareLMA(
quat_lma_enu_xyz, // quat_lma_xyz, // double [][] vect_x, quat_lma_enu_xyzatr, // quat_lma_xyz, // double [][] vect_x,
quat_lma_xyz, // double [][] vect_y, quat_lma_xyzatr, // double [][] vect_y,
null, // double [][] vect_w, all same weight null, // double [][] vect_w, all same weight
reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1 reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
quat0, // double [] quat0, quat0, // double [] quat0,
......
...@@ -5425,68 +5425,13 @@ public class OpticalFlow { ...@@ -5425,68 +5425,13 @@ public class OpticalFlow {
} }
// later move to the right place // later move to the right place
if (adjust_imu_orient) { // (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( QuadCLT.adjustImuOrient(
clt_parameters, // final CLTParameters clt_parameters, clt_parameters, //CLTParameters clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs, quadCLTs, // QuadCLT[] quadCLTs,
ref_index, // final int ref_index, ref_index, // int ref_index,
null, // double [][][] dxyzatr, earliest_scene, // int earliest_scene,
earliest_scene, // final int early_index, last_index, // int last_index,
last_index //(quadCLTs.length -1) // int last_index, debugLevel); // int debugLevel
);
double [][][] xyzatr = new double [quadCLTs.length][][];
ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
for (int nscene = earliest_scene; nscene <= last_index; nscene++) {
String ts = quadCLTs[nscene].getImageName();
xyzatr[nscene] = ers_ref.getSceneXYZATR(ts);
}
double [] rms = new double[5];
double [] quat = new double[4];
int quat_lma_mode = 03; // 4; // 3; // 2; // 1;
int debug_lev = debugLevel; // 3;
double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
double translation_weight = 1.0 / (avg_z + 1.0);
double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
clt_parameters, // CLTParameters clt_parameters,
quat_lma_mode, // int quat_lma_mode,
avg_z, // double avg_height,
translation_weight, // double translation_weight,
quadCLTs, // QuadCLT[] quadCLTs,
xyzatr, // double [][][] xyzatr,
pimu_xyzatr, // double [][][] ims_xyzatr,
ref_index, // int ref_index,
earliest_scene, // int early_index,
last_index, // int last_index,
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 (run_ly) {
if (debugLevel > -3) { if (debugLevel > -3) {
...@@ -5602,7 +5547,7 @@ public class OpticalFlow { ...@@ -5602,7 +5547,7 @@ public class OpticalFlow {
debugLevel+1); debugLevel+1);
if (debugLevel > -3) { if (debugLevel > -3) {
Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
System.out.println("Applying correction ot the IMS to world orientation (rotating around IMS vertical):"); System.out.println("Applying correction to the IMS to world orientation (rotating around IMS vertical):");
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV); double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3]; double [] corr_degrees = new double[3];
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI; for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
......
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