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 = "";
......
package com.elphel.imagej.cuas.rt;
import java.util.Arrays;
import java.util.concurrent.atomic.AtomicInteger;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.cuas.rt.TemporalKernelGenerator.ConsolidationKernel;
import com.elphel.imagej.tileprocessor.ImageDtt;
import com.elphel.imagej.tileprocessor.TileNeibs;
import ij.ImagePlus;
public class CuasRTUtils {
final static double N_SIGMA = 4.0; // for LoG kernel size
final double [] kernel2d;
final private int kernel2d_rad;
// contains pixel indices, approximately in reverse bit order for multithreading efficiency
// for the center area only, does not need to care about borders
final private int [] indx_center_2d;
// remaining near-border pixel indices
final private int [] indx_margins_2d;
// all pixels indices
final private int [] indx_all_2d;
// same for 3x3 velocities
final private int kernel3d3_rad;
final private int [] indx_all_3d3;
final private int [] indx_center_3d3;
final private int [] indx_margins_3d3;
final private int pixel_decimate;
final private int velocity_decimate;
final private int velocity_radius;
final private double [] temporal_weights;
final private double [][][][][] kernel5d;
final private int kernel5d_rad;
final private int [] indx_all_5d;
final private int [] indx_center_5d;
final private int [] indx_margins_5d;
final ConsolidationKernel consolidationKernel;
// balance kernel - center = +1, sum of other weights = -1.
final private int width;
final private int height;
// final private int tl_offset;
public CuasRTUtils (
double psf_radius,
// int kernel2d_rad,
int kernel3d3_rad,
double[] temporal_weights,
int pixel_decimate,
int velocity_decimate,
int velocity_radius,
int width,
int height) {
this.width = width;
this.height = height;
// Setup 2D pre-filter kernel, based on LoG
kernel2d_rad = getRadiusLoGKernel(psf_radius,N_SIGMA);
kernel2d = getLoGKenel(psf_radius, kernel2d_rad);
int [][] indices = getIndices(
kernel2d_rad,
width,
height);
indx_all_2d = indices[0];
indx_center_2d = indices[1];
indx_margins_2d = indices[2];
// Setup 3D kernel to generate coarse velocities and input to the next pyramid level (center output)
this.kernel3d3_rad = kernel3d3_rad;
indices = getIndices(
kernel3d3_rad,
width,
height);
indx_all_3d3 = indices[0];
indx_center_3d3 = indices[1];
indx_margins_3d3 = indices[2];
// Setup 5D kernel for generating fine velocities at increased spatial resolution
this.temporal_weights = temporal_weights.clone();
normalize1d (this.temporal_weights);
this.pixel_decimate = pixel_decimate;
this.velocity_decimate = velocity_decimate;
this.velocity_radius = velocity_radius;
consolidationKernel = TemporalKernelGenerator.generateKernel(
temporal_weights, // double[] temporal_weights,
pixel_decimate, // int pixel_decimate,
velocity_decimate, // int velocity_decimate,
velocity_radius); // int velocity_radius)
kernel5d = consolidationKernel.getKernel();
kernel5d_rad = consolidationKernel.getSpatialRadius();
indices = getIndices(
kernel5d_rad,
width,
height);
indx_all_5d = indices[0];
indx_center_5d = indices[1];
indx_margins_5d = indices[2];
}
private void normalize1d(double [] data) {
double s = 0;
for (double d : data) s+= d;
for (int i = 0; i < data.length; i++) {
data[i] /= s;
}
}
private static int [][] getIndices(
int kernel_rad,
int width,
int height) {
int kernel_size = 2 * kernel_rad+1;
int [] i_all = new int [width*height];
int [] i_center = new int [(width-kernel_size)*(height-kernel_size)];
int [] i_margins = new int [i_all.length-i_center.length];
int n;
for (n = 0; (1 << n) < i_all.length; n++);
int shft = 32-n;
int iall = 0, icent=0, imarg=0;
for (int i = 0; i < 1 << shft; i++) {
int ri = Integer.reverse(i)>>shft;
if (ri < i_all.length) {
int y = i/width;
int x = i%width;
i_all[iall++] = i;
if ((x < kernel_rad) || (y < kernel_rad) || (x >= (width-kernel_rad)) || (y >= (height-kernel_rad))) {
i_margins[imarg++] = i;
} else {
i_center[icent++] = i;
}
}
}
int [][] indices = {i_all, i_center, i_margins};
return indices;
}
public void checkData(double [] data) {
if (data.length != (width * height)) {
throw new IllegalArgumentException("Data size = "+data.length+" does not match dimensions "+width+"*"+height+"="+(width*height));
}
}
public void checkData(double [][] data) {
if (data.length != (width * height)) {
throw new IllegalArgumentException("Data size = "+data.length+" does not match dimensions "+width+"*"+height+"="+(width*height));
}
}
public void checkKernel(double [] kernel, int rad) {
int kernel_size = 2 * rad + 1;
if (kernel.length != (kernel_size * kernel_size)) {
throw new IllegalArgumentException("Kernel length = "+kernel.length+" does not match kernel size (2*"+kernel2d_rad+"+1) * (2*"+kernel2d_rad+"+1) ="+(kernel_size * kernel_size));
}
}
/*
private void setupConvolve3d(
final double [] hist_weights, // same
final int decim_pix,
final int decim_vel,
final int rad_vel){
final int num_subpix = decim_pix * decim_pix;
final int num_layers = hist_weights.length;
final int num_src_vel = (2*this.kernel3d3_rad+1)*(2*this.kernel3d3_rad+1);
final int num_dst_vel = (2*rad_vel+1)*(2*rad_vel+1);
final double [][][][][] kern3d = new double [num_subpix][num_layers][][][];
for (int nsubpix_y = 0; nsubpix_y<decim_pix; nsubpix_y++) {
for (int nsubpix_x = 0; nsubpix_x<decim_pix; nsubpix_x++) {
int nsubpix = nsubpix_x + nsubpix_y * decim_pix;
for (int nlayer = 0; nlayer < num_layers; nlayer++) {
int pix_rng = 2 * nlayer; // from -pix_rng to +pix_rng+1 inclusive
int num_src_pix = 2 * pix_rng + 2;
kern3d[nsubpix][nlayer] = new double [num_src_pix * num_src_pix][num_src_vel][num_dst_vel];
// first calculate total energy, then how that apply this energy to specific velocity? how to mix mutual positions and
// source direction
// for nlayer > 0 use mutual position, for layer 0 use source direction
// Certain?
if (nlayer == 0) { // same layer - use source direction
} else { // one of the earliest layers - use mutual position
}
}
}
}
}
*/
public double [][][] convolve3d(
final double [][][] data, // outer index - historic layers, [0] - latest, second index - pixel, third - direction ([9])
double [][][] result_in){
final int vel_size = 2*velocity_radius + 1;
final int nlayers = data.length;
final double [][][] result = (result_in != null) ? result_in : new double [width*height][pixel_decimate*pixel_decimate][vel_size * vel_size];
final double [][][][][] kernel = consolidationKernel.getKernel();
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
final int [] coarse_vel_indx = consolidationKernel.getCoarseVIdx();
final int decimate_size = pixel_decimate*pixel_decimate;
for (int nhist = 0; nhist < nlayers; nhist++) {
final int fhist = nhist;
final int spat_rad = consolidationKernel.getSpatialRadius(); // may be later dependent on nhist for efficiency
// final int spat_dim = consolidationKernel.getSpatialDim();
// final int tl_offset = spat_rad * (width+1);
final double [][][][] kernel_hist = kernel[fhist];
final double [][] data_hist = data[fhist];
ai.set(0);
// processing center area, no need to care about margins
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nPix = ai.getAndIncrement(); nPix < indx_center_5d.length; nPix = ai.getAndIncrement()) {
int ipix_dst = indx_center_5d[nPix];
int target_x = ipix_dst % width;
int target_y = ipix_dst / width;
for (int sub_idx = 0; sub_idx < decimate_size; sub_idx++) {
for (int v_out_idx = 0; v_out_idx < kernel_hist.length; v_out_idx++) {
int src_coarse_v = coarse_vel_indx[v_out_idx];
if (src_coarse_v >=0) {
double accumulated_flux = 0.0;
// Outer spatial neighborhood limits defined by the pre-computed kernel
for (int dy = -spat_rad; dy <= spat_rad; dy++) {
int src_y = target_y + dy;
for (int dx = -spat_rad; dx <= spat_rad; dx++) {
int src_x = target_x + dx;
// 1. Boundary check for source image coordinates - not here
// 2. Locate the source macro-pixel index in your history array
int src_pixel_idx = src_y * width + src_x;
// 3. Pull historical energy from ONLY the mapped coarse velocity channel
double historical_value = data_hist[src_pixel_idx][src_coarse_v];
// 4. Multiply by the exact pre-calculated geometric weight for this sub-pixel layout
int k_dy = dy + spat_rad;
int k_dx = dx + spat_rad;
double weight = kernel_hist[v_out_idx][sub_idx][k_dy][k_dx];
accumulated_flux += historical_value * weight;
}
}
result[ipix_dst][sub_idx][v_out_idx] += accumulated_flux;
}
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
ai.set(0);
// processing center area, no need to care about margins
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nPix = ai.getAndIncrement(); nPix < indx_margins_5d.length; nPix = ai.getAndIncrement()) {
int ipix_dst = indx_margins_5d[nPix];
int target_x = ipix_dst % width;
int target_y = ipix_dst / width;
for (int sub_idx = 0; sub_idx < decimate_size; sub_idx++) {
for (int v_out_idx = 0; v_out_idx < kernel_hist.length; v_out_idx++) {
int src_coarse_v = coarse_vel_indx[v_out_idx];
if (src_coarse_v >=0) {
double accumulated_flux = 0.0;
// Outer spatial neighborhood limits defined by the pre-computed kernel
for (int dy = -spat_rad; dy <= spat_rad; dy++) {
int src_y = target_y + dy;
for (int dx = -spat_rad; dx <= spat_rad; dx++) {
int src_x = target_x + dx;
// 1. Boundary check for source image coordinates
if (src_x >= 0 && src_x < width && src_y >= 0 && src_y < height) {
// 2. Locate the source macro-pixel index in your history array
int src_pixel_idx = src_y * width + src_x;
// 3. Pull historical energy from ONLY the mapped coarse velocity channel
double historical_value = data_hist[src_pixel_idx][src_coarse_v];
// 4. Multiply by the exact pre-calculated geometric weight for this sub-pixel layout
int k_dy = dy + spat_rad;
int k_dx = dx + spat_rad;
double weight = kernel_hist[v_out_idx][sub_idx][k_dy][k_dx];
accumulated_flux += historical_value * weight;
}
}
}
result[ipix_dst][sub_idx][v_out_idx] += accumulated_flux;
}
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
}
return result;
}
public double[][] convolve3D3LReLU(
final double [] data,
final double [] data_prev,
double [][] result_in,
final double w_now, // normally 0.5, so center will propagate to the next pyramid levels
final double w_prev,// normally 0.5, so center will propagate to the next pyramid levels
final double alpha) {
final int kern_size_3d3 = 2 * kernel3d3_rad + 1;
final double [][] result = (result_in != null) ? result_in : new double [data.length][kern_size_3d3*kern_size_3d3];
checkData(data);
checkData(data_prev);
checkData(result);
final int tl_offset = kernel3d3_rad * (width+1);
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
// processing center area, no need to care about margins
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nPix = ai.getAndIncrement(); nPix < indx_center_3d3.length; nPix = ai.getAndIncrement()) {
int ipix_dst = indx_center_3d3[nPix];
int ipix_prev = ipix_dst- tl_offset;
int ikern = kern_size_3d3*kern_size_3d3;
double v0 = w_now * data[ipix_dst]; // will ve added to all velocity outputs
for (int iy = 0; iy < kern_size_3d3; iy++) {
for (int ix = 0; ix < kern_size_3d3; ix++) {
double v = data[ipix_prev++] * w_prev * v0;
result[ipix_dst][--ikern] = Math.max(v, alpha * v);
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
// marginal area, check indices
ai.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
TileNeibs tn = new TileNeibs(width, height);
for (int nPix = ai.getAndIncrement(); nPix < indx_margins_3d3.length; nPix = ai.getAndIncrement()) {
int ipix_dst = indx_margins_3d3[nPix];
int ikern = kern_size_3d3*kern_size_3d3;
double v0 = w_now * data[ipix_dst]; // will ve added to all velocity outputs
for (int iy = 0; iy < kern_size_3d3; iy++) {
int dy = iy-kernel3d3_rad;
for (int ix = 0; ix < kern_size_3d3; ix++) {
int dx = ix-kernel3d3_rad;
int ipix_prev = tn.getNeibIndex(ipix_dst, dx, dy);
double v = v0;
if (ipix_prev >= 0) {
v += data[ipix_prev] * w_prev;
}
result[ipix_dst][--ikern] = Math.max(v, alpha * v);
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
return result;
}
public double[] convolve2DLReLU(
final double [] data,
final double [] kernel, // square
double [] result_in,
final double alpha) {
// assuming symmetrical kernel, not inverting indices
final double [] result = (result_in != null) ? result_in : new double [data.length];
// verify dimensions;
checkData(data);
checkData(result);
checkKernel(kernel, kernel2d_rad);
final int tl_offset = kernel2d_rad * (width+1);
final int kernel_size = 2 * kernel2d_rad + 1;
final int row_inc = width- kernel_size;
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
// processing center area, no need to care about margins
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nPix = ai.getAndIncrement(); nPix < indx_center_2d.length; nPix = ai.getAndIncrement()) {
int ipix_dst = indx_center_2d[nPix];
int ipix_src = ipix_dst- tl_offset;
int ikern = 0;
double v = 0;
for (int iy = 0; iy < kernel_size; iy++) {
for (int ix = 0; ix < kernel_size; ix++) {
v+=data[ipix_src++]*kernel[ikern++];
}
ipix_src+=row_inc;
}
result[ipix_dst] = Math.max(v, alpha * v);
}
}
};
}
ImageDtt.startAndJoin(threads);
// marginal area, check indices
ai.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
TileNeibs tn = new TileNeibs(width, height);
for (int nPix = ai.getAndIncrement(); nPix < indx_margins_2d.length; nPix = ai.getAndIncrement()) {
int ipix_dst = indx_margins_2d[nPix];
double v = 0;
for (int iy = 0; iy < kernel_size; iy++) {
int dy = iy-kernel2d_rad;
for (int ix = 0; ix < kernel_size; ix++) {
int dx = ix-kernel2d_rad;
int ipix_src = tn.getNeibIndex(ipix_dst, dx, dy);
if (ipix_src >= 0) {
v+=data[ipix_src]*kernel[ix + iy*kernel_size];
}
}
}
result[ipix_dst] = Math.max(v, alpha * v);
}
}
};
}
ImageDtt.startAndJoin(threads);
return result;
}
/**
* Calculates the integer radius for the LoG kernel based on the PSF radius
* and the desired truncation boundary (n_sigma).
*/
public static int getRadiusLoGKernel(double psf_radius, double n_sigma) {
double sigma = psf_radius / Math.sqrt(2.0);
return (int) Math.ceil(sigma * n_sigma);
}
/**
* Generates a 1D flattened flux-normalized 2D Laplacian of Gaussian kernel.
* The sum of positive elements is exactly 1.0, and the sum of negative elements is exactly -1.0.
*/
public static double[] getLoGKenel(double psf_radius, int kernel_radius) {
double sigma = psf_radius / Math.sqrt(2.0);
double sigma2 = sigma * sigma;
int width = 2 * kernel_radius + 1;
double[] kernel = new double[width * width];
double sumPos = 0.0;
double sumNeg = 0.0;
// 1. Generate raw inverted LoG shape and accumulate sums
for (int y = -kernel_radius; y <= kernel_radius; y++) {
for (int x = -kernel_radius; x <= kernel_radius; x++) {
double r2 = x * x + y * y;
// Positive center, negative surround for bright blob detection
double raw = (1.0 - (r2 / (2.0 * sigma2))) * Math.exp(-r2 / (2.0 * sigma2));
int idx = (y + kernel_radius) * width + (x + kernel_radius);
kernel[idx] = raw;
if (raw > 0) {
sumPos += raw;
} else {
sumNeg += raw;
}
}
}
// 2. Apply strict flux-normalization
double absSumNeg = Math.abs(sumNeg);
for (int i = 0; i < kernel.length; i++) {
if (kernel[i] > 0) {
kernel[i] /= sumPos;
} else if (kernel[i] < 0) {
kernel[i] /= absSumNeg;
}
}
return kernel;
}
// debug methods
public ImagePlus showKernel2d(String title) {
if (title==null) {
title = "kernel2d";
}
ImagePlus imp_kernel2d = ShowDoubleFloatArrays.makeArrays(
kernel2d,
2 * kernel2d_rad + 1,
2 * kernel2d_rad + 1,
title);
return imp_kernel2d;
}
public double [] render3d3Flat(double [][] coarse_vel) {
int width41 = 4 * width+1;
int height41 = 4 * height+1;
double [] render_flat = new double [width41 * height41];
Arrays.fill(render_flat, Double.NaN);
for (int npix=0; npix < coarse_vel.length; npix++) if (coarse_vel[npix] !=null){
int py = npix/width;
int px = npix%width;
int pix4 = (px * 4 + 1) + (py * 4 + 1) * width41;
int cvi = 0;
for (int dy = 0; dy < 3; dy++) {
for (int dx = 0; dx < 3; dx++) {
render_flat[pix4++] = coarse_vel[npix][cvi++];
}
pix4 += width41 - 3; // next row and 3 pixels back
}
}
return render_flat;
}
public double [][] render3d3Sliced(double [][] coarse_vel) {
double [][] render_sliced = new double [9][width * height];
for (double [] slice:render_sliced) {
Arrays.fill(slice, Double.NaN);
}
for (int npix=0; npix < coarse_vel.length; npix++) if (coarse_vel[npix] !=null){
for (int s = 0; s < coarse_vel[npix].length; s++) {
render_sliced[s][npix] = coarse_vel[npix][s];
}
}
return render_sliced;
}
public String [] get9Arrows() {
String [] arrows= {
"\u2196", // Left-Up
"\u2191", // Up
"\u2197", // Right-Up
"\u2190", // Left
"\u25E6", // Bullet
"\u2192", // Right
"\u2199", // Left-Down
"\u2198", // Right-Down
"\u2193"}; // Down
return arrows;
}
public ImagePlus showKernel5d(String title) {
// consolidationKernel
// [num_subs][num_temps][pixels]
if (title == null) {
title = "kernel5d";
}
int spat_dim = consolidationKernel.getSpatialDim();
int coarse_dim = 3;
int coarse_num = coarse_dim*coarse_dim;
int render_dim = spat_dim * (coarse_dim+1);
int num_hist = kernel5d.length;
int num_vout = kernel5d[0].length;
int vout_dim = (int) Math.sqrt(num_vout);
int vout_rad = (vout_dim-1)/2;
int num_sub = kernel5d[0][0].length;
int sub_dim = (int) Math.sqrt(num_sub);
double [][][] rendered = new double [num_sub*num_hist][num_vout][(render_dim + 1) * (render_dim + 1)];
String [] top_titles = new String[num_sub*num_hist];
String [] slice_titles = new String[num_vout];
for (double [][] slice_hist:rendered) {
for (double [] slice:slice_hist) {
Arrays.fill(slice, Double.NaN);
}
}
for (int nvout = 0; nvout < num_vout; nvout++) {
int vout_v = nvout / vout_dim - vout_rad ;
int vout_h = nvout % vout_dim - vout_rad;
slice_titles[nvout] = nvout+ " ("+((vout_v >0) ?"+":"")+vout_v+":"+((vout_h >0) ?"+":"")+vout_h+")";
}
for (int nsub = 0; nsub < num_sub; nsub++) {
int sub_v = nsub / sub_dim;
int sub_h = nsub % sub_dim;
for (int nhist = 0; nhist < num_hist; nhist++) {
int num_tslice = nsub * num_hist + nhist;
top_titles[num_tslice] = sub_v+":"+sub_h+" - " + nhist;
for (int nvout = 0; nvout < num_vout; nvout++) {
int coarse_index = consolidationKernel.getCoarseVIdx()[nvout];
for (int py = 0; py < spat_dim; py++) {
int py0_rend = py * (coarse_dim + 1) +1; // ) * render_dim;
for (int px = 0; px < spat_dim; px++) {
int px0_rend = px * (coarse_dim + 1) +1; //
for (int coarse_i = 0; coarse_i < coarse_num; coarse_i++) {
double w = 0; // maybe opposite - use NaN here?
int coarse_v = coarse_i / coarse_dim;
int coarse_h = coarse_i % coarse_dim;
if (coarse_i == coarse_index) {
w = kernel5d[nhist][nvout][nsub][py][px]; // unused will be 0, not NaN;
}
int pix_rend = (py0_rend + coarse_v) * render_dim + (px0_rend + coarse_h);
rendered[num_tslice][nvout][pix_rend] = w;
}
}
}
}
}
}
ImagePlus imp = ShowDoubleFloatArrays.showArraysHyperstack(
rendered, // double[][][] pixels,
width, // int width,
title, // String title, "time_derivs-rt"+diff_time_rt+"-rxy"+diff_time_rxy,
slice_titles, // String [] titles, // all slices*frames titles or just slice titles or null
top_titles, // CuasMotionLMA.LMA_TITLES, // String [] frame_titles, // frame titles or null
false); // boolean show)
return imp;
}
// Display convolution result [vout][2*width*2*height]. Or hyperstack with x and y sliders?
/**
* Display a sequence of velocities per subpixel (or pixel if no decimation) layers as
* a hyperstack. The bottom slider (outer index) selects the output velocity, the top slider
* (and a scroll wheel) - scenes (or averaged scenes named by the last integrated)
* @param data [nscene][npix][nsubpix][nvelocity]
* @param scene_titles scene names or null
* @param title image title or null
* @return
*/
public ImagePlus showConvKernel5d(
double [][][][] data,
String [] scene_titles,
String title) {
int num_scenes = data.length;
int num_vout = kernel5d[0].length;
int vout_dim = (int) Math.sqrt(num_vout);
int vout_rad = (vout_dim-1)/2;
int num_sub = kernel5d[0][0].length;
int sub_dim = (int) Math.sqrt(num_sub);
int full_with = width * sub_dim;
int full_height = height * sub_dim;
if (title==null) {
title = "OutputConvKernel5D";
}
if (scene_titles == null) {
scene_titles = new String[num_scenes];
for (int i = 0; i < scene_titles.length; i++) {
scene_titles[i]= String.format("scene-%03d", i);
}
}
String [] v_titles = new String[num_vout];
for (int nvout = 0; nvout < num_vout; nvout++) {
int vout_v = nvout / vout_dim - vout_rad ;
int vout_h = nvout % vout_dim - vout_rad;
v_titles[nvout] = nvout+ " ("+((vout_v >0) ?"+":"")+vout_v+":"+((vout_h >0) ?"+":"")+vout_h+")";
}
double [][][] rendered = new double [num_vout][num_scenes][full_with*full_height];
for (double [][] d2 : rendered) for (double [] d1 : d2) Arrays.fill(d1, Double.NaN);
ImagePlus imp = ShowDoubleFloatArrays.showArraysHyperstack(
rendered, // double[][][] pixels,
width, // int width,
title, // String title, "time_derivs-rt"+diff_time_rt+"-rxy"+diff_time_rxy,
scene_titles, // slice_titles, // String [] titles, // all slices*frames titles or just slice titles or null
v_titles, // CuasMotionLMA.LMA_TITLES, // String [] frame_titles, // frame titles or null
false); // boolean show)
return imp;
}
}
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