Commit 21b47b81 authored by Andrey Filippov's avatar Andrey Filippov

Started Realtime tests implementation

parent b7ce93be
......@@ -8857,7 +8857,7 @@ public class CuasMotion {
debugLevel); // -1); // debugLevel); // int debugLevel)
}
}
boolean debug_extra = debugLevel > -5;
boolean debug_extra = debugLevel> 5; //-5;
if (save_filtered_low && debug_more && debug_extra) {
ImagePlus imp_ext = showTargetSequence(
targets_nonoverlap, // double [][][] vector_fields_sequence,
......@@ -10792,7 +10792,7 @@ public class CuasMotion {
public void processMovingTargetsMulti(
final boolean batch_mode,
final float [][] fpixels,
final float [] fpixels_avg,
final float [] fpixels_avg, // not used
final int debugLevel) {
int start_frame = 0;
boolean half_step = clt_parameters.imp.cuas_half_step; // true;
......
......@@ -54,7 +54,7 @@ public class CuasRanging {
public static final String TARGET_GLOBALS_SUFFIX = "-TARGET_GLOBALS";
public static final String TARGET_STATS_SUFFIX = "-TARGETS"; // *.csv
final boolean realtime_mode;
final QuadCLT center_CLT;
final QuadCLT [] scenes;
final double [][][] img_um;
......@@ -66,21 +66,115 @@ public class CuasRanging {
public CuasRanging (
CLTParameters clt_parameters,
boolean realtime_mode,
QuadCLT center_CLT,
QuadCLT [] scenes,
int debugLevel) {
this.realtime_mode = realtime_mode;
this.clt_parameters = clt_parameters;
this.center_CLT = center_CLT;
this.scenes = scenes;
this.img_um = new double [scenes.length][][];
}
public ImagePlus prepareFpixels(){
boolean save_linear_cuas = true;
double [][]combo_dsi = center_CLT.comboFromMain();
double [][] dls = {
combo_dsi[OpticalFlow.COMBO_DSN_INDX_DISP], // **** null on second scene sequence
combo_dsi[OpticalFlow.COMBO_DSN_INDX_LMA],
combo_dsi[OpticalFlow.COMBO_DSN_INDX_STRENGTH]
};
double [][] ds = OpticalFlow.conditionInitialDS(
true, // boolean use_conf, // use configuration parameters, false - use following
clt_parameters, // CLTParameters clt_parameters,
dls, // double [][] dls
center_CLT, // quadCLTs[ref_index], // QuadCLT scene,
debugLevel-1);
double [] disparity_fg = ds[0]; // combo_dsn_final[COMBO_DSN_INDX_DISP_FG];
double [] strength_fg = ds[1];
if (strength_fg != null) { // for FG/BG only, fixing for transformCameraVew()
for (int i = 0; i < disparity_fg.length; i++) {
if (!Double.isNaN(disparity_fg[i]) && (strength_fg[i] == 0)) strength_fg[i] = 0.01; // transformCameraVew ignores strength= 0
}
}
double [] xyz_offset= {0,0,0};
double [][] ds_vantage = new double[][] {disparity_fg, strength_fg};
boolean debug_vantage = false;
double [][] dbg_vantage = debug_vantage ? (new double[7][]): null;
if (dbg_vantage != null) {
for (int i = 0; i < 3; i++) {
dbg_vantage[i] = dls[i].clone();
}
for (int i = 0; i < 2; i++) {
dbg_vantage[i+3] = ds_vantage[i].clone();
}
}
ds_vantage = OpticalFlow.transformCameraVew(
null, // (debug_ds_fg_virt?"transformCameraVew":null), // final String title,
ds_vantage, // final double [][] dsrbg_camera_in,
xyz_offset, // xyz_offset, // _inverse[0], // final double [] scene_xyz, // camera center in world coordinates
OpticalFlow.ZERO3, // _inverse[1], // final double [] scene_atr, // camera orientation relative to world frame
center_CLT, // quadCLTs[ref_index], // final QuadCLT scene_QuadClt,
center_CLT, // quadCLTs[ref_index], // final QuadCLT reference_QuadClt,
8); // iscale); // final int iscale);
if (dbg_vantage != null) {
for (int i = 0; i < 2; i++) {
dbg_vantage[i+5] = ds_vantage[i].clone();
}
ShowDoubleFloatArrays.showArrays(
dbg_vantage,
center_CLT.getTileProcessor().getTilesX(),
center_CLT.getTileProcessor().getTilesY(),
true,
center_CLT.getImageName()+"-ds_vantage", // "-corr2d"+"-"+frame0+"-"+frame1+"-"+corr_pairs,
new String[] {"disp0","lma0", "str0", "disp","str","virt_disp", "virt_str"});
}
float [] average_pixels = (float []) center_CLT.getCenterAverage().getProcessor().getPixels();
float [][] average_channels = new float [][] {average_pixels}; // for future color images
double [] cuas_atr = OpticalFlow.ZERO3;
String scenes_suffix = center_CLT.getImageName()+"-CUAS"; // "1747829900_781803-SEQ-FG-MONO-FPN";
ImagePlus imp_targets= OpticalFlow.renderSceneSequence(
clt_parameters, // CLTParameters clt_parameters,
true, // center_CLT.hasCenterClt(), // boolean mode_cuas,
false, // clt_parameters.imp.um_mono, // boolean um_mono,
clt_parameters.imp.calculate_average, // boolean insert_average, // then add new parameter, keep add average
null, // int [] average_range,
average_channels, // average_slice,
clt_parameters.imp.subtract_average, // boolean subtract_average,
clt_parameters.imp.running_average, // int running_average,
null, // fov_tiles, // Rectangle fov_tiles,
1, // mode3d, // int mode3d,
false, // toRGB, // boolean toRGB,
xyz_offset, // double [] stereo_offset, // offset reference camera {x,y,z}
cuas_atr, // double [] stereo_atr, // offset reference orientation (cuas)
1, // sensor_mask, // int sensor_mask,
scenes_suffix, // String suffix,
ds_vantage[0], // selected_disparity, // double [] ref_disparity,
scenes, // QuadCLT [] quadCLTs,
center_CLT, // ref_index, // int ref_index,
ImageDtt.THREADS_MAX, // threadsMax, // int threadsMax,
debugLevel); // int debugLevel);
if (save_linear_cuas) {
center_CLT.saveImagePlusInModelDirectory(
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_targets); // imp_scenes); // ImagePlus imp)
}
return imp_targets;
}
public CuasMotion detectTargets(
UasLogReader uasLogReader,
boolean batch_mode){
boolean save_linear_cuas = true;
boolean save_um_cuas = true;
boolean save_noise_map = true;
boolean save_um_cuas = true;
boolean save_noise_map = true;
double um_sigma = clt_parameters.imp.um_sigma;
double um_weight = clt_parameters.imp.um_weight;
boolean mono_fixed = clt_parameters.imp.mono_fixed;
......@@ -131,7 +225,7 @@ public class CuasRanging {
if (!Double.isNaN(disparity_fg[i]) && (strength_fg[i] == 0)) strength_fg[i] = 0.01; // transformCameraVew ignores strength= 0
}
}
/*
double [] xyz_offset= {0,0,0};
double [][] ds_vantage = new double[][] {disparity_fg, strength_fg};
......@@ -198,6 +292,10 @@ public class CuasRanging {
null, // "GPU-SHIFTED-D"+clt_parameters.disparity, // String suffix,
imp_targets); // imp_scenes); // ImagePlus imp)
}
*/
ImagePlus imp_targets = prepareFpixels();
// always generating UM, even if not saving - needed for targets
String cuas_title = imp_targets.getTitle();
String um_suffix = "";
......
This diff is collapsed.
package com.elphel.imagej.cuas.rt;
public class TemporalConsolidator {
/**
* 1D Cumulative Distribution Function for a Trapezoid centered at 0.
* Guarantees exact fractional area calculations.
*/
private static double trapezoidCDF(double x, double w_top, double w_bot) {
double x0 = -w_bot / 2.0;
double x1 = -w_top / 2.0;
double x2 = w_top / 2.0;
double x3 = w_bot / 2.0;
if (x <= x0) return 0.0;
// Handle t=0 (Boxcar, no velocity blur)
if (Math.abs(w_bot - w_top) < 1e-9) {
if (x >= x3) return w_bot;
return (x - x0);
}
double m = 1.0 / (x1 - x0); // Rising slope
double area_rising = 0.5 * m * (x1 - x0) * (x1 - x0);
double area_flat = 1.0 * (x2 - x1);
if (x <= x1) {
return 0.5 * m * (x - x0) * (x - x0);
} else if (x <= x2) {
return area_rising + 1.0 * (x - x1);
} else if (x <= x3) {
double dx = x - x2;
return area_rising + area_flat + (1.0 * dx - 0.5 * m * dx * dx);
}
return area_rising + area_flat + 0.5 * m * (x3 - x2) * (x3 - x2);
}
/**
* Consolidates coarse temporal velocity outputs into a high-resolution grid.
*/
public static double[][][] consolidateTemporalLayers(
double[][][] historic_pixel_vel, // [historic_t][pixel][9_velocities]
double[] weights, // [historic_t] sums to 1.0
int width, int height,
int pixel_decimate,
int velocity_decimate,
int velocity_radius
) {
int num_pixels = width * height;
int num_subpixels = pixel_decimate * pixel_decimate;
int v_dim = 2 * velocity_radius + 1;
int num_velocities = v_dim * v_dim;
// Output: [pixel][subpixel][high_res_velocity]
double[][][] output = new double[num_pixels][num_subpixels][num_velocities];
int num_history = weights.length;
for (int p = 0; p < num_pixels; p++) {
int px = p % width;
int py = p / width;
for (int t = 0; t < num_history; t++) {
double w_t = weights[t];
if (w_t == 0) continue;
// Trapezoid width scales with time and velocity decimation
double blur_width = (double) t / velocity_decimate;
double w_bot = 1.0 + blur_width;
double w_top = Math.abs(1.0 - blur_width);
double total_area = 0.5 * (w_bot + w_top); // Used to normalize area
for (int vx = -velocity_radius; vx <= velocity_radius; vx++) {
for (int vy = -velocity_radius; vy <= velocity_radius; vy++) {
// 1. Map High-Res Velocity to parent Coarse Velocity (-1, 0, 1)
int coarse_vx = (int) Math.round((double) vx / velocity_decimate);
int coarse_vy = (int) Math.round((double) vy / velocity_decimate);
// If outside the 3x3 coarse grid, target exceeds tracking limit
if (Math.abs(coarse_vx) > 1 || Math.abs(coarse_vy) > 1) continue;
int coarse_v_idx = (coarse_vy + 1) * 3 + (coarse_vx + 1);
int v_out_idx = (vy + velocity_radius) * v_dim + (vx + velocity_radius);
// 2. Calculate the projected center of the past pixel
double proj_x = ((double) vx / velocity_decimate) * t;
double proj_y = ((double) vy / velocity_decimate) * t;
// Identify the source macro-pixel (Simplified: no border checks)
int src_px = px - (int) Math.round(proj_x);
int src_py = py - (int) Math.round(proj_y);
int src_p = src_py * width + src_px;
// Calculate the sub-pixel shift within the target macro-pixel
double shift_x = proj_x - Math.round(proj_x);
double shift_y = proj_y - Math.round(proj_y);
// 3. Integrate over Output Spatial Subpixels
for (int sx = 0; sx < pixel_decimate; sx++) {
// Subpixel boundaries relative to the projected beam center
double left_x = ((double) sx / pixel_decimate) - 0.5 - shift_x;
double right_x = ((double) (sx + 1) / pixel_decimate) - 0.5 - shift_x;
double weight_x = (trapezoidCDF(right_x, w_top, w_bot) - trapezoidCDF(left_x, w_top, w_bot)) / total_area;
if (weight_x <= 0) continue;
for (int sy = 0; sy < pixel_decimate; sy++) {
double top_y = ((double) sy / pixel_decimate) - 0.5 - shift_y;
double bottom_y = ((double) (sy + 1) / pixel_decimate) - 0.5 - shift_y;
double weight_y = (trapezoidCDF(bottom_y, w_top, w_bot) - trapezoidCDF(top_y, w_top, w_bot)) / total_area;
double total_weight = weight_x * weight_y * w_t;
if (total_weight > 0) {
int sub_idx = sy * pixel_decimate + sx;
output[p][sub_idx][v_out_idx] += total_weight * historic_pixel_vel[t][src_p][coarse_v_idx];
}
}
}
}
}
}
}
return output;
}
}
package com.elphel.imagej.cuas.rt;
public class TemporalKernelGenerator {
public static class ConsolidationKernel {
public final int spatialRadius;
public final int spatialDim; // 2 * spatialRadius + 1
// Maps high-res velocity index -> coarse 9-velocity index (0-8).
// Contains -1 if the high-res velocity falls outside the target tracking limit.
public final int[] coarseVIdx;
// The Convolution Tensor
// Dimensions: [historic_t][v_out_idx][sub_idx][dy][dx]
// dy and dx are indices 0 to spatialDim-1, representing offsets from -spatialRadius to +spatialRadius
public final double[][][][][] weights;
public ConsolidationKernel(int numHistory, int numVOut, int numSub, int sRad) {
this.spatialRadius = sRad;
this.spatialDim = 2 * sRad + 1;
this.coarseVIdx = new int[numVOut];
this.weights = new double[numHistory][numVOut][numSub][spatialDim][spatialDim];
}
public int getSpatialRadius() {
return spatialRadius;
}
public int getSpatialDim() {
return spatialDim;
}
public double [][][][][] getKernel(){
return weights;
}
public int [] getCoarseVIdx() {
return coarseVIdx;
}
}
/**
* 1D Cumulative Distribution Function for a Trapezoid centered at 0.
*/
private static double trapezoidCDF(double x, double w_top, double w_bot) {
double x0 = -w_bot / 2.0;
double x1 = -w_top / 2.0;
double x2 = w_top / 2.0;
double x3 = w_bot / 2.0;
if (x <= x0) return 0.0;
if (Math.abs(w_bot - w_top) < 1e-9) { // t=0 Boxcar
if (x >= x3) return w_bot;
return (x - x0);
}
double m = 1.0 / (x1 - x0);
double area_rising = 0.5 * m * (x1 - x0) * (x1 - x0);
double area_flat = 1.0 * (x2 - x1);
if (x <= x1) return 0.5 * m * (x - x0) * (x - x0);
if (x <= x2) return area_rising + 1.0 * (x - x1);
if (x <= x3) {
double dx = x - x2;
return area_rising + area_flat + (1.0 * dx - 0.5 * m * dx * dx);
}
return area_rising + area_flat + 0.5 * m * (x3 - x2) * (x3 - x2);
}
/**
* Generates the multi-dimensional consolidation tensor.
*/
public static ConsolidationKernel generateKernel(
double[] temporal_weights,
int pixel_decimate,
int velocity_decimate,
int velocity_radius
) {
int num_history = temporal_weights.length;
int num_subpixels = pixel_decimate * pixel_decimate;
int v_dim = 2 * velocity_radius + 1;
int num_velocities = v_dim * v_dim;
// Calculate the maximum required spatial neighborhood to catch all trapezoid spillover
double max_time = num_history > 0 ? num_history - 1 : 0;
double max_vel = (double) velocity_radius / velocity_decimate;
double max_shift = max_vel * max_time;
double max_width = 1.0 + max_time / velocity_decimate;
int spatial_radius = (int) Math.ceil(max_shift + max_width / 2.0);
ConsolidationKernel kernel = new ConsolidationKernel(num_history, num_velocities, num_subpixels, spatial_radius);
for (int vx = -velocity_radius; vx <= velocity_radius; vx++) {
for (int vy = -velocity_radius; vy <= velocity_radius; vy++) {
int v_out_idx = (vy + velocity_radius) * v_dim + (vx + velocity_radius);
// Map to coarse 3x3 velocity grid (-1, 0, 1)
int coarse_vx = (int) Math.round((double) vx / velocity_decimate);
int coarse_vy = (int) Math.round((double) vy / velocity_decimate);
if (Math.abs(coarse_vx) > 1 || Math.abs(coarse_vy) > 1) {
kernel.coarseVIdx[v_out_idx] = -1; // Outside tracking limit
continue;
}
kernel.coarseVIdx[v_out_idx] = (coarse_vy + 1) * 3 + (coarse_vx + 1);
for (int t = 0; t < num_history; t++) {
double time_weight = temporal_weights[t];
if (time_weight == 0) continue;
double blur_width = (double) t / velocity_decimate;
double w_bot = 1.0 + blur_width;
double w_top = Math.abs(1.0 - blur_width);
double total_area = 0.5 * (w_bot + w_top);
// Loop over the spatial neighborhood (source macro-pixels)
for (int dy = -spatial_radius; dy <= spatial_radius; dy++) {
for (int dx = -spatial_radius; dx <= spatial_radius; dx++) {
// Center of projected trapezoid from source pixel (dx, dy)
double proj_x = dx + 0.5 + ((double) vx / velocity_decimate) * t;
double proj_y = dy + 0.5 + ((double) vy / velocity_decimate) * t;
for (int sy = 0; sy < pixel_decimate; sy++) {
for (int sx = 0; sx < pixel_decimate; sx++) {
int sub_idx = sy * pixel_decimate + sx;
// Target subpixel bounds (relative to target macro-pixel 0,0)
double left_x = (double) sx / pixel_decimate;
double right_x = (double) (sx + 1) / pixel_decimate;
double top_y = (double) sy / pixel_decimate;
double bot_y = (double) (sy + 1) / pixel_decimate;
// Shift so trapezoid is centered at 0 for CDF
double weight_x = (trapezoidCDF(right_x - proj_x, w_top, w_bot) -
trapezoidCDF(left_x - proj_x, w_top, w_bot)) / total_area;
double weight_y = (trapezoidCDF(bot_y - proj_y, w_top, w_bot) -
trapezoidCDF(top_y - proj_y, w_top, w_bot)) / total_area;
double final_weight = weight_x * weight_y * time_weight;
if (final_weight > 0) {
kernel.weights[t][v_out_idx][sub_idx][dy + spatial_radius][dx + spatial_radius] = final_weight;
}
}
}
}
}
}
}
}
return kernel;
}
}
......@@ -1121,6 +1121,9 @@ min_str_neib_fpn 0.35
public double cuas_max_disp_diff = 0.05; // Maximal disparity difference during last change to consider disparity valid
public double cuas_min_disp_str = 0.4; // Minimal disparity strength to consider disparity valid
public double cuas_rng_limit = 5000; // maximal displayed distance to target
// CUAS Reltime
public boolean curt_en = false; // enable airplane mode
// Airplane mode
public boolean air_mode_en = false; // enable airplane mode
public boolean air_sync_ims = true; // IMS is synchronized
......@@ -3338,6 +3341,10 @@ min_str_neib_fpn 0.35
gd.addNumericField("Maximal displayed distance", this.cuas_rng_limit, 6,8,"m",
"Maximal displayed distance to target.");
gd.addTab("CUAS RT", "CUAS Real Time");
gd.addCheckbox ("CUAS realtime enable", this.curt_en,
"Enable testing of the realtime CUAS detection.");
//
gd.addTab("Airplane","Airplane mode (fast forward movement, low vertical speed");
gd.addCheckbox ("Enable airplane mode", this.air_mode_en,
"Apply algorithms specific to the airplane mode.");
......@@ -4808,6 +4815,7 @@ min_str_neib_fpn 0.35
this.cuas_min_disp_str = gd.getNextNumber();
this.cuas_rng_limit = gd.getNextNumber();
this.curt_en = gd.getNextBoolean();
this.air_mode_en = gd.getNextBoolean();
this.air_sync_ims = gd.getNextBoolean();
this.air_disp_corr = gd.getNextBoolean();
......@@ -6108,6 +6116,7 @@ min_str_neib_fpn 0.35
properties.setProperty(prefix+"cuas_min_disp_str", this.cuas_min_disp_str+""); // double
properties.setProperty(prefix+"cuas_rng_limit", this.cuas_rng_limit+""); // double
properties.setProperty(prefix+"curt_en", this.curt_en+""); // boolean
properties.setProperty(prefix+"air_mode_en", this.air_mode_en+""); // boolean
properties.setProperty(prefix+"air_sync_ims", this.air_sync_ims+""); // boolean
properties.setProperty(prefix+"air_disp_corr", this.air_disp_corr+""); // boolean
......@@ -7391,6 +7400,7 @@ min_str_neib_fpn 0.35
if (properties.getProperty(prefix+"cuas_min_disp_str")!=null) this.cuas_min_disp_str=Double.parseDouble(properties.getProperty(prefix+"cuas_min_disp_str"));
if (properties.getProperty(prefix+"cuas_rng_limit")!=null) this.cuas_rng_limit=Double.parseDouble(properties.getProperty(prefix+"cuas_rng_limit"));
if (properties.getProperty(prefix+"curt_en")!=null) this.curt_en=Boolean.parseBoolean(properties.getProperty(prefix+"curt_en"));
if (properties.getProperty(prefix+"air_mode_en")!=null) this.air_mode_en=Boolean.parseBoolean(properties.getProperty(prefix+"air_mode_en"));
if (properties.getProperty(prefix+"air_sync_ims")!=null) this.air_sync_ims=Boolean.parseBoolean(properties.getProperty(prefix+"air_sync_ims"));
if (properties.getProperty(prefix+"air_disp_corr")!=null) this.air_disp_corr=Boolean.parseBoolean(properties.getProperty(prefix+"air_disp_corr"));
......@@ -8655,6 +8665,7 @@ min_str_neib_fpn 0.35
imp.cuas_min_disp_str = this.cuas_min_disp_str;
imp.cuas_rng_limit = this.cuas_rng_limit;
imp.curt_en = this.curt_en;
imp.air_mode_en = this.air_mode_en;
imp.air_sync_ims = this.air_sync_ims;
imp.air_disp_corr = this.air_disp_corr;
......
......@@ -7238,18 +7238,42 @@ java.lang.NullPointerException
// Moved to the very end, after 3D
// boolean test_vegetation = true;
if (master_CLT.hasCenterClt() && clt_parameters.imp.curt_en) { // cuas mode
if (debugLevel >-3) {
System.out.println("===== Running CUAS realtime testing. =====");
}
CuasRanging cuasRanging = new CuasRanging (
clt_parameters, // CLTParameters clt_parameters,
false, // boolean realtime_mode,
master_CLT, // QuadCLT center_CLT,
quadCLTs, // QuadCLT [] scenes,
debugLevel); // int debugLevel) {
CuasMotion cuasMotion = cuasRanging.detectTargets(
uasLogReader, // UasLogReader uasLogReader,
batch_mode); // boolean batch_mode)
if (cuasMotion == null) {
System.out.println("Failed target detection. Probably, radar mode was selected but target file does not exist (created with \"CUAS Combine\" command)");
} else {
if (debugLevel > -4) {
System.out.println("Target detection DONE");
}
}
}
if (master_CLT.hasCenterClt() && clt_parameters.imp.cuas_targets_en) { // cuas mode
if (debugLevel >-3) {
System.out.println("===== Running CUAS ranging. =====");
}
CuasRanging cuasRanging = new CuasRanging (
clt_parameters, // CLTParameters clt_parameters,
master_CLT, // QuadCLT center_CLT,
quadCLTs, // QuadCLT [] scenes,
debugLevel); // int debugLevel) {
false, // boolean realtime_mode,
master_CLT, // QuadCLT center_CLT,
quadCLTs, // QuadCLT [] scenes,
debugLevel); // int debugLevel) {
CuasMotion cuasMotion = cuasRanging.detectTargets(
uasLogReader, // UasLogReader uasLogReader,
batch_mode); // boolean batch_mode)
uasLogReader, // UasLogReader uasLogReader,
batch_mode); // boolean batch_mode)
if (cuasMotion == null) {
System.out.println("Failed target detection. Probably, radar mode was selected but target file does not exist (created with \"CUAS Combine\" command)");
} else {
......
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