Commit 12743f36 authored by Andrey Filippov's avatar Andrey Filippov

Implemented setInitialOrientationsCuas

parent 379f3835
......@@ -84,6 +84,44 @@ public class CuasCenterLma {
return rslt;
}
public static double [][] getCenterATR(
double [][] scenes_atr,
int [] range,
int debugLevel) {
boolean [] param_select = new boolean[PARAMETER_NAMES.length];
Arrays.fill(param_select, true);
CuasCenterLma cuasCenterLma = new CuasCenterLma(
param_select, // boolean [] param_select,
scenes_atr, // double [][] scenes_atr,
range, // int [] range,
debugLevel); // int debugLevel)
double lambda = 0.1;
double lambda_scale_good = 0.5;
double lambda_scale_bad = 8.0;
double lambda_max = 100;
double rms_diff = 0.001;
int num_iter = 20;
int lmaResult = cuasCenterLma.runLma(
lambda, // double lambda, // 0.1
lambda_scale_good,// double lambda_scale_good,// 0.5
lambda_scale_bad, // double lambda_scale_bad, // 8.0
lambda_max, // double lambda_max, // 100
rms_diff, // double rms_diff, // 0.001
num_iter, // int num_iter, // 20
debugLevel); // int debug_level)
double [][] rslt = {cuasCenterLma.getCenter(),cuasCenterLma.getRadius()};
if (debugLevel > -3) {
System.out.println("lmaResult =" +lmaResult+" iterations, RMSE ="+
cuasCenterLma.getRMS()+" ("+cuasCenterLma.getInitialRMS()+")");
System.out.println("azimuth_center = "+ rslt[0][0]);
System.out.println(" tilt_center = "+ rslt[0][1]);
System.out.println(" average roll = "+ rslt[0][2]);
System.out.println("azimuth_radius = "+ rslt[1][0]);
System.out.println(" tilt_radius = "+ rslt[1][1]);
}
return rslt;
}
public CuasCenterLma(
boolean [] param_select,
QuadCLT [] quadCLTs,
......@@ -98,6 +136,21 @@ public class CuasCenterLma {
range, // int [] range,
debugLevel); //int debugLevel)
}
public CuasCenterLma(
boolean [] param_select,
double [][] scenes_atr,
int [] range,
int debugLevel) {
prepareLMA(
param_select, // boolean [] param_select,
scenes_atr, // double [][] scenes_atr,
range, // int [] range,
debugLevel); //int debugLevel)
}
public int prepareLMA(
boolean [] param_select,
QuadCLT [] quadCLTs,
......@@ -121,6 +174,29 @@ public class CuasCenterLma {
String ts = quadCLTs[nscene].getImageName();
scenes_atr[nscene] = ers_reference.getSceneATR(ts);
}
return prepareLMA(
param_select, // boolean [] param_select,
scenes_atr, // double [][] scenes_atr,
new int [] {earliest_scene,last_scene}, // int [] range,
debugLevel); // int debugLevel);
}
public int prepareLMA(
boolean [] param_select,
double [][] scenes_atr,
int [] range,
int debugLevel) {
earliest_scene = range[0];
last_scene = range[1];
int num_scenes = last_scene- earliest_scene + 1;
for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
// just checking it is not isolated
if (scenes_atr[nscene] == null) {
earliest_scene = nscene + 1;
break;
}
}
full_parameters_vector = new double[PARAM_PHASE + num_scenes];
sel_par_rindex = new int[full_parameters_vector.length];
Arrays.fill(full_parameters_vector, Double.NaN);
......
......@@ -1891,14 +1891,24 @@ public class GpuQuad{ // quad camera description
return;
}
if (wh == null) {
wh = new int[] {img_width, img_height};
}
if (!rectilinear) {
setConvolutionKernels(false); // set kernels if they are not set already
setBayerImages(false); // set Bayer images if this.quadCLT instance has new ones
}
int [] wh1 = handleWH(
wh, // int [] wh_in,
ref_scene); // boolean ref_scene);
if (wh == null) { // so wh will not change if was not null
wh = wh1;
}
int tilesX = wh[0] / GPUTileProcessor.DTT_SIZE;
int tilesY = wh[1] / GPUTileProcessor.DTT_SIZE;
/*
if (wh == null) {
wh = new int[] {img_width, img_height};
}
// kernel parameters: pointer to pointers
int tilesX = wh[0] / GPUTileProcessor.DTT_SIZE;
int tilesY = wh[1] / GPUTileProcessor.DTT_SIZE;
......@@ -1954,6 +1964,8 @@ public class GpuQuad{ // quad camera description
gpu_clt_wh = wh.clone();
}
}
*/
CUdeviceptr gpu_clt_selected = ref_scene ? gpu_clt_ref : gpu_clt;
int [] GridFullWarps = {1, 1, 1};
int [] ThreadsFullWarps = {1, 1, 1};
......@@ -2037,6 +2049,79 @@ public class GpuQuad{ // quad camera description
}
}
/**
* Handle image width and height, allocate/re-allocate GPU memory if size changed
* @param wh input {width, height}, use sensor dimensions if null.
* @param ref_scene use reference buffer, if false - main scene buffer
* @return updated width, height pair, non-null
*/
public int [] handleWH(
int [] wh,
boolean ref_scene) {
// int [] wh = (wh_in != null) ? wh_in : (new int[] {img_width, img_height});
if (wh == null) {
wh = new int[] {img_width, img_height};
}
// kernel parameters: pointer to pointers
int tilesX = wh[0] / GPUTileProcessor.DTT_SIZE;
int tilesY = wh[1] / GPUTileProcessor.DTT_SIZE;
// De-allocate if size mismatch, allocate if needed. Now it is the only place where clt is allocated
if (ref_scene) {
if ((gpu_clt_ref_wh != null) && ((gpu_clt_ref_wh[0] != wh[0]) || (gpu_clt_ref_wh[1] != wh[1]))) {
for (int ncam = 0; ncam < num_cams; ncam++) {
cuMemFree (gpu_clt_ref_h[ncam]);
}
cuMemFree (gpu_clt_ref);
gpu_clt_ref = null;
gpu_clt_ref_wh = null;
}
if (gpu_clt_ref == null) { // Allocate memory, create pointers for reference scene TD representation
long [] gpu_clt_ref_l = new long [num_cams];
gpu_clt_ref_h = new CUdeviceptr[num_cams];
for (int ncam = 0; ncam < num_cams; ncam++) {
gpu_clt_ref_h[ncam] = new CUdeviceptr();
cuMemAlloc(gpu_clt_ref_h[ncam],
tilesY * tilesX * num_colors * 4 * GPUTileProcessor.DTT_SIZE * GPUTileProcessor.DTT_SIZE * Sizeof.FLOAT );
}
gpu_clt_ref = new CUdeviceptr();
cuMemAlloc(gpu_clt_ref, num_cams * Sizeof.POINTER);
for (int ncam = 0; ncam < num_cams; ncam++) {
gpu_clt_ref_l[ncam] = GPUTileProcessor.getPointerAddress(gpu_clt_ref_h[ncam]);
}
cuMemcpyHtoD(gpu_clt_ref, Pointer.to(gpu_clt_ref_l), num_cams * Sizeof.POINTER);
gpu_clt_ref_wh = wh.clone();
}
} else { // same for main (not ref) memory
if ((gpu_clt_wh != null) && ((gpu_clt_wh[0] != wh[0]) || (gpu_clt_wh[1] != wh[1]))) {
for (int ncam = 0; ncam < num_cams; ncam++) {
cuMemFree (gpu_clt_h[ncam]);
}
cuMemFree (gpu_clt);
gpu_clt = null;
gpu_clt_wh = null;
}
if (gpu_clt == null) { // Allocate memory, create pointers for reference scene TD representation
long [] gpu_clt_l = new long [num_cams];
gpu_clt_h = new CUdeviceptr[num_cams];
for (int ncam = 0; ncam < num_cams; ncam++) {
gpu_clt_h[ncam] = new CUdeviceptr();
cuMemAlloc(gpu_clt_h[ncam],
tilesY * tilesX * num_colors * 4 * GPUTileProcessor.DTT_SIZE * GPUTileProcessor.DTT_SIZE * Sizeof.FLOAT );
}
gpu_clt = new CUdeviceptr();
cuMemAlloc(gpu_clt, num_cams * Sizeof.POINTER);
for (int ncam = 0; ncam < num_cams; ncam++) {
gpu_clt_l[ncam] = GPUTileProcessor.getPointerAddress(gpu_clt_h[ncam]);
}
cuMemcpyHtoD(gpu_clt, Pointer.to(gpu_clt_l), num_cams * Sizeof.POINTER);
gpu_clt_wh = wh.clone();
}
}
return wh;
}
public boolean reAllocateClt(
int [] wh,
boolean ref_scene) {
......
......@@ -1339,6 +1339,7 @@ public class ImageDtt extends ImageDttCPU {
/**
* Convert reference scene to FD and save result in extra GPU array for the future interscene correlation
* Geometry correction and images will come from gpuQuad instance -
* @param fclt load TD data and bypass execConvertDirect. If fclt.length==1 load same data to all sensor channels
* @param erase_clt erase CLT (<0 - do not erase, 0 - erase to 0.0, >0 - erase to NaN). Needed only for later IMCLT
* end rendering images. NaN produces sharp, distinct borders; 0f - blended
* @param wh if null, will uses sensor dimensions. Otherwise {width, height} in pixels
......@@ -1353,6 +1354,7 @@ public class ImageDtt extends ImageDttCPU {
* @param globalDebugLevel
*/
public void setReferenceTD(
final float [][] fclt,
final int erase_clt,
final int [] wh, // null (use sensor dimensions) or pair {width, height} in pixels
final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
......@@ -1384,9 +1386,28 @@ public class ImageDtt extends ImageDttCPU {
gpuQuad.updateTasks(
tp_tasks,
false); // boolean use_aux // while is it in class member? - just to be able to free
if (fclt != null) {
gpuQuad.handleWH( // allocate/reallocate GPU memory, that was normally done by gpuQuad.execConvertDirect()
wh, // int [] wh,
use_reference_buffer); // boolean ref_scene)
int sensor_mask = -1;
int num_cams = getNumSensors();
boolean merge_channels = (fclt.length == 1);
for (int ncam = 0; ncam < num_cams; ncam++) if (((1 << ncam) & sensor_mask) != 0){
int src_cam = merge_channels? 0: ncam;
if (src_cam < fclt.length) {
gpuQuad.setCltData( // for testing only
ncam, // int ncam,
fclt[src_cam], // float [] fclt, //
true); // boolean use_ref);
}
}
} else {
gpuQuad.execConvertDirect(use_reference_buffer, wh, erase_clt); // put results into a "reference" buffer
}
}
public void setRectilinearReferenceTD(
final int erase_clt,
final float [] fpixels_ref,
......
......@@ -253,7 +253,8 @@ public class IntersceneMatchParameters {
public boolean center_reference = false;
public boolean lock_position = false; // camera position is not changing, only oriantation (cuas mode)
public boolean lock_position = false; // camera position is not changing, only orientation (cuas mode)
public boolean cuas_rotation = false; // assumes lock_position, camera is rotating around the center
public boolean manual_correction = false; // once used for foliage to merge two sequences
public boolean overlap_sequences = false; // overlap sequences: scan down from the previous center
public boolean reset_photometric = true; // reset photometric calibration - once for each new series
......@@ -564,7 +565,10 @@ min_str_neib_fpn 0.35
public double min_ref_str_lma = 0.8; // 0.22; // For orientations: use only tiles of the reference scene DSI_MAIN is stronger
public double min_ref_frac = 0.2; // 0.22; if fraction number of reliable tiles is less than this, use best possible
public boolean ref_smooth = false; // smooth reference disparity for initial matching
// was overwritten in code to always be true ****
public boolean ref_smooth = true; // false; // smooth reference disparity for initial matching
public boolean ref_smooth_always = true; // in older code it was hard-wired true inside adjustDiffPairsLMAInterscene
// ref_smooth_always, when true mimics old behavior
public double ref_sigma = 2.0; // Gaussian sigma to smooth reference disparity for initial matching
public double ref_smooth_diff = 0.15; // discard smooth disparity if it differs by more from measured
......@@ -1247,6 +1251,8 @@ min_str_neib_fpn 0.35
"True for mapping with post-processing. If false, use the last scene as a reference (driving).");
gd.addCheckbox ("Lock camera position", this.lock_position,
"Only camera orientation changes, position is locked (cUAS mode).");
gd.addCheckbox ("CUAS mode", this.cuas_rotation,
"Assumes lock_position, camera is rotating around the center.");
gd.addCheckbox ("Manual correction", this.manual_correction,
"Once used for foliage to merge two sequences.");
gd.addCheckbox ("Overlap sequences", this.overlap_sequences,
......@@ -1786,6 +1792,8 @@ min_str_neib_fpn 0.35
gd.addMessage ("Smooth reference disparity for initial matching");
gd.addCheckbox ("Smooth reference disparity", this.ref_smooth,
"Smooth reference disparity for initial matching.");
gd.addCheckbox ("Smooth always", this.ref_smooth_always,
"In older code it was hard-wired true inside adjustDiffPairsLMAInterscene. Checked mimics that legacy behavior.");
gd.addNumericField("Reference disparity sigma", this.ref_sigma, 5,7,"pix",
"Gaussian sigma to smooth reference disparity for initial matching.");
gd.addNumericField("Max smooth offset", this.ref_smooth_diff, 5,7,"pix",
......@@ -2501,6 +2509,8 @@ min_str_neib_fpn 0.35
this.center_reference = gd.getNextBoolean();
this.lock_position = gd.getNextBoolean();
this.cuas_rotation = gd.getNextBoolean();
this.lock_position |= this.cuas_rotation;
this.manual_correction = gd.getNextBoolean();
this.overlap_sequences = gd.getNextBoolean();
this.reset_photometric = gd.getNextBoolean();
......@@ -2758,6 +2768,7 @@ min_str_neib_fpn 0.35
this.min_ref_str_lma = gd.getNextNumber();
this.min_ref_frac = gd.getNextNumber();
this.ref_smooth = gd.getNextBoolean();
ref_smooth_always = gd.getNextBoolean();
this.ref_sigma = gd.getNextNumber();
this.ref_smooth_diff = gd.getNextNumber();
this.sfm_filter = gd.getNextBoolean();
......@@ -3301,6 +3312,7 @@ min_str_neib_fpn 0.35
properties.setProperty(prefix+"center_reference", this.center_reference + ""); // boolean
properties.setProperty(prefix+"lock_position", this.lock_position + ""); // boolean
properties.setProperty(prefix+"cuas_rotation", this.cuas_rotation + ""); // boolean
properties.setProperty(prefix+"manual_correction", this.manual_correction + ""); // boolean
properties.setProperty(prefix+"overlap_sequences", this.overlap_sequences + ""); // boolean
properties.setProperty(prefix+"reset_photometric", this.reset_photometric + ""); // boolean
......@@ -3569,6 +3581,7 @@ min_str_neib_fpn 0.35
properties.setProperty(prefix+"min_ref_frac", this.min_ref_frac+""); // double
properties.setProperty(prefix+"ref_smooth", this.ref_smooth+""); // boolean
properties.setProperty(prefix+"ref_smooth_always", this.ref_smooth_always+""); // boolean
properties.setProperty(prefix+"ref_sigma", this.ref_sigma+""); // double
properties.setProperty(prefix+"ref_smooth_diff", this.ref_smooth_diff+""); // double
......@@ -4083,6 +4096,8 @@ min_str_neib_fpn 0.35
if (properties.getProperty(prefix+"center_reference")!=null) this.center_reference=Boolean.parseBoolean(properties.getProperty(prefix+"center_reference"));
if (properties.getProperty(prefix+"lock_position")!=null) this.lock_position=Boolean.parseBoolean(properties.getProperty(prefix+"lock_position"));
if (properties.getProperty(prefix+"cuas_rotation")!=null) this.cuas_rotation=Boolean.parseBoolean(properties.getProperty(prefix+"cuas_rotation"));
this.lock_position |= this.cuas_rotation;
if (properties.getProperty(prefix+"manual_correction")!=null) this.manual_correction=Boolean.parseBoolean(properties.getProperty(prefix+"manual_correction"));
if (properties.getProperty(prefix+"overlap_sequences")!=null) this.overlap_sequences=Boolean.parseBoolean(properties.getProperty(prefix+"overlap_sequences"));
if (properties.getProperty(prefix+"reset_photometric")!=null) this.reset_photometric=Boolean.parseBoolean(properties.getProperty(prefix+"reset_photometric"));
......@@ -4356,6 +4371,7 @@ min_str_neib_fpn 0.35
if (properties.getProperty(prefix+"min_ref_frac")!=null) this.min_ref_frac=Double.parseDouble(properties.getProperty(prefix+"min_ref_frac"));
if (properties.getProperty(prefix+"ref_smooth")!=null) this.ref_smooth=Boolean.parseBoolean(properties.getProperty(prefix+"ref_smooth"));
if (properties.getProperty(prefix+"ref_smooth_always")!=null) this.ref_smooth_always=Boolean.parseBoolean(properties.getProperty(prefix+"ref_smooth_always"));
if (properties.getProperty(prefix+"ref_sigma")!=null) this.ref_sigma=Double.parseDouble(properties.getProperty(prefix+"ref_sigma"));
if (properties.getProperty(prefix+"ref_smooth_diff")!=null) this.ref_smooth_diff=Double.parseDouble(properties.getProperty(prefix+"ref_smooth_diff"));
......@@ -4882,7 +4898,8 @@ min_str_neib_fpn 0.35
imp.patt_save_subdir = this.patt_save_subdir;
imp.center_reference = this.center_reference;
imp.lock_position = this.lock_position;
imp.lock_position = this.lock_position || this.cuas_rotation;
imp.cuas_rotation = this.cuas_rotation;
imp.manual_correction = this.manual_correction;
imp.overlap_sequences = this.overlap_sequences;
imp.reset_photometric = this.reset_photometric;
......@@ -5146,6 +5163,7 @@ min_str_neib_fpn 0.35
imp.min_ref_frac = this.min_ref_frac;
imp.ref_smooth = this.ref_smooth;
imp.ref_smooth_always = this.ref_smooth_always;
imp.ref_sigma = this.ref_sigma;
imp.ref_smooth_diff = this.ref_smooth_diff;
......
......@@ -1464,6 +1464,7 @@ public class QuadCLT extends QuadCLTCPU {
debugLevel); // final int globalDebugLevel);
} else {
image_dtt.setReferenceTD( // change to main?
null, // final float [][] fclt,
erase_clt, //final int erase_clt,
wh, // null, // final int [] wh, // null (use sensor dimensions) or pair {width, height} in pixels
clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
......@@ -1608,7 +1609,7 @@ public class QuadCLT extends QuadCLTCPU {
final boolean merge_channels,
final Rectangle full_woi_in, // show larger than sensor WOI in tiles (or null)
CLTParameters clt_parameters,
double [] disparity_ref,
double [] disparity_ref, // may be null if ref_pXpYD!=null
double [][] ref_pXpYD, // alternative to disparity_ref when reference is not uniform
// motion blur compensation
double mb_tau, // 0.008; // time constant, sec
......@@ -1667,8 +1668,8 @@ public class QuadCLT extends QuadCLTCPU {
int dbg_width = rendered_width/GPUTileProcessor.DTT_SIZE;
int dbg_height = pXpYD.length/dbg_width;
String [] dbg_titles = (mb_vectors!=null)?
(new String[] {"pX","pY","Disparity","mb_X","mb_Y","disparity_ref"}):
(new String[] {"pX","pY","Disparity","disparity_ref"});
(new String[] {"pX","pY","Disparity","mb_X","mb_Y"}):
(new String[] {"pX","pY","Disparity"});
double [][] dbg_img = new double [dbg_titles.length][pXpYD.length]; // 3 + ((mb_vectors!=null)? 2:0)][pXpYD.length];
for (int i = 0; i < dbg_img.length; i++) {
Arrays.fill(dbg_img[i], Double.NaN);
......@@ -1685,7 +1686,7 @@ public class QuadCLT extends QuadCLTCPU {
}
}
}
dbg_img[dbg_img.length-1] = disparity_ref;
// dbg_img[dbg_img.length-1] = disparity_ref;
ShowDoubleFloatArrays.showArrays( // out of boundary 15
dbg_img,
dbg_width,
......@@ -1756,6 +1757,7 @@ public class QuadCLT extends QuadCLTCPU {
debugLevel); // final int globalDebugLevel);
} else {
image_dtt.setReferenceTD( // change to main?
null, // final float [][] fclt,
erase_clt, //final int erase_clt,
wh, // null, // final int [] wh, // null (use sensor dimensions) or pair {width, height} in pixels
clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
......@@ -1923,6 +1925,7 @@ public class QuadCLT extends QuadCLTCPU {
debugLevel); // final int globalDebugLevel);
} else {
image_dtt.setReferenceTD( // change to main?
null, // final float [][] fclt,
erase_clt, //final int erase_clt,
wh, // null, // final int [] wh, // null (use sensor dimensions) or pair {width, height} in pixels
clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
......@@ -2117,6 +2120,7 @@ public class QuadCLT extends QuadCLTCPU {
debugLevel); // final int globalDebugLevel);
} else {
image_dtt.setReferenceTD( // change to main?
null, // final float [][] fclt,
erase_clt, //final int erase_clt,
null, // wh, // null, // final int [] wh, // null (use sensor dimensions) or pair {width, height} in pixels
clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
......@@ -2408,6 +2412,7 @@ public class QuadCLT extends QuadCLTCPU {
debugLevel); // final int globalDebugLevel);
} else {
image_dtt.setReferenceTD( // change to main?
null, // final float [][] fclt,
erase_clt, //final int erase_clt,
null, // wh, // null, // final int [] wh, // null (use sensor dimensions) or pair {width, height} in pixels
clt_parameters.img_dtt, // final ImageDttParameters imgdtt_params, // Now just extra correlation parameters, later will include, most others
......@@ -2744,11 +2749,15 @@ public class QuadCLT extends QuadCLTCPU {
* @param use_reference set the reference buffers (false - set main buffers)
*/
void setComboToTD(
final float [][] fclt,
final boolean merge_channels, // duplicate same data to all selected channels
final int sensor_mask, // only if merge_channels
final int [] whc, // if int[2], will return width, height
final boolean use_reference){
float [][] fclt,
boolean merge_channels, // duplicate same data to all selected channels
int sensor_mask, // only if merge_channels
int [] whc, // if int[2], will return width, height
boolean use_reference){
if (fclt == null) {
fclt = new float[][] {getCenterClt()};
merge_channels = true;
}
final int [] width_height = gpuQuad.getWH(use_reference);
final int num_colors = gpuQuad.getNumColors();
if (whc != null) {
......
......@@ -176,6 +176,7 @@ public class StructureFromMotion {
ref_scene, // QuadCLT ref_scene,
mb_ref_disparity, // double [] ref_disparity, // null or alternative reference disparity
ref_pXpYD, // double [][] ref_pXpYD,
null, // final float [][] fclt,
reliable_ref, // final boolean [] selection, // may be null, if not null do not process unselected tiles
margin, // final int margin,
// motion blur compensation
......@@ -2103,6 +2104,7 @@ public class StructureFromMotion {
scenes[0], // ref_scene, // QuadCLT ref_scene,
null, // ref_disparity, // double [] ref_disparity, // null or alternative reference disparity
scenes_pXpYD[0], // ref_pXpYD, // double [][] ref_pXpYD,
null, // final float [][] fclt,
null, // reliable_ref, // final boolean [] selection, // may be null, if not null do not process unselected tiles
margin, // final int margin,
// motion blur compensation
......
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