Skip to content

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

First working version of IMU/LWIR positioning fusion

parent 0835c213
Hide whitespace changes
Inline Side-by-side
Showing with 109 additions and 11 deletions
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @ 7de7fe06
......@@ -27,6 +27,9 @@ import java.util.ArrayList;
import java.util.Arrays;
import java.util.concurrent.atomic.AtomicInteger;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
import com.elphel.imagej.calibration.CalibrationFileManagement;
import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.cameras.ColorProcParameters;
......@@ -2068,7 +2071,7 @@ public class Interscene {
// optionally filter by averaging and "IMU" (actually current dxyzatr_dt).
// double avg_rlen
readjust_xy_ims = false;
// readjust_xy_ims = false;
if (lpf_xy && (reg_weight_xy > 0.0)) {
scenes_xyzatr_pull = QuadCLT.refineFromLPF(
......@@ -2082,6 +2085,62 @@ public class Interscene {
earliest_scene, // int early_index,
last_scene); // int last_index)
} else if (readjust_xy_ims && (reg_weight_xy > 0.0)) {
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
ref_index, // final int ref_index,
null, // double [][][] dxyzatr,
earliest_scene, // final int early_index,
last_scene //(quadCLTs.length -1) // int last_index,
);
double [] rms = new double[5];
double [] quat = new double[4];
int quat_lma_mode = 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);
//scenes_xyzatr
double [][][] rotated_xyzatr = QuadCLT.rotateImsToCameraXYZ(
clt_parameters, // CLTParameters clt_parameters,
quat_lma_mode, // int quat_lma_mode,
avg_z, // double avg_height,
translation_weight, // double translation_weight,
quadCLTs, // QuadCLT[] quadCLTs,
scenes_xyzatr, // double [][][] xyzatr,
pimu_xyzatr, // double [][][] ims_xyzatr,
ref_index, // int ref_index,
earliest_scene, // int early_index,
last_scene, // int last_index,
rms, // double [] rms, // null or double[5];
quat, // double [] quaternion, // null or double[4]
debug_lev); // int debugLevel
if (rotated_xyzatr == null) {
System.out.println("reAdjustPairsLMAInterscene(): QuadCLT.rotateImsToCameraXYZ() failed");
} else {
System.out.println("reAdjustPairsLMAInterscene(): QuadCLT.rotateImsToCameraXYZ() finished in "+((int) rms[4])+ "iterations.");
System.out.println("rms="+rms[0]+" ("+rms[2]+"), pure rms="+rms[1]+" ("+rms[3]+")");
if (debug_lev > -3) {
Rotation rot = new Rotation(quat[0],quat[1],quat[2],quat[3], false); // no normalization - see if can be scaled
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("ATR(rad)=["+angles[0]+", "+angles[1]+", "+angles[2]+"]");
System.out.println("ATR(deg)=["+degrees[0]+", "+degrees[1]+", "+degrees[2]+"]");
}
// TODO: Use quat[] to update corrections of the IMU relative to the camera?
for (int nscene = earliest_scene; nscene <= last_scene; nscene++) if (scenes_xyzatr[nscene] != null) {
scenes_xyzatr_pull[nscene] = modifyATRtoXYZ(
scenes_xyzatr[nscene], // double [][] cur_xyzatr, // careful with Z - using the new one
rotated_xyzatr[nscene][0], // double [] new_xyz,
avg_z); // double avg_z
}
}
// old version
/*
scenes_xyzatr_pull = QuadCLT.refineFromImsVelocities(
clt_parameters, // CLTParameters clt_parameters,
quadCLTs, // QuadCLT[] quadCLTs,
......@@ -2090,6 +2149,7 @@ public class Interscene {
ref_index, // int ref_index,
earliest_scene, // int early_index,
last_scene); // int last_index)
*/
}
if (copy_pull_current) { // freeze_xy_pull) {
System.out.println("reAdjustPairsLMAInterscene(): freezing X,Y to X,Y pull values");
......@@ -2560,6 +2620,30 @@ public class Interscene {
return earliest_scene;
}
/**
* Modify initial ATR to match change of the XYZ from the already adjusted pose.
* Uses the fact that movement in X is approximate equivalent to Azimuth rotation
* (positive X has the same effect as negative A multiplied by Z) and Y-movement
* has the same sign to Tilt.
* @param cur_xyzatr current (already adjusted by the image-based LMA) {{x,y,z},{a,t,r}
* @param new_xyz new (target from the IMS) {x,y,z}
* @param avg_z average altitude (Z)
* @return new pose {{x,y,z},{a,t,r} expected to have a close match between scenes
*/
public static double [][] modifyATRtoXYZ(
double [][] cur_xyzatr, // careful with Z - using the new one
double [] new_xyz,
double avg_z
){
double dx = new_xyz[0] - cur_xyzatr[0][0];
double dy = new_xyz[1] - cur_xyzatr[0][1];
// System.out.println(String.format("modifyATRtoXYZ(): dx=%7.3f, dy=%7.3f", dx, dy));
double [][] diff_xyzatr = new double[][] {{dx, dy, 0},{dx/avg_z, -dy/avg_z,0}};
double [][] new_xyzatr = ErsCorrection.combineXYZATR(cur_xyzatr,diff_xyzatr);
return new_xyzatr;
}
// Make it the only entry point
// uses *_dt from
// quadCLTs[nscene*].getErsCorrection().getErsXYZATR_dt()
......@@ -2683,7 +2767,15 @@ public class Interscene {
true); // boolean rectilinear)
if (debugLevel > -3) {
System.out.println("adjustDiffPairsLMAInterscene(): pull_offset="+pull_offset);
double dx = scene1_xyzatr_pull[0][0]-scene1_xyzatr[0][0];
double dy = scene1_xyzatr_pull[0][1]-scene1_xyzatr[0][1];
double da = (scene1_xyzatr_pull[1][0]-scene1_xyzatr[1][0])*average_z;
double dt = (scene1_xyzatr_pull[1][1]-scene1_xyzatr[1][1])*average_z;
// System.out.println(String.format("modifyATRtoXYZ(): dx=%8.4f, dy=%8.4f"+
// ", hda=%8.4f, hdt=%8.4f, dx+hda=%8.4f, dy-hdt=%8.4f"
// , dx, dy, da, dt, dx+da, dy-dt));
}
while (pull_offset > clt_parameters.imp.max_pull_jump) {
// so far assuming only X and Y to be modified. Z - won't harm, angles are not used now,
// but if yes - quaternions are needed. Note, that angles are updated and will not now match pull
......
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 7de7fe06
......@@ -5437,7 +5437,7 @@ public class OpticalFlow {
}
double [] rms = new double[5];
double [] quat = new double[4];
int quat_lma_mode = 1; // 2;
int quat_lma_mode = 2; // 1;
int debug_lev = 3;
double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
double translation_weight = 1.0 / (avg_z + 1.0);
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 7de7fe06
......@@ -1098,6 +1098,8 @@ public class QuadCLTCPU {
boolean use_rot,
boolean rectilinear
) {
double [][] xyzatr0_a = new double [][] {xyzatr0[0].clone(),xyzatr0[1].clone()}; // debugging
double [][] xyzatr1_a = new double [][] {xyzatr1[0].clone(),xyzatr1[1].clone()};
ErsCorrection ers = getErsCorrection();
double disp_avg = ers.getDisparityFromZ(average_z);
int [] wh = ers.getSensorWH();
......@@ -1117,17 +1119,19 @@ public class QuadCLTCPU {
xy[1], // double py, // pixel coordinate Y in the reference view
disp_avg, // double disparity, // this reference disparity
!rectilinear, // boolean distortedView, // This camera view is distorted (diff.rect), false - rectilinear
xyzatr0[0], // double [] reference_xyz, // this view position in world coordinates (typically zero3)
xyzatr0[1], // double [] reference_atr, // this view orientation relative to world frame (typically zero3)
xyzatr0_a[0], // double [] reference_xyz, // this view position in world coordinates (typically zero3)
xyzatr0_a[1], // double [] reference_atr, // this view orientation relative to world frame (typically zero3)
!rectilinear, // boolean distortedCamera, // camera view is distorted (false - rectilinear)
xyzatr1[0], // double [] camera_xyz, // camera center in world coordinates
xyzatr1[1], // double [] camera_atr, // camera orientation relative to world frame
xyzatr1_a[0], // double [] camera_xyz, // camera center in world coordinates
xyzatr1_a[1], // double [] camera_atr, // camera orientation relative to world frame
OpticalFlow.LINE_ERR); // double line_err); // threshold error in scan lines (1.0)
if (pXpYD != null) {
double dx = pXpYD[0]-xy[0]; // null pointer
double dy = pXpYD[1]-xy[1];
s2 += dx*dx+dy*dy;
s0+=1.0;
// System.out.println(String.format("estimateAverageShift(): dx=%8.4f, dy=%8.4f",dx, dy));
} else {
continue;
}
......
src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
View file @ 7de7fe06
......@@ -599,7 +599,8 @@ public class QuaternionLma {
final double q1 = vector[1];
final double q2 = vector[2];
final double q3 = vector[3];
final double [] vector_r = normSign(new double[] {-q0,q1,q2,q3}); // seems better with reversal
// final double [] vector_r = normSign(new double[] {-q0,q1,q2,q3}); // seems better with reversal
final double [] vector_r = normSign(new double[] { q0,q1,q2,q3});
if (jt != null) {
for (int i = 0; i < vector.length; i++) {
jt[i] = new double [weights.length];
......@@ -1231,7 +1232,9 @@ public class QuaternionLma {
double [] q,
double [] r) {
return new double [][] {
// t=s*q' d/dQ0
// for inverted {-q0,q1,q2,q3}
// t=s*q' d/dQ0
/*
{ 2*r[0]*q[0],
2*r[1]*q[0] + 2*r[2]*q[3] - 2*r[3]*q[2],
2*r[2]*q[0] + 2*r[3]*q[1] - 2*r[1]*q[3],
......@@ -1251,8 +1254,8 @@ public class QuaternionLma {
-2*r[3]*q[1] - 2*r[2]*q[0] + 2*r[1]*q[3],
-2*r[3]*q[2] + 2*r[1]*q[0] + 2*r[2]*q[3],
-2*r[1]*q[1] - 2*r[2]*q[2] - 2*r[3]*q[3]}
/*
*/
// for non-inverted {q0,q1,q2,q3}
// t=s*q' d/dQ0
{ 2*r[0]*q[0],
2*r[1]*q[0] + 2*r[2]*q[3] - 2*r[3]*q[2],
......@@ -1273,7 +1276,6 @@ public class QuaternionLma {
2*r[3]*q[1] + 2*r[2]*q[0] - 2*r[1]*q[3],
2*r[3]*q[2] - 2*r[1]*q[0] - 2*r[2]*q[3],
2*r[1]*q[1] + 2*r[2]*q[2] + 2*r[3]*q[3]}
*/
};
}
public static double [][] composeQR_QdQ_(
......
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