Commit 54ecece0 authored by Andrey Filippov's avatar Andrey Filippov

moving objects detection during pose matching

parent 2790d745
...@@ -519,7 +519,7 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)? ...@@ -519,7 +519,7 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)?
public double [] getIMU() { public double [] getIMU() {
// double [] imu = {vector[IMU_INDEX + 0], vector[IMU_INDEX + 1], vector[IMU_INDEX + 2],vector[IMU_INDEX + 3], vector[IMU_INDEX + 4], vector[IMU_INDEX + 5]}; // double [] imu = {vector[IMU_INDEX + 0], vector[IMU_INDEX + 1], vector[IMU_INDEX + 2],vector[IMU_INDEX + 3], vector[IMU_INDEX + 4], vector[IMU_INDEX + 5]};
int indx = getZoomIndex(); int indx = getIMUIndex(); // Was bug: getZoomIndex();
double [] imu = new double[6]; double [] imu = new double[6];
for (int i = 0; i < imu.length; i++) { for (int i = 0; i < imu.length; i++) {
imu[i] = vector[indx + i]; imu[i] = vector[indx + i];
...@@ -528,10 +528,15 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)? ...@@ -528,10 +528,15 @@ public class CorrVector{ // TODO: Update to non-quad (extract to a file first)?
} }
public void setIMU(double [] imu) { public void setIMU(double [] imu) {
int indx = getZoomIndex(); int indx = getIMUIndex(); // Was bug: getZoomIndex();
for (int i = 0; i < imu.length; i++) { for (int i = 0; i < imu.length; i++) {
vector [indx + i] = imu[i]; vector [indx + i] = imu[i];
} }
System.out.print("setIMU(");
for (int i = 0; i < imu.length; i++) {
System.out.print(imu[i]+",");
}
System.out.println(")");
} }
public double setZoomsFromF(double [] f) { // USED in lwir public double setZoomsFromF(double [] f) { // USED in lwir
......
...@@ -67,22 +67,22 @@ public class IntersceneLma { ...@@ -67,22 +67,22 @@ public class IntersceneLma {
full_vector[ErsCorrection.DP_DAZ],full_vector[ErsCorrection.DP_DTL],full_vector[ErsCorrection.DP_DRL]}; full_vector[ErsCorrection.DP_DAZ],full_vector[ErsCorrection.DP_DTL],full_vector[ErsCorrection.DP_DRL]};
} }
public double [] getSceneERSXYZ(boolean initial) { public double [] getSceneERSXYZ(boolean initial) { // never used
double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector); double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector);
return new double[] { return new double[] {
full_vector[ErsCorrection.DP_DSVX],full_vector[ErsCorrection.DP_DSVY],full_vector[ErsCorrection.DP_DSVZ]}; full_vector[ErsCorrection.DP_DSVX],full_vector[ErsCorrection.DP_DSVY],full_vector[ErsCorrection.DP_DSVZ]};
} }
public double [] getSceneERSATR(boolean initial) { public double [] getSceneERSATR(boolean initial) { // never used
double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector); double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector);
return new double[] { return new double[] {
full_vector[ErsCorrection.DP_DSVAZ],full_vector[ErsCorrection.DP_DSVTL],full_vector[ErsCorrection.DP_DSVRL]}; full_vector[ErsCorrection.DP_DSVAZ],full_vector[ErsCorrection.DP_DSVTL],full_vector[ErsCorrection.DP_DSVRL]};
} }
public double [] getReferenceERSXYZ(boolean initial) { public double [] getReferenceERSXYZ(boolean initial) { // never used
double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector); double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector);
return new double[] { return new double[] {
full_vector[ErsCorrection.DP_DSVX],full_vector[ErsCorrection.DP_DSVY],full_vector[ErsCorrection.DP_DSVZ]}; full_vector[ErsCorrection.DP_DSVX],full_vector[ErsCorrection.DP_DSVY],full_vector[ErsCorrection.DP_DSVZ]};
} }
public double [] getReferenceERSATR(boolean initial) { public double [] getReferenceERSATR(boolean initial) { // never used
double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector); double [] full_vector = initial? backup_parameters_full: getFullVector(parameters_vector);
return new double[] { return new double[] {
full_vector[ErsCorrection.DP_DSVAZ],full_vector[ErsCorrection.DP_DSVTL],full_vector[ErsCorrection.DP_DSVRL]}; full_vector[ErsCorrection.DP_DSVAZ],full_vector[ErsCorrection.DP_DSVTL],full_vector[ErsCorrection.DP_DSVRL]};
...@@ -389,7 +389,7 @@ public class IntersceneLma { ...@@ -389,7 +389,7 @@ public class IntersceneLma {
Matrix wjtjlambda = new Matrix(getWJtJlambda( Matrix wjtjlambda = new Matrix(getWJtJlambda(
lambda, // *10, // temporary lambda, // *10, // temporary
this.last_jt)); // double [][] jt) this.last_jt)); // double [][] jt)
if (debug_level > 2) { if (debug_level > 3) {
try { try {
System.out.println("getFxDerivs(): getChecksum(this.y_vector)="+ getChecksum(this.y_vector)); System.out.println("getFxDerivs(): getChecksum(this.y_vector)="+ getChecksum(this.y_vector));
System.out.println("getFxDerivs(): getChecksum(this.weights)="+ getChecksum(this.weights)); System.out.println("getFxDerivs(): getChecksum(this.weights)="+ getChecksum(this.weights));
...@@ -793,7 +793,7 @@ public class IntersceneLma { ...@@ -793,7 +793,7 @@ public class IntersceneLma {
fx [i + 2 * macrotile_centers.length] = vector[i]; // - parameters_initial[i]; // scale will be combined with weights fx [i + 2 * macrotile_centers.length] = vector[i]; // - parameters_initial[i]; // scale will be combined with weights
jt[i][i + 2 * macrotile_centers.length] = 1.0; // scale will be combined with weights jt[i][i + 2 * macrotile_centers.length] = 1.0; // scale will be combined with weights
} }
if (debug_level > 2) { if (debug_level > 3) {
try { try {
System.out.println ("getFxDerivs(): getChecksum(fx)="+getChecksum(fx)); System.out.println ("getFxDerivs(): getChecksum(fx)="+getChecksum(fx));
if (jt != null) { if (jt != null) {
......
...@@ -116,6 +116,22 @@ public class IntersceneMatchParameters { ...@@ -116,6 +116,22 @@ public class IntersceneMatchParameters {
public double maybe_avg = 0.005; // maybe average strength public double maybe_avg = 0.005; // maybe average strength
public double shure_avg = 0.015; // sure average strength public double shure_avg = 0.015; // sure average strength
// Reference scene disparity
public boolean use_combo_dsi = true; // use interscene DSI if available (instead of the single-scene)
public boolean use_lma_dsi = true; // only use reference DSI tiles that have LMA (strong) disparity
// Remove moving objects (goal is not to detect slightest movement, but to improve pose matching
public boolean mov_en = true; // enable detection/removal of the moving objects during pose matching
public double mov_sigma = 1.5; // pix - weighted-blur offsets before detection
// next two to prevent motion detection while errors are too big
public double mov_max_std = 0.5; // pix
public double mov_thresh_rel = 3.5; // .0; // exceed average error
public double mov_thresh_abs= 0.5; // sqrt(dx^2+dy^2) in moving areas
public double mov_clust_max = 1.5; // cluster maximum should exceed threshold this times
public int mov_grow = 4; // grow detected moving area
public boolean mov_show = true; // show debug images for movement detection
public int mov_debug_level = 1; // >0 verbose
//LMA parameters //LMA parameters
public boolean [] adjust_atr = new boolean [] {true,true,true}; public boolean [] adjust_atr = new boolean [] {true,true,true};
...@@ -126,7 +142,7 @@ public class IntersceneMatchParameters { ...@@ -126,7 +142,7 @@ public class IntersceneMatchParameters {
public int max_LMA = 25; // hard limit on LMA iterations public int max_LMA = 25; // hard limit on LMA iterations
public double max_rms = 2.0; // maximal RMS to consider adjustment to be a failure public double max_rms = 2.0; // maximal RMS to consider adjustment to be a failure
// Debug and visualisation // Debug and visualization
public boolean scene_is_ref_test= false; // correlate ref-2-ref for testing public boolean scene_is_ref_test= false; // correlate ref-2-ref for testing
private boolean render_ref = true; // render reference scene private boolean render_ref = true; // render reference scene
private boolean render_scene = true; // render scene to be correlated with ref private boolean render_scene = true; // render scene to be correlated with ref
...@@ -146,6 +162,10 @@ public class IntersceneMatchParameters { ...@@ -146,6 +162,10 @@ public class IntersceneMatchParameters {
public boolean show2dCorrelations() {return (debug_level>1) && show_2d_correlations;} public boolean show2dCorrelations() {return (debug_level>1) && show_2d_correlations;}
public boolean showMotionVectors() {return (debug_level>1) && show_motion_vectors;} public boolean showMotionVectors() {return (debug_level>1) && show_motion_vectors;}
public boolean showCorrMotion() {return (debug_level>0) && show_motion_vectors;} public boolean showCorrMotion() {return (debug_level>0) && show_motion_vectors;}
public boolean showMovementDetection(){return (debug_level>0) && mov_show;}
public int movDebugLevel() {return (debug_level > -1) ? mov_debug_level : 0;}
public IntersceneMatchParameters() { public IntersceneMatchParameters() {
...@@ -317,6 +337,32 @@ public class IntersceneMatchParameters { ...@@ -317,6 +337,32 @@ public class IntersceneMatchParameters {
gd.addNumericField("\"Sure\" average of strengths", this.shure_avg, 5,7,"", gd.addNumericField("\"Sure\" average of strengths", this.shure_avg, 5,7,"",
"Definitely sufficient average of defined tiles strengths (will non continue looking for better)."); "Definitely sufficient average of defined tiles strengths (will non continue looking for better).");
gd.addMessage ("Reference scene disparity");
gd.addCheckbox ("Use combo DSI (if available)", this.use_combo_dsi,
"Use interscene DSI if available (instead of the single-scene)");
gd.addCheckbox ("LMA tiles only", this.use_lma_dsi,
"Use only strong correlation tiles (with LMA available) for interscene correlation (pose matching)");
gd.addMessage ("Detect and remove moving objects from pose matching");
gd.addCheckbox ("Enable movement detection/elimination", this.mov_en,
"Detect and mask areas with detected movement to improve pose matching");
gd.addNumericField("Detection blur sigma", this.mov_sigma, 6,7,"pix",
"Blur squared difference (dx^2+dy^2) befopre thresholding for movement detection");
gd.addNumericField("Max weighted mismatch std", this.mov_max_std, 6,7,"pix",
"Do not try to detect moving objects until approximate pose match is achieved (standard deviation)");
gd.addNumericField("Relative threshold", this.mov_thresh_rel, 6,7,"x",
"Moving areas over standard deviation. Both relative and absolute should be exceeded.");
gd.addNumericField("Absolute threshold", this.mov_thresh_abs, 6,7,"pix",
"Moving areas sqrt(dx*dx+dy*dy). Both relative and absolute should be exceeded.");
gd.addNumericField("Cluster max over threshold", this.mov_clust_max, 6,7,"",
"Moving cluster should contain tile with this exceed over thresholds");
gd.addNumericField("Moving cluster grow", this.mov_grow, 0,3,"",
"Standard grow values - 1 - ortho, 2 - diagonal, 3 - twice orto, 4 - twice diagonal");
gd.addCheckbox ("Show movement debug images", this.mov_show,
"Disabled if 'Debug Level for interscene match' < 1");
gd.addNumericField("Debug level for movement detection (0/1)", this.mov_debug_level, 0,3,"",
"Disabled if 'Debug Level for interscene match' < 0");
gd.addMessage ("LMA parameters"); gd.addMessage ("LMA parameters");
gd.addCheckbox ("Azimuth", this.adjust_atr[0], gd.addCheckbox ("Azimuth", this.adjust_atr[0],
"Adjust scene camera azimuth with LMA"); "Adjust scene camera azimuth with LMA");
...@@ -447,6 +493,20 @@ public class IntersceneMatchParameters { ...@@ -447,6 +493,20 @@ public class IntersceneMatchParameters {
this.shure_sum = gd.getNextNumber(); this.shure_sum = gd.getNextNumber();
this.maybe_avg = gd.getNextNumber(); this.maybe_avg = gd.getNextNumber();
this.shure_avg = gd.getNextNumber(); this.shure_avg = gd.getNextNumber();
this.use_combo_dsi = gd.getNextBoolean();
this.use_lma_dsi = gd.getNextBoolean();
this.mov_en = gd.getNextBoolean();
this.mov_sigma = gd.getNextNumber();
this.mov_max_std = gd.getNextNumber();
this.mov_thresh_rel = gd.getNextNumber();
this.mov_thresh_abs = gd.getNextNumber();
this.mov_clust_max = gd.getNextNumber();
this.mov_grow = (int) gd.getNextNumber();
this.mov_show = gd.getNextBoolean();
this.mov_debug_level = (int) gd.getNextNumber();
this.adjust_atr[0] = gd.getNextBoolean(); this.adjust_atr[0] = gd.getNextBoolean();
this.adjust_atr[1] = gd.getNextBoolean(); this.adjust_atr[1] = gd.getNextBoolean();
this.adjust_atr[2] = gd.getNextBoolean(); this.adjust_atr[2] = gd.getNextBoolean();
...@@ -548,6 +608,18 @@ public class IntersceneMatchParameters { ...@@ -548,6 +608,18 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"shure_sum", this.shure_sum+""); // double properties.setProperty(prefix+"shure_sum", this.shure_sum+""); // double
properties.setProperty(prefix+"maybe_avg", this.maybe_avg+""); // double properties.setProperty(prefix+"maybe_avg", this.maybe_avg+""); // double
properties.setProperty(prefix+"shure_avg", this.shure_avg+""); // double properties.setProperty(prefix+"shure_avg", this.shure_avg+""); // double
properties.setProperty(prefix+"use_combo_dsi", this.use_combo_dsi+""); // boolean
properties.setProperty(prefix+"use_lma_dsi", this.use_lma_dsi+""); // boolean
properties.setProperty(prefix+"mov_en", this.mov_en+""); // boolean
properties.setProperty(prefix+"mov_sigma", this.mov_sigma+""); // double
properties.setProperty(prefix+"mov_max_std", this.mov_max_std+""); // double
properties.setProperty(prefix+"mov_thresh_rel", this.mov_thresh_rel+""); // double
properties.setProperty(prefix+"mov_thresh_abs", this.mov_thresh_abs+""); // double
properties.setProperty(prefix+"mov_clust_max", this.mov_clust_max+""); // double
properties.setProperty(prefix+"mov_grow", this.mov_grow+""); // int
properties.setProperty(prefix+"mov_show", this.mov_show+""); // boolean
properties.setProperty(prefix+"mov_debug_level", this.mov_debug_level+""); // int
properties.setProperty(prefix+"adjust_atr_0", this.adjust_atr[0]+""); // boolean properties.setProperty(prefix+"adjust_atr_0", this.adjust_atr[0]+""); // boolean
properties.setProperty(prefix+"adjust_atr_1", this.adjust_atr[1]+""); // boolean properties.setProperty(prefix+"adjust_atr_1", this.adjust_atr[1]+""); // boolean
properties.setProperty(prefix+"adjust_atr_2", this.adjust_atr[2]+""); // boolean properties.setProperty(prefix+"adjust_atr_2", this.adjust_atr[2]+""); // boolean
...@@ -566,7 +638,6 @@ public class IntersceneMatchParameters { ...@@ -566,7 +638,6 @@ public class IntersceneMatchParameters {
properties.setProperty(prefix+"show_2d_correlations", this.show_2d_correlations+"");// boolean properties.setProperty(prefix+"show_2d_correlations", this.show_2d_correlations+"");// boolean
properties.setProperty(prefix+"show_motion_vectors", this.show_motion_vectors+""); // boolean properties.setProperty(prefix+"show_motion_vectors", this.show_motion_vectors+""); // boolean
properties.setProperty(prefix+"debug_level", this.debug_level+""); // int properties.setProperty(prefix+"debug_level", this.debug_level+""); // int
properties.setProperty(prefix+"test_ers", this.test_ers+""); // boolean properties.setProperty(prefix+"test_ers", this.test_ers+""); // boolean
properties.setProperty(prefix+"test_ers0", this.test_ers0+""); // int properties.setProperty(prefix+"test_ers0", this.test_ers0+""); // int
properties.setProperty(prefix+"test_ers1", this.test_ers1+""); // int properties.setProperty(prefix+"test_ers1", this.test_ers1+""); // int
...@@ -647,6 +718,19 @@ public class IntersceneMatchParameters { ...@@ -647,6 +718,19 @@ public class IntersceneMatchParameters {
if (properties.getProperty(prefix+"shure_sum")!=null) this.shure_sum=Double.parseDouble(properties.getProperty(prefix+"shure_sum")); if (properties.getProperty(prefix+"shure_sum")!=null) this.shure_sum=Double.parseDouble(properties.getProperty(prefix+"shure_sum"));
if (properties.getProperty(prefix+"maybe_avg")!=null) this.maybe_avg=Double.parseDouble(properties.getProperty(prefix+"maybe_avg")); if (properties.getProperty(prefix+"maybe_avg")!=null) this.maybe_avg=Double.parseDouble(properties.getProperty(prefix+"maybe_avg"));
if (properties.getProperty(prefix+"shure_avg")!=null) this.shure_avg=Double.parseDouble(properties.getProperty(prefix+"shure_avg")); if (properties.getProperty(prefix+"shure_avg")!=null) this.shure_avg=Double.parseDouble(properties.getProperty(prefix+"shure_avg"));
if (properties.getProperty(prefix+"use_combo_dsi")!=null) this.use_combo_dsi=Boolean.parseBoolean(properties.getProperty(prefix+"use_combo_dsi"));
if (properties.getProperty(prefix+"use_lma_dsi")!=null) this.use_lma_dsi=Boolean.parseBoolean(properties.getProperty(prefix+"use_lma_dsi"));
if (properties.getProperty(prefix+"mov_en")!=null) this.mov_en=Boolean.parseBoolean(properties.getProperty(prefix+"mov_en"));
if (properties.getProperty(prefix+"mov_sigma")!=null) this.mov_sigma=Double.parseDouble(properties.getProperty(prefix+"mov_sigma"));
if (properties.getProperty(prefix+"mov_max_std")!=null) this.mov_max_std=Double.parseDouble(properties.getProperty(prefix+"mov_max_std"));
if (properties.getProperty(prefix+"mov_thresh_rel")!=null) this.mov_thresh_rel=Double.parseDouble(properties.getProperty(prefix+"mov_thresh_rel"));
if (properties.getProperty(prefix+"mov_thresh_abs")!=null) this.mov_thresh_abs=Double.parseDouble(properties.getProperty(prefix+"mov_thresh_abs"));
if (properties.getProperty(prefix+"mov_clust_max")!=null) this.mov_clust_max=Double.parseDouble(properties.getProperty(prefix+"mov_clust_max"));
if (properties.getProperty(prefix+"mov_grow")!=null) this.mov_grow=Integer.parseInt(properties.getProperty(prefix+"mov_grow"));
if (properties.getProperty(prefix+"mov_show")!=null) this.mov_show=Boolean.parseBoolean(properties.getProperty(prefix+"mov_show"));
if (properties.getProperty(prefix+"mov_debug_level")!=null) this.mov_debug_level=Integer.parseInt(properties.getProperty(prefix+"mov_debug_level"));
if (properties.getProperty(prefix+"adjust_atr_0")!=null) this.adjust_atr[0]=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_atr_0")); if (properties.getProperty(prefix+"adjust_atr_0")!=null) this.adjust_atr[0]=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_atr_0"));
if (properties.getProperty(prefix+"adjust_atr_1")!=null) this.adjust_atr[1]=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_atr_1")); if (properties.getProperty(prefix+"adjust_atr_1")!=null) this.adjust_atr[1]=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_atr_1"));
if (properties.getProperty(prefix+"adjust_atr_2")!=null) this.adjust_atr[2]=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_atr_2")); if (properties.getProperty(prefix+"adjust_atr_2")!=null) this.adjust_atr[2]=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_atr_2"));
...@@ -747,6 +831,19 @@ public class IntersceneMatchParameters { ...@@ -747,6 +831,19 @@ public class IntersceneMatchParameters {
imp.shure_sum = this.shure_sum; imp.shure_sum = this.shure_sum;
imp.maybe_avg = this.maybe_avg; imp.maybe_avg = this.maybe_avg;
imp.shure_avg = this.shure_avg; imp.shure_avg = this.shure_avg;
imp.use_combo_dsi = this.use_combo_dsi;
imp.use_lma_dsi = this.use_lma_dsi;
imp.mov_en = this.mov_en;
imp.mov_sigma = this.mov_sigma;
imp.mov_max_std = this.mov_max_std;
imp.mov_thresh_rel = this.mov_thresh_rel;
imp.mov_thresh_abs = this.mov_thresh_abs;
imp.mov_clust_max = this.mov_clust_max;
imp.mov_grow = this.mov_grow;
imp.mov_show = this.mov_show;
imp.mov_debug_level = this.mov_debug_level;
imp.adjust_atr[0] = this.adjust_atr[0]; imp.adjust_atr[0] = this.adjust_atr[0];
imp.adjust_atr[1] = this.adjust_atr[1]; imp.adjust_atr[1] = this.adjust_atr[1];
imp.adjust_atr[2] = this.adjust_atr[2]; imp.adjust_atr[2] = this.adjust_atr[2];
......
...@@ -2776,15 +2776,15 @@ public class OpticalFlow { ...@@ -2776,15 +2776,15 @@ public class OpticalFlow {
} }
} }
final double [] disparity_ref = dref; final double [] disparity_ref = dref;
final int tilesX_ref = ref_w; // final int tilesX_ref = ref_w;
final int tilesY_ref = ref_h; // final int tilesY_ref = ref_h;
final int tiles = tilesX*tilesY; final int tiles = tilesX*tilesY;
final int transform_size = tp.getTileSize(); final int transform_size = tp.getTileSize();
final double [][] pXpYD= new double [tiles][]; final double [][] pXpYD= new double [tiles][];
final ErsCorrection ersSceneCorrection = scene_QuadClt.getErsCorrection(); final ErsCorrection ersSceneCorrection = scene_QuadClt.getErsCorrection();
final ErsCorrection ersReferenceCorrection = (reference_QuadClt!=null)? reference_QuadClt.getErsCorrection(): ersSceneCorrection; final ErsCorrection ersReferenceCorrection = (reference_QuadClt!=null)? reference_QuadClt.getErsCorrection(): ersSceneCorrection;
if (reference_QuadClt!=null) { if (reference_QuadClt!=null) {
ersReferenceCorrection.setupERS(); // just in case - setUP using instance paRAMETERS ersReferenceCorrection.setupERS(); // just in case - setUP using instance parameters
} }
ersSceneCorrection.setupERS(); ersSceneCorrection.setupERS();
final Thread[] threads = ImageDtt.newThreadArray(threadsMax); final Thread[] threads = ImageDtt.newThreadArray(threadsMax);
...@@ -3709,6 +3709,7 @@ public class OpticalFlow { ...@@ -3709,6 +3709,7 @@ public class OpticalFlow {
scenes_xyzatr[i] = adjustPairsLMAInterscene( scenes_xyzatr[i] = adjustPairsLMAInterscene(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
reference_QuadClt, // QuadCLT reference_QuadCLT, reference_QuadClt, // QuadCLT reference_QuadCLT,
null, // double [] ref_disparity, // null or alternative reference disparity
scene_QuadClt, // QuadCLT scene_QuadCLT, scene_QuadClt, // QuadCLT scene_QuadCLT,
scenes_xyzatr[i][0], // xyz scenes_xyzatr[i][0], // xyz
scenes_xyzatr[i][1], // atr scenes_xyzatr[i][1], // atr
...@@ -4163,6 +4164,7 @@ public class OpticalFlow { ...@@ -4163,6 +4164,7 @@ public class OpticalFlow {
scenes_xyzatr[scene_index] = adjustPairsLMAInterscene( scenes_xyzatr[scene_index] = adjustPairsLMAInterscene(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
quadCLTs[ref_index], // QuadCLT reference_QuadCLT, quadCLTs[ref_index], // QuadCLT reference_QuadCLT,
null, // double [] ref_disparity, // null or alternative reference disparity
scene_QuadClt, // QuadCLT scene_QuadCLT, scene_QuadClt, // QuadCLT scene_QuadCLT,
scenes_xyzatr[scene_index][0], // xyz scenes_xyzatr[scene_index][0], // xyz
scenes_xyzatr[scene_index][1], // atr scenes_xyzatr[scene_index][1], // atr
...@@ -4249,6 +4251,7 @@ public class OpticalFlow { ...@@ -4249,6 +4251,7 @@ public class OpticalFlow {
} }
} }
testERS( testERS(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
test_ers0, // int indx0, // reference scene in a pair test_ers0, // int indx0, // reference scene in a pair
...@@ -4457,6 +4460,7 @@ public class OpticalFlow { ...@@ -4457,6 +4460,7 @@ public class OpticalFlow {
ZERO3, // final double [] scene_xyz, // camera center in world coordinates ZERO3, // final double [] scene_xyz, // camera center in world coordinates
ZERO3, // new double[] {.1,0.1,.1}, // ZERO3, // final double [] scene_atr, // camera orientation relative to world frame ZERO3, // new double[] {.1,0.1,.1}, // ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
quadCLTs[ref_index], // final QuadCLT scene, quadCLTs[ref_index], // final QuadCLT scene,
quadCLTs[ref_index], // final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
true, // toRGB, // final boolean toRGB, true, // toRGB, // final boolean toRGB,
"GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix, "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
threadsMax, // int threadsMax, threadsMax, // int threadsMax,
...@@ -4472,6 +4476,7 @@ public class OpticalFlow { ...@@ -4472,6 +4476,7 @@ public class OpticalFlow {
ZERO3, // final double [] scene_xyz, // camera center in world coordinates ZERO3, // final double [] scene_xyz, // camera center in world coordinates
ZERO3, // final double [] scene_atr, // camera orientation relative to world frame ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
quadCLTs[ref_index], // final QuadCLT scene, quadCLTs[ref_index], // final QuadCLT scene,
quadCLTs[ref_index], // final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
false, // toRGB, // final boolean toRGB, false, // toRGB, // final boolean toRGB,
"GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix, "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
threadsMax, // int threadsMax, threadsMax, // int threadsMax,
...@@ -4493,6 +4498,7 @@ public class OpticalFlow { ...@@ -4493,6 +4498,7 @@ public class OpticalFlow {
ZERO3, // final double [] scene_xyz, // camera center in world coordinates ZERO3, // final double [] scene_xyz, // camera center in world coordinates
ZERO3, // final double [] scene_atr, // camera orientation relative to world frame ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
quadCLTs[ref_index], // final QuadCLT scene, quadCLTs[ref_index], // final QuadCLT scene,
quadCLTs[ref_index], // final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
true, // toRGB, // final boolean toRGB, true, // toRGB, // final boolean toRGB,
"GPU-SHIFTED-FOREGROUND", // String suffix, "GPU-SHIFTED-FOREGROUND", // String suffix,
threadsMax, // int threadsMax, threadsMax, // int threadsMax,
...@@ -4508,6 +4514,7 @@ public class OpticalFlow { ...@@ -4508,6 +4514,7 @@ public class OpticalFlow {
ZERO3, // final double [] scene_xyz, // camera center in world coordinates ZERO3, // final double [] scene_xyz, // camera center in world coordinates
ZERO3, // final double [] scene_atr, // camera orientation relative to world frame ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
quadCLTs[ref_index], // final QuadCLT scene, quadCLTs[ref_index], // final QuadCLT scene,
quadCLTs[ref_index], // final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
false, // toRGB, // final boolean toRGB, false, // toRGB, // final boolean toRGB,
"GPU-SHIFTED-FOREGROUND", // String suffix, "GPU-SHIFTED-FOREGROUND", // String suffix,
threadsMax, // int threadsMax, threadsMax, // int threadsMax,
...@@ -4529,6 +4536,7 @@ public class OpticalFlow { ...@@ -4529,6 +4536,7 @@ public class OpticalFlow {
ZERO3, // final double [] scene_xyz, // camera center in world coordinates ZERO3, // final double [] scene_xyz, // camera center in world coordinates
ZERO3, // final double [] scene_atr, // camera orientation relative to world frame ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
quadCLTs[ref_index], // final QuadCLT scene, quadCLTs[ref_index], // final QuadCLT scene,
quadCLTs[ref_index], // final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
true, // final boolean toRGB, true, // final boolean toRGB,
"GPU-SHIFTED-BACKGROUND", // String suffix, "GPU-SHIFTED-BACKGROUND", // String suffix,
threadsMax, // int threadsMax, threadsMax, // int threadsMax,
...@@ -4544,6 +4552,7 @@ public class OpticalFlow { ...@@ -4544,6 +4552,7 @@ public class OpticalFlow {
ZERO3, // final double [] scene_xyz, // camera center in world coordinates ZERO3, // final double [] scene_xyz, // camera center in world coordinates
ZERO3, // final double [] scene_atr, // camera orientation relative to world frame ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
quadCLTs[ref_index], // final QuadCLT scene, quadCLTs[ref_index], // final QuadCLT scene,
quadCLTs[ref_index], // final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
false, // final boolean toRGB, false, // final boolean toRGB,
"GPU-SHIFTED-BACKGROUND", // String suffix, "GPU-SHIFTED-BACKGROUND", // String suffix,
threadsMax, // int threadsMax, threadsMax, // int threadsMax,
...@@ -4614,7 +4623,12 @@ public class OpticalFlow { ...@@ -4614,7 +4623,12 @@ public class OpticalFlow {
QuadCLT [] quadCLTs, QuadCLT [] quadCLTs,
int debugLevel) { int debugLevel) {
// First create a pair of images, similar to renderSceneSequence() // First create a pair of images, similar to renderSceneSequence()
boolean toRGB = true; // boolean toRGB = true;
boolean show_color = clt_parameters.imp.show_mapped_color;
boolean show_mono = clt_parameters.imp.show_mapped_mono;
boolean use_combo_dsi = clt_parameters.imp.use_combo_dsi;
boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
int ref_index = quadCLTs.length-1; int ref_index = quadCLTs.length-1;
indx0 = ref_index; // disregard initial setting, set to reference indx0 = ref_index; // disregard initial setting, set to reference
int tilesX = quadCLTs[ref_index].getTileProcessor().getTilesX(); int tilesX = quadCLTs[ref_index].getTileProcessor().getTilesX();
...@@ -4637,14 +4651,29 @@ public class OpticalFlow { ...@@ -4637,14 +4651,29 @@ public class OpticalFlow {
quadCLTs[ref_index], // QuadCLT scene, quadCLTs[ref_index], // QuadCLT scene,
debugLevel); debugLevel);
double [] disparity_fg = ds[0]; // combo_dsn_final[COMBO_DSN_INDX_DISP_FG]; double [] disparity_fg = ds[0]; // combo_dsn_final[COMBO_DSN_INDX_DISP_FG];
// double d
double [] interscene_ref_disparity = null; // keep null to use old single-scene disparity for interscene matching
if (use_combo_dsi) {
interscene_ref_disparity = ds[0].clone(); // use_lma_dsi ?
if (use_lma_dsi) {
for (int i = 0; i < interscene_ref_disparity.length; i++) {
if (Double.isNaN(dls[1][i])) {
interscene_ref_disparity[i] = Double.NaN;
}
}
}
}
// QuadCLT [] other_ref = {quadCLTs[indx1],quadCLTs[indx0]}; // QuadCLT [] other_ref = {quadCLTs[indx1],quadCLTs[indx0]};
int [] other_ref = {indx1, indx0}; int [] other_ref = {indx1, indx0};
ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection(); ErsCorrection ers_reference = quadCLTs[ref_index].getErsCorrection();
ImageStack stack_scenes = null; ImageStack stack_scenes_color = null;
ImageStack stack_scenes_mono = null;
int sensor_num = 0; int sensor_num = 0;
int sensor_mask = 1 << sensor_num; int sensor_mask = 1 << sensor_num;
String suffix = quadCLTs[indx0].getImageName()+"-"+quadCLTs[indx1].getImageName()+"-ERS_TEST"; String suffix_color = quadCLTs[indx0].getImageName()+"-"+quadCLTs[indx1].getImageName()+"-ERS_TEST-COLOR";
String suffix_mono = quadCLTs[indx0].getImageName()+"-"+quadCLTs[indx1].getImageName()+"-ERS_TEST-MONO";
double [][] dxyzatr_dt = new double[quadCLTs.length][]; double [][] dxyzatr_dt = new double[quadCLTs.length][];
for (int iscene = 0; iscene <2; iscene++) { for (int iscene = 0; iscene <2; iscene++) {
int nscene = other_ref[iscene]; int nscene = other_ref[iscene];
...@@ -4674,28 +4703,61 @@ public class OpticalFlow { ...@@ -4674,28 +4703,61 @@ public class OpticalFlow {
scene_ers_xyz_dt, // double [] ers_xyz_dt, scene_ers_xyz_dt, // double [] ers_xyz_dt,
scene_ers_atr_dt); // double [] ers_atr_dt)(ers_scene_original_xyz_dt); scene_ers_atr_dt); // double [] ers_atr_dt)(ers_scene_original_xyz_dt);
} }
ImagePlus imp_scene = QuadCLT.renderGPUFromDSI(
sensor_mask, // final int sensor_mask, if (show_color) {
null, // final Rectangle full_woi_in, // show larger than sensor WOI (or null) ImagePlus imp_scene_color = QuadCLT.renderGPUFromDSI(
clt_parameters, // CLTParameters clt_parameters, sensor_mask, // final int sensor_mask,
disparity_fg, // double [] disparity_ref, null, // final Rectangle full_woi_in, // show larger than sensor WOI (or null)
scene_xyz, // final double [] scene_xyz, // camera center in world coordinates clt_parameters, // CLTParameters clt_parameters,
scene_atr, // final double [] scene_atr, // camera orientation relative to world frame disparity_fg, // double [] disparity_ref,
quadCLTs[nscene], // final QuadCLT scene, scene_xyz, // final double [] scene_xyz, // camera center in world coordinates
toRGB, // final boolean toRGB, scene_atr, // final double [] scene_atr, // camera orientation relative to world frame
"", // String suffix, no suffix here quadCLTs[nscene], // final QuadCLT scene,
threadsMax, // int threadsMax, quadCLTs[ref_index], // final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
debugLevel); // int debugLevel) true, // final boolean toRGB,
if (stack_scenes == null) { "", // String suffix, no suffix here
stack_scenes = new ImageStack(imp_scene.getWidth(),imp_scene.getHeight()); threadsMax, // int threadsMax,
debugLevel); // int debugLevel)
if (stack_scenes_color == null) {
stack_scenes_color = new ImageStack(imp_scene_color.getWidth(),imp_scene_color.getHeight());
}
stack_scenes_color.addSlice(
nscene+":"+ts,
imp_scene_color.getStack().getPixels(sensor_num+1));
}
if (show_mono) {
ImagePlus imp_scene_mono = QuadCLT.renderGPUFromDSI(
sensor_mask, // final int sensor_mask,
null, // final Rectangle full_woi_in, // show larger than sensor WOI (or null)
clt_parameters, // CLTParameters clt_parameters,
disparity_fg, // double [] disparity_ref,
scene_xyz, // final double [] scene_xyz, // camera center in world coordinates
scene_atr, // final double [] scene_atr, // camera orientation relative to world frame
quadCLTs[nscene], // final QuadCLT scene,
quadCLTs[ref_index], // final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
false, // final boolean toRGB,
"", // String suffix, no suffix here
threadsMax, // int threadsMax,
debugLevel); // int debugLevel)
if (stack_scenes_mono == null) {
stack_scenes_mono = new ImageStack(imp_scene_mono.getWidth(),imp_scene_mono.getHeight());
}
stack_scenes_mono.addSlice(
nscene+":"+ts,
imp_scene_mono.getStack().getPixels(sensor_num+1));
} }
stack_scenes.addSlice(
nscene+":"+ts,
imp_scene.getStack().getPixels(sensor_num+1));
} }
ImagePlus imp_scenes = new ImagePlus(suffix, stack_scenes); if (show_color) {
imp_scenes.getProcessor().resetMinAndMax(); ImagePlus imp_scenes_color = new ImagePlus(suffix_color, stack_scenes_color);
imp_scenes.show(); imp_scenes_color.getProcessor().resetMinAndMax();
imp_scenes_color.show();
}
if (show_mono) {
ImagePlus imp_scenes_mono = new ImagePlus(suffix_mono, stack_scenes_mono);
imp_scenes_mono.getProcessor().resetMinAndMax();
imp_scenes_mono.show();
}
String [] vel_names = {"Vx","Vy","Vz","Vaz","Vtl","Vrl"}; String [] vel_names = {"Vx","Vy","Vz","Vaz","Vtl","Vrl"};
String [] vel_units = {"m/s","m/s","m/s","rad/s","rad/s","rad/s","rad/s"}; String [] vel_units = {"m/s","m/s","m/s","rad/s","rad/s","rad/s","rad/s"};
System.out.println("Pair: ref. scene "+indx0+": ("+quadCLTs[indx0].getImageName()+")"+ System.out.println("Pair: ref. scene "+indx0+": ("+quadCLTs[indx0].getImageName()+")"+
...@@ -4719,9 +4781,30 @@ public class OpticalFlow { ...@@ -4719,9 +4781,30 @@ public class OpticalFlow {
scene_ers_xyz_dt, // double [] ers_xyz_dt, scene_ers_xyz_dt, // double [] ers_xyz_dt,
scene_ers_atr_dt); // double [] ers_atr_dt)(ers_scene_original_xyz_dt); scene_ers_atr_dt); // double [] ers_atr_dt)(ers_scene_original_xyz_dt);
double [] lma_rms = new double[2]; double [] lma_rms = new double[2];
double []ref_dt = dxyzatr_dt[other_ref[1]];
double []scene_dt = dxyzatr_dt[other_ref[0]];
double []ref_xyz_dt = ZERO3; // {ref_dt[0],ref_dt[1],ref_dt[2]};
// double []ref_xyz_dt = {ref_dt[0],ref_dt[1],ref_dt[2]};
// double []ref_xyz_dt = {-ref_dt[0],-ref_dt[1],-ref_dt[2]};
double k = 1.0;
double []ref_atr_dt = {k*ref_dt[3],k*ref_dt[4],k*ref_dt[5]};
double []scene_xyz_dt = ZERO3; // {scene_dt[0],scene_dt[1],scene_dt[2]};
// double []scene_xyz_dt = {scene_dt[0],scene_dt[1],scene_dt[2]};
// double []scene_xyz_dt = {-scene_dt[0],-scene_dt[1],-scene_dt[2]};
double []scene_atr_dt = {k*scene_dt[3],k*scene_dt[4],k*scene_dt[5]};
// first - try ATR only
quadCLTs[other_ref[0]].getErsCorrection().setErsDt( // scene
scene_xyz_dt, // double [] ers_xyz_dt,
scene_atr_dt); // double [] ers_atr_dt)(ers_scene_original_xyz_dt);
quadCLTs[other_ref[1]].getErsCorrection().setErsDt( // reference
ref_xyz_dt, // double [] ers_xyz_dt,
ref_atr_dt); // double [] ers_atr_dt)(ers_scene_original_xyz_dt);
double [][] adjusted_xyzatr = adjustPairsLMAInterscene( double [][] adjusted_xyzatr = adjustPairsLMAInterscene(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
quadCLTs[other_ref[1]], // QuadCLT reference_QuadCLT, quadCLTs[other_ref[1]], // QuadCLT reference_QuadCLT,
null, // double [] ref_disparity, // null or alternative reference disparity
quadCLTs[other_ref[0]], // QuadCLT scene_QuadCLT, quadCLTs[other_ref[0]], // QuadCLT scene_QuadCLT,
scene_xyz_pre, // xyz scene_xyz_pre, // xyz
scene_atr_pre, // atr scene_atr_pre, // atr
...@@ -4732,44 +4815,80 @@ public class OpticalFlow { ...@@ -4732,44 +4815,80 @@ public class OpticalFlow {
clt_parameters.imp.debug_level); // 1); // -1); // int debug_level); clt_parameters.imp.debug_level); // 1); // -1); // int debug_level);
System.out.println("lma_rms={"+lma_rms[0]+","+lma_rms[1]+"}"); System.out.println("lma_rms={"+lma_rms[0]+","+lma_rms[1]+"}");
ImageStack stack_adjusted = null; ImageStack stack_adjusted_color = null;
String suffix_adjusted = quadCLTs[indx0].getImageName()+"-"+quadCLTs[indx1].getImageName()+"-ERS_TEST_ADJUSTED"; ImageStack stack_adjusted_mono = null;
String suffix_adjusted_color = quadCLTs[indx0].getImageName()+"-"+quadCLTs[indx1].getImageName()+"-ERS_TEST_ADJUSTED-COLOR";
String suffix_adjusted_mono = quadCLTs[indx0].getImageName()+"-"+quadCLTs[indx1].getImageName()+"-ERS_TEST_ADJUSTED-MONO";
for (int iscene = 0; iscene <2; iscene++) { for (int iscene = 0; iscene <2; iscene++) {
int nscene = other_ref[iscene]; int nscene = other_ref[iscene];
String ts = quadCLTs[nscene].getImageName(); String ts = quadCLTs[nscene].getImageName();
double [] scene_xyz = ZERO3; double [] scene_xyz = ZERO3;
double [] scene_atr = ZERO3; double [] scene_atr = ZERO3;
if (nscene != ref_index) { if (nscene != ref_index) {
scene_xyz = ers_reference.getSceneXYZ(ts); // keep current/last adjusted
scene_atr = ers_reference.getSceneATR(ts); scene_xyz = adjusted_xyzatr[0]; // ers_reference.getSceneXYZ(ts);
scene_atr = adjusted_xyzatr[1]; // ers_reference.getSceneATR(ts);
/*
double [] scene_ers_xyz_dt1 = ers_reference.getSceneErsXYZ_dt(ts); double [] scene_ers_xyz_dt1 = ers_reference.getSceneErsXYZ_dt(ts);
double [] scene_ers_atr_dt1 = ers_reference.getSceneErsATR_dt(ts); double [] scene_ers_atr_dt1 = ers_reference.getSceneErsATR_dt(ts);
quadCLTs[nscene].getErsCorrection().setErsDt( quadCLTs[nscene].getErsCorrection().setErsDt(
scene_ers_xyz_dt1, // double [] ers_xyz_dt, scene_ers_xyz_dt1, // double [] ers_xyz_dt,
scene_ers_atr_dt1); // double [] ers_atr_dt)(ers_scene_original_xyz_dt); scene_ers_atr_dt1); // double [] ers_atr_dt)(ers_scene_original_xyz_dt);
*/
} }
ImagePlus imp_scene = QuadCLT.renderGPUFromDSI( if (show_color) {
sensor_mask, // final int sensor_mask, ImagePlus imp_scene_color = QuadCLT.renderGPUFromDSI(
null, // final Rectangle full_woi_in, // show larger than sensor WOI (or null) sensor_mask, // final int sensor_mask,
clt_parameters, // CLTParameters clt_parameters, null, // final Rectangle full_woi_in, // show larger than sensor WOI (or null)
disparity_fg, // double [] disparity_ref, clt_parameters, // CLTParameters clt_parameters,
scene_xyz, // final double [] scene_xyz, // camera center in world coordinates disparity_fg, // double [] disparity_ref,
scene_atr, // final double [] scene_atr, // camera orientation relative to world frame scene_xyz, // final double [] scene_xyz, // camera center in world coordinates
quadCLTs[nscene], // final QuadCLT scene, scene_atr, // final double [] scene_atr, // camera orientation relative to world frame
toRGB, // final boolean toRGB, quadCLTs[nscene], // final QuadCLT scene,
"", // String suffix, no suffix here quadCLTs[ref_index], // final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
threadsMax, // int threadsMax, true, // final boolean toRGB,
debugLevel); // int debugLevel) "", // String suffix, no suffix here
if (stack_adjusted == null) { threadsMax, // int threadsMax,
stack_adjusted = new ImageStack(imp_scene.getWidth(),imp_scene.getHeight()); debugLevel); // int debugLevel)
if (stack_adjusted_color == null) {
stack_adjusted_color = new ImageStack(imp_scene_color.getWidth(),imp_scene_color.getHeight());
}
stack_adjusted_color.addSlice(
nscene+":"+ts,
imp_scene_color.getStack().getPixels(sensor_num+1));
}
if (show_mono) {
ImagePlus imp_scene_mono = QuadCLT.renderGPUFromDSI(
sensor_mask, // final int sensor_mask,
null, // final Rectangle full_woi_in, // show larger than sensor WOI (or null)
clt_parameters, // CLTParameters clt_parameters,
disparity_fg, // double [] disparity_ref,
scene_xyz, // final double [] scene_xyz, // camera center in world coordinates
scene_atr, // final double [] scene_atr, // camera orientation relative to world frame
quadCLTs[nscene], // final QuadCLT scene,
quadCLTs[ref_index], // final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
false, // final boolean toRGB,
"", // String suffix, no suffix here
threadsMax, // int threadsMax,
debugLevel); // int debugLevel)
if (stack_adjusted_mono == null) {
stack_adjusted_mono = new ImageStack(imp_scene_mono.getWidth(),imp_scene_mono.getHeight());
}
stack_adjusted_mono.addSlice(
nscene+":"+ts,
imp_scene_mono.getStack().getPixels(sensor_num+1));
} }
stack_adjusted.addSlice(
nscene+":"+ts,
imp_scene.getStack().getPixels(sensor_num+1));
} }
ImagePlus imp_adjusted = new ImagePlus(suffix_adjusted, stack_adjusted); if (show_color) {
imp_adjusted.getProcessor().resetMinAndMax(); ImagePlus imp_adjusted_color = new ImagePlus(suffix_adjusted_color, stack_adjusted_color);
imp_adjusted.show(); imp_adjusted_color.getProcessor().resetMinAndMax();
imp_adjusted_color.show();
}
if (show_mono) {
ImagePlus imp_adjusted_mono = new ImagePlus(suffix_adjusted_mono, stack_adjusted_mono);
imp_adjusted_mono.getProcessor().resetMinAndMax();
imp_adjusted_mono.show();
}
return; return;
} }
...@@ -4869,6 +4988,7 @@ public class OpticalFlow { ...@@ -4869,6 +4988,7 @@ public class OpticalFlow {
scene_xyz, // final double [] scene_xyz, // camera center in world coordinates scene_xyz, // final double [] scene_xyz, // camera center in world coordinates
scene_atr, // final double [] scene_atr, // camera orientation relative to world frame scene_atr, // final double [] scene_atr, // camera orientation relative to world frame
quadCLTs[nscene], // final QuadCLT scene, quadCLTs[nscene], // final QuadCLT scene,
quadCLTs[ref_index], // final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
toRGB, // final boolean toRGB, toRGB, // final boolean toRGB,
// "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
"", // String suffix, no suffix here "", // String suffix, no suffix here
...@@ -4968,6 +5088,7 @@ public class OpticalFlow { ...@@ -4968,6 +5088,7 @@ public class OpticalFlow {
double [][][] coord_motion = interCorrPair( // new double [tilesY][tilesX][][]; double [][][] coord_motion = interCorrPair( // new double [tilesY][tilesX][][];
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
reference_QuadClt, // QuadCLT reference_QuadCLT, reference_QuadClt, // QuadCLT reference_QuadCLT,
null, // double [] ref_disparity, // null or alternative reference disparity
scene_QuadClt, // QuadCLT scene_QuadCLT, scene_QuadClt, // QuadCLT scene_QuadCLT,
pose[0], // camera_xyz0, // xyz pose[0], // camera_xyz0, // xyz
atr, // camera_atr0, // pose[1], // atr atr, // camera_atr0, // pose[1], // atr
...@@ -5201,6 +5322,7 @@ public class OpticalFlow { ...@@ -5201,6 +5322,7 @@ public class OpticalFlow {
scenes_xyzatr[i] = adjustPairsLMAInterscene( scenes_xyzatr[i] = adjustPairsLMAInterscene(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
reference_QuadClt, // QuadCLT reference_QuadCLT, reference_QuadClt, // QuadCLT reference_QuadCLT,
null, // double [] ref_disparity, // null or alternative reference disparity
scene_QuadClt, // QuadCLT scene_QuadCLT, scene_QuadClt, // QuadCLT scene_QuadCLT,
combo_XYZATR[0], // xyz combo_XYZATR[0], // xyz
combo_XYZATR[1], // atr combo_XYZATR[1], // atr
...@@ -5307,6 +5429,7 @@ public class OpticalFlow { ...@@ -5307,6 +5429,7 @@ public class OpticalFlow {
scenes_xyzatr[i] = adjustPairsLMAInterscene( scenes_xyzatr[i] = adjustPairsLMAInterscene(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
reference_QuadClt, // QuadCLT reference_QuadCLT, reference_QuadClt, // QuadCLT reference_QuadCLT,
null, // double [] ref_disparity, // null or alternative reference disparity
scene_QuadClt, // QuadCLT scene_QuadCLT, scene_QuadClt, // QuadCLT scene_QuadCLT,
combo_XYZATR[0], // xyz combo_XYZATR[0], // xyz
combo_XYZATR[1], // atr combo_XYZATR[1], // atr
...@@ -5445,6 +5568,7 @@ public class OpticalFlow { ...@@ -5445,6 +5568,7 @@ public class OpticalFlow {
scenes_xyzatr[i] = adjustPairsLMAInterscene( scenes_xyzatr[i] = adjustPairsLMAInterscene(
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
reference_QuadClt, // QuadCLT reference_QuadCLT, reference_QuadClt, // QuadCLT reference_QuadCLT,
null, // double [] ref_disparity, // null or alternative reference disparity
scenes[i], // QuadCLT scene_QuadCLT, scenes[i], // QuadCLT scene_QuadCLT,
scene_xyz, // combo_XYZATR[0], // xyz scene_xyz, // combo_XYZATR[0], // xyz
scene_atr, // combo_XYZATR[1], // atr scene_atr, // combo_XYZATR[1], // atr
...@@ -10353,6 +10477,7 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad ...@@ -10353,6 +10477,7 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad
public double [][][] interCorrPair( // return [tilesX*telesY]{ref_pXpYD, dXdYS} public double [][][] interCorrPair( // return [tilesX*telesY]{ref_pXpYD, dXdYS}
CLTParameters clt_parameters, CLTParameters clt_parameters,
QuadCLT ref_scene, QuadCLT ref_scene,
double [] ref_disparity, // null or alternative reference disparity
QuadCLT scene, QuadCLT scene,
double [] scene_xyz, double [] scene_xyz,
double [] scene_atr, double [] scene_atr,
...@@ -10373,20 +10498,20 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad ...@@ -10373,20 +10498,20 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad
boolean toRGB = clt_parameters.imp.toRGB ; // true; boolean toRGB = clt_parameters.imp.toRGB ; // true;
boolean show_coord_motion = clt_parameters.imp.showCorrMotion(); // mae its own boolean show_coord_motion = clt_parameters.imp.showCorrMotion(); // mae its own
int erase_clt = (toRGB? clt_parameters.imp.show_color_nan : clt_parameters.imp.show_mono_nan) ? 1:0; int erase_clt = (toRGB? clt_parameters.imp.show_color_nan : clt_parameters.imp.show_mono_nan) ? 1:0;
boolean use_lma_dsi = clt_parameters.imp.use_lma_dsi;
boolean mov_en = clt_parameters.imp.mov_en; // true; // enable detection/removal of the moving objects during pose matching
boolean mov_debug_images = clt_parameters.imp.showMovementDetection();
int mov_debug_level = clt_parameters.imp.movDebugLevel() ;
if (scene_is_ref_test) { if (scene_is_ref_test) {
scene_xyz = ZERO3.clone(); scene_xyz = ZERO3.clone();
scene_atr = ZERO3.clone(); scene_atr = ZERO3.clone();
ref_scene = scene; // or opposite: scene = ref_scene; ref_scene = scene; // or opposite: scene = ref_scene;
} }
int tilesX = tp.getTilesX(); int tilesX = tp.getTilesX();
int tilesY = tp.getTilesY(); int tilesY = tp.getTilesY();
// double [][][] coord_motion = new double [tilesX*tilesY][][];
double [][][] coord_motion = null; // new double [2][tilesX*tilesY][]; double [][][] coord_motion = null; // new double [2][tilesX*tilesY][];
final double [][][] motion_vectors = show_motion_vectors?new double [tilesY *tilesX][][]:null; final double [][][] motion_vectors = show_motion_vectors?new double [tilesY *tilesX][][]:null;
final float [][][] fclt_corr = ((accum_2d_corr != null) || show_2d_correlations) ? final float [][][] fclt_corr = ((accum_2d_corr != null) || show_2d_correlations) ?
(new float [tilesX * tilesY][][]) : null; (new float [tilesX * tilesY][][]) : null;
if (debug_level > 1) { if (debug_level > 1) {
...@@ -10407,11 +10532,13 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad ...@@ -10407,11 +10532,13 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad
ref_scene.getGPU().setGpu_debug_level(debug_level - 4); // monitor GPU ops >=-1 ref_scene.getGPU().setGpu_debug_level(debug_level - 4); // monitor GPU ops >=-1
} }
final double disparity_corr = 0.0; // (z_correction == 0) ? 0.0 : geometryCorrection.getDisparityFromZ(1.0/z_correction); final double disparity_corr = 0.0; // (z_correction == 0) ? 0.0 : geometryCorrection.getDisparityFromZ(1.0/z_correction);
// double [][] dsrbg_ref= ref_scene.getDSRBG(); //ref_disparity
double [][] dls= ref_scene.getDLS(); if (ref_disparity == null) {
ref_disparity = ref_scene.getDLS()[use_lma_dsi?1:0];
}
double [][] ref_pXpYD = transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls double [][] ref_pXpYD = transformToScenePxPyD( // full size - [tilesX*tilesY], some nulls
null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null) null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null)
dls[0], // final double [] disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?) ref_disparity, // dls[0], // final double [] disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
ZERO3, // final double [] scene_xyz, // camera center in world coordinates ZERO3, // final double [] scene_xyz, // camera center in world coordinates
ZERO3, // final double [] scene_atr, // camera orientation relative to world frame ZERO3, // final double [] scene_atr, // camera orientation relative to world frame
ref_scene, // final QuadCLT scene_QuadClt, ref_scene, // final QuadCLT scene_QuadClt,
...@@ -10430,17 +10557,10 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad ...@@ -10430,17 +10557,10 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad
margin, // final int margin, // do not use tiles if their centers are closer to the edges margin, // final int margin, // do not use tiles if their centers are closer to the edges
null, // final boolean [] valid_tiles, null, // final boolean [] valid_tiles,
threadsMax); // final int threadsMax) // maximal number of threads to launch threadsMax); // final int threadsMax) // maximal number of threads to launch
/*
double [] scene_ers_xyz_dt = ers_reference.getSceneErsXYZ_dt(ts);
double [] scene_ers_atr_dt = ers_reference.getSceneErsATR_dt(ts);
scene.getErsCorrection().setErsDt(
scene_ers_xyz_dt, // double [] ers_xyz_dt,
scene_ers_atr_dt); // double [] ers_atr_dt)(ers_scene_original_xyz_dt);
//setupERS() will be inside transformToScenePxPyD()
*/
double [][] scene_pXpYD = transformToScenePxPyD( // will be null for disparity == NaN, total size - tilesX*tilesY double [][] scene_pXpYD = transformToScenePxPyD( // will be null for disparity == NaN, total size - tilesX*tilesY
null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null) null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null)
dls[0], // final double [] disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?) ref_disparity, // dls[0], // final double [] disparity_ref, // invalid tiles - NaN in disparity (maybe it should not be masked by margins?)
scene_xyz, // final double [] scene_xyz, // camera center in world coordinates scene_xyz, // final double [] scene_xyz, // camera center in world coordinates
scene_atr, // final double [] scene_atr, // camera orientation relative to world frame scene_atr, // final double [] scene_atr, // camera orientation relative to world frame
scene, // final QuadCLT scene_QuadClt, scene, // final QuadCLT scene_QuadClt,
...@@ -10458,7 +10578,6 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad ...@@ -10458,7 +10578,6 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad
threadsMax); // final int threadsMax) // maximal number of threads to launch threadsMax); // final int threadsMax) // maximal number of threads to launch
if (ref_scene.hasGPU()) { // direct convert to reference_clt if (ref_scene.hasGPU()) { // direct convert to reference_clt
// float [][][][] fcorr_td = new float[tilesY][tilesX][][];
float [][][][] fcorr_td = null; // no accumulation, use data in GPU float [][][][] fcorr_td = null; // no accumulation, use data in GPU
ref_scene.saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry ref_scene.saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
image_dtt.setReferenceTD( image_dtt.setReferenceTD(
...@@ -10520,41 +10639,53 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad ...@@ -10520,41 +10639,53 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad
double [][][] dcorr_tiles = (fclt_corr != null)? (new double [tp_tasks.length][][]):null; double [][][] dcorr_tiles = (fclt_corr != null)? (new double [tp_tasks.length][][]):null;
// will use num_acc with variable number of accumulations (e.g. clusters) // will use num_acc with variable number of accumulations (e.g. clusters)
coord_motion = image_dtt.clt_process_tl_interscene( // convert to pixel domain and process correlations already prepared in fcorr_td and/or fcorr_combo_td coord_motion = image_dtt.clt_process_tl_interscene( // convert to pixel domain and process correlations already prepared in fcorr_td and/or fcorr_combo_td
clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
fcorr_td, // final float [][][][] fcorr_td, // [tilesY][tilesX][pair][4*64] transform domain representation of all selected corr pairs fcorr_td, // final float [][][][] fcorr_td, // [tilesY][tilesX][pair][4*64] transform domain representation of all selected corr pairs
null, // float [][][] num_acc, // number of accumulated tiles [tilesY][tilesX][pair] (or null). Can be inner null if not used in tp_tasks null, // float [][][] num_acc, // number of accumulated tiles [tilesY][tilesX][pair] (or null). Can be inner null if not used in tp_tasks
null, // double [] dcorr_weight, // alternative to num_acc, compatible with CPU processing (only one non-zero enough) null, // double [] dcorr_weight, // alternative to num_acc, compatible with CPU processing (only one non-zero enough)
clt_parameters.gpu_corr_scale, // final double gpu_corr_scale, // 0.75; // reduce GPU-generated correlation values clt_parameters.gpu_corr_scale, // final double gpu_corr_scale, // 0.75; // reduce GPU-generated correlation values
clt_parameters.getGpuFatZeroInter(ref_scene.isMonochrome()), // final double gpu_fat_zero, // clt_parameters.getGpuFatZero(is_mono);absolute == 30.0 clt_parameters.getGpuFatZeroInter(ref_scene.isMonochrome()), // final double gpu_fat_zero, // clt_parameters.getGpuFatZero(is_mono);absolute == 30.0
image_dtt.transform_size - 1, // final int gpu_corr_rad, // = transform_size - 1 ? image_dtt.transform_size - 1, // final int gpu_corr_rad, // = transform_size - 1 ?
// The tp_tasks data should be decoded from GPU to get coordinates // The tp_tasks data should be decoded from GPU to get coordinates
tp_tasks, // final TpTask [] tp_tasks, // data from the reference frame - will be applied to LMA for the integrated correlations tp_tasks, // final TpTask [] tp_tasks, // data from the reference frame - will be applied to LMA for the integrated correlations
// to be converted to float (may be null) // to be converted to float (may be null)
dcorr_tiles, // final double [][][] dcorr_tiles, // [tile][pair_abs, sparse][(2*transform_size-1)*(2*transform_size-1)] // if null - will not calculate dcorr_tiles, // final double [][][] dcorr_tiles, // [tile][pair_abs, sparse][(2*transform_size-1)*(2*transform_size-1)] // if null - will not calculate
ref_pXpYD, // final double [][] pXpYD, // pXpYD for the reference scene ref_pXpYD, // final double [][] pXpYD, // pXpYD for the reference scene
motion_vectors, // final double [][][] motion_vectors, // [tilesY*tilesX][][] -> [][][num_sel_sensors+1][2] motion_vectors, // final double [][][] motion_vectors, // [tilesY*tilesX][][] -> [][][num_sel_sensors+1][2]
clt_parameters.imp.run_poly, // final boolean run_poly, // polynomial max, if false - centroid clt_parameters.imp.run_poly, // final boolean run_poly, // polynomial max, if false - centroid
clt_parameters.imp.use_partial, // final boolean use_partial, // find motion vectors for individual pairs, false - for sum only clt_parameters.imp.use_partial, // final boolean use_partial, // find motion vectors for individual pairs, false - for sum only
clt_parameters.imp.centroid_radius,// final double centroid_radius, // 0 - use all tile, >0 - cosine window around local max clt_parameters.imp.centroid_radius,// final double centroid_radius, // 0 - use all tile, >0 - cosine window around local max
clt_parameters.imp.n_recenter, // final int n_recenter, // when cosine window, re-center window this many times clt_parameters.imp.n_recenter, // final int n_recenter, // when cosine window, re-center window this many times
clt_parameters.imp.td_weight, // final double td_weight, // mix correlations accumulated in TD with clt_parameters.imp.td_weight, // final double td_weight, // mix correlations accumulated in TD with
clt_parameters.imp.pd_weight, // final double pd_weight, // correlations (post) accumulated in PD clt_parameters.imp.pd_weight, // final double pd_weight, // correlations (post) accumulated in PD
clt_parameters.imp.td_nopd_only, // final boolean td_nopd_only, // only use TD accumulated data if no safe PD is available for the tile. clt_parameters.imp.td_nopd_only, // final boolean td_nopd_only, // only use TD accumulated data if no safe PD is available for the tile.
clt_parameters.imp.min_str, // final double min_str, // = 0.25; clt_parameters.imp.min_str, // final double min_str, // = 0.25;
clt_parameters.imp.min_str_sum, // final double min_str_sum, // = 0.8; // 5; clt_parameters.imp.min_str_sum, // final double min_str_sum, // = 0.8; // 5;
clt_parameters.imp.min_neibs, // final int min_neibs, // 2; // minimal number of strong neighbors (> min_str) clt_parameters.imp.min_neibs, // final int min_neibs, // 2; // minimal number of strong neighbors (> min_str)
clt_parameters.imp.weight_zero_neibs, // final double weight_zero_neibs,// 0.2; // Reduce weight for no-neib (1.0 for all 8) clt_parameters.imp.weight_zero_neibs, // final double weight_zero_neibs,// 0.2; // Reduce weight for no-neib (1.0 for all 8)
clt_parameters.imp.half_disparity, // final double half_disparity, // 5.0; // Reduce weight twice for this disparity clt_parameters.imp.half_disparity, // final double half_disparity, // 5.0; // Reduce weight twice for this disparity
clt_parameters.imp.half_avg_diff, // final double half_avg_diff, // 0.2; // when L2 of x,y difference from average of neibs - reduce twice clt_parameters.imp.half_avg_diff, // final double half_avg_diff, // 0.2; // when L2 of x,y difference from average of neibs - reduce twice
clt_parameters.tileX, // final int debug_tileX, clt_parameters.tileX, // final int debug_tileX,
clt_parameters.tileY, // final int debug_tileY, clt_parameters.tileY, // final int debug_tileY,
threadsMax, // final int threadsMax, // maximal number of threads to launch threadsMax, // final int threadsMax, // maximal number of threads to launch
debug_level); // final int globalDebugLevel); debug_level);
// final int globalDebugLevel);
// ImageDtt.convertFcltCorr(
// dcorr_tiles, // double [][][] dcorr_tiles,// [tile][sparse, correlation pair][(2*transform_size-1)*(2*transform_size-1)] // if null - will not calculate if (mov_en) {
// fclt_corr); // float [][][] fclt_corr) // new float [tilesX * tilesY][][] or null String debug_image_name = mov_debug_images ? (scene.getImageName()+"-"+ref_scene.getImageName()+"-movements"): null;
boolean [] move_mask = getMovementMask(
clt_parameters, // CLTParameters clt_parameters,
coord_motion[1], // double [][] motion, // only x,y,w components
tilesX, // int tilesX,
debug_image_name, // String debug_image_name,
mov_debug_level); // int debug_level);
if (move_mask != null) {
for (int nTile=0; nTile < move_mask.length; nTile++) if (move_mask[nTile]) {
coord_motion[1][nTile]= null;
}
}
}
float [][][] fclt_corr1 = ImageDtt.convertFcltCorr( // partial length, matching corr_indices = gpuQuad.getCorrIndices(); // also sets num_corr_tiles float [][][] fclt_corr1 = ImageDtt.convertFcltCorr( // partial length, matching corr_indices = gpuQuad.getCorrIndices(); // also sets num_corr_tiles
dcorr_tiles, // double [][][] dcorr_tiles,// [tile][sparse, correlation pair][(2*transform_size-1)*(2*transform_size-1)] // if null - will not calculate dcorr_tiles, // double [][][] dcorr_tiles,// [tile][sparse, correlation pair][(2*transform_size-1)*(2*transform_size-1)] // if null - will not calculate
fclt_corr); // float [][][] fclt_corr) // new float [tilesX * tilesY][][] or null fclt_corr); // float [][][] fclt_corr) // new float [tilesX * tilesY][][] or null
...@@ -10640,7 +10771,7 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad ...@@ -10640,7 +10771,7 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad
} }
if (show_coord_motion) { if (show_coord_motion) {
//coord_motion //coord_motion
String [] mvTitles = {"dx", "dy", "conf", "pX", "pY","Disp"}; String [] mvTitles = {"dx", "dy", "conf", "pX", "pY","Disp"}; // ,"blurX","blurY", "blur"};
double [][] dbg_img = new double [mvTitles.length][tilesX*tilesY]; double [][] dbg_img = new double [mvTitles.length][tilesX*tilesY];
for (int l = 0; l < dbg_img.length; l++) { for (int l = 0; l < dbg_img.length; l++) {
Arrays.fill(dbg_img[l], Double.NaN); Arrays.fill(dbg_img[l], Double.NaN);
...@@ -10675,8 +10806,202 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad ...@@ -10675,8 +10806,202 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad
System.out.println ("interCorrPair(): num_defined = "+num_defined+ System.out.println ("interCorrPair(): num_defined = "+num_defined+
", sum_strength = "+sum_strength+", avg_strength = "+(sum_strength/num_defined)); ", sum_strength = "+sum_strength+", avg_strength = "+(sum_strength/num_defined));
} }
return coord_motion; return coord_motion;
} }
public static boolean [] getMovementMask(
CLTParameters clt_parameters,
double [][] motion, // only x,y,w components
int tilesX,
String debug_image_name,
int debug_level)
{
boolean show_debug_images = (debug_image_name != null);
String [] mvTitles = {"dx", "dy", "conf", "blur", "blurX","blurY","clust","mask"};
boolean mov_en = clt_parameters.imp.mov_en; // true; // enable detection/removal of the moving objects during pose matching
if (!mov_en) {
return null;
}
double mov_sigma = clt_parameters.imp.mov_sigma; // 1.5; // pix - weighted-blur offsets before detection
// next two to prevent motion detection while errors are too big
double mov_max_std = clt_parameters.imp.mov_max_std; // 0.5; // pix
double mov_thresh_rel = clt_parameters.imp.mov_thresh_rel; // 5.0; // exceed average error
double mov_thresh_abs= clt_parameters.imp.mov_thresh_abs; // 1.0; // sqrt(dx^2+dy^2) in moving areas
double mov_clust_max = clt_parameters.imp.mov_clust_max; // 1.5; // cluster maximum should exceed threshold this times
int mov_grow = clt_parameters.imp.mov_grow; // 4; // grow detected moving area
double mov_max_std2 = mov_max_std * mov_max_std;
double mov_thresh_rel2 = mov_thresh_rel* mov_thresh_rel;
double mov_thresh_abs2 = mov_thresh_abs * mov_thresh_abs;
int tilesY = motion.length / tilesX;
double [] wa = new double[3];
for (int nTile = 0; nTile < motion.length; nTile++) if (motion[nTile] != null) {
double w = motion[nTile][2]; //? 1.0; //
wa[0]+=w;
wa[1]+=w*motion[nTile][0];
wa[2]+=w*motion[nTile][1];
}
wa[1] /= wa[0];
wa[2] /= wa[0];
double [][] mov_obj = new double[show_debug_images?4:2][tilesX*tilesY];
double sl2 = 0;
for (int nTile = 0; nTile < motion.length; nTile++) if (motion[nTile] != null) {
double w = motion[nTile][2]; //? 1.0; //
mov_obj[0][nTile]= w;
double x = motion[nTile][0]-wa[1];
double y = motion[nTile][1]-wa[2];
double l2 = x*x + y*y;
mov_obj[1][nTile]= w*l2;
if (show_debug_images) {
mov_obj[2][nTile]= w*x;
mov_obj[3][nTile]= w*y;
}
sl2+=w*l2;
}
sl2/=wa[0];
if (debug_level>0) {
System.out.println("getMovementMask(): std="+Math.sqrt(sl2));
}
double [][] dbg_img = null;
if(show_debug_images) {
dbg_img = new double [mvTitles.length][tilesX*tilesY];
for (int l = 0; l < dbg_img.length; l++) {
Arrays.fill(dbg_img[l], Double.NaN);
}
}
boolean [] move_mask = null;
process_movements:
{
if (sl2 > mov_max_std2) {
if (debug_level>0) {
System.out.println("getMovementMask(): Too high scene mismatch: std="+Math.sqrt(sl2)+" > "+mov_max_std+", aborting movement processing");
}
break process_movements;
}
DoubleGaussianBlur gb = new DoubleGaussianBlur();
gb.blurDouble(mov_obj[0], tilesX, tilesY, mov_sigma, mov_sigma, 0.01);
gb.blurDouble(mov_obj[1], tilesX, tilesY, mov_sigma, mov_sigma, 0.01);
for (int nTile = 0; nTile < motion.length; nTile++) {
mov_obj[1][nTile] /= mov_obj[0][nTile];
}
if (show_debug_images) {
gb.blurDouble(mov_obj[2], tilesX, tilesY, mov_sigma, mov_sigma, 0.01);
gb.blurDouble(mov_obj[3], tilesX, tilesY, mov_sigma, mov_sigma, 0.01);
for (int nTile = 0; nTile < motion.length; nTile++) {
mov_obj[2][nTile] /= mov_obj[0][nTile];
mov_obj[3][nTile] /= mov_obj[0][nTile];
}
for (int nTile = 0; nTile < motion.length; nTile++) {
if (motion[nTile] != null) {
dbg_img[0][nTile] = motion[nTile][0];
dbg_img[1][nTile] = motion[nTile][1];
dbg_img[2][nTile] = motion[nTile][2];
}
dbg_img[3][nTile] = mov_obj[1][nTile];
dbg_img[4][nTile] = mov_obj[2][nTile];
dbg_img[5][nTile] = mov_obj[3][nTile];
}
}
double mov_thresh = sl2 * mov_thresh_rel2;
if (mov_thresh < mov_thresh_abs2) {
mov_thresh = mov_thresh_abs2;
}
int num_over = 0;
boolean [] over_thresh = new boolean [mov_obj[0].length];
for (int nTile = 0; nTile < motion.length; nTile++) if (mov_obj[1][nTile] > mov_thresh){
over_thresh[nTile] = true;
num_over ++;
}
if (num_over == 0) {
if (debug_level>0) {
System.out.println("getMovementMask(): No moving tiles exceed ="+Math.sqrt(mov_thresh)+", aborting movement processing");
}
break process_movements;
}
// Find clusters
TileNeibs tn = new TileNeibs(tilesX,tilesY);
int [] clusters = tn.enumerateClusters(
over_thresh, // boolean [] tiles,
false); // boolean ordered)
// check clusters contain super-threshold tile
double max_cluster = mov_thresh*mov_clust_max; // each cluster should have larger tile value to survive
int max_clust_num = 0;
for (int nTile=0; nTile < clusters.length;nTile++) if (clusters[nTile] > max_clust_num) {
max_clust_num = clusters[nTile];
}
double [] clust_max = new double [max_clust_num];
for (int nTile=0; nTile < clusters.length;nTile++) if (clusters[nTile] > 0) {
int i = clusters[nTile]-1;
if (mov_obj[1][nTile] > clust_max[i]) {
clust_max[i] = mov_obj[1][nTile];
}
}
int num_good_clusters = 0;
boolean [] good_clusters = new boolean[clust_max.length];
for (int i = 0; i < clust_max.length; i++) if (clust_max[i] >= max_cluster){
good_clusters[i] = true;
num_good_clusters ++;
}
if (debug_level>0) {
System.out.println("getMovementMask(): Got "+max_clust_num+" clusters, of them good (>"+max_cluster+") - "+num_good_clusters );
for (int i = 0; i < clust_max.length; i++) {
System.out.println("getMovementMask(): Cluster "+i+(good_clusters[i]?"*":" ")+", max = "+clust_max[i]);
}
}
if(show_debug_images) {
for (int nTile = 0; nTile < motion.length; nTile++) if (clusters[nTile] > 0){
dbg_img[6][nTile] = clusters[nTile];
}
}
if (num_good_clusters == 0) {
if (debug_level>0) {
System.out.println("getMovementMask(): No strong enough moving clusters (of total "+clust_max.length+"), aborting movement processing");
}
break process_movements;
}
move_mask = new boolean [clusters.length];
for (int nTile=0; nTile < clusters.length; nTile++) if (clusters[nTile] > 0) {
move_mask[nTile] = good_clusters[clusters[nTile] -1];
}
tn.growSelection(
mov_grow, // int grow, // grow tile selection by 1 over non-background tiles 1: 4 directions, 2 - 8 directions, 3 - 8 by 1, 4 by 1 more
move_mask, // boolean [] tiles,
null); // boolean [] prohibit);
if(show_debug_images) {
for (int nTile = 0; nTile < motion.length; nTile++) if (move_mask[nTile]){
dbg_img[7][nTile] = 1.0;
}
}
}
if(show_debug_images) {
(new ShowDoubleFloatArrays()).showArrays( // out of boundary 15
dbg_img,
tilesX,
tilesY,
true,
debug_image_name,
mvTitles);
}
if (debug_level>0) {
if (move_mask != null) {
int num_mov = 0;
for (int nTile=0; nTile<move_mask.length;nTile++) if (move_mask[nTile]) {
num_mov++;
}
System.out.println("getMovementMask(): Created moving objects mask of "+num_mov+" tiles.");
} else {
System.out.println("getMovementMask(): No moving objects mask is created.");
}
}
return move_mask;
}
public double[][] adjustPairsLMA( public double[][] adjustPairsLMA(
CLTParameters clt_parameters, CLTParameters clt_parameters,
...@@ -10945,6 +11270,7 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad ...@@ -10945,6 +11270,7 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad
public double[][] adjustPairsLMAInterscene( public double[][] adjustPairsLMAInterscene(
CLTParameters clt_parameters, CLTParameters clt_parameters,
QuadCLT reference_QuadClt, QuadCLT reference_QuadClt,
double [] ref_disparity, // null or alternative reference disparity
QuadCLT scene_QuadClt, QuadCLT scene_QuadClt,
double [] camera_xyz, double [] camera_xyz,
double [] camera_atr, double [] camera_atr,
...@@ -10967,9 +11293,11 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad ...@@ -10967,9 +11293,11 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad
double [][][] coord_motion = null; double [][][] coord_motion = null;
int nlma = 0; int nlma = 0;
for (; nlma < clt_parameters.imp.max_cycles; nlma ++) { for (; nlma < clt_parameters.imp.max_cycles; nlma ++) {
//remove outliers (small moving objects) with histogram
coord_motion = interCorrPair( // new double [tilesY][tilesX][][]; coord_motion = interCorrPair( // new double [tilesY][tilesX][][];
clt_parameters, // CLTParameters clt_parameters, clt_parameters, // CLTParameters clt_parameters,
reference_QuadClt, // QuadCLT reference_QuadCLT, reference_QuadClt, // QuadCLT reference_QuadCLT,
ref_disparity, // double [] ref_disparity, // null or alternative reference disparity
scene_QuadClt, // QuadCLT scene_QuadCLT, scene_QuadClt, // QuadCLT scene_QuadCLT,
camera_xyz0, // xyz camera_xyz0, // xyz
camera_atr0, // pose[1], // atr camera_atr0, // pose[1], // atr
...@@ -11013,7 +11341,7 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad ...@@ -11013,7 +11341,7 @@ public double[][] correlateIntersceneDebug( // only uses GPU and quad
} }
} }
if (clt_parameters.imp.debug_level > -2) { if (clt_parameters.imp.debug_level > -2) {
String [] lines1 = intersceneLma.printOldNew(false); // boolean allvectors) String [] lines1 = intersceneLma.printOldNew((clt_parameters.imp.debug_level > -1)); // false); // boolean allvectors)
System.out.println("Adjusted interscene, iteration="+nlma+ System.out.println("Adjusted interscene, iteration="+nlma+
", last RMS = "+intersceneLma.getLastRms()[0]+ ", last RMS = "+intersceneLma.getLastRms()[0]+
" (pure RMS = "+intersceneLma.getLastRms()[1]+")"+ " (pure RMS = "+intersceneLma.getLastRms()[1]+")"+
......
...@@ -2045,6 +2045,7 @@ public class QuadCLT extends QuadCLTCPU { ...@@ -2045,6 +2045,7 @@ public class QuadCLT extends QuadCLTCPU {
final double [] scene_xyz, // camera center in world coordinates final double [] scene_xyz, // camera center in world coordinates
final double [] scene_atr, // camera orientation relative to world frame final double [] scene_atr, // camera orientation relative to world frame
final QuadCLT scene, final QuadCLT scene,
final QuadCLT ref_scene, // now - may be null - for testing if scene is rotated ref
final boolean toRGB, final boolean toRGB,
String suffix, String suffix,
int threadsMax, int threadsMax,
...@@ -2056,7 +2057,7 @@ public class QuadCLT extends QuadCLTCPU { ...@@ -2056,7 +2057,7 @@ public class QuadCLT extends QuadCLTCPU {
scene_xyz, // final double [] scene_xyz, // camera center in world coordinates scene_xyz, // final double [] scene_xyz, // camera center in world coordinates
scene_atr, // final double [] scene_atr, // camera orientation relative to world frame scene_atr, // final double [] scene_atr, // camera orientation relative to world frame
scene, // final QuadCLT scene_QuadClt, scene, // final QuadCLT scene_QuadClt,
null, // scene, // final QuadCLT reference_QuadClt, // now - may be null - for testing if scene is rotated ref ref_scene, // final QuadCLT reference_QuadClt, // now - may be null - for testing if scene is rotated ref
threadsMax); // int threadsMax) threadsMax); // int threadsMax)
int rendered_width = scene.getErsCorrection().getSensorWH()[0]; int rendered_width = scene.getErsCorrection().getSensorWH()[0];
if (full_woi_in != null) { if (full_woi_in != null) {
...@@ -2238,8 +2239,7 @@ public class QuadCLT extends QuadCLTCPU { ...@@ -2238,8 +2239,7 @@ public class QuadCLT extends QuadCLTCPU {
return null; return null;
} }
saveQuadClt(); // to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
// GPU-specific // GPU-specific
boolean showCoord = debugLevel > 1; boolean showCoord = debugLevel > 1;
boolean is_mono = isMonochrome(); boolean is_mono = isMonochrome();
......
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