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
Expand all 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);
}
}
......
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