Skip to content

I
imagej-elphel
Commit 77381e24 authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

Unfinished mods

parent 2014ae08
Hide whitespace changes
Inline Side-by-side
Showing with 502 additions and 58 deletions
src/main/java/com/elphel/imagej/ims/Imx5.java
View file @ 77381e24
......@@ -9,6 +9,7 @@ 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;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import com.elphel.imagej.tileprocessor.ErsCorrection;
......@@ -206,6 +207,18 @@ public class Imx5 {
rslt_rot.getQ2(),
rslt_rot.getQ3()};
}
/**
* Get up (for IMS) vector relative to the camera reference frame
* @param ims_atr mount to camera correction
* @return {x,y,z} of the IMS-up relative to the camera frame (reference scene)
*/
public static double [] getUpAxis(
double [] ims_atr ) {// -> mount_to_cam
Rotation mount_to_cam = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
ims_atr[0], ims_atr[1], ims_atr[2]);
return mount_to_cam.applyInverseTo(new Vector3D(0, 0, 1)).toArray();
}
public static double [] quaternionImsToCam(
......
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ 77381e24
......@@ -263,7 +263,9 @@ public class Interscene {
ref_index,// ref_indx,
cent_index, // earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment); // String comment);
ego_comment, // String comment);
debugLevel); // int debugLevel);
if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path);
}
......@@ -335,7 +337,8 @@ public class Interscene {
cent_index,// ref_indx,
earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment); // String comment);
ego_comment, // String comment);
debugLevel); // int debugLevel)
if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path);
}
......@@ -370,7 +373,8 @@ public class Interscene {
cent_index,// ref_indx,
earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment); // String comment);
ego_comment, // String comment);
debugLevel); // int debugLevel)
if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path);
}
......@@ -2043,7 +2047,8 @@ public class Interscene {
earliest_scene, // int start_scene,
last_scene, // int end1_scene,
scenes_xyzatr, // double [][][] scenes_xyzatr,
half_run_range); // double half_run_range
half_run_range, // double half_run_range
debugLevel); // int debugLevel);
break;
default: // do nothing - already read
}
......@@ -2084,7 +2089,8 @@ public class Interscene {
ref_index, // int ref_index,
earliest_scene, // int early_index,
last_scene); // int last_index)
} else if (readjust_xy_ims && (reg_weight_xy > 0.0)) {
// } else if (readjust_xy_ims && (reg_weight_xy > 0.0)) {
} else if (readjust_xy_ims) {
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
......@@ -2095,7 +2101,7 @@ public class Interscene {
);
double [] rms = new double[5];
double [] quat = new double[4];
int quat_lma_mode = 2; // 1; // 2;
int quat_lma_mode = QuaternionLma.MODE_COMBO_LOCAL; // 2; // 1; // 2;
int debug_lev = debugLevel; // 3;
// double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
double translation_weight = 1.0 / (avg_z + 1.0);
......@@ -5155,7 +5161,8 @@ public class Interscene {
int ref_index,
int earliest_scene,
String path,
String comment) {
String comment,
int debugLevel) {
double [] ims_ortho = clt_parameters.imp.ims_ortho;
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
QuadCLT ref_scene = quadCLTs[ref_index];
......@@ -5170,8 +5177,18 @@ public class Interscene {
quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index,
earliest_scene, // int earliest_scene,
rms); // double [] rms // null or double[2];
rms, // double [] rms // null or double[2];
debugLevel); // int debugLevel
if (debugLevel > -3) {
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):");
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("quatCorr=["+quatCorr[0]+", "+quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[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]+"]");
}
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
QuadCLT scene = quadCLTs[nscene];
......@@ -5196,6 +5213,7 @@ public class Interscene {
"\tINS->X\tINS->Y\tINS->Z"+
"\tabs_A_ned\tabs_T_ned\tabs_R_ned\trel_A_ned\trel_T_ned\trel_R_ned"+
"\tabs_A_enu\tabs_T_enu\tabs_R_enu\trel_A_enu\trel_T_enu\trel_R_enu"+
"\traw_A_enu\traw_T_enu\traw_R_enu"+
"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
......@@ -5216,7 +5234,8 @@ public class Interscene {
earliest_scene, // int start_scene,
quadCLTs.length-1, // int end1_scene,
scenes_xyzatr, // double [][][] scenes_xyzatr, // 5.0
clt_parameters.ofp.lpf_series); // half_run_range); // double half_run_range
clt_parameters.ofp.lpf_series, // half_run_range); // double half_run_range
debugLevel); // int debugLevel);
}
......@@ -5324,7 +5343,7 @@ public class Interscene {
double [] ned = Imx5.nedFromLla (d2.lla, d2_ref.lla);
double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
double [] ims_xyz = Imx5.applyQuaternionTo(double_qn2b, ned, false);
//"\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"
sb.append("\t"+ned[0]+ "\t"+ned[1]+ "\t"+ned[2]); // global axes
sb.append("\t"+ims_xyz[0]+ "\t"+ims_xyz[1]+ "\t"+ims_xyz[2]); // imu axes
......@@ -5335,6 +5354,9 @@ public class Interscene {
double [] cam_quat_enu =Imx5.quaternionImsToCam(d2.getQEnu(),
ims_mount_atr, // new double[] {0, 0.13, 0},
ims_ortho);
double [] cam_quat_enu_raw =Imx5.quaternionImsToCam(d2.getQEnu(),
new double[3], // new double[] {0, 0.13, 0},
ims_ortho);
double [] cam_xyz1 = Imx5.applyQuaternionTo(cam_quat1, ned, false);
double [] cam_xyz2 = Imx5.applyQuaternionTo(cam_quat2, ned, false);
......@@ -5352,14 +5374,16 @@ 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
//"\tcam_X1\tcam_Y1\tcam_Z1\tcam_X2\tcam_Y2\tcam_Z2"
// NED without ims_mount_atr correction \tcam_X1\tcam_Y1\tcam_Z1
sb.append("\t"+cam_xyz1[0]+ "\t"+cam_xyz1[1]+ "\t"+cam_xyz1[2]); //
// NED with ims_mount_atr correction
// NED with ims_mount_atr correction \tcam_X2\tcam_Y2\tcam_Z2
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]); // 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
//"\tned->X\tned->Y\tned->Z\tenu->X\tenu->Y\tenu->Z"
sb.append("\t"+cam_xyz_ned[0]+ "\t"+cam_xyz_ned[1]+ "\t"+cam_xyz_ned[2]); // \tned->X\tned->Y\tned->Z
sb.append("\t"+cam_xyz_enu[0]+ "\t"+cam_xyz_enu[1]+ "\t"+cam_xyz_enu[2]); // WITH ims_mount_atr, NO quatCorr \tenu->X\tenu->Y\tenu->Z
//"\tINS->X\tINS->Y\tINS->Z"
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 "\tINS->X\tINS->Y\tINS->Z"
double [] scene_abs_atr = Imx5.quatToCamAtr(cam_quat2);
......@@ -5370,6 +5394,7 @@ public class Interscene {
double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
double [][] ims_scene_xyzatr_enu = {ZERO3, scene_abs_atr_enu};
double [] scene_raw_atr_enu = Imx5.quatToCamAtr(cam_quat_enu_raw); // not corrected by ims_mount_atr
double [] scene_rel_atr_enu=ErsCorrection.combineXYZATR(
ims_scene_xyzatr_enu,
......@@ -5380,6 +5405,8 @@ public class Interscene {
// "\tabs_A_enu\tabs_T_enu\tabs_R_enu\trel_A_enu\trel_T_enu\trel_R_enu"
sb.append("\t"+scene_abs_atr_enu[0]+ "\t"+scene_abs_atr_enu[1]+ "\t"+scene_abs_atr_enu[2]); //
sb.append("\t"+scene_rel_atr_enu[0]+ "\t"+scene_rel_atr_enu[1]+ "\t"+scene_rel_atr_enu[2]); //
// "\traw_A_enu\traw_T_enu\traw_R_enu"+
sb.append("\t"+scene_raw_atr_enu[0]+ "\t"+scene_raw_atr_enu[1]+ "\t"+scene_raw_atr_enu[2]); //
// "\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv"
sb.append("\t"+uvw_dir[0]+ "\t"+uvw_dir[1]+ "\t"+uvw_dir[2]); // wrong
sb.append("\t"+uvw_inv[0]+ "\t"+uvw_inv[1]+ "\t"+uvw_inv[2]); // correct
......@@ -5412,9 +5439,20 @@ public class Interscene {
sb.append("\n");
}
// test QuaternionLMA here
// Add another data
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;
sb.append("New ATR mount (rad):[\t"+new_atr[0]+"\t"+new_atr[1]+"\t"+new_atr[2]+"]\n");
sb.append("New ATR mount (deg):[\t"+degrees[0]+"\t"+degrees[1]+"\t"+degrees[2]+"]\n");
// double [] quatCorr
Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
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;
sb.append("quatCorr=[\t"+quatCorr[0]+"\t"+quatCorr[1]+"\t"+quatCorr[2]+"\t"+quatCorr[3]+"]\n");
sb.append("ATR(rad)=[\t"+corr_angles[0]+"\t"+corr_angles[1]+"\t"+corr_angles[2]+"]\n");
sb.append("ATR(deg)=[\t"+corr_degrees[0]+"\t"+corr_degrees[1]+"\t"+corr_degrees[2]+"]\n");
if (path!=null) {
String footer=(comment != null) ? ("Comment: "+comment): "";
......@@ -5425,8 +5463,9 @@ public class Interscene {
new TextWindow("Sharpness History", header, sb.toString(), 1000,900);
}
}
public static double [] getQuaternionCorrection(
@Deprecated
// adjusts by all 3 axis rotation
public static double [] getQuaternionCorrection_Old(
CLTParameters clt_parameters,
QuadCLT [] quadCLTs,
int ref_index,
......@@ -5503,6 +5542,90 @@ public class Interscene {
return quaternionLma.getQuaternion();
}
}
public static double [] getQuaternionCorrection(
CLTParameters clt_parameters,
QuadCLT [] quadCLTs,
int ref_index,
int earliest_scene,
double [] rms, // null or double[2];
int debugLevel
) {
double [] ims_ortho = clt_parameters.imp.ims_ortho;
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
QuadCLT ref_scene = quadCLTs[ref_index];
ErsCorrection ers_reference = ref_scene.getErsCorrection();
double [][] quat_lma_xyz = new double [quadCLTs.length][];
double [][] quat_lma_enu_xyz = new double [quadCLTs.length][];
Did_ins_2 d2_ref = quadCLTs[ref_index].did_ins_2;
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
QuadCLT scene = quadCLTs[nscene];
if (nscene == ref_index) {
quat_lma_xyz[nscene] = new double[3];
} else {
String ts = scene.getImageName();
quat_lma_xyz[nscene] = ers_reference.getSceneXYZ(ts);
}
Did_ins_2 d2 = scene.did_ins_2;
double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
quat_lma_enu_xyz[nscene] = Imx5.applyQuaternionTo(
Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
ims_mount_atr,
ims_ortho),
enu,
false);
}
double [] up_axis = Imx5.getUpAxis(
ims_mount_atr); // double [] ims_atr )
double lambda = clt_parameters.imp.quat_lambda; // 0.1;
double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
double lambda_scale_bad = clt_parameters.imp.quat_lambda_scale_bad; // 8.0;
double lambda_max = clt_parameters.imp.quat_lambda_max; // 100;
double rms_diff = clt_parameters.imp.quat_rms_diff; // 0.001;
int num_iter = clt_parameters.imp.quat_num_iter; // 20;
boolean last_run = false;
int debug_level = debugLevel;
QuaternionLma quaternionLma = new QuaternionLma();
quaternionLma.prepareCompassLMA(
quat_lma_enu_xyz, // quat_lma_xyz, // double [][] vect_x,
quat_lma_xyz, // double [][] vect_y,
null, // double [][] vect_w, all same weight
up_axis, // double [] vector_up, // Up in the IMS axes nearest to the camera Z (rotated by - ims_mount_atr)
debug_level); // int debug_level)
int lma_result = quaternionLma.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
lambda, // double lambda, // 0.1
lambda_scale_good,// double lambda_scale_good,// 0.5
lambda_scale_bad, // double lambda_scale_bad, // 8.0
lambda_max, // double lambda_max, // 100
rms_diff, // double rms_diff, // 0.001
num_iter, // int num_iter, // 20
last_run, // boolean last_run,
debug_level); // int debug_level)
if (lma_result < 0) {
return null;
} else {
if (rms != null) { // null or double[2];
double [] last_rms = quaternionLma.getLastRms();
rms[0] = last_rms[0];
rms[1] = last_rms[1];
if (rms.length >= 4) {
double [] initial_rms = quaternionLma.getInitialRms();
rms[2] = initial_rms[0];
rms[3] = initial_rms[1];
if (rms.length >= 5) {
rms[4] = lma_result;
}
}
}
if (debugLevel > -3) {
System.out.println("getQuaternionCorrection(): Rotated around IMS-vertical by "+quaternionLma.getQuaternion()[0]+" rad");
System.out.println("getQuaternionCorrection(): Rotated around IMS-vertical by "+quaternionLma.getQuaternion()[0]*180/Math.PI+" degrees");
}
return quaternionLma.getAxisQuat();
}
}
static double [] test_xyz_ned(
double [] ned,
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 77381e24
......@@ -5032,7 +5032,9 @@ public class OpticalFlow {
ref_index,// ref_indx,
earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment); // String comment);
ego_comment, // String comment);
debugLevel); // int debugLevel);
if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path);
}
......@@ -5386,7 +5388,8 @@ public class OpticalFlow {
ref_index,// ref_indx,
earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment); // String comment);
ego_comment, // String comment);
debugLevel); // int debugLevel)
if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path);
}
......@@ -5438,7 +5441,7 @@ public class OpticalFlow {
}
double [] rms = new double[5];
double [] quat = new double[4];
int quat_lma_mode = 4; // 3; // 2; // 1;
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);
......@@ -5583,7 +5586,8 @@ public class OpticalFlow {
quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index,
earliest_scene, // int earliest_scene,
quat_rms); // double [] rms // null or double[2];
quat_rms, // double [] rms // null or double[2];
debugLevel); // int debugLevel
if (quatCorr != null) {
int num_iter = (int) quat_rms[4];
if (debugLevel> -3) {
......@@ -5596,6 +5600,16 @@ public class OpticalFlow {
quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
null, // String path, // full name with extension or w/o path to use x3d directory
debugLevel+1);
if (debugLevel > -3) {
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):");
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("quatCorr=["+quatCorr[0]+", "+quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[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]+"]");
}
} else {
if (debugLevel> -3) {
System.out.println("Failed to perform attitude correction with QuaternionLma.");
......@@ -5616,7 +5630,9 @@ public class OpticalFlow {
ref_index,// ref_indx,
earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment); // String comment);
ego_comment, // String comment);
debugLevel); // int debugLevel)
if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path);
}
......@@ -5628,7 +5644,8 @@ public class OpticalFlow {
ref_index,// ref_indx,
earliest_scene, // int earliest_scene,
ego_path, // String path,
ego_comment); // String comment);
ego_comment, // String comment);
debugLevel); // int debugLevel)
}
}
boolean test_ground = false; // true;
......@@ -8256,7 +8273,8 @@ public class OpticalFlow {
int start_scene,
int end_scene,
double [][][] scenes_xyzatr, // <=0 use +/-1 or +0 if other are not available
double half_run_range
double half_run_range,
int debugLevel
){
double [][][] ers_xyzatr = new double [scenes_xyzatr.length][][];
if (half_run_range <=0 ) {
......@@ -8314,9 +8332,13 @@ public class OpticalFlow {
for (int m = 0; m < sy.length; m++) {
for (int d = 0; d < sy[m].length; d++) {
if (scenes_xyzatr[ns]== null ) {
System.out.println("getVelocitiesFromScenes():scenes_xyzatr["+ns+"]== null");
if (debugLevel > -1) {
System.out.println("getVelocitiesFromScenes():scenes_xyzatr["+ns+"]== null");
}
}else if (scenes_xyzatr[ns][m]== null ) {
System.out.println("getVelocitiesFromScenes():scenes_xyzatr["+ns+"]["+m+"] == null");
if (debugLevel > -1) {
System.out.println("getVelocitiesFromScenes():scenes_xyzatr["+ns+"]["+m+"] == null");
}
} else {
double y = scenes_xyzatr[ns][m][d];
sy [m][d] += w * y;
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 77381e24
......@@ -256,6 +256,7 @@ public class QuadCLTCPU {
double [] rms, // null or double[5];
int debugLevel
) {
final boolean use3 = (quat_lma_mode == 3); // true;
double lambda = clt_parameters.imp.quat_lambda; // 0.1;
double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
double lambda_scale_bad = clt_parameters.imp.quat_lambda_scale_bad; // 8.0;
......@@ -264,7 +265,7 @@ public class QuadCLTCPU {
int num_iter = clt_parameters.imp.quat_num_iter; // 20;
boolean last_run = false;
double reg_w = clt_parameters.imp.quat_reg_w; // 0.25;
double [] quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity
double [] quat0 = use3? new double[3] : new double [] {1.0, 0.0, 0.0, 0.0}; // identity
QuaternionLma quaternionLma = new QuaternionLma();
if ((quat_lma_mode == 2) || (quat_lma_mode == 4)) {
double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR
......
src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
View file @ 77381e24
......@@ -33,7 +33,19 @@ import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
import Jama.Matrix;
public class QuaternionLma {
private final static int REGLEN = 1; // number of extra (regularization) samples
private final static int REGLEN = 1; // number of extra (regularization) samples
public static final int MODE_XYZ = 0;
public static final int MODE_XYZQ = 1;
public static final int MODE_COMBO = 2;
public static final int MODE_XYZQ_LOCAL = 3;
public static final int MODE_COMBO_LOCAL = 4;
public static final int MODE_COMPASS = 5;
// public static final int MODE_XYZ3 = 6;
// public static final int MODE_XYZQ3 = 7;
// public static final int MODE_COMBO3 = 8;
// public static final int MODE_XYZQ_LOCAL3 = 9;
// public static final int MODE_COMBO_LOCAL3 = 10;
private int N = 0;
// Mode2 - compensating camera uncertainty:dpx/dx ~= -dpx/daz/height(m) dpy/dy ~= dpy/dtl/height(m)
private int mode = 0; // 0 xyz, 1 - xyz,quat, 2: Z/x, 2Q3, 2Q2-X/h, 2Q1+Y/h
......@@ -52,17 +64,85 @@ public class QuaternionLma {
private double pure_weight; // weight of samples only
private double [] last_ymfx = null;
private double [][] last_jt = null;
private double [] axis = null;
public double [] getQuaternion() {
if (parameters_vector.length == 3) {
return new double [] {
getQ0(parameters_vector),
parameters_vector[0],
parameters_vector[1],
parameters_vector[2]};
}
return parameters_vector;
}
public double [] getLastRms() {
return last_rms;
}
public double [] getInitialRms() {
return initial_rms;
}
public double [] getAxis() {
return axis;
}
public double [] getAxisQuat() {
double c = Math.cos(parameters_vector[0]/2), s = Math.sin(parameters_vector[0]/2);
return new double [] { c, s*axis[0], s*axis[1], s*axis[2]};
}
public void prepareCompassLMA(
double [][] vect_x, // GPS-derived X,Y,Z relative to the reference frame
double [][] vect_y, // Camera X,Y,Z relative to the reference frame
double [][] vect_w,
double [] vector_up, // Up in the IMS axes nearest to the camera Z (rotated by - ims_mount_atr)
final int debug_level) {
double axis_l = Math.sqrt(
vector_up[0]*vector_up[0]+
vector_up[1]*vector_up[1]+
vector_up[2]*vector_up[2]);
axis = new double [] {vector_up[0]/axis_l, vector_up[1]/axis_l, vector_up[2]/axis_l};
N = vect_x.length;
pure_weight = 1.0;
mode = MODE_COMPASS;
samples = 3;
samples_x = 3;
x_vector = new double [3* N];
y_vector = new double [3* N];
weights = new double [3* N];
parameters_vector = new double[1];
double sw = 0;
for (int i = 0; i < N; i++) {
if ((vect_x[i]== null) || (vect_y[i]== null)) {
for (int j = 0; j < samples; j++) {
x_vector[samples * i + j] = 0.0;
y_vector[samples * i + j] = 0.0;
weights[samples*i + j] = 0.0;
}
} else {
for (int j = 0; j < 3; j++) {
x_vector[samples * i + j] = vect_x[i][j];
y_vector[samples * i + j] = vect_y[i][j];
double w = (vect_w != null)? vect_w[i][j] : 1.0;
weights[samples*i + j] = w;
sw += w;
}
}
}
double k = (pure_weight)/sw;
for (int i = 0; i < weights.length; i++) weights[i] *= k;
last_jt = new double [parameters_vector.length][];
if (debug_level > 0) {
debugYfX ( "", // String pfx,
y_vector); // double [] data)
debugYfX ( "PIMU-", // String pfx,
x_vector); // double [] data)
}
}
public void prepareLMA(
double [][] vect_x,
double [][] vect_y,
......@@ -72,7 +152,7 @@ public class QuaternionLma {
final int debug_level) {
N = vect_x.length;
pure_weight = 1.0 - reg_w;
mode = 0;
mode = MODE_XYZ;
samples = 3;
samples_x = 3;
x_vector = new double [3* N];
......@@ -115,7 +195,7 @@ public class QuaternionLma {
double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
double [] quat0,
final int debug_level) {
if (mode != 1) {
if (mode != MODE_XYZQ) {
prepareLMAMode3(
mode, // int mode,
vect_x, // double [][][] vect_x, // []{{x,y,z},{a,t,r}}
......@@ -203,13 +283,14 @@ public class QuaternionLma {
samples = 7;
samples_x = 7;
pure_weight = 1.0 - reg_w;
int extra_samples = (quat0.length < 4)? 0:REGLEN;
x_vector = new double [samples * N];
y_vector = new double [samples * N + REGLEN];
y_inv_vector = new double [samples_x * N + REGLEN];
weights = new double [samples * N + REGLEN];
y_vector = new double [samples * N + extra_samples];
y_inv_vector = new double [samples_x * N + extra_samples];
weights = new double [samples * N + extra_samples];
parameters_vector = quat0.clone();
double [] tr_w = new double [] {translation_weight, 1.0-translation_weight};
// double [] tr_w = new double [] {translation_weight, 1.0-translation_weight};
double sw = 0;
for (int i = 0; i < N; i++) {
if ((vect_x[i]== null) || (vect_y[i]== null)) {
......@@ -255,12 +336,12 @@ public class QuaternionLma {
System.arraycopy(inv_y[1], 0, y_inv_vector, samples_x * i + 3, 4);
double wt = translation_weight*((vect_w != null)? vect_w[i] : 1.0);
double wr = 0.75*(1.0 - translation_weight)*((vect_w != null)? vect_w[i] : 1.0);
double wr = (1.0 - translation_weight)*((vect_w != null)? vect_w[i] : 1.0);
for (int j = 0; j < 3; j++) {
weights[samples * i + j] = wt;
sw += wt;
}
for (int j = 0; j < 4; j++) {
for (int j = 1; j < 4; j++) {// 0 - q0, near 1.0
weights[samples * i + 3 + j] = wr;
sw += wr;
}
......@@ -268,8 +349,10 @@ public class QuaternionLma {
}
double k = (pure_weight)/sw;
for (int i = 0; i < weights.length; i++) weights[i] *= k;
weights [samples * N] = 1.0 - pure_weight;
y_inv_vector[samples * N] = 1.0;
if (parameters_vector.length >=4) {
weights [samples * N] = 1.0 - pure_weight;
y_inv_vector[samples * N] = 1.0;
}
last_jt = new double [parameters_vector.length][];
if (debug_level > 0) {
debugYfX ( "Y-INV-", // String pfx,
......@@ -289,7 +372,7 @@ public class QuaternionLma {
double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
double [] quat0,
final int debug_level) {
if (mode != 2) {
if (mode != MODE_COMBO) {
prepareLMAMode4(
mode, // int mode,
avg_height, // double avg_height,
......@@ -452,24 +535,37 @@ public class QuaternionLma {
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) {
switch (mode) {
case 1:return getFxDerivs6Dof(
case MODE_XYZQ:return getFxDerivs6Dof(
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
case 2:return getFxDerivsVisual( // fill change
case MODE_COMBO:return getFxDerivsVisual( // fill change
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
case 3:return getFxDerivs6DofMode3(
case MODE_XYZQ_LOCAL:
if (parameters_vector.length < 4) {
return getFxDerivs6DofMode33(
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
} else {
return getFxDerivs6DofMode3(
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
}
case MODE_COMBO_LOCAL:
return getFxDerivsVisualMode4( // fill change
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
case 4:return getFxDerivsVisualMode4( // fill change
case MODE_COMPASS:return getFxDerivsCompass( // fill change
vector, // double [] vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
}
// remains here for mode 0
// remains here for mode MODE_XYZ
double [] fx = new double [weights.length];
final double q0 = vector[0];
final double q1 = vector[1];
......@@ -499,6 +595,40 @@ public class QuaternionLma {
return fx;
}
private double [] getFxDerivsCompass(
double [] vector,
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) {
double c = Math.cos(vector[0]/2), s = Math.sin(vector[0]/2);
//axis
double [] fx = new double [weights.length];
final double [] q = new double [] { c/2, s*axis[0]/2, s*axis[1]/2, s*axis[2]/2};
double [] dq_dv = new double [] {-s/2, c*axis[0]/2, c*axis[1]/2, c*axis[2]/2};
if (jt != null) {
for (int i = 0; i < vector.length; i++) {
jt[i] = new double [weights.length];
}
}
double [] xyz_rot;
double [][] xyz_dq;
for (int i = 0; i < N; i++) {
int i3 = 3 * i;
final double [] xyz = new double [] {x_vector[i3 + 0],x_vector[i3 + 1],x_vector[i3 + 2]};
xyz_rot = applyTo(q, xyz);
System.arraycopy(xyz_rot, 0, fx, i3, 3);
if (jt != null) {
xyz_dq = applyToDQ(q, xyz);
for (int j = 0; j < 3; j++) {
jt[0][i3+j] = 0.0;
for (int k = 0; k < 4; k++) {
jt[0][i3+j] += dq_dv[k] * xyz_dq[k][j];
}
}
}
}
return fx;
}
private double [] getFxDerivs6Dof(
double [] vector,
final double [][] jt, // should be null or initialized with [vector.length][]
......@@ -641,16 +771,121 @@ public class QuaternionLma {
}
return fx;
}
private double [] getFxDerivs6DofMode33(
double [] vector, //
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level) {
double [] fx = new double [weights.length];
final double q1 = vector[0];
final double q2 = vector[1];
final double q3 = vector[2];
final double q0 = getQ0(vector);
// final double [] vector_r = normSign(new double[] { q0,q1,q2,q3});
final double [] vector_r = new double[] { q0,q1,q2,q3};
if (jt != null) {
for (int i = 0; i < vector.length; i++) {
jt[i] = new double [weights.length];
// jt[i][samples * N] = 2 * vector[i];
}
}
// fx[samples * N] = q0*q0 + q1*q1 + q2*q2 + q3*q3;
double [] xyz_rot;
double [] quat_rot;
double [][] xyz_dq;
double [][] quat_dq;
double [][] inv_y = new double [][] {new double[3],new double[4]};
for (int i = 0; i < N; i++) {
int i7 = samples * i;
has_data:{
for (int j = 0; j < samples; j++) {
if (weights[i7+j] > 0) {
break has_data;
}
}
continue; // nothing to process for this scene
}
// translations
final double [] xyz = new double [] {x_vector[i7 + 0],x_vector[i7 + 1],x_vector[i7 + 2]};
// rotations
final double [] quat_r = {x_vector[i7 + 3],x_vector[i7 + 4],x_vector[i7 + 5],x_vector[i7 + 6]};
xyz_rot = applyTo(vector_r, xyz);
quat_rot = composeQR_Q(vector_r, quat_r);
System.arraycopy(y_inv_vector, i7, inv_y[0], 0, 3);
System.arraycopy(y_inv_vector, i7+3, inv_y[1], 0, 4);
double [][] comb_y = combineTransRot( //
inv_y[0], // double [] xyz_src, // transformation to apply to (was reference_xyz)
inv_y[1], // double [] quat_src, // transformation to apply to (was reference_atr)
xyz_rot, // double [] xyz_target, // to which is applied (was scene_xyz)
quat_rot); // double [] quat_target // to which is applied (was scene_atr)
System.arraycopy(comb_y[0], 0, fx, i7, 3);
System.arraycopy(comb_y[1], 0, fx, i7+3, 4);
if (jt != null) {
xyz_dq = applyToDQ(vector_r, xyz);
double [][] xyz_dq_local = new double [xyz_dq.length][];
for (int j = 0; j < xyz_dq.length; j++) {
xyz_dq_local[j] =combineTransRot(
null, // double [] xyz_src, // transformation to apply to (was reference_xyz)
inv_y[1], // double [] quat_src, // transformation to apply to (was reference_atr)
xyz_dq[j], // double [] xyz_target, // to which is applied (was scene_xyz)
null)[0]; // double [] quat_target // to which is applied (was scene_atr)
}
double [][] xyz_dq_local3 = dQuat123(
vector, // double [] q123,
xyz_dq_local, // double [][] dq0123, // [4][]
q0); // double q0)
for (int j = 0; j < xyz_dq_local3.length; j++) { // xyz_dq_local3.length==3
System.arraycopy(xyz_dq_local3[j], 0, jt[j], i7, 3);
}
quat_dq = composeQR_QdQ(vector_r,quat_r);
// 2 alternative ways with the same result
if (debug_level < 1000) {
double [][] invy_mat = qMat(inv_y[1]);
double [][] quat_dq_local = mulMat(quat_dq, invy_mat);
double [][] quat_dq_local3 =dQuat123(
vector, // double [] q123,
quat_dq_local, // double [][] dq0123, // [4][]
q0); // double q0)
for (int j = 0; j < quat_dq_local3.length; j++) {
System.arraycopy(quat_dq_local3[j], 0, jt[j], i7+3, 4);
}
} else {
double [][] dcomp_dsecond = composeDR(inv_y[1]);
double [][] quat_dq_local1 = new double [4][];
for (int j = 0; j < quat_dq_local1.length; j++) {
quat_dq_local1[j] = mulMat(dcomp_dsecond, quat_dq[j]);
}
double [][] quat_dq_local13 =dQuat123(
vector, // double [] q123,
quat_dq_local1, // double [][] dq0123, // [4][]
q0); // double q0)
for (int j = 0; j < quat_dq_local13.length; j++) {
System.arraycopy(quat_dq_local13[j], 0, jt[j], i7+3, 4);
}
}
}
}
return fx;
}
private double compareJT(
double [] vector,
double delta) {
double [] errors=new double [vector.length];
double [][] jt = new double [vector.length][];
System.out.println("Parameters vector = ["+vector[0]+", "+vector[1]+", "+vector[2]+", "+vector[3]+"]");
double [] fx_center = getFxDerivs(
// System.out.println("Parameters vector = ["+vector[0]+", "+vector[1]+", "+vector[2]+", "+vector[3]+"]");
System.out.print("Parameters vector = [");
for (int i = 0; i < vector.length; i++) {
System.out.print(vector[i]);
if (i < (vector.length -1)) System.out.print(", ");
}
System.out.println("]");
getFxDerivs(
vector,
jt, // final double [][] jt, // should be null or initialized with [vector.length][]
1); // debug_level);
......@@ -659,18 +894,39 @@ public class QuaternionLma {
delta, // final double delta,
-1); // final int debug_level)
for (int n = 0; n < weights.length; n++) if (weights[n] > 0) {
System.out.print(String.format("%3d",n));
for (int i = 0; i < vector.length; i++) {
System.out.print(String.format("\t%12.9f",jt[i][n]));
}
for (int i = 0; i < vector.length; i++) {
System.out.print(String.format("\t%12.9f",jt_delta[i][n]));
}
for (int i = 0; i < vector.length; i++) {
System.out.print(String.format("\t%12.9f",jt[i][n]-jt_delta[i][n]));
}
System.out.println();
/*
System.out.println(String.format(
"%3d\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f",
n, jt[0][n], jt[1][n], jt[2][n], jt[3][n],
jt_delta[0][n], jt_delta[1][n], jt_delta[2][n], jt_delta[3][n],
jt[0][n]-jt_delta[0][n],jt[1][n]-jt_delta[1][n],jt[2][n]-jt_delta[2][n],jt[3][n]-jt_delta[3][n]));
*/
for (int i = 0; i < vector.length; i++) {
errors[i] = Math.max(errors[i], jt[i][n]-jt_delta[i][n]);
}
}
for (int i = 0; i < vector.length; i++) {
System.out.print("\t\t");
}
for (int i = 0; i < vector.length; i++) {
System.out.print(String.format("\t%12.9f",errors[i]));
}
/*
System.out.println(String.format(
"-\t-\t-\t-\t-\t-\t-\t-\t-\t%12.9f\t%12.9f\t%12.9f\t%12.9f",
errors[0], errors[1], errors[2], errors[3]));
*/
double err=0;
for (int i = 0; i < vector.length; i++) {
err = Math.max(errors[i], err);
......@@ -874,7 +1130,7 @@ public class QuaternionLma {
) {
final double [] wymfw = new double [fx.length];
double s_rms=0;
double rms_pure=0;
double rms_pure=Double.NaN;
// int num_comp = use_6dof? 7 : 3;
for (int i = 0; i < fx.length; i++) {
double d = y_vector[i] - fx[i];
......@@ -892,7 +1148,9 @@ public class QuaternionLma {
s_rms += d * wd;
}
double rms = Math.sqrt(s_rms); // assuming sum_weights == 1.0;
if (Double.isNaN(rms_pure)) {
rms_pure=rms;
}
if (rms_fp != null) {
rms_fp[0] = rms;
rms_fp[1] = rms_pure;
......@@ -1210,7 +1468,7 @@ 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 == 0 ) {
if ((mode == MODE_XYZ) || ((mode == MODE_COMPASS))) {
System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s",
"N",pfx+"X",pfx+"Y",pfx+"Z"));
......@@ -1221,7 +1479,7 @@ public class QuaternionLma {
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+...]
} else if ((mode == MODE_XYZQ) || (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",
......@@ -1594,5 +1852,32 @@ public class QuaternionLma {
}
return tmat;
}
public static double getQ0(double [] q123) {
return Math.sqrt(1.0-(q123[0]*q123[0]+q123[1]*q123[1]+q123[2]*q123[2]));
}
public static double [] dQuat123(
double [] q123,
double [] dq0123, // [4]
double q0) { // null or double[3]
double [] dq123 = new double[3];
for (int i = 0; i < dq123.length; i++) {
dq123[i]=dq0123[i+1]- dq0123[0] * q123[i]/q0;
}
return dq123;
}
public static double [][] dQuat123(
double [] q123,
double [][] dq0123, // [4][]
double q0) { // null or double[3]
double [][] dq123 = new double[3][dq0123[0].length];
for (int n = 0; n < dq123[0].length; n++) {
for (int i = 0; i < dq123.length; i++) {
dq123[i][n]=dq0123[i+1][n]- dq0123[0][n]*q123[i]/q0;
}
}
return dq123;
}
}
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