Skip to content

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

Added attitude correction from IMS to camera by images

parent e9aa9780
Show whitespace changes
Inline Side-by-side
Showing with 678 additions and 20 deletions
src/main/java/com/elphel/imagej/ims/Imx5.java
View file @ db9a0992
......@@ -175,7 +175,7 @@ public class Imx5 {
quat_enu_rot.getQ3()};
}
public static double [] applyQuternionTo(double[]quat, double[] vector, boolean inverse) {
public static double [] applyQuaternionTo(double[]quat, double[] vector, boolean inverse) {
Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
double [] rslt = new double[3];
if (inverse) {
......@@ -187,6 +187,27 @@ public class Imx5 {
return rslt;
}
public static double [] applyQuaternionToQuaternion(
double[] quat,
double[] targ_quat,
boolean inverse) {
Rotation ims_rot = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
Rotation targ_rot = new Rotation(targ_quat[0],targ_quat[1],targ_quat[2],targ_quat[3],true);
Rotation rslt_rot;
if (inverse) {
rslt_rot=ims_rot.applyInverseTo(targ_rot);
} else {
rslt_rot=ims_rot.applyTo(targ_rot);
}
return new double[] {
rslt_rot.getQ0(),
rslt_rot.getQ1(),
rslt_rot.getQ2(),
rslt_rot.getQ3()};
}
public static double [] quaternionImsToCam(
double [] quat, // ims_to_ned
double [] ims_atr, // -> mount_to_cam
......
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ db9a0992
......@@ -430,7 +430,7 @@ public class Interscene {
d2.getQEnu(),
ims_mount_atr,
ims_ortho);
double [] cam_xyz_enu = Imx5.applyQuternionTo(
double [] cam_xyz_enu = Imx5.applyQuaternionTo(
// Offset in the reference scene frame, not in the current scene
cam_quat_ref_enu, // cam_quat_enu, Offset in reference scene frame
enu, // absolute offset (East, North, Up) in meters
......@@ -3789,6 +3789,15 @@ public class Interscene {
int num_processed = 0;
double [][][] scenes_xyzatr = new double [quadCLTs.length][][];
double [][][] scenes_xyzatr_dt = new double [quadCLTs.length][][];
// for testing QuaternionLma
double [] rms = new double [2];
double [] quatCorr = getQuaternionCorrection(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index,
earliest_scene, // int earliest_scene,
rms); // double [] rms // null or double[2];
for (int nscene = earliest_scene; nscene < quadCLTs.length; nscene++) {
QuadCLT scene = quadCLTs[nscene];
......@@ -3817,6 +3826,7 @@ public class Interscene {
String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
"\tcam_X1\tcam_Y1\tcam_Z1\tcam_X2\tcam_Y2\tcam_Z2"+
"\tned->X\tned->Y\tned->Z\tenu->X\tenu->Y\tenu->Z"+
"\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"+
"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
......@@ -3904,13 +3914,13 @@ public class Interscene {
//imsToCamRotations(double [] ims_theta, boolean rev_order, boolean rev_matrix )
double [] double_qn2b = d2.getQn2b();
double [] double_uvw = d2.getUvw();
double [] uvw_dir = Imx5.applyQuternionTo(double_qn2b, double_uvw, false); // bad
double [] uvw_inv = Imx5.applyQuternionTo(double_qn2b, double_uvw, true); // good
double [] uvw_dir = Imx5.applyQuaternionTo(double_qn2b, double_uvw, false); // bad
double [] uvw_inv = Imx5.applyQuaternionTo(double_qn2b, double_uvw, true); // good
//Converting from local uvw to NED: (qn2b).applyInverseTo(uvw,ned) results in [vNorth, vEast, vUp]
double [] ned = Imx5.nedFromLla (d2.lla, d2_ref.lla);
double [] enu = Imx5.enuFromLla (d2.lla, d2_ref.lla);
double [] ims_xyz = Imx5.applyQuternionTo(double_qn2b, ned, false);
double [] ims_xyz = Imx5.applyQuaternionTo(double_qn2b, ned, false);
// String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
// "\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
......@@ -3925,8 +3935,8 @@ public class Interscene {
ims_mount_atr, // new double[] {0, 0.13, 0},
ims_ortho);
double [] cam_xyz1 = Imx5.applyQuternionTo(cam_quat1, ned, false);
double [] cam_xyz2 = Imx5.applyQuternionTo(cam_quat2, ned, false);
double [] cam_xyz1 = Imx5.applyQuaternionTo(cam_quat1, ned, false);
double [] cam_xyz2 = Imx5.applyQuaternionTo(cam_quat2, ned, false);
double [] cam_xyz_ned = test_xyz_ned(
Imx5.nedFromLla (d2.lla, d2_ref.lla), // double [] ned,double [] ned,
d2_ref.getQn2b(), // double[] quat_ned,
......@@ -3939,19 +3949,20 @@ public class Interscene {
ims_mount_atr, // double [] ims_mount_atr,
ims_ortho); //double [] ims_ortho)
*/
double [] cam_xyz_enu = Imx5.applyQuternionTo(
double [] cam_xyz_enu = Imx5.applyQuaternionTo(
Imx5.quaternionImsToCam(d2_ref.getQEnu(), // double[] quat_enu,
ims_mount_atr,
ims_ortho),
enu,
false);
double [] quat_lma_enu_xyz = Imx5.applyQuaternionTo(quatCorr,cam_xyz_enu,false);
sb.append("\t"+cam_xyz1[0]+ "\t"+cam_xyz1[1]+ "\t"+cam_xyz1[2]); //
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]); //
double [] scene_abs_atr = Imx5.quatToCamAtr(cam_quat2);
......@@ -3993,6 +4004,10 @@ public class Interscene {
// add lpf
sb.append("\n");
}
// test QuaternionLMA here
if (path!=null) {
String footer=(comment != null) ? ("Comment: "+comment): "";
......@@ -4004,6 +4019,84 @@ public class Interscene {
}
}
public static double [] getQuaternionCorrection(
CLTParameters clt_parameters,
QuadCLT [] quadCLTs,
int ref_index,
int earliest_scene,
double [] rms // null or double[2];
) {
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][3];
double [][] quat_lma_enu_xyz = new double [quadCLTs.length][3];
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 lambda = 0.1;
double lambda_scale_good = 0.5;
double lambda_scale_bad = 8.0;
double lambda_max = 100;
double rms_diff = 0.001;
int num_iter = 20;
boolean last_run = false;
double reg_w = 0.25;
double [] quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity
int debug_level = 1;
QuaternionLma quaternionLma = new QuaternionLma();
quaternionLma.prepareLMA(
quat_lma_enu_xyz, // quat_lma_xyz, // double [][] vect_x,
quat_lma_xyz, // double [][] vect_y,
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
quat0, // double [] quat0,
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;
}
}
}
return quaternionLma.getQuaternion();
}
}
static double [] test_xyz_ned(
double [] ned,
double[] quat_ned,
......@@ -4013,7 +4106,7 @@ public class Interscene {
double [] cam_quat2 =Imx5.quaternionImsToCam(quat_ned,
ims_mount_atr,
ims_ortho);
return Imx5.applyQuternionTo(cam_quat2, ned, false);
return Imx5.applyQuaternionTo(cam_quat2, ned, false);
}
static double [] test_xyz_enu(
......@@ -4025,7 +4118,7 @@ public class Interscene {
double [] cam_quat2 =Imx5.quaternionImsToCam(quat_enu,
ims_mount_atr,
ims_ortho);
return Imx5.applyQuternionTo(cam_quat2, enu, false);
return Imx5.applyQuaternionTo(cam_quat2, enu, false);
}
......
src/main/java/com/elphel/imagej/tileprocessor/IntersceneLma.java
View file @ db9a0992
/**
**
** IntersceneLma - Adjust camera egomotion with LMA
**
** Copyright (C) 2020-2023 Elphel, Inc.
**
** -----------------------------------------------------------------------------**
**
** IntersceneLma.java is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by
** the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with this program. If not, see <http://www.gnu.org/licenses/>.
** -----------------------------------------------------------------------------**
**
*/
package com.elphel.imagej.tileprocessor;
import java.io.ByteArrayOutputStream;
......
src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java
View file @ db9a0992
......@@ -85,6 +85,9 @@ public class IntersceneMatchParameters {
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 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 generate_egomotion = true; // generate egomotion table (image-based and ims)
public boolean generate_mapped = true; // generate any of the sequences - Tiff, video, mono or stereo
public boolean reuse_video = false; // dry-run video generation, just reassemble video fragments (active if !generate_mapped)
......@@ -536,6 +539,10 @@ 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 ("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,
"Apply camera orientation correction from predicted by IMS.");
gd.addMessage ("Generate/show scene sequences");
gd.addCheckbox ("Generate egomotion table", this.generate_egomotion,
......@@ -1227,6 +1234,8 @@ 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.calc_quat_corr = gd.getNextBoolean();
this.apply_quat_corr = gd.getNextBoolean();
this.generate_egomotion = gd.getNextBoolean();
this.generate_mapped = gd.getNextBoolean();
this.reuse_video = gd.getNextBoolean();
......@@ -1609,6 +1618,8 @@ 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+"calc_quat_corr", this.calc_quat_corr+""); // boolean
properties.setProperty(prefix+"apply_quat_corr", this.apply_quat_corr+""); // boolean
properties.setProperty(prefix+"generate_egomotion", this.generate_egomotion+""); // boolean
properties.setProperty(prefix+"generate_mapped", this.generate_mapped+""); // boolean
properties.setProperty(prefix+"reuse_video", this.reuse_video+""); // boolean
......@@ -1951,6 +1962,8 @@ 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+"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+"generate_egomotion")!=null) this.generate_egomotion=Boolean.parseBoolean(properties.getProperty(prefix+"generate_egomotion"));
if (properties.getProperty(prefix+"generate_mapped")!=null) this.generate_mapped=Boolean.parseBoolean(properties.getProperty(prefix+"generate_mapped"));
if (properties.getProperty(prefix+"reuse_video")!=null) this.reuse_video=Boolean.parseBoolean(properties.getProperty(prefix+"reuse_video"));
......@@ -2320,6 +2333,8 @@ 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.calc_quat_corr = this.calc_quat_corr;
imp.apply_quat_corr = this.apply_quat_corr;
imp.generate_egomotion = this.generate_egomotion;
imp.generate_mapped = this.generate_mapped;
imp.reuse_video = this.reuse_video;
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ db9a0992
......@@ -4655,8 +4655,9 @@ 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 calc_quat_corr = clt_parameters.imp.calc_quat_corr; // calculate camera orientation correction from predicted by IMS
boolean apply_quat_corr = clt_parameters.imp.apply_quat_corr; // apply camera orientation correction from predicted by IMS
boolean generate_egomotion = clt_parameters.imp.generate_egomotion; // generate egomotion table (image-based and ims)
boolean generate_mapped = clt_parameters.imp.generate_mapped &&
(gen_seq_mono_color[0] || gen_seq_mono_color[1]); // generate sequences - Tiff and/or video
boolean export3d = clt_parameters.imp.export3d; // true;
......@@ -5329,6 +5330,30 @@ public class OpticalFlow {
}
}
if (calc_quat_corr) {
double [] quat_rms = new double [5];
double [] quatCorr = Interscene.getQuaternionCorrection(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index,
earliest_scene, // int earliest_scene,
quat_rms); // double [] rms // null or double[2];
if (quatCorr != null) {
int num_iter = (int) quat_rms[4];
if (debugLevel> -3) {
System.out.println("LMA done on iteration "+num_iter+
" full RMS="+quat_rms[0]+" ("+quat_rms[2]+"), pure RMS="+quat_rms[1]+" ("+quat_rms[3]+")");
System.out.println("Correction quaternion = ["+quatCorr[0]+", "+
quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]");
}
quadCLTs[ref_index].setQuatCorr(quatCorr);
quadCLT_main.setQuatCorr(quatCorr);
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 (generate_egomotion) {
boolean ego_show = !clt_parameters.batch_run; //true;
String ego_path = quadCLTs[ref_index].getX3dDirectory()+Prefs.getFileSeparator()+
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ db9a0992
......@@ -185,6 +185,7 @@ public class QuadCLTCPU {
public Did_gps_pos did_gps2_pos = null;
public Did_gps_pos did_gps1_ubx_pos = null;
public String ims_last_path = null;
public double [] quat_corr = null; // correction for IMS camera frame to actual camera frame (for reference frames)
// latest (or any? first also?) scene in a sequence has a timestamp of the reference scene
public String timestamp_reference = null;
......@@ -196,6 +197,12 @@ public class QuadCLTCPU {
return timestamp_reference;
}
public double [] getQuatCorr() {
return quat_corr;
}
public void setQuatCorr(double[] quat) {
quat_corr = quat;
}
public double [][] getDxyzatrIms(
CLTParameters clt_parameters,
boolean scale) {
......@@ -207,8 +214,8 @@ public class QuadCLTCPU {
double [] double_uvw = did_ins_2.getUvw();
double [] omegas=new double[] {did_pimu.theta[0]/did_pimu.dt,
did_pimu.theta[1]/did_pimu.dt, did_pimu.theta[2]/did_pimu.dt};
double [] cam_dxyz = Imx5.applyQuternionTo(quat_ims_cam, double_uvw, false);
double [] cam_datr = Imx5.applyQuternionTo(quat_ims_cam, omegas, false);
double [] cam_dxyz = Imx5.applyQuaternionTo(quat_ims_cam, double_uvw, false);
double [] cam_datr = Imx5.applyQuaternionTo(quat_ims_cam, omegas, false);
// a (around Y),t (around X), r (around Z)
double [][] dxyzatyr = new double [][] {cam_dxyz,{cam_datr[1],cam_datr[0],cam_datr[2]}};
if (scale) {
......@@ -692,14 +699,26 @@ public class QuadCLTCPU {
double [] ims_ortho = clt_parameters.imp.ims_ortho;
double [] ims_mount_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
double [] ims_mount_xyz = clt_parameters.imp.ims_mount_xyz; // not yet used
// double [] ref_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_ref_enu);
// double [][] ims_ref_xyzatr_enu = {ZERO3, ref_abs_atr_enu};
boolean apply_quat_corr = clt_parameters.imp.apply_quat_corr; // apply camera orientation correction from predicted by IMS
Did_ins_2 d2 = did_ins_2;
double [] cam_quat_enu =Imx5.quaternionImsToCam(
d2.getQEnu(),
ims_mount_atr,
ims_ortho);
//
double [] quat_corr = getQuatCorr();
if (apply_quat_corr && (quat_corr != null)) {
cam_quat_enu=Imx5.applyQuaternionToQuaternion(quat_corr, cam_quat_enu, false);
if (debug_level > -3) {
System.out.println("Applying attitude correction from quaternion ["+
quat_corr[0]+", "+quat_corr[1]+", "+quat_corr[2]+", "+quat_corr[3]+"] to the ground plane");
}
} else {
if (debug_level > -3) {
System.out.println("No attitude correction is applied to the ground plane");
}
}
double [] scene_abs_atr_enu = Imx5.quatToCamAtr(cam_quat_enu);
double [][] dls = getDLS();
if (dls==null) {
......@@ -3737,6 +3756,12 @@ public class QuadCLTCPU {
if (this.timestamp_last != null) {
properties.setProperty(prefix+"timestamp_last", this.timestamp_last);
}
if (this.quat_corr != null) {
properties.setProperty(prefix+"quat_corr", IntersceneMatchParameters.doublesToString(this.quat_corr));
}
return properties;
}
......@@ -3894,12 +3919,17 @@ public class QuadCLTCPU {
if (properties.getProperty(prefix+"timestamp_reference")!=null) {
this.timestamp_reference= (String) properties.getProperty(prefix+"timestamp_reference");
}
if (properties.getProperty(prefix+"timestamp_first")!=null) {
this.timestamp_first= (String) properties.getProperty(prefix+"timestamp_first");
}
if (properties.getProperty(prefix+"timestamp_last")!=null) {
this.timestamp_last= (String) properties.getProperty(prefix+"timestamp_last");
}
if (properties.getProperty(prefix+"quat_corr")!=null) {
this.quat_corr= IntersceneMatchParameters.StringToDoubles(properties.getProperty(prefix+"quat_corr"),4);
}
}
......
src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java 0 → 100644
View file @ db9a0992
/**
**
** QuaternionLma - Find quaternion to best transform a set of input 3D vectors
** into a set of output 3D vectors
**
** Copyright (C) 2023 Elphel, Inc.
**
** -----------------------------------------------------------------------------**
**
** QuaternionLma.java is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by
** the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** This program is distributed in the hope that it will be useful,
** but WITHOUT ANY WARRANTY; without even the implied warranty of
** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with this program. If not, see <http://www.gnu.org/licenses/>.
** -----------------------------------------------------------------------------**
**
*/
package com.elphel.imagej.tileprocessor;
import java.util.concurrent.atomic.AtomicInteger;
import Jama.Matrix;
public class QuaternionLma {
private final static int REGLEN = 1; // number of extra (regularization) samples
private int N = 0;
private double [] last_rms = null; // {rms, rms_pure}, matching this.vector
private double [] good_or_bad_rms = null; // just for diagnostics, to read last (failed) rms
private double [] initial_rms = null; // {rms, rms_pure}, first-calcualted rms
private double [] parameters_vector = null;
private double [] x_vector = null;
private double [] y_vector = null;
private double [] weights; // normalized so sum is 1.0 for all - samples and extra regularization
private double pure_weight; // weight of samples only
private double [] last_ymfx = null;
private double [][] last_jt = null;
public double [] getQuaternion() {
return parameters_vector;
}
public double [] getLastRms() {
return last_rms;
}
public double [] getInitialRms() {
return initial_rms;
}
public void prepareLMA(
double [][] vect_x,
double [][] vect_y,
double [][] vect_w,
double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
double [] quat0,
final int debug_level) {
N = vect_x.length;
pure_weight = 1.0 - reg_w;
x_vector = new double [3* N];
y_vector = new double [3* N + REGLEN];
weights = new double [3* N + REGLEN];
parameters_vector = quat0.clone();
double sw = 0;
for (int i = 0; i < N; i++) {
for (int j = 0; j < 3; j++) {
x_vector[3 * i + j] = vect_x[i][j];
y_vector[3 * i + j] = vect_y[i][j];
double w = (vect_w != null)? vect_w[i][j] : 1.0;
weights[3*i + j] = w;
sw += w;
}
}
double k = (pure_weight)/sw;
for (int i = 0; i < weights.length; i++) weights[i] *= k;
weights[3 * N] = 1.0 - pure_weight;
y_vector[3 * N] = 1.0;
last_jt = new double [parameters_vector.length][];
}
// TODO: Consider adding differences between x and y for regularization (or it won't work)
// goal - to minimize "unneded" rotation along the commonn axis
private double [] getFxDerivs(
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 q0 = vector[0];
final double q1 = vector[1];
final double q2 = vector[2];
final double q3 = vector[3];
if (jt != null) {
for (int i = 0; i < vector.length; i++) {
jt[i] = new double [weights.length];
jt[i][3 * N] = 2 * vector[i];
}
}
fx[3 * N] = q0*q0 + q1*q1 + q2 * q2 + q3*q3;
for (int i = 0; i < N; i++) {
int i3 = 3 * i;
final double x = x_vector[i3 + 0];
final double y = x_vector[i3 + 1];
final double z = x_vector[i3 + 2];
final double s = q1 * x + q2 * y + q3 * z;
fx[i3 + 0] = 2 * (q0 * (x * q0 - (q2 * z - q3 * y)) + s * q1) - x;
fx[i3 + 1] = 2 * (q0 * (y * q0 - (q3 * x - q1 * z)) + s * q2) - y;
fx[i3 + 2] = 2 * (q0 * (z * q0 - (q1 * y - q2 * x)) + s * q3) - z;
if (jt != null) {
jt[0][i3 + 0] = 4*x*q0 - 2*z*q2 + 2*y*q3;
jt[1][i3 + 0] = 2*s + 2*q1*x;
jt[2][i3 + 0] = 2*z*q0 + 2*q1*y;
jt[3][i3 + 0] = 2*y*q0 + 2*q1*z;
jt[0][i3 + 1] = 4*y*q0 - 2*x*q3 + 2*z*q1;
jt[1][i3 + 1] = 2*z*q0 + 2*x*q2;
jt[2][i3 + 1] = 2*s + 2*y*q2;
jt[3][i3 + 1] =-2*x*q0+ 2*z*q2;
jt[0][i3 + 2] = 4*z*q0 - 2*y*q1 + 2*x*q2;
jt[1][i3 + 2] =-2*y*q0 + 2*x*q3;
jt[2][i3 + 2] = 2*x*q0 + 2*y*q3;
jt[3][i3 + 2] = 2*s + 2*z*q3;
}
}
return fx;
}
private double [] getYminusFxWeighted(
final double [] fx,
final double [] rms_fp // null or [2]
) {
final double [] wymfw = new double [fx.length];
double s_rms=0;
double rms_pure=0;
for (int i = 0; i < fx.length; i++) {
double d = y_vector[i] - fx[i];
double wd = d * weights[i];
if (Double.isNaN(wd)) {
System.out.println("getYminusFxWeighted(): weights["+i+"]="+weights[i]+", wd="+wd+
", y_vector[i]="+y_vector[i]+", fx[i]="+fx[i]);
wd = 0.0;
d = 0.0;
}
if (i == (3 * N)) {
rms_pure = Math.sqrt(s_rms/pure_weight);;
}
wymfw[i] = wd;
s_rms += d * wd;
}
double rms = Math.sqrt(s_rms); // assuming sum_weights == 1.0;
if (rms_fp != null) {
rms_fp[0] = rms;
rms_fp[1] = rms_pure;
}
return wymfw;
}
// reusing multithreaded
private double [][] getWJtJlambda( // USED in lwir
final double lambda,
final double [][] jt)
{
final int num_pars = jt.length;
final int num_pars2 = num_pars * num_pars;
final int nup_points = jt[0].length;
final double [][] wjtjl = new double [num_pars][num_pars];
final Thread[] threads = ImageDtt.newThreadArray(ImageDtt.THREADS_MAX);
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int indx = ai.getAndIncrement(); indx < num_pars2; indx = ai.getAndIncrement()) {
int i = indx / num_pars;
int j = indx % num_pars;
if (j >= i) {
double d = 0.0;
for (int k = 0; k < nup_points; k++) {
if (jt[i][k] != 0) {
d+=0;
}
d += weights[k]*jt[i][k]*jt[j][k];
}
wjtjl[i][j] = d;
if (i == j) {
wjtjl[i][j] += d * lambda;
} else {
wjtjl[j][i] = d;
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
return wjtjl;
}
public int runLma( // <0 - failed, >=0 iteration number (1 - immediately)
double lambda, // 0.1
double lambda_scale_good,// 0.5
double lambda_scale_bad, // 8.0
double lambda_max, // 100
double rms_diff, // 0.001
int num_iter, // 20
boolean last_run,
int debug_level) {
boolean [] rslt = {false,false};
this.last_rms = null; // remove?
int iter = 0;
for (iter = 0; iter < num_iter; iter++) {
rslt = lmaStep(
lambda,
rms_diff,
debug_level);
if (rslt == null) {
return -1; // false; // need to check
}
if (debug_level > 1) {
System.out.println("LMA step "+iter+": {"+rslt[0]+","+rslt[1]+"} full RMS= "+good_or_bad_rms[0]+
" ("+initial_rms[0]+"), pure RMS="+good_or_bad_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
}
if (rslt[1]) {
break;
}
if (rslt[0]) { // good
lambda *= lambda_scale_good;
} else {
lambda *= lambda_scale_bad;
if (lambda > lambda_max) {
break; // not used in lwir
}
}
}
if (rslt[0]) { // better
if (iter >= num_iter) { // better, but num tries exceeded
if (debug_level > 1) System.out.println("Step "+iter+": Improved, but number of steps exceeded maximal");
} else {
if (debug_level > 1) System.out.println("Step "+iter+": LMA: Success");
}
} else { // improved over initial ?
if (last_rms[0] < initial_rms[0]) { // NaN
rslt[0] = true;
if (debug_level > 1) System.out.println("Step "+iter+": Failed to converge, but result improved over initial");
} else {
if (debug_level > 1) System.out.println("Step "+iter+": Failed to converge");
}
}
boolean show_intermediate = true;
if (show_intermediate && (debug_level > 0)) {
System.out.println("LMA: full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
}
if (debug_level > 2){
String [] lines1 = printOldNew(false); // boolean allvectors)
System.out.println("iteration="+iter);
for (String line : lines1) {
System.out.println(line);
}
}
if (debug_level > 0) {
if ((debug_level > 1) || last_run) { // (iter == 1) || last_run) {
if (!show_intermediate) {
System.out.println("LMA: iter="+iter+", full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
}
String [] lines = printOldNew(false); // boolean allvectors)
for (String line : lines) {
System.out.println(line);
}
}
}
if ((debug_level > -2) && !rslt[0]) { // failed
if ((debug_level > 1) || (iter == 1) || last_run) {
System.out.println("LMA failed on iteration = "+iter);
String [] lines = printOldNew(true); // boolean allvectors)
for (String line : lines) {
System.out.println(line);
}
}
System.out.println();
}
return rslt[0]? iter : -1;
}
private boolean [] lmaStep(
double lambda,
double rms_diff,
int debug_level) {
boolean [] rslt = {false,false};
// maybe the following if() branch is not needed - already done in prepareLMA !
if (this.last_rms == null) { //first time, need to calculate all (vector is valid)
last_rms = new double[2];
if (debug_level > 1) {
System.out.println("lmaStep(): first step");
}
double [] fx = getFxDerivs(
parameters_vector, // double [] vector,
last_jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
last_ymfx = getYminusFxWeighted(
fx, // final double [] fx,
last_rms); // final double [] rms_fp // null or [2]
this.initial_rms = this.last_rms.clone();
this.good_or_bad_rms = this.last_rms.clone();
if (debug_level > -1) { // temporary
/*
dbgYminusFxWeight(
this.last_ymfx,
this.weights,
"Initial_y-fX_after_moving_objects");
*/
}
if (last_ymfx == null) {
return null; // need to re-init/restart LMA
}
// TODO: Restore/implement
if (debug_level > 3) {
/*
dbgJacobians(
corr_vector, // GeometryCorrection.CorrVector corr_vector,
1E-5, // double delta,
true); //boolean graphic)
*/
}
}
Matrix y_minus_fx_weighted = new Matrix(this.last_ymfx, this.last_ymfx.length);
Matrix wjtjlambda = new Matrix(getWJtJlambda(
lambda, // *10, // temporary
this.last_jt)); // double [][] jt)
if (debug_level>2) {
System.out.println("JtJ + lambda*diag(JtJ");
wjtjlambda.print(18, 6);
}
Matrix jtjl_inv = null;
try {
jtjl_inv = wjtjlambda.inverse(); // check for errors
} catch (RuntimeException e) {
rslt[1] = true;
if (debug_level > 0) {
System.out.println("Singular Matrix!");
}
return rslt;
}
if (debug_level>2) {
System.out.println("(JtJ + lambda*diag(JtJ).inv()");
jtjl_inv.print(18, 6);
}
//last_jt has NaNs
Matrix jty = (new Matrix(this.last_jt)).times(y_minus_fx_weighted);
if (debug_level>2) {
System.out.println("Jt * (y-fx)");
jty.print(18, 6);
}
Matrix mdelta = jtjl_inv.times(jty);
if (debug_level>2) {
System.out.println("mdelta");
mdelta.print(18, 6);
}
double scale = 1.0;
double [] delta = mdelta.getColumnPackedCopy();
double [] new_vector = parameters_vector.clone();
for (int i = 0; i < parameters_vector.length; i++) {
new_vector[i] += scale * delta[i];
}
double [] fx = getFxDerivs(
new_vector, // double [] vector,
last_jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
double [] rms = new double[2];
last_ymfx = getYminusFxWeighted(
fx, // final double [] fx,
rms); // final double [] rms_fp // null or [2]
if (debug_level > 2) {
/*
dbgYminusFx(this.last_ymfx, "next y-fX");
dbgXY(new_vector, "XY-correction");
*/
}
if (last_ymfx == null) {
return null; // need to re-init/restart LMA
}
this.good_or_bad_rms = rms.clone();
if (rms[0] < this.last_rms[0]) { // improved
rslt[0] = true;
rslt[1] = rms[0] >=(this.last_rms[0] * (1.0 - rms_diff));
this.last_rms = rms.clone();
this.parameters_vector = new_vector.clone();
if (debug_level > 2) {
// print vectors in some format
/*
System.out.print("delta: "+corr_delta.toString()+"\n");
System.out.print("New vector: "+new_vector.toString()+"\n");
System.out.println();
*/
}
} else { // worsened
rslt[0] = false;
rslt[1] = false; // do not know, caller will decide
// restore state
fx = getFxDerivs(
parameters_vector, // double [] vector,
last_jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
last_ymfx = getYminusFxWeighted(
fx, // final double [] fx,
this.last_rms); // final double [] rms_fp // null or [2]
if (last_ymfx == null) {
return null; // need to re-init/restart LMA
}
if (debug_level > 2) {
/*
dbgJacobians(
corr_vector, // GeometryCorrection.CorrVector corr_vector,
1E-5, // double delta,
true); //boolean graphic)
*/
}
}
return rslt;
}
//TODO: implement
public String [] printOldNew(boolean allvectors) {
return new String[] {};
}
}
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