Skip to content

I
imagej-elphel
Commit 692610be authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

Debugging, before refactoring

parent 2c6707fb
Show whitespace changes
Inline Side-by-side
Showing with 400 additions and 35 deletions
src/main/java/com/elphel/imagej/cuas/CuasMotion.java
View file @ 692610be
...@@ -1912,6 +1912,7 @@ public class CuasMotion { ...@@ -1912,6 +1912,7 @@ public class CuasMotion {
return vf; return vf;
} }
@Deprecated
public static double [][][] getTargetsFromHyper(String path){ public static double [][][] getTargetsFromHyper(String path){
int [] wh = new int [2]; int [] wh = new int [2];
String [][] pvf_top_titles = new String[1][]; String [][] pvf_top_titles = new String[1][];
...@@ -2016,7 +2017,8 @@ public class CuasMotion { ...@@ -2016,7 +2017,8 @@ public class CuasMotion {
int num_tiles = targets_file[0][0].length; int num_tiles = targets_file[0][0].length;
double [][][] target_sequence = new double [num_seq][num_tiles][]; double [][][] target_sequence = new double [num_seq][num_tiles][];
for (int nseq=0; nseq < num_seq; nseq++) { for (int nseq=0; nseq < num_seq; nseq++) {
for (int ntile = 0; ntile < num_tiles; ntile++) if (!Double.isNaN(targets_file[0][nseq][ntile])){ for (int ntile = 0; ntile < num_tiles; ntile++) if (!Double.isNaN(targets_file[CuasMotionLMA.RSLT_X][nseq][ntile]) ||
!Double.isNaN(targets_file[CuasMotionLMA.RSLT_FL_PX][nseq][ntile])){
double [] v = new double[num_fields_augmented]; // cut to originally calculated fields double [] v = new double[num_fields_augmented]; // cut to originally calculated fields
for (int i = 0; i < num_fields; i++) { for (int i = 0; i < num_fields; i++) {
v[i] = targets_file[i][nseq][ntile]; v[i] = targets_file[i][nseq][ntile];
...@@ -4408,6 +4410,7 @@ public class CuasMotion { ...@@ -4408,6 +4410,7 @@ public class CuasMotion {
double target_vx, double target_vx,
double target_vy) { double target_vy) {
double ifov = clt_parameters.imp.cuas_ifov; // 0.05; // degree per pixel double ifov = clt_parameters.imp.cuas_ifov; // 0.05; // degree per pixel
int px0 = clt_parameters.imp.cuas_px0; // 283; // pixel with known azimuth int px0 = clt_parameters.imp.cuas_px0; // 283; // pixel with known azimuth
int py0 = clt_parameters.imp.cuas_py0; // 386; // pixel with known elevation int py0 = clt_parameters.imp.cuas_py0; // 386; // pixel with known elevation
double az0 = clt_parameters.imp.cuas_az0; // 201.5; // degrees for cuas_px0; double az0 = clt_parameters.imp.cuas_az0; // 201.5; // degrees for cuas_px0;
......
src/main/java/com/elphel/imagej/cuas/CuasMotionLMA.java
View file @ 692610be
...@@ -87,19 +87,22 @@ public class CuasMotionLMA { ...@@ -87,19 +87,22 @@ public class CuasMotionLMA {
public static final int RSLT_DISP_DIFF= 45; public static final int RSLT_DISP_DIFF= 45;
public static final int RSLT_DISP_STR = 46; public static final int RSLT_DISP_STR = 46;
public static final int RSLT_RANGE = 47; public static final int RSLT_RANGE = 47;
public static final int RSLT_GLOBAL = 48; public static final int RSLT_GLOBAL = 48; // local target ID (1-based)
public static final int RSLT_GLENGTH = 49; public static final int RSLT_GLENGTH = 49; // length (symmetrical around this one) for which disparity and range are provided
public static final int RSLT_GDISPARITY= 50; public static final int RSLT_GDISPARITY= 50;
public static final int RSLT_GDISP_DIFF= 51; public static final int RSLT_GDISP_DIFF= 51;
public static final int RSLT_GDISP_STR = 52; public static final int RSLT_GDISP_STR = 52;
public static final int RSLT_GRANGE = 53; public static final int RSLT_GRANGE = 53; // global range - calculated for long sequences (and its fractions)
public static final int RSLT_FL_PX = 54; // flight log px public static final int RSLT_FL_PX = 54; // flight log px
public static final int RSLT_FL_PY = 55; // flight log py public static final int RSLT_FL_PY = 55; // flight log py
public static final int RSLT_FL_DISP = 56; // flight log true disparity public static final int RSLT_FL_DISP = 56; // flight log true disparity
public static final int RSLT_FL_RANGE = 57; // flight log range (meters) public static final int RSLT_FL_RANGE = 57; // flight log range (meters)
public static final int RSLT_INFINITY = 58; // disparity at infinity used for range calculation public static final int RSLT_INFINITY = 58; // disparity at infinity used for range calculation
public static final int RSLT_TARGET_ID = 59; // unique target id for the whole sequence of segments. 1 is reserved for the UAS public static final int RSLT_TARGET_ID = 59; // unique target id for the whole sequence of segments. 1 is reserved for the UAS
public static final int RSLT_LEN = RSLT_TARGET_ID + 1; public static final int RSLT_VEL_AXIAL = 60; // axial velocity (positive - away), range derivative (m/s), calculated from the difference to the previous/next series range
public static final int RSLT_VEL_LATERAL = 61; // lateral velocity ( calculated from angular and RSLT_GRANGE
public static final int RSLT_RANGE_LIN = 62; // range linear interpolated using previous/next series if the same target is present there
public static final int RSLT_LEN = RSLT_RANGE_LIN + 1;
public static final String [] LMA_TITLES = public static final String [] LMA_TITLES =
{"X-OFFS","Y-OFFS", "AMPLITUDE", "RADIUS","RAD_POS", "OVERSHOOT","OFFSET","RMSE","RMSE/A","MAX2A","ITERATIONS", {"X-OFFS","Y-OFFS", "AMPLITUDE", "RADIUS","RAD_POS", "OVERSHOOT","OFFSET","RMSE","RMSE/A","MAX2A","ITERATIONS",
...@@ -118,7 +121,7 @@ public class CuasMotionLMA { ...@@ -118,7 +121,7 @@ public class CuasMotionLMA {
"Disparity","Disparity-Diff","Strength","Range","Global-index", "Disparity","Disparity-Diff","Strength","Range","Global-index",
"segment-length","segment-disparity","segment_disp-diff", "segment-strength", "segment-range", "segment-length","segment-disparity","segment_disp-diff", "segment-strength", "segment-range",
"FLOG-px","FLOG-pY","FLOG-DISP","FLOG-range","infinity", "FLOG-px","FLOG-pY","FLOG-DISP","FLOG-range","infinity",
"GTarget-ID"}; "GTarget-ID","Range-derivative"};
public static final String EXTRA_SLICE_DISCARD_ON_LOAD = "Targets"; public static final String EXTRA_SLICE_DISCARD_ON_LOAD = "Targets";
public static final int FAIL_NONE = 0; public static final int FAIL_NONE = 0;
public static final int FAIL_MOTION = 1; // motion strength/fraction too low public static final int FAIL_MOTION = 1; // motion strength/fraction too low
......
src/main/java/com/elphel/imagej/cuas/CuasMultiSeries.java
View file @ 692610be
...@@ -5,11 +5,13 @@ import java.io.File; ...@@ -5,11 +5,13 @@ import java.io.File;
import java.nio.file.Path; import java.nio.file.Path;
import java.util.ArrayList; import java.util.ArrayList;
import java.util.Arrays; import java.util.Arrays;
import java.util.Collections;
import java.util.Comparator; import java.util.Comparator;
import java.util.concurrent.atomic.AtomicInteger; import java.util.concurrent.atomic.AtomicInteger;
import com.elphel.imagej.cameras.CLTParameters; import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.gpu.GPUTileProcessor; import com.elphel.imagej.gpu.GPUTileProcessor;
import com.elphel.imagej.tileprocessor.GeometryCorrection;
import com.elphel.imagej.tileprocessor.ImageDtt; import com.elphel.imagej.tileprocessor.ImageDtt;
import com.elphel.imagej.tileprocessor.QuadCLT; import com.elphel.imagej.tileprocessor.QuadCLT;
...@@ -20,17 +22,27 @@ import ij.io.FileSaver; ...@@ -20,17 +22,27 @@ import ij.io.FileSaver;
public class CuasMultiSeries { public class CuasMultiSeries {
public static final int TARGET_INDEX_NONE = 0; public static final int TARGET_INDEX_NONE = 0;
public static final int TARGET_INDEX_UAS = 1; public static final int TARGET_INDEX_UAS = 1;
public static final int AVG_RANGE = 0;
public static final int AVG_TIMESTASMP = 1;
public static final int AVG_KEYFRAME = 2;
public static final int AVG_DISPARITY = 3;
public static final int AVG_VELOCIY = 4;
public static final int AVG_LENGTH = AVG_VELOCIY+1;
final CLTParameters clt_parameters; final CLTParameters clt_parameters;
final File [] model_dirs; final File [] model_dirs;
final String [] model_names; final String [] model_names;
final double [][][][] targets_multi_series; // [nser][nseq][ntile][nfield] final double [][][][] targets_multi_series; // [nser][nseq][ntile][nfield]
final int [][][][] linked_targets_multi; // [nser][ntarg][nseq]{ntile,0} // was {ntile, alt} final int [][][][] linked_targets_multi; // [nser][ntarg][nseq]{ntile,0} // was {ntile, alt}
final int [][] target_map; // [nser][ntarg] global target index, 1 for UAS final int [][] target_map; // [nser][ntarg] global target index, 1 for UAS
int [][][] target_rmap; // [target_id-1][num_pair]{nser,nltarget}
final int [][] local_map; // [nser][ntarg] final int [][] local_map; // [nser][ntarg]
final int [][][] local_imap; // [nser][ntarg_grp]{ntarg1, ...} final int [][][] local_imap; // [nser][ntarg_grp]{ntarg1, ...}
final int [][][] targets_start_end; //[nser][ntarg]{start_nseq,end_nseq} final int [][][] targets_start_end; //[nser][ntarg]{start_nseq,end_nseq}
final int [][] uas_tiles; final int [][] uas_tiles; // [nser][nseq]
final String [][] scene_titles; double [][][] avg_range_ts; // [target_id][nser]{average_range, average_ts}
final String [][] scene_titles; // keyframe scene titles/timestamps
int global_targets = 0; int global_targets = 0;
QuadCLT master_CLT = null; QuadCLT master_CLT = null;
// int debugLevel = 0; // int debugLevel = 0;
...@@ -42,6 +54,7 @@ public class CuasMultiSeries { ...@@ -42,6 +54,7 @@ public class CuasMultiSeries {
String version) { String version) {
// System.out.println ("CuasMultiSeries(): path="+path); // System.out.println ("CuasMultiSeries(): path="+path);
this.clt_parameters = clt_parameters; this.clt_parameters = clt_parameters;
double infinity = clt_parameters.imp.cuas_infinity;
File [] scene_dirs = (new File(path)).listFiles(); // may contain non-directories, will be filtered by filterScenes File [] scene_dirs = (new File(path)).listFiles(); // may contain non-directories, will be filtered by filterScenes
Arrays.sort(scene_dirs); Arrays.sort(scene_dirs);
int num_series = scene_dirs.length; int num_series = scene_dirs.length;
...@@ -66,6 +79,7 @@ public class CuasMultiSeries { ...@@ -66,6 +79,7 @@ public class CuasMultiSeries {
null, // String [][] pvf_top_titles, null, // String [][] pvf_top_titles,
scen_titles_w, // String [][] pvf_titles, scen_titles_w, // String [][] pvf_titles,
target_file.toString()); target_file.toString());
scene_titles[nser] = scen_titles_w[0]; scene_titles[nser] = scen_titles_w[0];
linked_targets_multi[nser] = CuasMotion.getLinkedTargets(targets_multi_series[nser]); // [nser][ntarg(zero-based]{ntile,0} linked_targets_multi[nser] = CuasMotion.getLinkedTargets(targets_multi_series[nser]); // [nser][ntarg(zero-based]{ntile,0}
target_map[nser] = new int [linked_targets_multi[nser].length]; target_map[nser] = new int [linked_targets_multi[nser].length];
...@@ -91,6 +105,10 @@ public class CuasMultiSeries { ...@@ -91,6 +105,10 @@ public class CuasMultiSeries {
targets_start_end[nser][ntarg][1] = i-1; targets_start_end[nser][ntarg][1] = i-1;
} }
} }
recalculateAllRanges(
targets_multi_series, // final double [][][][] targets_multi_series, // [nser][nseq][ntile][nfield]
infinity, // final double new_infinity,
master_CLT.getGeometryCorrection()); // final GeometryCorrection gc) {
printAssignmentStats(); printAssignmentStats();
// show missing here // show missing here
return; return;
...@@ -134,6 +152,14 @@ public class CuasMultiSeries { ...@@ -134,6 +152,14 @@ public class CuasMultiSeries {
printAssignmentStats(); printAssignmentStats();
printAssignments(); printAssignments();
printReverseAssignments();
double min_disparity_range = 0.02;
double min_disparity_velocity = 0.08;
avg_range_ts = getAverageRangeTimestamp(
min_disparity_range, // double min_disparity_range){
min_disparity_velocity); // double min_disparit_velocity);
printAverageRanges(avg_range_ts);
printAverageVsUASRanges(avg_range_ts);
saveUpdatedTargets(); saveUpdatedTargets();
return; return;
} }
...@@ -160,7 +186,7 @@ public class CuasMultiSeries { ...@@ -160,7 +186,7 @@ public class CuasMultiSeries {
} }
public void printLocalAssignments() { public void printLocalAssignments() {
System.out.println("void printLocalAssignments(): local target group assignments"); System.out.println("printLocalAssignments(): local target group assignments");
for (int nseg = 0; nseg < local_map.length; nseg++) { for (int nseg = 0; nseg < local_map.length; nseg++) {
System.out.print(nseg+ " ("+model_names[nseg]+") ("+local_map[nseg].length+"): "); System.out.print(nseg+ " ("+model_names[nseg]+") ("+local_map[nseg].length+"): ");
for (int i = 0; i < local_map[nseg].length; i++) { for (int i = 0; i < local_map[nseg].length; i++) {
...@@ -172,7 +198,7 @@ public class CuasMultiSeries { ...@@ -172,7 +198,7 @@ public class CuasMultiSeries {
} }
public void printAssignments() { public void printAssignments() {
System.out.println("void printAssignments(): local-to-global target assignments"); System.out.println("printAssignments(): local-to-global target assignments");
for (int nseg = 0; nseg < target_map.length; nseg++) { for (int nseg = 0; nseg < target_map.length; nseg++) {
System.out.print(nseg+ " ("+model_names[nseg]+") ("+target_map[nseg].length+"): "); System.out.print(nseg+ " ("+model_names[nseg]+") ("+target_map[nseg].length+"): ");
for (int i = 0; i < target_map[nseg].length; i++) { for (int i = 0; i < target_map[nseg].length; i++) {
...@@ -183,6 +209,48 @@ public class CuasMultiSeries { ...@@ -183,6 +209,48 @@ public class CuasMultiSeries {
return; return;
} }
public void printReverseAssignments() {
System.out.println("printReverseAssignments(): global-to-local target assignments");
for (int ngtarg = 0; ngtarg < target_rmap.length; ngtarg++) {
int target_id = ngtarg+1;
System.out.print(target_id+" ("+target_rmap[ngtarg].length+"): ");
for (int i = 0; i < target_rmap[ngtarg].length; i++) {
System.out.print(target_rmap[ngtarg][i][0]+":"+target_rmap[ngtarg][i][1]+" ");
}
System.out.println();
}
return;
}
public void printAverageRanges(double [][][] avg_range_ts) {
System.out.println("printAverageRanges(): global targets ranges in different series");
for (int ngtarg = 0; ngtarg < avg_range_ts.length; ngtarg++) {
int target_id = ngtarg +1;
System.out.print(target_id+": ");
for (int nser = 0; nser < avg_range_ts[ngtarg].length; nser++) if (avg_range_ts[ngtarg][nser] != null) {
System.out.print(nser+":"+avg_range_ts[ngtarg][nser][AVG_RANGE]+"("+avg_range_ts[ngtarg][nser][AVG_VELOCIY]+
")["+avg_range_ts[ngtarg][nser][AVG_DISPARITY]+"] ");
}
System.out.println();
}
return;
}
public void printAverageVsUASRanges(double [][][] avg_range_ts) {
System.out.println("printAverageVsUASRanges(): Compare average UAS range with flight log");
int ngtarg = 0;
System.out.println("scene,range,fl_range,axial_velocity,disparity");
for (int nser = 0; nser < avg_range_ts[ngtarg].length; nser++) if (avg_range_ts[ngtarg][nser] != null) {
int mid_seq = (int) +avg_range_ts[ngtarg][nser][2];
int fl_tile = uas_tiles[nser][mid_seq];
double fl_range = targets_multi_series[nser][mid_seq][fl_tile][CuasMotionLMA.RSLT_FL_RANGE];
System.out.println(nser+", "+avg_range_ts[ngtarg][nser][AVG_RANGE]+", "+fl_range+", "+avg_range_ts[ngtarg][nser][AVG_VELOCIY]+
", "+avg_range_ts[ngtarg][nser][AVG_DISPARITY]);
}
return;
}
/** /**
* Get total number of local targets and number of assigned ones * Get total number of local targets and number of assigned ones
* Sets global_targets to the total number of global targets * Sets global_targets to the total number of global targets
...@@ -480,7 +548,7 @@ public class CuasMultiSeries { ...@@ -480,7 +548,7 @@ public class CuasMultiSeries {
int debugLevel) { int debugLevel) {
int debug_min = -2; // if >= then print int debug_min = -2; // if >= then print
boolean debug = debugLevel>= debug_min; boolean debug = debugLevel>= debug_min;
final GeometryCorrection gc = master_CLT.getGeometryCorrection();
final int tmtch_ends = clt_parameters.imp.cuas_tmtch_ends; final int tmtch_ends = clt_parameters.imp.cuas_tmtch_ends;
// final int tmtch_gaps = clt_parameters.imp.cuas_tmtch_gaps; // final int tmtch_gaps = clt_parameters.imp.cuas_tmtch_gaps;
final double tmtch_apix = clt_parameters.imp.cuas_tmtch_apix; final double tmtch_apix = clt_parameters.imp.cuas_tmtch_apix;
...@@ -549,10 +617,14 @@ public class CuasMultiSeries { ...@@ -549,10 +617,14 @@ public class CuasMultiSeries {
// TODO: average ranging if there are several local targets corresponding to the same object // TODO: average ranging if there are several local targets corresponding to the same object
double [] middle_target0 = getMiddleTarget( double [] middle_target0 = getMiddleTarget(
prev_ser, // int nser, prev_ser, // int nser,
ntarg0); // int local_target) ntarg0, // int local_target)
null); // int [] seq_tile) {
double [] middle_target1 = getMiddleTarget( double [] middle_target1 = getMiddleTarget(
nSer, // int nser, nSer, // int nser,
ntarg1); // int local_target) ntarg1, // int local_target)
null); // int [] seq_tile) {
// only check matches if disparity is sufficient // only check matches if disparity is sufficient
boolean range_match = true; boolean range_match = true;
if ((middle_target0 != null) && (middle_target1 != null) && // do not have measured disparity at all if ((middle_target0 != null) && (middle_target1 != null) && // do not have measured disparity at all
...@@ -563,6 +635,7 @@ public class CuasMultiSeries { ...@@ -563,6 +635,7 @@ public class CuasMultiSeries {
double avelocity = (range1 - range0)/dt; double avelocity = (range1 - range0)/dt;
double lvelocity = getLateralVelocity ( double lvelocity = getLateralVelocity (
clt_parameters, //CLTParameters clt_parameters, clt_parameters, //CLTParameters clt_parameters,
gc, // GeometryCorrection gc,
middle_target0, // double [] target0, // should contain RSLT_VX, RSLT_VX, RSLT_GRANGE, middle_target0, // double [] target0, // should contain RSLT_VX, RSLT_VX, RSLT_GRANGE,
middle_target1, // double [] target1, // should contain RSLT_VX, RSLT_VX, RSLT_GRANGE, middle_target1, // double [] target1, // should contain RSLT_VX, RSLT_VX, RSLT_GRANGE,
fps); // double fps){ // velocity_scale times fps fps); // double fps){ // velocity_scale times fps
...@@ -673,15 +746,162 @@ public class CuasMultiSeries { ...@@ -673,15 +746,162 @@ public class CuasMultiSeries {
} }
} }
// print more debug // print more debug
target_rmap = new int [next_global_target_id - 1][][];
ai.set(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nGtarg = ai.getAndIncrement(); nGtarg < target_rmap.length; nGtarg = ai.getAndIncrement()) {
int target_id = nGtarg+1;
ArrayList<Point> local_list = new ArrayList<Point>(); // .x - nser, .y - ntarg(0-based)
for (int nser = 0; nser < target_map.length; nser++) {
for (int ntarg = 0; ntarg < target_map[nser].length; ntarg++) {
if (target_map[nser][ntarg] == target_id) {
local_list.add(new Point(nser,ntarg));
}
}
}
// sort list by the timestamps
Collections.sort(local_list, new Comparator<Point>() {
@Override
public int compare(Point lhs, Point rhs) {
return Double.compare(getLocalTargetStartTimestamp(lhs.x, lhs.y), getLocalTargetStartTimestamp(rhs.x, rhs.y));
}
});
target_rmap[nGtarg]= new int [local_list.size()][];
for (int i = 0; i <target_rmap[nGtarg].length; i++) {
Point p = local_list.get(i);
target_rmap[nGtarg][i] = new int [] {p.x, p.y};
}
}
}
};
}
ImageDtt.startAndJoin(threads);
return next_global_target_id; return next_global_target_id;
} }
public double [][][] getAverageRangeTimestamp(
double min_disparit_range,
double min_disparit_velocity){
final double [][][] avg_rts = new double [target_rmap.length][target_map.length][];
final double infinity = clt_parameters.imp.cuas_infinity;
final GeometryCorrection gc = master_CLT.getGeometryCorrection();
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
int [] seq_tile = new int [2];
for (int nGtarg = ai.getAndIncrement(); nGtarg < target_rmap.length; nGtarg = ai.getAndIncrement()) {
int [][] rmap = target_rmap[nGtarg];
// int num_occ = target_rmap[nGtarg].length; // number of local targets with the same target_id
int target_id = nGtarg+1;
int indx_first = 0;
int indx_next =0;
for (; indx_first < rmap.length; indx_first = indx_next) {
int nser = rmap[indx_first][0];
double ts_start = getKeyTimeStamp(
nser, // int nser,
0); // int nseq)
int last_in_ser = targets_multi_series[nser].length - 1;
double ts_end = getKeyTimeStamp(
nser, // int nser,
last_in_ser); // int nseq)
indx_next = indx_first+1;
for (; (indx_next < rmap.length) && (rmap[indx_next][0] == nser);indx_next++);
int indx_last = indx_next -1;
// calculate average disparity if multiple
double sumw = 0, sumw_disp = 0, sumw_ts = 0;
for (int indx = indx_first; indx <= indx_last; indx++) {
int ltarg = rmap[indx][1]; // OOB
double [] middle_target = getMiddleTarget(
nser, // int nser,
ltarg, // int local_target,
seq_tile); // int [] seq_tile)
if (middle_target != null) {
double w = middle_target[CuasMotionLMA.RSLT_GLENGTH];
double disparity = middle_target[CuasMotionLMA.RSLT_GDISPARITY];
double ts = getKeyTimeStamp(
nser, // int nser,
seq_tile[0]); // int nseq)
sumw += w;
sumw_disp += w * disparity;
sumw_ts += w * ts;
}
}
if (sumw > 0) { // if 0, keep undefined (null)
double avg_disp = sumw_disp/sumw - infinity;
double avg_ts = sumw_ts/sumw;
double avg_range = (avg_disp >= min_disparit_range) ? gc.getZFromDisparity(avg_disp):Double.POSITIVE_INFINITY;
int avg_key_frame = (int) Math.round(last_in_ser * (avg_ts - ts_start)/(ts_end - ts_start));
avg_rts[nGtarg][nser] = new double [AVG_LENGTH];
avg_rts[nGtarg][nser][AVG_RANGE] = avg_range;
avg_rts[nGtarg][nser][AVG_TIMESTASMP] = avg_ts;
avg_rts[nGtarg][nser][AVG_KEYFRAME] = avg_key_frame;
avg_rts[nGtarg][nser][AVG_DISPARITY] = avg_disp;
avg_rts[nGtarg][nser][AVG_VELOCIY] = Double.NaN;
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
// calculate derivatives, save as the last item in avg_rts[nGtarg][nser]
int num_ser = targets_multi_series.length;
for (int ngtarg = 0; ngtarg < target_rmap.length; ngtarg++) {
for (int nser = 0; nser < num_ser; nser++) {
double [] avg = avg_rts[ngtarg][nser];
if ((avg != null) && (avg[AVG_DISPARITY] > min_disparit_velocity)) {
double v_prev = Double.NaN;
double v_next = Double.NaN;
double [] avg_prev = (nser > 0) ? avg_rts[ngtarg][nser - 1]: null;
double [] avg_next = (nser < (num_ser-1)) ? avg_rts[ngtarg][nser + 1]: null;
if ((avg_prev != null) && (avg_prev[AVG_DISPARITY] > min_disparit_velocity)) {
double dt = avg[AVG_TIMESTASMP] - avg_prev[AVG_TIMESTASMP];
double dr = avg[AVG_RANGE] - avg_prev[AVG_RANGE];
v_prev = dr / dt;
}
if ((avg_next != null) && (avg_next[AVG_DISPARITY] > min_disparit_velocity)) {
double dt = avg_next[AVG_TIMESTASMP] - avg[AVG_TIMESTASMP];
double dr = avg_next[AVG_RANGE] - avg[AVG_RANGE];
v_next = dr / dt;
}
avg[AVG_VELOCIY] = (Double.isNaN(v_prev)? 0 : v_prev) + (Double.isNaN(v_next)? 0 : v_next);
if (!Double.isNaN(v_prev) && !Double.isNaN(v_next)) {
avg[AVG_VELOCIY] /= 2;
}
}
}
}
return avg_rts;
}
public double getFps(int nser) { public double getFps(int nser) {
double fps = (CuasMotion.getCorrInc(clt_parameters) * (scene_titles[nser].length - 1.0))/(QuadCLT.getTimeStamp(scene_titles[nser][scene_titles[nser].length-1]) - double fps = (CuasMotion.getCorrInc(clt_parameters) * (scene_titles[nser].length - 1.0))/(QuadCLT.getTimeStamp(scene_titles[nser][scene_titles[nser].length-1]) -
QuadCLT.getTimeStamp(scene_titles[nser][0])); QuadCLT.getTimeStamp(scene_titles[nser][0]));
return fps; return fps;
} }
public double getLocalTargetStartTimestamp(
int nser,
int ltarg) {
return getKeyTimeStamp(
nser, // int nser,
targets_start_end[nser][ltarg][0]); // int nseq)
}
public double getKeyTimeStamp(
int nser,
int nseq) {
String sts = scene_titles[nser][nseq];
double ts = QuadCLT.getTimeStamp(sts);
return ts;
}
private void removeConflictPairs( private void removeConflictPairs(
ArrayList<Point> pair_list, // in each pair, .x is earlier than .y ArrayList<Point> pair_list, // in each pair, .x is earlier than .y
...@@ -763,12 +983,14 @@ public class CuasMultiSeries { ...@@ -763,12 +983,14 @@ public class CuasMultiSeries {
* Find the "middle" keyframe that contanis disparity/range for the specified local target index (0-based) * Find the "middle" keyframe that contanis disparity/range for the specified local target index (0-based)
* @param nser series number containing specified local target * @param nser series number containing specified local target
* @param local_target local * @param local_target local
* @param seq_tile - null or int[2] - will return [nseq_middle, ntile_middle]
* @return target array containing the longest averaged disparity and range for the specified target * @return target array containing the longest averaged disparity and range for the specified target
*/ */
private double [] getMiddleTarget( private double [] getMiddleTarget(
int nser, int nser,
int local_target) { int local_target,
int [][] lt = linked_targets_multi[nser][local_target]; int [] seq_tile) {
int [][] lt = linked_targets_multi[nser][local_target]; // OOB
int len_middle = -1; int len_middle = -1;
int nseq_middle = -1; int nseq_middle = -1;
int ntile_middle = -1; int ntile_middle = -1;
...@@ -790,6 +1012,10 @@ public class CuasMultiSeries { ...@@ -790,6 +1012,10 @@ public class CuasMultiSeries {
} }
} }
if (len_middle >= 0) { if (len_middle >= 0) {
if (seq_tile != null) {
seq_tile[0] = nseq_middle;
seq_tile[1] = ntile_middle;
}
return targets_multi_series[nser][nseq_middle][ntile_middle]; return targets_multi_series[nser][nseq_middle][ntile_middle];
} else { } else {
return null; return null;
...@@ -797,6 +1023,9 @@ public class CuasMultiSeries { ...@@ -797,6 +1023,9 @@ public class CuasMultiSeries {
} }
private double getMispatch2( private double getMispatch2(
CLTParameters clt_parameters, CLTParameters clt_parameters,
double [] target0, double [] target0,
...@@ -830,34 +1059,156 @@ public class CuasMultiSeries { ...@@ -830,34 +1059,156 @@ public class CuasMultiSeries {
double err2 = dpx*dpx + dpy * dpy; double err2 = dpx*dpx + dpy * dpy;
return err2; return err2;
} }
@Deprecated
private static double getLateralVelocity (
CLTParameters clt_parameters,
double [] target0, // should contain RSLT_VX, RSLT_VX, RSLT_GRANGE,
double [] target1, // should contain RSLT_VX, RSLT_VX, RSLT_GRANGE,
double fps){ // velocity_scale times fps
// TODO: Improve to use actual geometry
double ifov_deg = clt_parameters.imp.cuas_ifov; // 0.05; // degree per pixel
double ifov = ifov_deg / 180 * Math.PI;
double velocity_scale = CuasMotion.getFrameVelocityScale(clt_parameters); // == 1.0/clt_parameters.imp.cuas_corr_offset;
double range = target0[CuasMotionLMA.RSLT_GRANGE]; // meters
double vpx = 0.5*(target0[CuasMotionLMA.RSLT_VX] + target1[CuasMotionLMA.RSLT_VX]) * velocity_scale * fps;
double vpy = 0.5*(target0[CuasMotionLMA.RSLT_VY] + target1[CuasMotionLMA.RSLT_VY]) * velocity_scale * fps;
double vxm = vpx * ifov * range;
double vym = vpy * ifov * range;
double vm = Math.sqrt(vxm*vxm+vym*vym);
return vm;
}
/** /**
* Get lateral velocity in m/s *
* @param clt_parameters * @param clt_parameters
* @param gc
* @param target0 * @param target0
* @param target1 * @param target1
* @param range
* @param fps * @param fps
* @return * @return
*/ */
private double getLateralVelocity (
private static double getLateralVelocity (
CLTParameters clt_parameters, CLTParameters clt_parameters,
GeometryCorrection gc,
double [] target0, // should contain RSLT_VX, RSLT_VX, RSLT_GRANGE, double [] target0, // should contain RSLT_VX, RSLT_VX, RSLT_GRANGE,
double [] target1, // should contain RSLT_VX, RSLT_VX, RSLT_GRANGE, double [] target1, // should contain RSLT_VX, RSLT_VX, RSLT_GRANGE,
double range,
double fps){ // velocity_scale times fps double fps){ // velocity_scale times fps
// TODO: Improve to use actual geometry double ifov = gc.getIFOV();
double ifov = clt_parameters.imp.cuas_ifov; // 0.05; // degree per pixel
double ifov_rad = ifov / 180 * Math.PI;
double velocity_scale = CuasMotion.getFrameVelocityScale(clt_parameters); // == 1.0/clt_parameters.imp.cuas_corr_offset; double velocity_scale = CuasMotion.getFrameVelocityScale(clt_parameters); // == 1.0/clt_parameters.imp.cuas_corr_offset;
double range = target0[CuasMotionLMA.RSLT_GRANGE]; // meters
double vpx = 0.5*(target0[CuasMotionLMA.RSLT_VX] + target1[CuasMotionLMA.RSLT_VX]) * velocity_scale * fps; double vpx = 0.5*(target0[CuasMotionLMA.RSLT_VX] + target1[CuasMotionLMA.RSLT_VX]) * velocity_scale * fps;
double vpy = 0.5*(target0[CuasMotionLMA.RSLT_VY] + target1[CuasMotionLMA.RSLT_VY]) * velocity_scale * fps; double vpy = 0.5*(target0[CuasMotionLMA.RSLT_VY] + target1[CuasMotionLMA.RSLT_VY]) * velocity_scale * fps;
double vxm = vpx * ifov_rad * range; double vxm = vpx * ifov * range;
double vym = vpy * ifov_rad * range; double vym = vpy * ifov * range;
double vm = Math.sqrt(vxm*vxm+vym*vym); double vm = Math.sqrt(vxm*vxm+vym*vym);
return vm; return vm;
}
private static double getLateralVelocity (
CLTParameters clt_parameters,
GeometryCorrection gc,
double [] target0, // should contain RSLT_VX, RSLT_VX, RSLT_GRANGE,
double [] target1, // should contain RSLT_VX, RSLT_VX, RSLT_GRANGE,
double fps){ // velocity_scale times fps
double range = 0.5 * (target0[CuasMotionLMA.RSLT_GRANGE] + target0[CuasMotionLMA.RSLT_GRANGE]); // meters
return getLateralVelocity (
clt_parameters,
gc,
target0, // should contain RSLT_VX, RSLT_VX, RSLT_GRANGE,
target1, // should contain RSLT_VX, RSLT_VX, RSLT_GRANGE,
range,
fps); // velocity_scale times fps
}
public double recalculateRangeAfterInfinityChange(
double old_range,
double old_infinity,
double new_infinity) {
GeometryCorrection gc = master_CLT.getGeometryCorrection();
return recalculateRangeAfterInfinityChange(
old_range,
old_infinity,
new_infinity,
gc);
} }
public static double recalculateRangeAfterInfinityChange(
double old_range,
double old_infinity,
double new_infinity,
GeometryCorrection gc) {
if (Double.isNaN(old_range)) {
return Double.NaN;
}
if (Double.isInfinite(old_range)) {
return Double.POSITIVE_INFINITY;
}
double disparity = gc.getDisparityFromZ(old_range) + old_infinity -new_infinity;
if (disparity <= 0) {
return Double.POSITIVE_INFINITY;
}
return gc.getZFromDisparity(disparity);
}
public static void recalculateAllRanges(
final double [][][][] targets_multi_series, // [nser][nseq][ntile][nfield]
final double new_infinity,
final GeometryCorrection gc) {
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nSer = ai.getAndIncrement(); nSer < targets_multi_series.length; nSer = ai.getAndIncrement()) {
for (int nseq = 0; nseq < targets_multi_series[nSer].length; nseq++) {
for (int ntile = 0; ntile < targets_multi_series[nSer][nseq].length; ntile++) {
double [] target = targets_multi_series[nSer][nseq][ntile];
if (target != null) { // && !Double.isNaN(target[CuasMotionLMA.RSLT_INFINITY])) {
double old_infinity = target[CuasMotionLMA.RSLT_INFINITY];
if (!Double.isNaN(old_infinity) && (old_infinity != new_infinity)) {
target[CuasMotionLMA.RSLT_RANGE] = recalculateRangeAfterInfinityChange(
target[CuasMotionLMA.RSLT_RANGE],
old_infinity,
new_infinity,
gc);
double old_range = target[CuasMotionLMA.RSLT_GRANGE];
target[CuasMotionLMA.RSLT_GRANGE] = recalculateRangeAfterInfinityChange(
target[CuasMotionLMA.RSLT_GRANGE],
old_infinity,
new_infinity,
gc);
double new_range = target[CuasMotionLMA.RSLT_GRANGE];
double range_scale = new_range/old_range;
if ((range_scale > 0.1) && (range_scale < 10)) { // to deal with NaN and infinities
target[CuasMotionLMA.RSLT_VEL_AXIAL] *= range_scale; // could be NaN - OK
target[CuasMotionLMA.RSLT_VEL_LATERAL] *= range_scale; // could be NaN - OK
}
target[CuasMotionLMA.RSLT_RANGE_LIN] = recalculateRangeAfterInfinityChange(
target[CuasMotionLMA.RSLT_RANGE_LIN],
old_infinity,
new_infinity,
gc);
target[CuasMotionLMA.RSLT_INFINITY] = new_infinity;
}
}
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
return;
}
public void setupGlobalTargets( public void setupGlobalTargets(
) { ) {
......
src/main/java/com/elphel/imagej/tileprocessor/GeometryCorrection.java
View file @ 692610be
...@@ -1797,14 +1797,22 @@ public class GeometryCorrection { ...@@ -1797,14 +1797,22 @@ public class GeometryCorrection {
return (1000.0 * SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / this.pixelSize) / z; return (1000.0 * SCENE_UNITS_SCALE * this.focalLength * this.disparityRadius / this.pixelSize) / z;
} }
/**
public double getFOVPix(){ // get ratio of 1 pixel X/Y to Z (distance to object) // USED in lwir * Get Intrinsic FOV - angular size of 1 pixel
* @return IFOV
*/
public double getIFOV(){ // get Intrinsic FOV - angular size of 1 pixel. Ratio of 1 pixel X/Y to Z (distance to object) // USED in lwir
return 0.001 * this.pixelSize / this.focalLength; return 0.001 * this.pixelSize / this.focalLength;
} }
public double getIFOVDegrees() {
return getIFOV() * 180 /Math.PI;
}
public double getFOVWidth(){ // get FOV ratio: width to distance // USED in lwir public double getFOVWidth(){ // get FOV ratio: width to distance // USED in lwir
return this.pixelCorrectionWidth * 0.001 * this.pixelSize / this.focalLength; return this.pixelCorrectionWidth * 0.001 * this.pixelSize / this.focalLength;
} }
public double getFOVHeight(){ // get FOV ratio: width to distance // USED in lwir public double getFOVHeight(){ // get FOV ratio: width to distance // USED in lwir
return this.pixelCorrectionHeight * 0.001 * this.pixelSize / this.focalLength; return this.pixelCorrectionHeight * 0.001 * this.pixelSize / this.focalLength;
} }
......
src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java
View file @ 692610be
...@@ -898,7 +898,7 @@ min_str_neib_fpn 0.35 ...@@ -898,7 +898,7 @@ min_str_neib_fpn 0.35
public boolean cuas_reset_disparity = true; // reset target disparities from infinity public boolean cuas_reset_disparity = true; // reset target disparities from infinity
public double cuas_initial_disparity = 1.0; // Start correlation with this disparity (in addition to infinity) after reset public double cuas_initial_disparity = 1.0; // Start correlation with this disparity (in addition to infinity) after reset
public double cuas_infinity = 0.65; // disparity at infinity for targets public double cuas_infinity = 0.6466;// disparity at infinity for targets
public boolean cuas_rng_img = false; // Generate/save per-sensor target images public boolean cuas_rng_img = false; // Generate/save per-sensor target images
public boolean cuas_rng_glob= true; // Generate/save integrated target disparities (one per target) TODO: add first/second half public boolean cuas_rng_glob= true; // Generate/save integrated target disparities (one per target) TODO: add first/second half
......
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @ 692610be
...@@ -17117,7 +17117,7 @@ public class QuadCLTCPU { ...@@ -17117,7 +17117,7 @@ public class QuadCLTCPU {
boolean noalpha, // only with fillBlack, otherwise ignored boolean noalpha, // only with fillBlack, otherwise ignored
int debugLevel) int debugLevel)
{ {
double backdropPixels = 2.0/geometryCorrection.getFOVPix(); double backdropPixels = 2.0/geometryCorrection.getIFOV();
if (debugLevel > -1) { if (debugLevel > -1) {
System.out.println("backdropPixels = "+backdropPixels); System.out.println("backdropPixels = "+backdropPixels);
} }
......
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