Skip to content

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

Next snapshot

parent 93c00b54
Expand all Show whitespace changes
Inline Side-by-side
Showing with 1070 additions and 57 deletions
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 0835c213
...@@ -39,6 +39,9 @@ import java.util.concurrent.atomic.AtomicBoolean; ...@@ -39,6 +39,9 @@ import java.util.concurrent.atomic.AtomicBoolean;
import java.util.concurrent.atomic.AtomicInteger; import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.atomic.DoubleAccumulator; import java.util.concurrent.atomic.DoubleAccumulator;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
import java.util.concurrent.ThreadLocalRandom; import java.util.concurrent.ThreadLocalRandom;
import com.elphel.imagej.calibration.CalibrationFileManagement; import com.elphel.imagej.calibration.CalibrationFileManagement;
...@@ -5434,8 +5437,16 @@ public class OpticalFlow { ...@@ -5434,8 +5437,16 @@ public class OpticalFlow {
} }
double [] rms = new double[5]; double [] rms = new double[5];
double [] quat = new double[4]; double [] quat = new double[4];
int quat_lma_mode = 1; // 2;
int debug_lev = 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( double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
clt_parameters, // CLTParameters clt_parameters, 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, quadCLTs, // QuadCLT[] quadCLTs,
xyzatr, // double [][][] xyzatr, xyzatr, // double [][][] xyzatr,
pimu_xyzatr, // double [][][] ims_xyzatr, pimu_xyzatr, // double [][][] ims_xyzatr,
...@@ -5444,10 +5455,9 @@ public class OpticalFlow { ...@@ -5444,10 +5455,9 @@ public class OpticalFlow {
last_index, // int last_index, last_index, // int last_index,
rms, //double [] rms, // null or double[5]; rms, //double [] rms, // null or double[5];
quat, // double [] quaternion, // null or double[4] quat, // double [] quaternion, // null or double[4]
0); // int debugLevel debug_lev); // int debugLevel
}
}
if (run_ly) { if (run_ly) {
if (debugLevel > -3) { if (debugLevel > -3) {
System.out.println("**** Running LY adjustments *****"); System.out.println("**** Running LY adjustments *****");
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 0835c213
...@@ -242,8 +242,11 @@ public class QuadCLTCPU { ...@@ -242,8 +242,11 @@ public class QuadCLTCPU {
* @return quaternion componets * @return quaternion componets
*/ */
public static double [] getRotationFromXYZCameraIms( public static double [] getRotationFromXYZATRCameraIms(
CLTParameters clt_parameters, CLTParameters clt_parameters,
int quat_lma_mode,
double avg_height,
double translation_weight,
QuadCLT[] quadCLTs, QuadCLT[] quadCLTs,
double [][][] xyzatr, double [][][] xyzatr,
double [][][] ims_xyzatr, double [][][] ims_xyzatr,
...@@ -253,12 +256,6 @@ public class QuadCLTCPU { ...@@ -253,12 +256,6 @@ public class QuadCLTCPU {
double [] rms, // null or double[5]; double [] rms, // null or double[5];
int debugLevel int debugLevel
) { ) {
double [][] vect_y = new double [quadCLTs.length][]; // camera XYZ
double [][] vect_x = new double [quadCLTs.length][]; // IMS XYZ
for (int nscene = early_index; nscene <= last_index; nscene++) {
vect_x[nscene] = ims_xyzatr[nscene][0];
vect_y[nscene] = xyzatr[nscene][0];
}
double lambda = clt_parameters.imp.quat_lambda; // 0.1; 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_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_scale_bad = clt_parameters.imp.quat_lambda_scale_bad; // 8.0;
...@@ -269,6 +266,44 @@ public class QuadCLTCPU { ...@@ -269,6 +266,44 @@ public class QuadCLTCPU {
double reg_w = clt_parameters.imp.quat_reg_w; // 0.25; 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 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity
QuaternionLma quaternionLma = new QuaternionLma(); QuaternionLma quaternionLma = new QuaternionLma();
if (quat_lma_mode == 2) {
double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR
double [][][] vect_x = new double [quadCLTs.length][][]; // IMS XYZATR
for (int nscene = early_index; nscene <= last_index; nscene++) {
vect_x[nscene] = ims_xyzatr[nscene];
vect_y[nscene] = xyzatr[nscene];
}
quaternionLma.prepareLMA(
avg_height, // double avg_height,
vect_x, // double [][][] vect_x,
vect_y, // double [][][] vect_y,
null, // double [] vect_w, all same weight
translation_weight, // double translation_weight, // 0.0 ... 1.0;
quat0, // double [] quat0,
debugLevel); // int debug_level)
} else if (quat_lma_mode == 1) {
double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR
double [][][] vect_x = new double [quadCLTs.length][][]; // IMS XYZATR
for (int nscene = early_index; nscene <= last_index; nscene++) {
vect_x[nscene] = ims_xyzatr[nscene];
vect_y[nscene] = xyzatr[nscene];
}
quaternionLma.prepareLMA(
vect_x, // double [][][] vect_x,
vect_y, // double [][][] vect_y,
null, // double [] vect_w, all same weight
translation_weight, // double translation_weight, // 0.0 ... 1.0;
reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
quat0, // double [] quat0,
debugLevel); // int debug_level)
} else {
double [][] vect_y = new double [quadCLTs.length][]; // camera XYZ
double [][] vect_x = new double [quadCLTs.length][]; // IMS XYZ
for (int nscene = early_index; nscene <= last_index; nscene++) {
vect_x[nscene] = ims_xyzatr[nscene][0];
vect_y[nscene] = xyzatr[nscene][0];
}
quaternionLma.prepareLMA( quaternionLma.prepareLMA(
vect_x, // quat_lma_xyz, // double [][] vect_x, vect_x, // quat_lma_xyz, // double [][] vect_x,
vect_y, // double [][] vect_y, vect_y, // double [][] vect_y,
...@@ -276,6 +311,8 @@ public class QuadCLTCPU { ...@@ -276,6 +311,8 @@ public class QuadCLTCPU {
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,
debugLevel); // int debug_level) debugLevel); // int debug_level)
}
int lma_result = quaternionLma.runLma( // <0 - failed, >=0 iteration number (1 - immediately) int lma_result = quaternionLma.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
lambda, // double lambda, // 0.1 lambda, // double lambda, // 0.1
lambda_scale_good,// double lambda_scale_good,// 0.5 lambda_scale_good,// double lambda_scale_good,// 0.5
...@@ -307,6 +344,9 @@ public class QuadCLTCPU { ...@@ -307,6 +344,9 @@ public class QuadCLTCPU {
public static double [][][] rotateImsToCameraXYZ( public static double [][][] rotateImsToCameraXYZ(
CLTParameters clt_parameters, CLTParameters clt_parameters,
int quat_lma_mode,
double avg_height,
double translation_weight,
QuadCLT[] quadCLTs, QuadCLT[] quadCLTs,
double [][][] xyzatr, double [][][] xyzatr,
double [][][] ims_xyzatr, double [][][] ims_xyzatr,
...@@ -317,8 +357,11 @@ public class QuadCLTCPU { ...@@ -317,8 +357,11 @@ public class QuadCLTCPU {
double [] quaternion, // null or double[4] double [] quaternion, // null or double[4]
int debugLevel int debugLevel
) { ) {
double [] quat = getRotationFromXYZCameraIms( double [] quat = getRotationFromXYZATRCameraIms(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
quat_lma_mode, // int quat_lma_mode,
avg_height, // double avg_height,
translation_weight, // double translation_weight,
quadCLTs, // QuadCLT[] quadCLTs, quadCLTs, // QuadCLT[] quadCLTs,
xyzatr, // double [][][] xyzatr, xyzatr, // double [][][] xyzatr,
ims_xyzatr, // double [][][] ims_xyzatr, ims_xyzatr, // double [][][] ims_xyzatr,
...@@ -336,17 +379,27 @@ public class QuadCLTCPU { ...@@ -336,17 +379,27 @@ public class QuadCLTCPU {
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
double [][][] rotated_xyzatr = new double [quadCLTs.length][][]; // orientation from camera, xyz from rotated IMS double [][][] rotated_xyzatr = new double [quadCLTs.length][][]; // orientation from camera, xyz from rotated IMS
double [] rotated_xyz = new double[3]; double [] rotated_xyz = new double[3];
// double [] rotated_atr = new double[3]; Rotation rot_invert = rot.revert();
for (int nscene = early_index; nscene <= last_index; nscene++) { for (int nscene = early_index; nscene <= last_index; nscene++) {
rot.applyTo(ims_xyzatr[nscene][0], rotated_xyz); rot.applyTo(ims_xyzatr[nscene][0], rotated_xyz);
Rotation scene_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV, Rotation scene_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
xyzatr[nscene][1][0], xyzatr[nscene][1][1], xyzatr[nscene][1][2]); xyzatr[nscene][1][0], xyzatr[nscene][1][1], xyzatr[nscene][1][2]);
Rotation rotation_atr = rot.applyTo(scene_atr); Rotation ims_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
ims_xyzatr[nscene][1][0], ims_xyzatr[nscene][1][1], ims_xyzatr[nscene][1][2]);
// Rotation rotation_atr = rot.applyTo(scene_atr);
Rotation rotation_atr = rot.applyTo(ims_atr);
Rotation rotation_atr2 = rotation_atr.applyTo(rot_invert); // applyInverseTo?
rotated_xyzatr[nscene] = new double [][] {rotated_xyz.clone(), rotated_xyzatr[nscene] = new double [][] {rotated_xyz.clone(),
rotation_atr.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)}; rotation_atr2.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
} }
if (debugLevel > -1) { if (debugLevel > -1) {
double [] angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] degrees = new double[3];
for (int i = 0; i < 3; i++) degrees[i]=angles[i]*180/Math.PI;
System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]"); System.out.println("quat=["+quat[0]+", "+quat[1]+", "+quat[2]+", "+quat[3]+"]");
System.out.println("ATR(rad)=["+angles[0]+", "+angles[1]+", "+angles[2]+"]");
System.out.println("ATR(deg)=["+degrees[0]+", "+degrees[1]+", "+degrees[2]+"]");
System.out.println(String.format("%3s"+ System.out.println(String.format("%3s"+
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+ "\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+ "\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"+
......
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