Skip to content

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

Debugging LY for CUAS

parent 4e8c0cea
Hide whitespace changes
Inline Side-by-side
Showing with 703 additions and 55 deletions
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @ 4bbf80f0
......@@ -37,6 +37,7 @@ import java.util.Collections;
import java.util.Comparator;
import java.util.List;
import java.util.Properties;
import java.util.Random;
import java.util.concurrent.atomic.AtomicBoolean;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.atomic.DoubleAccumulator;
......@@ -5656,7 +5657,8 @@ public class OpticalFlow {
// reduce some conditions as they are implied by (operation_mode == RESULTS_BUILD_SEQ_LY)
// && run_ly && (run_ly_mode > 0) && (run_ly_mode <= 3) && is_first_scene
boolean exec_ly_correction = (operation_mode == RESULTS_BUILD_SEQ_LY) && force_initial_orientations;
boolean exec_ly_correction = (operation_mode == RESULTS_BUILD_SEQ_LY) &&
(force_initial_orientations || (extr_corr == null));
if (operation_mode == RESULTS_BUILD_SEQ_LY) {
if (debugLevel >-3) {
......@@ -6718,34 +6720,54 @@ java.lang.NullPointerException
if (debugLevel > -3) {
System.out.println("**** Running LY adjustments *****");
}
int max_ly_length = clt_parameters.imp.run_ly_scenes;
QuadCLT [] lim_quadCLTs = quadCLTs;
// int lim_ref_index = ref_index;
if ((max_ly_length > 0) && (quadCLTs.length > max_ly_length)) {
int [] lim_ref_indices = new int[1];
lim_quadCLTs = limitScenes(
max_ly_length, // final int max_num_scenes,
quadCLTs, // final QuadCLT [] scenes,
master_CLT); // final QuadCLT ref_scene)
// for real reference scenes it will remain in lim_quadCLTs, for virtual - it will not be added
}
// Calculate and fill per-scene target disparities as scene.dsrbg
double [] ref_target_disparity=master_CLT.getDLS()[0]; // 0];
// filter scenes, keeping only processed
// now (11/24/2025) intepolateSceneDisparity() returns nulls for the scenes that do not have orientation
/*
double [][] interpolated_disparities = intepolateSceneDisparity(
clt_parameters, // final CLTParameters clt_parameters,
lim_quadCLTs, // final QuadCLT [] scenes,
lim_ref_index, // final int indx_ref,
ref_target_disparity, // final double [] disparity_ref, // disparity in the reference view tiles (Double.NaN - invalid)
debugLevel + 2); // final int debug_level
// using master_CLT to work for CUAS?
*/
// m,aster_CLT can be one of the scenes or not
double [][] interpolated_disparities = intepolateSceneDisparity(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT [] scenes,
ref_index, // final int indx_ref,
lim_quadCLTs, // final QuadCLT [] scenes,
master_CLT, // final QuadCLT ref_scene,
ref_target_disparity, // final double [] disparity_ref, // disparity in the reference view tiles (Double.NaN - invalid)
debugLevel + 2); // final int debug_level
ArrayList<Integer> scene_list = new ArrayList<Integer>();
int oriented_ref_index=-1;
for (int i = 0; i < quadCLTs.length; i++) if (interpolated_disparities[i] != null){ // should be for ref_index
double [][] dsrbg=quadCLTs[i].getDSRBG();
for (int i = 0; i < lim_quadCLTs.length; i++) if (interpolated_disparities[i] != null){ // should be for ref_index
double [][] dsrbg=lim_quadCLTs[i].getDSRBG();
if (dsrbg == null) {
quadCLTs[i].dsrbg = new double[1][];
quadCLTs[i].dsrbg[0] = interpolated_disparities[i];
lim_quadCLTs[i].dsrbg = new double[1][];
lim_quadCLTs[i].dsrbg[0] = interpolated_disparities[i];
}
if (i == ref_index) oriented_ref_index = scene_list.size();
scene_list.add(i);
//double [] target_disparity = scene.getDSRBG()[0];
}
if (oriented_ref_index < 0) {
throw new IllegalArgumentException ("buildSeries(): interpolated_disparities undefined for ref_index="+ref_index);
}
// if (oriented_ref_index < 0) {
// throw new IllegalArgumentException ("buildSeries(): interpolated_disparities undefined for ref_index="+ref_index);
// }
QuadCLT [] oriented_scenes = new QuadCLT[scene_list.size()];
for (int i = 0; i <oriented_scenes.length; i++) {
oriented_scenes[i] =quadCLTs[scene_list.get(i)];
oriented_scenes[i] =lim_quadCLTs[scene_list.get(i)];
}
if (debugLevel > -3) {
System.out.println("Using "+oriented_scenes.length+" oriented scenes (of "+quadCLTs.length+") for LY adjustment.");
......@@ -6753,7 +6775,7 @@ java.lang.NullPointerException
adjustLYSeries(
quadCLT_main, // QuadCLT quadCLT_main, // update extrinsics here too
oriented_scenes, // QuadCLT [] quadCLTs, // was quadCLTs,
oriented_ref_index, // int ref_index,
master_CLT, // oriented_ref_index, // int ref_index,
clt_parameters, // CLTParameters clt_parameters,
run_ly_mode, // int run_ly_mode, // +1 - lazy eye, +2 - infinity
run_ly_ims, // boolean run_ly_ims, // adjust infinity (if enabled) using horizontal movement from the IMS
......@@ -6780,7 +6802,8 @@ java.lang.NullPointerException
// Copy LY calibration to the index scene (last in the sequence)
CorrVector corr_vector = oriented_scenes[oriented_ref_index].getGeometryCorrection().getCorrVector();
// CorrVector corr_vector = oriented_scenes[oriented_ref_index].getGeometryCorrection().getCorrVector();
CorrVector corr_vector = master_CLT.getGeometryCorrection().getCorrVector();
index_scenes[0].getGeometryCorrection().setCorrVector(corr_vector); // updated system corr vector with the current updated one
// quadCLT_main.getGeometryCorrection().setCorrVector(corr_vector); // already set in adjustLYSeries()
......@@ -6803,9 +6826,6 @@ java.lang.NullPointerException
QuadCLTCPU.moveDir(x3d_path_ref, x3d_path_ref_ly);
return null;
}
if (photo_en && !reuse_video) {
if (debugLevel > -3) {
......@@ -8817,23 +8837,424 @@ java.lang.NullPointerException
}
}
}
if (xyz1 == null) {
System.out.println("correctInfinityFromIMS(): not enough data");
return false;
if (xyz1 == null) {
System.out.println("correctInfinityFromIMS(): not enough data");
return false;
}
*/
return true;
}
/*
public void adjustLYSeries(
QuadCLT quadCLT_main, // update extrinsics here too
QuadCLT [] quadCLTs,
int ref_index,
CLTParameters clt_parameters,
int run_ly_mode, // +1 - lazy eye, +2 - infinity
boolean run_ly_ims, // adjust infinity (if enabled) using horizontal movement from the IMS
ColorProcParameters colorProcParameters,
CorrectionColorProc.ColorGainsParameters channelGainParameters,
EyesisCorrectionParameters.RGBParameters rgbParameters,
EyesisCorrectionParameters.EquirectangularParameters equirectangularParameters,
Properties properties,
boolean reset_from_extrinsics,
final int threadsMax, // maximal number of threads to launch
final boolean updateStatus,
final int debugLevel) throws Exception
{
MultisceneLY.MSLY_MODE adjust_mode = MultisceneLY.MSLY_MODE.INF_NOINF;
switch (run_ly_mode) {
case 1: adjust_mode = MultisceneLY.MSLY_MODE.NOINF_ONLY; break;
case 2: adjust_mode = MultisceneLY.MSLY_MODE.INF_ONLY; break;
case 3: adjust_mode = MultisceneLY.MSLY_MODE.INF_NOINF; break;
default:
System.out.println("Invalid LY/infinity adjustment mode");
return;
}
if (clt_parameters.ofp.pattern_mode) {
adjust_mode = MultisceneLY.MSLY_MODE.NOINF_ONLY;
}
boolean pattern_mode = clt_parameters.ofp.pattern_mode;
this.startTime=System.nanoTime();
boolean proc_infinity = (adjust_mode == MultisceneLY.MSLY_MODE.INF_ONLY) || (adjust_mode == MultisceneLY.MSLY_MODE.INF_NOINF); // true;
boolean lma_only = true; // use clt_parameters
double dbg_disparity_offset = 0.0; // 0.1
double inf_disp_ref = 0.0;
/// int ref_scene_index = quadCLTs.length-1;
/// if (pattern_mode) {
/// ref_scene_index = clt_parameters.ofp.center_index;
/// }
// QuadCLT ref_scene = quadCLTs[ref_scene_index];
QuadCLT ref_scene = quadCLTs[ref_index]; // 05.04.2025
String composite_suffix = "-INTER-INTRA-LMA"; // is already read if available!
String num_corr_max_suffix = "-NUM-CORR-MAX";
int [] wh = new int[2];
double [][] composite_ds = ref_scene.readDoubleArrayFromModelDirectory(
composite_suffix, // String suffix,
0, // int num_slices, // (0 - all)
wh); // int [] wh)
// composite_ds == null if no file
MultisceneLY multisceneLY = new MultisceneLY (
quadCLTs[0].getNumSensors(),
threadsMax, // int threadsMax, // maximal number of threads to launch
updateStatus); // boolean updateStatus);
// read interscene composite data
boolean [][] is_scene_infinity = null; // per scene, per tile - is infinity.
int tilesX = ref_scene.tp.getTilesX();
int tilesY = ref_scene.tp.getTilesY();
int clustersX = (int) Math.ceil(1.0 * tilesX / clt_parameters.lyms_clust_size);
int clustersY = (int) Math.ceil(1.0 * tilesY / clt_parameters.lyms_clust_size);
int clusters = clustersX * clustersY;
boolean debug = debugLevel > -3;
if (proc_infinity) {
if (run_ly_ims) {
// make sure ref_scene is reference (center if selected)
double [] disparity_dsi = ref_scene.getDLS()[1]; // null
double [] strength = ref_scene.getDLS()[2];
double sw=0,swd=0;
for (int i = 0; i < disparity_dsi.length; i++) {
if (!Double.isNaN(disparity_dsi[i])) {
sw += strength[i];
swd += strength[i]*disparity_dsi[i];
}
}
double disp_avg = swd/sw;
double avg_z = ref_scene.getGeometryCorrection().getZFromDisparity(disp_avg);
double scale_img = getImgImsScale( // correctInfinityFromIMS(
quadCLTs, // QuadCLT [] quadCLTs,
ref_scene, // QuadCLT master_CLT)
0, // earliest_scene); // int earliest_scene)
-1); // int latest_scene) { // -1 will use quadCLTs.length -1;
inf_disp_ref = disp_avg * (1.0 - scale_img);
System.out.println("inf_disp_ref="+inf_disp_ref);
} else {
double [] inf_avg = new double[1];
boolean [] ref_inf = MultisceneLY.getComboInfinity(
ref_scene.tp, // TileProcessor tp,
composite_ds, // double [][] composite_ds,
clt_parameters.lyms_far_inf, // double far_inf,
clt_parameters.lyms_near_inf, // double near_inf,
clt_parameters.lyms_far_fract, // double far_fract,
clt_parameters.lyms_inf_range_offs, // double inf_range_offs,
clt_parameters.lyms_inf_range, // double inf_range,
clt_parameters.lyms_min_inf_str, // double min_inf_str,
clt_parameters.lyms_min_fg_str, // double min_fg_str,
inf_avg, // double [] inf_avg,
debug); // boolean debug)
is_scene_infinity = MultisceneLY.infinityPerScene( // null pointer
quadCLTs, // QuadCLT [] scenes,
inf_avg[0], // double inf_disp_ref, // average disparity at infinity for ref scene
ref_inf, // boolean [] infinity_ref,
debug, // boolean debug,
threadsMax); // int threadsMax)
inf_disp_ref = inf_avg[0];
}
}
// Read or generate+save number of correlation maximums per scene, per tile
// Will use only tiles with one and only one correlation maximum
int [][] numCorrMax = new int [quadCLTs.length][]; // ->
double [][] dNumCorrMax =ref_scene.readDoubleArrayFromModelDirectory(
num_corr_max_suffix, // String suffix,
0, // int num_slices, // (0 - all)
wh); // int [] wh)
if ((dNumCorrMax != null) && (dNumCorrMax.length == numCorrMax.length)) {
for (int nscene = 0; nscene < numCorrMax.length; nscene++) {
numCorrMax[nscene] = new int[dNumCorrMax[nscene].length];
for (int i = 0; i < dNumCorrMax[nscene].length; i++) {
numCorrMax[nscene][i] = (int) dNumCorrMax[nscene][i];
}
}
} else {
for (int i = 0; i < quadCLTs.length; i++) { // require DSI for each model
numCorrMax[i] = multisceneLY.getNumCorrMax(
clt_parameters, // CLTParameters clt_parameters,
lma_only, // boolean lma_only,
quadCLTs[i], // QuadCLT scene, // ordered by increasing timestamps
debugLevel); // int debug_level)
if (debugLevel > -3) {
System.out.println("\n============ ran multisceneLY.getNumCorrMax for i = "+i+" of "+quadCLTs.length);
}
}
dNumCorrMax = new double[numCorrMax.length][numCorrMax[0].length];
for (int nscene = 0; nscene < numCorrMax.length; nscene++) {
dNumCorrMax[nscene] = new double[dNumCorrMax[nscene].length];
for (int i = 0; i < dNumCorrMax[nscene].length; i++) {
dNumCorrMax[nscene][i] = numCorrMax[nscene][i];
}
}
ref_scene.saveDoubleArrayInModelDirectory( // error
num_corr_max_suffix, // String suffix,
null, // null, // String [] labels, // or null
dNumCorrMax, // dbg_data, // double [][] data,
ref_scene.tp.getTilesX(), // int width,
ref_scene.tp.getTilesY()); // int height)
}
// valid tile - one and only one maximum
boolean [][] valid_tile = new boolean[numCorrMax.length][numCorrMax[0].length];
for (int nscene = 0; nscene < numCorrMax.length; nscene++) {
for (int nTile = 0; nTile < numCorrMax[nscene].length; nTile++) {
valid_tile[nscene][nTile] = numCorrMax[nscene][nTile] == 1; // only single-maximum
if (clt_parameters.lyms_margin > 0) {
int tileX = nTile % tilesX;
int tileY = nTile / tilesX;
if ( (tileY < clt_parameters.lyms_margin) ||
(tileX < clt_parameters.lyms_margin) ||
(tileY >= (tilesY - clt_parameters.lyms_margin)) ||
(tileX >= (tilesX - clt_parameters.lyms_margin))) {
valid_tile[nscene][nTile] = false;
}
}
}
}
int [][] num_tiles2 = new int[2][];
double [][][] target_disparities = new double [2][][];
boolean use_tarz = false; // true; //false;
double delta = 0.001; // 0.01;
boolean debug_derivs = false;
ExtrinsicAdjustment ea = new ExtrinsicAdjustment (
ref_scene.getErsCorrection(), // GeometryCorrection gc,
clt_parameters.lyms_clust_size, // int clusterSize,
clustersX, // int clustersX,
clustersY); // int clustersY)
boolean [] force_disparity = new boolean[clusters];
boolean apply_extrinsic = (clt_parameters.ly_corr_scale != 0.0);
// int max_tries = 5; // organize cycle with comparing results
int max_tries = clt_parameters.lym_iter; // 25;
double inf_min = -1.0;
double inf_max = 1.0;
double [] old_new_rms = new double [2];
double min_sym_update = clt_parameters.getLymChange(ref_scene.isAux()); // 4e-6; // stop iterations if no angle changes more than this
double comp_diff = min_sym_update + 1; // (> min_sym_update)
double [] disparity_offset = new double [clusters]; // 0.1
if (dbg_disparity_offset != 0.0) {
Arrays.fill(disparity_offset, dbg_disparity_offset);
}
int nrefine = 4;
for (int num_iter = 0; num_iter < max_tries; num_iter++){
// (re-)measure LY here, update all scenes? or just accumulater difference?
double [][][] lazy_eye_data2 = MultisceneLY.getLYDataInfNoinf(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT [] scenes, // ordered by increasing timestamps
valid_tile, // final boolean [][] valid_tile, // tile with lma and single correlation maximum
inf_disp_ref, // final double inf_disp_ref, // average disparity at infinity for ref scene // is_scene_infinity
is_scene_infinity, // final boolean [][] is_scene_infinity, // may be null, if not - may be infinity from the composite depth map
target_disparities, // final double[][] target_disparities,
dbg_disparity_offset, // final double dbg_disparity_offset,
num_tiles2, // final int [][] in_num_tiles, // null or number of tiles per cluster to multiply strength
null, // final CorrVector corr_vector_delta, // null or extrinsic vector offset, applied to all scenes
nrefine, // final int nrefine, // number of disparity refines for non-inf
threadsMax, // final int threadsMax,
debugLevel); // final int debug_level);
if (debug_derivs && (num_iter == 0)) { // only once if any
MultisceneLY.debugLYDerivatives(
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT [] scenes, // ordered by increasing timestamps
lazy_eye_data2, // double [][][] lazy_eye_data2, // inf, no_inf
valid_tile, // final boolean [][] valid_tile, // tile with lma and single correlation maximum
inf_disp_ref, // final double inf_disp_ref, // average disparity at infinity for ref scene // is_scene_infinity
is_scene_infinity, //final boolean [][] is_scene_infinity, // may be null, if not - may be infinity from the composite depth map
threadsMax, // final int threadsMax, // maximal number of threads to launch
delta, // double delta,
use_tarz, // boolean use_tarz, // derivatives by tarz, not symmetrical vectors
debugLevel); // final int debugLevel);
}
if (clt_parameters.lyms_show_images) {
for (int nly = 00; nly < lazy_eye_data2.length; nly++) if (lazy_eye_data2[nly] != null ) {
ea.showInput(
lazy_eye_data2[nly], // double[][] data,
quadCLTs[ref_index].getImageName()+"-drv_reference-"+MultisceneLY.SINF_NOINF[nly]);// String title);
}
}
double [][] lazy_eye_data = MultisceneLY.mergeLY(
adjust_mode, // MSLY_MODE adjust_mode,
lazy_eye_data2, // double [][][] lazy_eye_data2,
force_disparity); // boolean [] force_disparity // null or [clusters]
ea.setForceDisparity(force_disparity);
if (clt_parameters.lyms_show_images) {
ea.showInput(
lazy_eye_data, // double[][] data,
quadCLTs[ref_index].getImageName()+"-ly_combo");// String title);
}
CorrVector corr_vector = ea.solveCorr (
clt_parameters.ly_marg_fract, // double marg_fract, // part of half-width, and half-height to reduce weights
clt_parameters.ly_inf_en, // boolean use_disparity, // adjust disparity-related extrinsics
// 1.0 - to skip filtering infinity
inf_min, //double inf_min_disparity, // minimal disparity for infinity
inf_max, // double inf_max_disparity, // minimal disparity for infinity
clt_parameters.ly_inf_min_broad, // inf_min_disp_abs, // minimal disparity for infinity (absolute)
clt_parameters.ly_inf_max_broad, // maximal disparity for infinity (absolute)
clt_parameters.ly_inf_tilt, // boolean en_infinity_tilt, // select infinity tiles form right/left tilted (false - from average)
clt_parameters.ly_right_left, // boolean infinity_right_left, // balance weights between right and left halves of infinity
clt_parameters.ly_aztilt_en, // boolean use_aztilts, // Adjust azimuths and tilts excluding disparity
clt_parameters.ly_diff_roll_en, // boolean use_diff_rolls, // Adjust differential rolls (3 of 4 angles)
// clt_parameters.ly_inf_force, // boolean force_convergence, // if true try to adjust convergence (disparity, symmetrical parameter 0) even with no disparity
clt_parameters.ly_min_forced, // int min_num_forced, // minimal number of clusters with forced disparity to use it
// data, using just radial distortions
clt_parameters.ly_com_roll, // boolean common_roll, // Enable common roll (valid for high disparity range only)
clt_parameters.ly_focalLength , // boolean corr_focalLength, // Correct scales (focal length temperature? variations)
clt_parameters.ly_ers_rot, // boolean ers_rot, // Enable ERS correction of the camera rotation
clt_parameters.getErsForw(), // boolean ers_forw, // Enable ERS correction of the camera linear movement in z direction
clt_parameters.getErsSide(), // boolean ers_side, // Enable ERS correction of the camera linear movement in x direction
clt_parameters.getErsVert(), // boolean ers_vert, // Enable ERS correction of the camera linear movement in y direction
// add balancing-related here?
clt_parameters.ly_par_sel, // int manual_par_sel, // Manually select the parameter mask bit 0 - sym0, bit1 - sym1, ... (0 - use boolean flags, != 0 - ignore boolean flags)
clt_parameters.ly_weight_infinity, //0.3, // double weight_infinity, // 0.3, total weight of infinity tiles fraction (0.0 - 1.0)
clt_parameters.ly_weight_disparity, //0.0, // double weight_disparity, // 0.0 disparity weight relative to the sum of 8 lazy eye values of the same tile
clt_parameters.ly_weight_disparity_inf,//0.5, // double weight_disparity_inf,// 0.5 disparity weight relative to the sum of 8 lazy eye values of the same tile for infinity
clt_parameters.ly_max_disparity_far, //5.0, // double max_disparity_far, // 5.0 reduce weights of near tiles proportional to sqrt(max_disparity_far/disparity)
clt_parameters.ly_max_disparity_use, //5.0, // double max_disparity_use, // 5.0 (default 1000)disable near objects completely - use to avoid ERS
0.0, // clt_parameters.ly_inf_min_dfe, //1.75,// double min_dfe, // = 1.75;
0.0, // clt_parameters.ly_inf_max_dfe, //5.0, // double max_dfe, // = 5.0; // <=0 - disable feature
// moving objects filtering
false, // clt_parameters.ly_moving_en, // boolean moving_en, // enable filtering areas with potentially moving objects
clt_parameters.ly_moving_apply, // boolean moving_apply, // apply filtering areas with potentially moving objects
clt_parameters.ly_moving_sigma, // double moving_sigma, // blurring sigma for moving objects = 1.0;
clt_parameters.ly_max_mov_disparity, // double max_mov_disparity, // disparity limit for moving objects detection = 75.0;
clt_parameters.ly_rad_to_hdiag_mov, // double rad_to_hdiag_mov, // radius to half-diagonal ratio to remove high-distortion corners = 0.7 ; // 0.8
clt_parameters.ly_max_mov_average, // double max_mov_average, // do not attempt to detect moving objects if ERS is not accurate for terrain = .25;
clt_parameters.ly_mov_min_L2, // double mov_min_L2, // threshold for moving objects = 0.75;
lazy_eye_data, // scan.getLazyEyeData(), // dsxy, // double [][] measured_dsxy,
force_disparity, // scan.getLazyEyeForceDisparity(), // null, // boolean [] force_disparity, // boolean [] force_disparity,
false, // boolean use_main, // corr_rots_aux != null;
ref_scene.getGeometryCorrection().getCorrVector(), // CorrVector corr_vector,
old_new_rms, // double [] old_new_rms, // should be double[2]
debugLevel); // + 5);// int debugLevel) >=2 to show images
if (debugLevel > -2){
System.out.println("Old extrinsic corrections:");
System.out.println(ref_scene.getGeometryCorrection().getCorrVector().toString());
}
if (corr_vector != null) {
CorrVector diff_corr = corr_vector.diffFromVector(ref_scene.getGeometryCorrection().getCorrVector());
comp_diff = diff_corr.getNorm(); // apply this to all scenes
if (debugLevel > -2){
System.out.println("New extrinsic corrections:");
System.out.println(corr_vector.toString());
}
if (debugLevel > -3){
System.out.println("Increment extrinsic corrections:");
System.out.println(diff_corr.toString());
}
ref_scene.gpuResetCorrVector(); // next time GPU will need to set correction vector (and re-calculate offsets?)
if (apply_extrinsic){
// Apply correction to all scenes (adding, as ERS can be different)
// will need to update all scenes GC (write back to disk), but only after all are done
for (int i = 0; i < quadCLTs.length; i++) {
QuadCLT scene = quadCLTs[i];
CorrVector scene_vector = scene.getGeometryCorrection().getCorrVector();
scene_vector.incrementVector(diff_corr, 1.0); // no scale here
scene.getGeometryCorrection().setCorrVector(scene_vector);
}
quadCLTs[ref_index].getGeometryCorrection().setCorrVector(corr_vector); // updated system corr vector with the current updated one
// was in TwoQuadCLT:
quadCLT_main.getGeometryCorrection().setCorrVector(corr_vector); // updated system corr vector with the current updated one
System.out.println("Extrinsic correction updated (can be disabled by setting clt_parameters.ly_corr_scale = 0.0) ");
} else {
System.out.println("Correction is not applied according clt_parameters.ly_corr_scale == 0.0) ");
}
} else {
if (debugLevel > -3){
System.out.println("LMA failed"); // What now?
}
}
boolean done = (comp_diff < min_sym_update) || (num_iter == (max_tries - 1));
// System.out.println("done="+done);
if (debugLevel > -10) { // should work even in batch mode
System.out.println("#### extrinsicsCLT(): iteration step = "+(num_iter + 1) + " ( of "+max_tries+") change = "+
comp_diff + " ("+min_sym_update+"), previous RMS = " + old_new_rms[0]+
" final RMS = " + old_new_rms[1]+ " (debugLevel = "+debugLevel+")");
}
if (debugLevel > -10) {
if ((debugLevel > -3) || done) {
System.out.println("New extrinsic corrections:");
System.out.println(ref_scene.getGeometryCorrection().getCorrVector().toString());
}
}
if (comp_diff < min_sym_update) {
break;
}
inf_disp_ref = 0.0; // after first adjustment set infinity to 0.0
}
// Now update correction vector for all scenes to disk - how was it read?
// update
if (debugLevel > -200) {
if (debugLevel > -199) {
return;
}
}
// debug images
if (debugLevel > -2) {
double [][] dbg_numCorrMax = new double[numCorrMax.length][];
for (int i = 0; i < dbg_numCorrMax.length; i++) {
dbg_numCorrMax[i] = new double[numCorrMax[i].length];
for (int ntile=0; ntile < numCorrMax[i].length; ntile++) {
dbg_numCorrMax[i][ntile] = numCorrMax[i][ntile];
}
}
ShowDoubleFloatArrays.showArrays(
dbg_numCorrMax,
tilesX,
tilesY,
true,
"numCorrMax");
double [][] dbg_num_tiles = new double[2][];
for (int i = 0; i < num_tiles2.length; i++ ) {
dbg_num_tiles[i] = new double [clusters];
for (int nClust = 0; nClust < clusters; nClust++) {
dbg_num_tiles[i][nClust] = num_tiles2[i][nClust];
}
}
ShowDoubleFloatArrays.showArrays(
dbg_num_tiles,
clustersX,
clustersY,
true,
"clusters_num_inf_boinf",
new String[] {"inf tiles", "noinf tiles"});
}
if (debugLevel > -2) {
System.out.println("adjustLYSeries() Done");
}
*/
return true;
}
System.out.println("End of adjustLYSeries()");
}
*/
public void adjustLYSeries(
QuadCLT quadCLT_main, // update extrinsics here too
QuadCLT [] quadCLTs,
int ref_index,
QuadCLT ref_scene,
CLTParameters clt_parameters,
int run_ly_mode, // +1 - lazy eye, +2 - infinity
boolean run_ly_ims, // adjust infinity (if enabled) using horizontal movement from the IMS
......@@ -8875,7 +9296,7 @@ java.lang.NullPointerException
/// }
// QuadCLT ref_scene = quadCLTs[ref_scene_index];
QuadCLT ref_scene = quadCLTs[ref_index]; // 05.04.2025
/// QuadCLT ref_scene = quadCLTs[ref_index]; // 05.04.2025
String composite_suffix = "-INTER-INTRA-LMA"; // is already read if available!
String num_corr_max_suffix = "-NUM-CORR-MAX";
int [] wh = new int[2];
......@@ -9068,7 +9489,8 @@ java.lang.NullPointerException
for (int nly = 00; nly < lazy_eye_data2.length; nly++) if (lazy_eye_data2[nly] != null ) {
ea.showInput(
lazy_eye_data2[nly], // double[][] data,
quadCLTs[ref_index].getImageName()+"-drv_reference-"+MultisceneLY.SINF_NOINF[nly]);// String title);
// quadCLTs[ref_index].getImageName()+"-drv_reference-"+MultisceneLY.SINF_NOINF[nly]);// String title);
ref_scene.getImageName()+"-drv_reference-"+MultisceneLY.SINF_NOINF[nly]);// String title);
}
}
double [][] lazy_eye_data = MultisceneLY.mergeLY(
......@@ -9079,7 +9501,7 @@ java.lang.NullPointerException
if (clt_parameters.lyms_show_images) {
ea.showInput(
lazy_eye_data, // double[][] data,
quadCLTs[ref_index].getImageName()+"-ly_combo");// String title);
ref_scene.getImageName()+"-ly_combo");// String title);
}
CorrVector corr_vector = ea.solveCorr (
clt_parameters.ly_marg_fract, // double marg_fract, // part of half-width, and half-height to reduce weights
......@@ -9152,7 +9574,8 @@ java.lang.NullPointerException
scene_vector.incrementVector(diff_corr, 1.0); // no scale here
scene.getGeometryCorrection().setCorrVector(scene_vector);
}
quadCLTs[ref_index].getGeometryCorrection().setCorrVector(corr_vector); // updated system corr vector with the current updated one
// quadCLTs[ref_index].getGeometryCorrection().setCorrVector(corr_vector); // updated system corr vector with the current updated one
ref_scene.getGeometryCorrection().setCorrVector(corr_vector); // updated system corr vector with the current updated one
// was in TwoQuadCLT:
quadCLT_main.getGeometryCorrection().setCorrVector(corr_vector); // updated system corr vector with the current updated one
System.out.println("Extrinsic correction updated (can be disabled by setting clt_parameters.ly_corr_scale = 0.0) ");
......@@ -9223,31 +9646,13 @@ java.lang.NullPointerException
new String[] {"inf tiles", "noinf tiles"});
}
/* TODO: move all adjustments there?
if (lazy_eye_data != null) {
multisceneLY.processLYdata( // TODO: move all adjustments there?
clt_parameters, // final CLTParameters clt_parameters,
adjust_mode, // MSLY_MODE adjust_mode,
quadCLTs, // final QuadCLT [] scenes, // ordered by increasing timestamps
lazy_eye_data2, // final double [][][] lazy_eye_data,
valid_tile, // final boolean [][] valid_tile, // tile with lma and single correlation maximum
inf_disp_ref, // final double inf_disp_ref, // average disparity at infinity for ref scene // is_scene_infinity
is_scene_infinity, // final boolean [][] is_scene_infinity, // may be null, if not - may be infinity from the composite depth map
false, // boolean update_disparity, // re-measure disparity before measuring LY
threadsMax, // final int threadsMax, // maximal number of threads to launch
updateStatus, // final boolean updateStatus,
debugLevel); // final int debugLevel)
}
*/
if (debugLevel > -2) {
System.out.println("adjustLYSeries() Done");
}
System.out.println("End of adjustLYSeries()");
}
public static void testERS(
......@@ -14338,7 +14743,50 @@ java.lang.NullPointerException
return combo_dsn;
}
/**
* Limit scenes list size for LY adjustment, preserve reference scene, throw away random
* @param max_num_scenes maximal number of scenes
* @param scenes array of QuadCLT scenes
* @param indx_ref int[1] - will return index of the reference scene in the new list
* @return ordered array of QuadCLT scenes, including reference scene, limited by max_num_scenes. contains reference scene
*/
public static QuadCLT [] limitScenes(
final int max_num_scenes,
final QuadCLT [] scenes,
final QuadCLT ref_scene) {
if (scenes.length <= max_num_scenes) {
return scenes;
}
// final QuadCLT ref_scene = scenes[indx_ref];
final ErsCorrection ers_reference = ref_scene.getErsCorrection();
final String ref_name = ref_scene.getImageName();
ArrayList<Integer> scene_list = new ArrayList<Integer>();
int indx_ref = -1;
for (int nscene = 0; nscene < scenes.length; nscene++) { // if (nscene != indx_ref){ // should be for ref_index
String ts = scenes[nscene].getImageName();
if (ref_name.equals(ts)) {
indx_ref = nscene; // later add reference scene
} else {
if ((ers_reference.getSceneXYZ(ts) != null) && (ers_reference.getSceneATR(ts) != null)){
scene_list.add(nscene); // scene is not matched
}
}
}
Random r= new Random();
int add_ref = (indx_ref >= 0) ? 1 : 0;
while ((scene_list.size() + add_ref) > max_num_scenes) {
scene_list.remove(r.nextInt(scene_list.size()));
}
if (indx_ref >= 0) {
scene_list.add(indx_ref);
}
Collections.sort(scene_list);
QuadCLT [] filtered_scenes = new QuadCLT[scene_list.size()];
for (int indx = 0; indx < filtered_scenes.length; indx++) {
filtered_scenes[indx] = scenes[scene_list.get(indx)];
}
return filtered_scenes;
}
public static double [][] intepolateSceneDisparity(
final CLTParameters clt_parameters,
......@@ -14539,6 +14987,206 @@ java.lang.NullPointerException
}
return disparity_scenes;
}
public static double [][] intepolateSceneDisparity(
final CLTParameters clt_parameters,
final QuadCLT [] scenes,
final QuadCLT ref_scene,
final double [] disparity_ref, // disparity in the reference view tiles (Double.NaN - invalid)
final int debug_level){
final int scene_extrap_irad = 1;
final double scene_extrap_rad = scene_extrap_irad + 0.5;
final ErsCorrection ers_reference = ref_scene.getErsCorrection();
final int tilesX = ref_scene.getTileProcessor().getTilesX();
final int tilesY = ref_scene.getTileProcessor().getTilesY();
final int tileSize = ref_scene.getTileProcessor().getTileSize();
double max_rad2 = scene_extrap_rad * scene_extrap_rad * tileSize * tileSize; // in pixels
final double [][] disparity_scenes = new double [scenes.length][]; // [tilesX*tilesY];
final int dbg_tileX=-70;
final int dbg_tileY=-19;
final int dbg_tile=dbg_tileY * tilesX + dbg_tileX;
if (debug_level > -1) {
System.out.print("Correlating scene ");
}
final String ref_name = ref_scene.getImageName();
for (int nscene = scenes.length-1; nscene >= 0; nscene--) {
String ts = scenes[nscene].getImageName();
double [][] scene_pXpYD;
if (ref_name.equals(ts)){ // never for (virtual) center
// transform to self - maybe use a method that sets central points
scene_pXpYD = transformToScenePxPyD( // check it is all 0.5
null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null)
disparity_ref, // 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_atr, // camera orientation relative to world frame
ref_scene, // final QuadCLT scene_QuadClt,
ref_scene); // final QuadCLT reference_QuadClt)
disparity_scenes[nscene] = disparity_ref; // .clone();
// for (int i = 0; i < ref_pXpYD.length; i++) {
// ref_pXpYD[i] = scene_pXpYD[i];
// }
} else {
final Matrix [][] scene_approx = new Matrix[disparity_ref.length][];
double [] scene_xyz = ers_reference.getSceneXYZ(ts);
double [] scene_atr = ers_reference.getSceneATR(ts);
if ((scene_xyz == null) || (scene_atr == null)){
continue; // scene is not matched
}
disparity_scenes[nscene] = new double [tilesX*tilesY];
Arrays.fill(disparity_scenes[nscene], Double.NaN);
double [] scene_ers_xyz_dt = ers_reference.getSceneErsXYZ_dt(ts);
double [] scene_ers_atr_dt = ers_reference.getSceneErsATR_dt(ts);
scenes[nscene].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_prefilter = transformToScenePxPyD( // will be null for disparity == NaN, total size - tilesX*tilesY
null, // final Rectangle [] extra_woi, // show larger than sensor WOI (or null)
disparity_ref, // 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_atr, // final double [] scene_atr, // camera orientation relative to world frame
scenes[nscene], // final QuadCLT scene_QuadClt,
ref_scene); // final QuadCLT reference_QuadClt)
double max_overlap = 0.6;
double min_adisp_cam = 0.2;
double min_rdisp_cam = 0.03;
double [][] scene_ds =conditionInitialDS(
clt_parameters, // CLTParameters clt_parameters,
scenes[nscene], // QuadCLT scene,
-1); // int debug_level);
if (scene_ds != null) {
double [] disparity_cam = scene_ds[0]; // null; // for now
scene_pXpYD = filterBG (
ref_scene.getTileProcessor(), // final TileProcessor tp,
scene_pXpYD_prefilter, // final double [][] pXpYD,
max_overlap, // final double max_overlap,
null, // disparity_cam, // final double [] disparity_cam,
min_adisp_cam, // final double min_adisp_cam,
min_rdisp_cam, // final double min_rdisp_cam,
clt_parameters.tileX, // final int dbg_tileX,
clt_parameters.tileY, // final int dbg_tileY,
0); // 1); //debug_level); // final int debug_level);
} else {
scene_pXpYD = scene_pXpYD_prefilter;
}
// acummulate single-thread
// tileSize
for (int nTile = 0; nTile < scene_pXpYD.length; nTile++) if ((scene_pXpYD[nTile] != null) && !Double.isNaN(scene_pXpYD[nTile][2])) {
// final int scene_extrap_irad = 1;
// double max_rad2 = scene_extrap_irad * scene_extrap_irad;
// int tileX0=nTile % tilesX;
// int tileY0=nTile / tilesX;
int tileX0=(int) Math.floor(scene_pXpYD[nTile][0]/tileSize);
int tileY0=(int) Math.floor(scene_pXpYD[nTile][1]/tileSize);
if ((debug_level > -1) && (Math.abs(tileY0-dbg_tileY) < 2) && (Math.abs(tileX0-dbg_tileX) < 2)) {
System.out.println("nTile="+nTile+", tilX0="+tileX0+", tileY0="+tileY0);
}
for (int dTy = -scene_extrap_irad; dTy <= scene_extrap_irad; dTy++) {
int tileY = tileY0 + dTy;
double dy = tileY * tileSize + tileSize/2 - scene_pXpYD[nTile][1];
if ((tileY >=0) && (tileY < tilesY)) {
for (int dTx = -scene_extrap_irad; dTx <= scene_extrap_irad; dTx++) {
int tileX = tileX0 + dTx;
if ((tileX >=0) && (tileX < tilesX)) {
if ((debug_level > -1) && (tileY == dbg_tileY) && (tileX == dbg_tileX)) {
System.out.println("tileX="+tileX+", tileY="+tileY);
}
double dx = tileX * tileSize + tileSize/2 - scene_pXpYD[nTile][0];
double rad2 = dy*dy+dx*dx;
if (rad2 < max_rad2) {
int tile = tileY * tilesX + tileX;
double w = 1 - (rad2/max_rad2);
if (scene_approx[tile] == null) {
scene_approx[tile] = new Matrix[2];
scene_approx[tile][0] = new Matrix(3,3); // A
scene_approx[tile][1] = new Matrix(3,1); // B
}
double d = scene_pXpYD[nTile][2];
double dsx = w *dx;
double dsy = w *dy;
double dsx2 = dsx*dx;
double dsy2 = dsy*dy;
double dsxy = dsx*dy;
double ds0 = w;
double dsxd = dsx * d;
double dsyd = dsy * d;
double dsd = ds0 * d;
double [][] A = scene_approx[tile][0].getArray();
A[0][0] += dsx2;
A[0][1] += dsxy;
A[0][2] += dsx;
A[1][1] += dsy2;
A[1][2] += dsy;
A[2][2] += ds0;
double [][] B = scene_approx[tile][1].getArray();
B[0][0] += dsxd;
B[1][0] += dsyd;
B[2][0] += dsd;
/* ax + by + c ~= d
a * sx2 + b * sxy + c * sx - sxd = 0
a * sxy + b * sy2 + c * sy - syd = 0
a * sx + b * sy + c * s0 - sd = 0
| sx2 sxy sx | | a | | sxd |
| sxy sy2 sy | * | b | = | syd |
| sx sy s0 | | c | | sd | */
}
}
}
}
}
}
for (int nTile = 0; nTile < scene_pXpYD.length; nTile++) if (scene_approx[nTile] != null) {
if (debug_level > -1) {
int tileY = nTile / tilesX;
int tileX = nTile % tilesX;
if ((tileY == dbg_tileY) && (tileX == dbg_tileX)) {
System.out.println("tileX="+tileX+", tileY="+tileY);
}
}
double [][] A = scene_approx[nTile][0].getArray();
if (A[2][2] > 0) {
A[1][0] = A[0][1];
A[2][0] = A[0][2];
A[2][1] = A[1][2];
try {
Matrix abc = scene_approx[nTile][0].solve(scene_approx[nTile][1]);
disparity_scenes[nscene][nTile] = abc.get(2, 0) + abc.get(0, 0)*tileSize/2 + abc.get(1, 0)*tileSize/2;
} catch (RuntimeException e){
// Use just average of disparities
disparity_scenes[nscene][nTile] = scene_approx[nTile][1].get(2,0) / A[2][2]; // Double.NaN;
}
}
}
}
if (debug_level > -1) {
if (ref_name.equals(ts)){ // never for (virtual) center
// if (nscene == indx_ref) {
System.out.print("reference ");
// System.out.println("Correlating reference scene"); // , nrefine = "+nrefine);
} else {
System.out.print(nscene+ " ");
// System.out.println("Correlating scene "+nscene); // +nrefine+":"+nscene);
}
}
} // for (int nscene = scenes.length-1; nscene >= 0; nscene--) {
if (debug_level > -1) {
System.out.println();
}
if (clt_parameters.lyms_show_images) {
ShowDoubleFloatArrays.showArrays(
disparity_scenes,
tilesX,
tilesY,
true,
ref_scene.getImageName()+"-disparity_scenes");
}
return disparity_scenes;
}
// Cleaned up and optimized version to reduce memory usage (on-the-fly integration, not saving full correlation data)
......
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