Skip to content

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

Debugging OrthoMultiLMA

parent 352aa9e1
Hide whitespace changes
Inline Side-by-side
Showing with 697 additions and 63 deletions
src/main/java/com/elphel/imagej/orthomosaic/ComboMatch.java
View file @ b8b1f945
......@@ -157,13 +157,14 @@ public class ComboMatch {
// boolean show_map_stats = false;
boolean show_combo = false; // true;
boolean create_overlaps = false;
boolean create_map = false;
// boolean show_combo_mask = false; // generate image mas (where it is defined)_
// boolean use_alt = false;
// boolean show_centers = true;
boolean use_saved_collection = true; // false;
boolean save_collection = true;
boolean process_correlation = true; // use false to save new version of data
int num_tries_fit = 10;
int num_tries_fit = 10;
boolean update_match = true; // use false to save new version of data
boolean render_match = false; // true;
boolean test_multi_lma = false;
......@@ -231,6 +232,7 @@ public class ComboMatch {
// gd.addCheckbox ("Show statistics for ortho images", show_map_stats, "Generate and show statistics for ortho maps.");
gd.addCheckbox ("Show combo maps/stats", show_combo, "Generate/save combo maps and stats.");
gd.addCheckbox ("Create overlap pairs", create_overlaps, "Create scene pairs overlaps.");
gd.addCheckbox ("Create map", create_map, "Create combined map from pairwise matches.");
// gd.addCheckbox ("Show combo image mask", show_combo_mask, "Display combo binary image.");
// gd.addCheckbox ("Show altitude combo image", use_alt, "Load and process altitude maps.");
gd.addNumericField("Remove fraction of worst matches", frac_remove, 3,7,"", "When fitting scenes remove this fraction of worst match.");
......@@ -286,6 +288,7 @@ public class ComboMatch {
show_combo = gd.getNextBoolean();
create_overlaps = gd.getNextBoolean();
create_map = gd.getNextBoolean();
frac_remove = gd.getNextNumber();
metric_error= gd.getNextNumber();
if (use_marked_image ) { // will only be used if found and asked
......@@ -768,7 +771,14 @@ public class ComboMatch {
} // final int debugLevel);
}
if (create_map) {
OrthoMultiLMA.buildOrthoMap(
clt_parameters, // CLTParameters clt_parameters,
maps_collection, // OrthoMapsCollection maps_collection
orthoMapsCollection_path); // String orthoMapsCollection_path
return true;
}
if (create_overlaps) {
boolean ok =maps_collection.getIntersectedPairs(
clt_parameters, // CLTParameters clt_parameters,
......
src/main/java/com/elphel/imagej/orthomosaic/OrthoMultiLMA.java
View file @ b8b1f945
......@@ -28,12 +28,15 @@ package com.elphel.imagej.orthomosaic;
import java.awt.Point;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.concurrent.atomic.AtomicInteger;
import com.elphel.imagej.cameras.CLTParameters;
import com.elphel.imagej.tileprocessor.ImageDtt;
import Jama.Matrix;
public class OrthoMultiLMA {
final boolean move_only;
private double [] last_rms = null; // {rms, rms_pure}, matching this.vector
private double [] good_or_bad_rms = null; // just for diagnostics, to read last (failed) rms
private double [] initial_rms = null; // {rms, rms_pure}, first-calcualted rms
......@@ -48,14 +51,20 @@ public class OrthoMultiLMA {
private double [] last_ymfx = null;
private double [][] last_jt = null;
private PairwiseOrthoMatch [][] matches = null;
private ArrayList<Point> pairs = null;
private int [][] pairs = null;
private int [][] pairs_ni = null; // non-intersecting pairs for multi-threaded processing
private int [] indices = null;
private double [][] offsets = null; // scene offsets (rd)
private int num_scenes = 0;
private int num_pairs = 0;
public OrthoMultiLMA (boolean move_only) {
this.move_only = move_only;
}
public static boolean testMultiLMA(
CLTParameters clt_parameters,
OrthoMapsCollection maps_collection,
int [] indices) {
boolean move_only = false;
int scene0 = indices[0], scene1 = indices[1];
PairwiseOrthoMatch direct = maps_collection.ortho_maps[scene0].getMatch(maps_collection.ortho_maps[scene1].getName());
double [] enuOffset = maps_collection.ortho_maps[scene1].enuOffsetTo(maps_collection.ortho_maps[scene0]);
......@@ -75,12 +84,14 @@ public class OrthoMultiLMA {
double [][][] derivs_delta = new double [12][][];
double delta = 1E-7;
getAffineAndDerivetives_delta(
move_only, //boolean move_only,
delta, // double delta,
affine0, // double [][] affine00,
affine1, // double [][] affine10,
rd, // double [] rd,
derivs_delta); // double [][][] derivs)
double [][] affine = getAffineAndDerivetives(
double [][] affine = getAffineAndDerivatives(
move_only, //boolean move_only,
affine0, // double [][] affine00,
affine1, // double [][] affine10,
rd, // double [] rd,
......@@ -96,6 +107,58 @@ public class OrthoMultiLMA {
System.out.println();
return true;
}
public static int buildOrthoMap(
CLTParameters clt_parameters,
OrthoMapsCollection maps_collection,
String orthoMapsCollection_path
) {
int debugLevel = 3;
boolean move_only = false;
double [] val_coord = null; // 1 - valid, 0 - invalid, minimize coordinates errors
double position_pull = 0.1;
boolean use_inv = false;
double overlap_pow = 2.0; // match weight as overlap fraction to this power
int [] indices = maps_collection.getScenesSelection(
null, // boolean select_all,
" to build a map"); // String purpose)
OrthoMultiLMA oml = new OrthoMultiLMA(move_only);
double lambda = 0.1;
double lambda_scale_good = 0.5;
double lambda_scale_bad = 8.0;
double lambda_max = 100;
double rms_diff = 0.0001;
int num_iter = 20;
boolean last_run = false;
oml.prepareLMA (
clt_parameters, // CLTParameters clt_parameters,
maps_collection, // OrthoMapsCollection maps_collection,
indices, // int [] indices,
val_coord, // double [] val_coord, // 1 - valid, 0 - invalid, minimize coordinates errors
position_pull, // double position_pull,
use_inv, // boolean use_inv,
overlap_pow, // double overlap_pow, // match weight as overlap fraction to this power
debugLevel); // int debugLevel)
int lma_rslt = oml.runLma( // <0 - failed, >=0 iteration number (1 - immediately)
lambda, // double lambda, // 0.1
lambda_scale_good, // double lambda_scale_good,// 0.5
lambda_scale_bad, // double lambda_scale_bad, // 8.0
lambda_max, // double lambda_max, // 100
rms_diff, // double rms_diff, // 0.001
num_iter, // int num_iter, // 20
last_run, // boolean last_run,
null, // dbg_lma_prefix, // String dbg_prefix,
debugLevel); // int debug_level)
System.out.println("lma_rslt="+lma_rslt);
double [] rms = oml.getRms();
double [] initial_rms = oml.getInitialRms();
if (debugLevel > 0) {
System.out.println("LMA: full RMS="+rms[0]+" ("+initial_rms[0]+"), pure RMS="+rms[1]+" ("+initial_rms[1]+")");
}
return 0;
}
public int prepareLMA (
CLTParameters clt_parameters,
OrthoMapsCollection maps_collection,
......@@ -108,7 +171,8 @@ public class OrthoMultiLMA {
this.indices = indices;
num_scenes = indices.length;
matches = new PairwiseOrthoMatch[indices.length][indices.length];
pairs = new ArrayList<Point>();
ArrayList<Point> pairs_list = new ArrayList<Point>();
// ArrayList<Double> offset_list = new ArrayList<Double>();
for (int i = 0; i < num_scenes-1; i++) {
int scene0 = indices[i];
for (int j = i+1; j < num_scenes; j++){
......@@ -128,61 +192,95 @@ public class OrthoMultiLMA {
inv_match = match.getInverse(rd);
}
}
matches[i][j] = match;
matches[j][i] = inv_match;
pairs.add(new Point(i,j)); // only once?
if (match != null) {
matches[i][j] = match;
matches[j][i] = inv_match;
pairs_list.add(new Point(i,j)); // only once?
}
}
}
num_pairs = pairs.size();
num_pairs = pairs_list.size();
offsets = new double [num_pairs][2];
pairs = new int[num_pairs][2];
for (int np = 0; np < num_pairs; np++) {
Point pair = pairs_list.get(np);
pairs[np] = new int[] {pair.x, pair.y};
double [] enuOffset = maps_collection.ortho_maps[indices[pair.x]].enuOffsetTo(maps_collection.ortho_maps[indices[pair.y]]);
offsets[np] = new double[] {enuOffset[0], -enuOffset[1]}; // {right,down} of the image
}
pairs_ni = createNonIntersectingPairs(pairs);
if (val_coord == null) { // maybe not needed - use weights
val_coord = new double [indices.length];
Arrays.fill(val_coord, 1.0);
}
y_vector = new double [6*num_pairs + 2 * num_scenes]; // last 2 * num_scenes will stay 0
weights = new double [6*num_pairs + 2 * num_scenes];
parameters_vector = new double [6*num_scenes]; // maybe will need move-only mode?
int n62 = move_only ? 2 : 6;
int n13 = move_only ? 1 : 3;
y_vector = new double [n62*num_pairs + 2 * num_scenes]; // last 2 * num_scenes will stay 0
weights = new double [n62*num_pairs + 2 * num_scenes];
parameters_vector = new double [n62*num_scenes]; // maybe will need move-only mode?
for (int n = 0; n < num_scenes; n++) {
double [][] affine = maps_collection.ortho_maps[indices[n]].getAffine();
System.arraycopy(affine[0], 0, parameters_vector, 6*n, 3);
System.arraycopy(affine[1], 0, parameters_vector, 6*n + 3, 3);
y_vector[6*num_pairs+2*n + 0] = affine[0][2];
y_vector[6*num_pairs+2*n + 1] = affine[1][2];
System.arraycopy(affine[0], 0, parameters_vector, n62*n, n13);
System.arraycopy(affine[1], 0, parameters_vector, n62*n + n13, n13);
y_vector[n62*num_pairs+2*n + 0] = affine[0][2];
y_vector[n62*num_pairs+2*n + 1] = affine[1][2];
}
double sw=0;
for (int np = 0; np < num_pairs; np++) {
Point pair_ind = pairs.get(np);
PairwiseOrthoMatch match = matches[pair_ind.x][pair_ind.y];
PairwiseOrthoMatch match = matches[pairs[np][0]][pairs[np][1]];
double [][] affine_pair = match.getAffine();
System.arraycopy(affine_pair[0], 0, y_vector, 6*np, 3);
System.arraycopy(affine_pair[1], 0, y_vector, 6*np + 3, 3);
if (move_only) {
System.arraycopy(affine_pair[0], 2, y_vector, n62*np, 1);
System.arraycopy(affine_pair[1], 2, y_vector, n62*np + 1, 1);
} else {
System.arraycopy(affine_pair[0], 0, y_vector, n62*np, 3);
System.arraycopy(affine_pair[1], 0, y_vector, n62*np + 3, 3);
}
double overlap = match.overlap;
double [][] jtj = match.jtj;
double [] w = new double [6];
w[0] = jtj[0][0];w[1]=jtj[1][1]; w[2] = jtj[4][4]; // a00, a01, b0
w[3] = jtj[2][2];w[4]=jtj[3][3]; w[5] = jtj[5][5]; // a10, a11, b1
double [] w = new double [n62];
if (move_only) {
w[0] = jtj[4][4]; w[1] = jtj[5][5];
} else {
w[0] = jtj[0][0];w[1]=jtj[1][1]; w[2] = jtj[4][4]; // a00, a01, b0
w[3] = jtj[2][2];w[4]=jtj[3][3]; w[5] = jtj[5][5]; // a10, a11, b1
}
double w_overlap = Math.pow(overlap, overlap_pow);
for (int i = 0; i < w.length; i++) {
w[i] = Math.sqrt(w[i]) * w_overlap;
sw+=w[i];
}
System.arraycopy(w, 0, weights, 6*np, 6);
System.arraycopy(w, 0, weights, n62*np, n62);
}
int wi = 6*num_pairs;
double swp = sw;
int wi = n62 * num_pairs;
for (int n = 0; n < num_scenes; n++) {
double w = val_coord[n]*position_pull;
weights[wi++] = w;
weights[wi++] = w;
sw+= 2*w;
}
pure_weight = swp/sw;
double s = 1.0/sw;
for (int i = 0; i <weights.length;i++) {
weights[i] *=s;
}
last_jt = new double [parameters_vector.length][];
return weights.length;
}
public static double [][] getAffineAndDerivetives_delta(
boolean move_only,
double delta,
double [][] affine00,
double [][] affine10,
double [] rd,
double [][][] derivs) {
double [][] affine = getAffineAndDerivetives(
double [][] affine = getAffineAndDerivatives(
move_only, //boolean move_only,
affine00, // double [][] affine0,
affine10, // double [][] affine1,
rd, // double [] rd,
......@@ -195,13 +293,15 @@ public class OrthoMultiLMA {
{affines0[0][0].clone(), affines0[0][1].clone()},
{affines0[1][0].clone(), affines0[1][1].clone()}};
affines[naff][row][col]= affines0[naff][row][col] - 0.5*delta;
double [][] affine_minus = getAffineAndDerivetives(
double [][] affine_minus = getAffineAndDerivatives(
move_only, //boolean move_only,
affines[0], // double [][] affine0,
affines[1], // double [][] affine1,
rd, // double [] rd,
null); // double [][][] derivs)
affines[naff][row][col]= affines0[naff][row][col] + 0.5*delta;
double [][] affine_plus = getAffineAndDerivetives(
double [][] affine_plus = getAffineAndDerivatives(
move_only, //boolean move_only,
affines[0], // double [][] affine0,
affines[1], // double [][] affine1,
rd, // double [] rd,
......@@ -218,24 +318,93 @@ public class OrthoMultiLMA {
}
return affine;
}
public static double [][] getAffineAndDerivetives(
public static double [][] getAffineAndDerivatives(
boolean move_only,
double [][] affine0,
double [][] affine1,
double [] rd,
double [][][] derivs) { // null or double[12] - will return affine derivatives by each of affine0, affine1
Matrix A0 = new Matrix (
new double [][] {{affine0[0][0],affine0[0][1]},{affine0[1][0],affine0[1][1]}});
Matrix B0 = new Matrix(new double [][] {{affine0[0][2]},{affine0[1][2]}});
Matrix A1 = new Matrix (
new double [][] {{affine1[0][0],affine1[0][1]},{affine1[1][0],affine1[1][1]}});
Matrix B1 = new Matrix(new double [][] {{affine1[0][2]},{affine1[1][2]}});
Matrix V = new Matrix(new double [][] {{-rd[0]},{-rd[1]}});
double [][][] derivs3) { // null or double[12] - will return affine derivatives by each of affine0, affine1
double [] params = new double[move_only? 4 : 12];
if (move_only) {
params[0] = affine0[0][2];
params[1] = affine0[1][2];
params[2] = affine1[0][2];
params[3] = affine1[1][2];
} else {
System.arraycopy(affine0[0], 0, params, 0, 3);
System.arraycopy(affine0[1], 0, params, 3, 3);
System.arraycopy(affine1[0], 0, params, 6, 3);
System.arraycopy(affine1[1], 0, params, 9, 3);
}
double [][] derivs = (derivs3 !=null)? (new double[move_only? 4 : 12][]) : null;
double [] l_affine= getAffineAndDerivatives( // [2]/[6]
move_only, //boolean move_only,
params, // double [] params,
0, // int pindx0,
1, // int pindx1,
rd, // double [] rd,
derivs); // double [][][] derivs)
double [][] affine;
if (move_only) {
affine = new double [][]{
{1,0,l_affine[0]},
{0,1,l_affine[1]}};
if (derivs3 !=null) {
for (int i = 0; i < 4; i++) {
derivs3[3*i+2] = new double [][] {
{0,0,derivs[i][0]},
{0,0,derivs[i][1]}};
}
}
} else {
affine = new double [][]{{l_affine[0],l_affine[1],l_affine[2]},{l_affine[3],l_affine[4],l_affine[5]}};
if (derivs3 !=null) {
for (int i = 0; i < derivs3.length; i++) {
derivs3[i] = new double [][] {
{derivs[i][0],derivs[i][1],derivs[i][2]},
{derivs[i][3],derivs[i][4],derivs[i][5]}};
}
}
}
return affine;
}
public static double [] getAffineAndDerivatives(
boolean move_only,
double [] params,
int pindx0,
int pindx1,
double [] rd,
double [][] derivs) { // null or double[12] ([4] for move_only) - will return affine derivatives by each of affine0, affine1
int indx0, indx1;
double [][] aff0_a, aff0_b, aff1_a, aff1_b;
if (move_only) {
indx0 = 2 * pindx0;
indx1 = 2 * pindx1;
aff0_a = new double [][] {{1, 0},{0, 1}};
aff0_b = new double [][] {{params[indx0+0]},{params[indx0+1]}};
aff1_a = new double [][] {{1, 0},{0, 1}};
aff1_b = new double [][] {{params[indx1+0]},{params[indx1+1]}};
} else {
indx0 = 6 * pindx0;
indx1 = 6 * pindx1;
aff0_a = new double [][] {{params[indx0+0],params[indx0+1]},{params[indx0+3],params[indx0+4]}};
aff0_b = new double [][] {{params[indx0+2]},{params[indx0+5]}};
aff1_a = new double [][] {{params[indx1+0],params[indx1+1]},{params[indx1+3],params[indx1+4]}};
aff1_b = new double [][] {{params[indx1+2]},{params[indx1+5]}};
}
Matrix A0 = new Matrix (aff0_a);
Matrix B0 = new Matrix (aff0_b);
Matrix A1 = new Matrix (aff1_a);
Matrix B1 = new Matrix (aff1_b);
Matrix V = new Matrix (new double [][] {{-rd[0]},{-rd[1]}});
Matrix A = A1.times(A0.inverse());
Matrix B = A.times(V.minus(B0)).minus(A1.times(V)).plus(B1);
double [][] affine = new double[][] {
{A.get(0,0),A.get(0,1), B.get(0,0)},
{A.get(1,0),A.get(1,1), B.get(1,0)}};
Matrix B = A.times(V.minus(B0)).minus(A1.times(V)).plus(B1);
double [] affine = move_only? (new double[] {B.get(0,0), B.get(1,0)}) :
(new double[] {A.get(0,0),A.get(0,1), B.get(0,0), A.get(1,0),A.get(1,1), B.get(1,0)});
if (derivs != null) {
double [][] a = A.getArray();
double [][] a0 = A0.getArray();
......@@ -272,7 +441,6 @@ public class OrthoMultiLMA {
double [][] da_da101 = {{-idet*a0[1][0], idet*a0[0][0]},{0,0}};
double [][] da_da110 = {{ 0, 0 },{ idet*a0[1][1],-idet*a0[0][1]}};
double [][] da_da111 = {{ 0, 0 },{-idet*a0[1][0], idet*a0[0][0]}};
double [][][][] da_da1 = {{da_da100, da_da101}, {da_da110, da_da111}};
double [][][][][] da_da = {da_da0, da_da1};
double [] db_da000 = {
......@@ -302,29 +470,40 @@ public class OrthoMultiLMA {
da_da111[1][0]*vb0[0]+da_da111[1][1]*vb0[1] - v[1]};
double [][][] db_da1 = {{db_da100,db_da101},{db_da110,db_da111}};
double [][][][] db_da = {db_da0,db_da1};
double [] db_db00 = {-a[0][0],-a[1][0]};
double [] db_db01 = {-a[0][1],-a[1][1]};
double [] db_db10 = {1,0};
double [] db_db11 = {0,1};
double [][][] db_db = {{db_db00,db_db01},{db_db10,db_db11}};
// fill in derivatives, skipping B - only 2x2 A
for (int naff = 0; naff < da_da.length; naff++) {
for (int row = 0; row < da_da[naff].length; row++) {
for (int col = 0; col < 3; col++) {
int indx = naff*6+row*3+col;
derivs[indx] = new double[2][3];
if ((col < da_da[naff][row].length) ) {
for (int i = 0; i < 2; i++) {
for (int j = 0; j< 2; j++) {
derivs[indx][i][j] = da_da[naff][row][col][i][j];
if (move_only) {
for (int naff = 0; naff < da_da.length; naff++) {
for (int row = 0; row < da_da[naff].length; row++) {
int indx = naff*2+row;
derivs[indx] = new double[2]; // [2][3];
for (int i = 0; i < 2; i++) {
derivs[indx][i] = db_db[naff][row][i];
}
}
}
} else {
// fill in derivatives, skipping B - only 2x2 A
for (int naff = 0; naff < da_da.length; naff++) {
for (int row = 0; row < da_da[naff].length; row++) {
for (int col = 0; col < 3; col++) {
int indx = naff*6+row*3+col;
derivs[indx] = new double[6]; // [2][3];
if ((col < da_da[naff][row].length) ) {
for (int i = 0; i < 2; i++) {
for (int j = 0; j< 2; j++) {
derivs[indx][3 * i + j] = da_da[naff][row][col][i][j];
}
derivs[indx][3 * i + 2] = db_da[naff][row][col][i];
}
derivs[indx][i][2] = db_da[naff][row][col][i];
} else { // d/db
for (int i = 0; i < 2; i++) {
derivs[indx][3 * i + 2] = db_db[naff][row][i];
}
}
} else { // d/db
for (int i = 0; i < 2; i++) {
derivs[indx][i][2] = db_db[naff][row][i];
}
}
}
}
......@@ -332,6 +511,452 @@ public class OrthoMultiLMA {
}
return affine; // _alt;
}
private static int [][] createNonIntersectingPairs(int [][] pairs) {
ArrayList<Integer> pairs_list = new ArrayList<Integer>();
for (int i = 0; i < pairs.length; i++) {
pairs_list.add(i);
}
int max_scene = 0;
for (int [] pair:pairs) {
max_scene=Math.max(max_scene, pair[0]);
max_scene=Math.max(max_scene, pair[1]);
}
ArrayList<ArrayList<Integer>> list_list_pairs = new ArrayList<ArrayList<Integer>>();
while (!pairs_list.isEmpty()) {
boolean [] used = new boolean [max_scene+1];
ArrayList<Integer> pairs_new = new ArrayList<Integer>();
for (int i = 0; i < pairs_list.size(); ) { // no increment here!
int ip = pairs_list.get(i);
int [] p = pairs[ip];
if (used[p[0]] || used[p[1]]) {
i++;
} else {
used[p[0]] = true;
used[p[1]] = true;
pairs_new.add(ip);
pairs_list.remove(i);
}
}
list_list_pairs.add(pairs_new);
}
int [][] pairs_ni = new int [list_list_pairs.size()][];
for (int n = 0; n < pairs_ni.length; n++) {
pairs_list = list_list_pairs.get(n);
pairs_ni[n] = new int [pairs_list.size()];
for (int i = 0;i < pairs_ni[n].length; i++) {
pairs_ni[n][i] = pairs_list.get(i);
}
}
return pairs_ni;
}
public int runLma( // <0 - failed, >=0 iteration number (1 - immediately)
double lambda, // 0.1
double lambda_scale_good,// 0.5
double lambda_scale_bad, // 8.0
double lambda_max, // 100
double rms_diff, // 0.001
int num_iter, // 20
boolean last_run,
String dbg_prefix,
int debug_level)
{
boolean [] rslt = {false,false};
this.last_rms = null; // remove?
int iter = 0;
if (dbg_prefix != null) {
// debugStateImage(dbg_prefix+"-initial");
}
for (iter = 0; iter < num_iter; iter++) {
rslt = lmaStep(
lambda,
rms_diff,
debug_level);
if (dbg_prefix != null) {
// debugStateImage(dbg_prefix+"-step_"+iter);
}
if (rslt == null) {
return -1; // false; // need to check
}
if (debug_level > 1) {
System.out.println("LMA step "+iter+": {"+rslt[0]+","+rslt[1]+"} full RMS= "+good_or_bad_rms[0]+
" ("+initial_rms[0]+"), pure RMS="+good_or_bad_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
}
if (rslt[1]) {
break;
}
if (rslt[0]) { // good
lambda *= lambda_scale_good;
} else {
lambda *= lambda_scale_bad;
if (lambda > lambda_max) {
break; // not used in lwir
}
}
}
if (rslt[0]) { // better
if (iter >= num_iter) { // better, but num tries exceeded
if (debug_level > 1) System.out.println("Step "+iter+": Improved, but number of steps exceeded maximal");
} else {
if (debug_level > 1) System.out.println("Step "+iter+": LMA: Success");
}
} else { // improved over initial ?
if (last_rms[0] < initial_rms[0]) { // NaN
rslt[0] = true;
if (debug_level > 1) System.out.println("Step "+iter+": Failed to converge, but result improved over initial");
} else {
if (debug_level > 1) System.out.println("Step "+iter+": Failed to converge");
}
}
boolean show_intermediate = true;
if (show_intermediate && (debug_level > 0)) {
System.out.println("LMA: full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
}
if (debug_level > 2){
System.out.println("iteration="+iter);
}
if (debug_level > 0) {
if ((debug_level > 1) || last_run) { // (iter == 1) || last_run) {
if (!show_intermediate) {
System.out.println("LMA: iter="+iter+", full RMS="+last_rms[0]+" ("+initial_rms[0]+"), pure RMS="+last_rms[1]+" ("+initial_rms[1]+") + lambda="+lambda);
}
}
}
if ((debug_level > -2) && !rslt[0]) { // failed
if ((debug_level > 1) || (iter == 1) || last_run) {
System.out.println("LMA failed on iteration = "+iter);
}
System.out.println();
}
return rslt[0]? iter : -1;
}
private boolean [] lmaStep(
double lambda,
double rms_diff,
int debug_level) {
boolean [] rslt = {false,false};
// maybe the following if() branch is not needed - already done in prepareLMA !
if (this.last_rms == null) { //first time, need to calculate all (vector is valid)
last_rms = new double[2];
if (debug_level > 1) {
System.out.println("lmaStep(): first step");
}
double [] fx = getFxDerivs(
parameters_vector, // double [] vector,
last_jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
last_ymfx = getYminusFxWeighted(
fx, // final double [] fx,
last_rms); // final double [] rms_fp // null or [2]
this.initial_rms = this.last_rms.clone();
this.good_or_bad_rms = this.last_rms.clone();
if (debug_level > -1) { // temporary
/*
dbgYminusFxWeight(
this.last_ymfx,
this.weights,
"Initial_y-fX_after_moving_objects");
*/
}
if (last_ymfx == null) {
return null; // need to re-init/restart LMA
}
// TODO: Restore/implement
if (debug_level > 3) {
/*
dbgJacobians(
corr_vector, // GeometryCorrection.CorrVector corr_vector,
1E-5, // double delta,
true); //boolean graphic)
*/
}
}
Matrix y_minus_fx_weighted = new Matrix(this.last_ymfx, this.last_ymfx.length);
Matrix wjtjlambda = new Matrix(getWJtJlambda(
lambda, // *10, // temporary
this.last_jt)); // double [][] jt)
if (debug_level>2) {
System.out.println("JtJ + lambda*diag(JtJ");
wjtjlambda.print(18, 6);
}
Matrix jtjl_inv = null;
try {
jtjl_inv = wjtjlambda.inverse(); // check for errors
} catch (RuntimeException e) {
rslt[1] = true;
if (debug_level > 0) {
System.out.println("Singular Matrix!");
}
return rslt;
}
if (debug_level>2) {
System.out.println("(JtJ + lambda*diag(JtJ).inv()");
jtjl_inv.print(18, 6);
}
//last_jt has NaNs
Matrix jty = (new Matrix(this.last_jt)).times(y_minus_fx_weighted);
if (debug_level>2) {
System.out.println("Jt * (y-fx)");
jty.print(18, 6);
}
Matrix mdelta = jtjl_inv.times(jty);
if (debug_level>2) {
System.out.println("mdelta");
mdelta.print(18, 6);
}
double scale = 1.0;
double [] delta = mdelta.getColumnPackedCopy();
double [] new_vector = parameters_vector.clone();
for (int i = 0; i < parameters_vector.length; i++) {
new_vector[i] += scale * delta[i];
}
double [] fx = getFxDerivs(
new_vector, // double [] vector,
last_jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
double [] rms = new double[2];
last_ymfx = getYminusFxWeighted(
// vector_XYS, // final double [][] vector_XYS,
fx, // final double [] fx,
rms); // final double [] rms_fp // null or [2]
if (debug_level > 2) {
/*
dbgYminusFx(this.last_ymfx, "next y-fX");
dbgXY(new_vector, "XY-correction");
*/
}
if (last_ymfx == null) {
return null; // need to re-init/restart LMA
}
this.good_or_bad_rms = rms.clone();
if (rms[0] < this.last_rms[0]) { // improved
rslt[0] = true;
rslt[1] = rms[0] >=(this.last_rms[0] * (1.0 - rms_diff));
this.last_rms = rms.clone();
this.parameters_vector = new_vector.clone();
if (debug_level > 2) {
// print vectors in some format
/*
System.out.print("delta: "+corr_delta.toString()+"\n");
System.out.print("New vector: "+new_vector.toString()+"\n");
System.out.println();
*/
}
} else { // worsened
rslt[0] = false;
rslt[1] = false; // do not know, caller will decide
// restore state
fx = getFxDerivs(
parameters_vector, // double [] vector,
last_jt, // final double [][] jt, // should be null or initialized with [vector.length][]
debug_level); // final int debug_level)
last_ymfx = getYminusFxWeighted(
fx, // final double [] fx,
this.last_rms); // final double [] rms_fp // null or [2]
if (last_ymfx == null) {
return null; // need to re-init/restart LMA
}
if (debug_level > 2) {
/*
dbgJacobians(
corr_vector, // GeometryCorrection.CorrVector corr_vector,
1E-5, // double delta,
true); //boolean graphic)
*/
}
}
return rslt;
}
private double [][] getWJtJlambda(
final double lambda,
final double [][] jt)
{
final int num_pars = jt.length;
final int num_pars2 = num_pars * num_pars;
final int nup_points = jt[0].length;
final double [][] wjtjl = new double [num_pars][num_pars];
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 indx = ai.getAndIncrement(); indx < num_pars2; indx = ai.getAndIncrement()) {
int i = indx / num_pars;
int j = indx % num_pars;
if (j >= i) {
double d = 0.0;
for (int k = 0; k < nup_points; k++) {
if (jt[i][k] != 0) {
d+=0;
}
d += weights[k]*jt[i][k]*jt[j][k];
}
wjtjl[i][j] = d;
if (i == j) {
wjtjl[i][j] += d * lambda;
} else {
wjtjl[j][i] = d;
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
return wjtjl;
}
private double [] getYminusFxWeighted(
final double [] fx,
final double [] rms_fp // null or [2]
) {
final int n62 = move_only ? 2 : 6;
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
final double [] wymfw = new double [fx.length];
double [] swd2 = new double[num_pairs];
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nPair = ai.getAndIncrement(); nPair < num_pairs; nPair = ai.getAndIncrement()) {
int np6 = n62*nPair;
for (int i1 = 0; i1 < n62; i1++) {
int i = np6+i1;
double d = y_vector[i] - fx[i];
double wd = d * weights[i];
wymfw[i] = wd;
swd2[nPair] += d * wd;
}
}
}
};
}
ImageDtt.startAndJoin(threads);
double s_rms = 0;
for (int nPair = 0; nPair < num_pairs; nPair++) {
s_rms += swd2[nPair];
}
double s_rms_pure = s_rms/pure_weight;
// num_scenes is not that large; sum w/o threads
for (int nScene = 0; nScene < num_scenes; nScene++) {
for (int i1 = 0; i1 < 2; i1++) {
int i = n62*num_pairs + 2 * nScene + i1;
double d = y_vector[i] - fx[i];
double wd = d * weights[i];
wymfw[i] = wd;
s_rms += d * wd;
}
}
if (rms_fp != null) {
rms_fp[0] = Math.sqrt(s_rms);
if (rms_fp.length > 1) {
rms_fp[1] = Math.sqrt(s_rms_pure);
}
}
return wymfw;
}
public double [] getRms() {
return last_rms;
}
public double [] getInitialRms() {
return initial_rms;
}
private double [] getFxDerivs(
final double [] vector,
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level)
{
final int n62 = move_only ? 2 : 6;
final double [] fx = new double [weights.length]; // weights.length]; : weights.length :
if (jt != null) {
for (int i = 0; i < jt.length; i++) {
jt[i] = new double [weights.length]; // weights.length];
}
}
final Thread[] threads = ImageDtt.newThreadArray();
final AtomicInteger ai = new AtomicInteger(0);
for (int pair_group = 0; pair_group<pairs_ni.length; pair_group++) {
final int fpair_group = pair_group;
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
double [][] jt_frac = (jt != null)? (new double [2 * n62][]):null;
for (int iPair = ai.getAndIncrement(); iPair < pairs_ni[fpair_group].length; iPair = ai.getAndIncrement()) {
int nPair = pairs_ni[fpair_group][iPair]; // index in pairs[][]
int [] pair = pairs[nPair];
double [] fx_frac = getAffineAndDerivatives(
move_only,
vector, // double [] params,
pair[0], // int pindx0,
pair[1], // int pindx1,
offsets[nPair], // double [] rd,
jt_frac); // double [][] derivs)
System.arraycopy(
fx_frac,
0,
fx,
n62 * nPair,
n62);
if (jt_frac != null) {
for (int ns = 0; ns < 2; ns++) {
for (int nsp = 0; nsp < n62; nsp++) {
for (int i = 0; i < n62; i++) {
jt[n62 * pairs[nPair][ns] + nsp][n62 * nPair + i] +=
jt_frac[n62 * ns +nsp][i];
}
}
}
}
}
}
};
}
ImageDtt.startAndJoin(threads);
ai.set(0);
}
final int pull_indx = n62 * num_pairs;
final int [] ioffs = {move_only? 0:2,move_only? 1:5};
for (int ithread = 0; ithread < threads.length; ithread++) {
threads[ithread] = new Thread() {
public void run() {
for (int nScene = ai.getAndIncrement(); nScene < num_scenes; nScene = ai.getAndIncrement()){
fx[pull_indx + 2 * nScene + 0] = vector[n62 * nScene + ioffs[0]];
fx[pull_indx + 2 * nScene + 1] = vector[n62 * nScene + ioffs[1]];
if (jt!= null) {
jt[n62 * nScene + ioffs[0]][pull_indx + 2 * nScene + 0] = 1.0;
jt[n62 * nScene + ioffs[1]][pull_indx + 2 * nScene + 1] = 1.0;
}
}
}
};
}
ImageDtt.startAndJoin(threads);
return fx;
}
}
src/main/java/com/elphel/imagej/orthomosaic/OrthoPairLMA.java
View file @ b8b1f945
......@@ -720,7 +720,7 @@ public class OrthoPairLMA {
}
private double [][] getWJtJlambda( // USED in lwir
private double [][] getWJtJlambda(
final double lambda,
final double [][] jt)
{
......@@ -766,7 +766,6 @@ public class OrthoPairLMA {
final double [][] jt, // should be null or initialized with [vector.length][]
final int debug_level)
{
// final double [][] affine = {{vector[0],vector[1],vector[4]},{vector[2],vector[3],vector[5]}};
final double [] fx = new double [weights.length]; // weights.length]; : weights.length :
if (jt != null) {
for (int i = 0; i < jt.length; i++) {
......
src/main/java/com/elphel/imagej/tileprocessor/Corr2dLMA.java
View file @ b8b1f945
......@@ -2583,7 +2583,7 @@ public class Corr2dLMA {
fx[i] = this.weights[i] * d;
rms += fx[i]*d; // sum of weights
}
rms_pure = Math.sqrt(rms)/this.pure_weight;
rms_pure = Math.sqrt(rms)/this.pure_weight; // Not Math.sqrt(rms/this.pure_weight)?
for (int i = num_samples; i < num_points; i++) {
double d = (values[i] - fx[i]);
fx[i] = this.weights[i] * d;
......
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