Improving mount correction, implementing gyro (omegas) correction (54e00356) · Commits · Elphel / imagej-elphel

src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java

+63 −2

Original line number	Diff line number	Diff line
		@@ -114,6 +114,10 @@ public class IntersceneMatchParameters {
		public int min_num_orient = 2; // make from parameters, should be >= 1
		public int min_num_interscene = 2; // make from parameters, should be >= 1
		public boolean adjust_imu_orient = false;// adjust IMU misalignment to the camera
		public boolean apply_imu_orient = true; // apply IMU misalignment to the camera if adjusted
		public boolean adjust_gyro = false;// adjust qyro omegas offsets
		public boolean apply_gyro = true; // apply adjusted qyro omegas offsets

		public boolean calc_quat_corr = true; // calculate camera orientation correction from predicted by IMS
		public boolean apply_quat_corr = true; // apply camera orientation correction from predicted by IMS
		public boolean use_quat_corr = true; // Use orientation correction everywhere if available
		@@ -451,6 +455,11 @@ public class IntersceneMatchParameters {
		public int quat_num_iter = 20;
		public double quat_reg_w = 0.25;

		public double quat_max_ratio = 5.0; // max derivatives difference
		public double quat_max_change = 0.1; // radians
		public double quat_min_transl = 5.0; // meters to adjust by translation
		public double quat_min_rot = 0.1; // radians to adjust by rotation

		public boolean stereo_merge = true;
		public int stereo_gap = 32; // pixels between right and left frames
		public double stereo_intereye = 63.5; // mm
		@@ -522,6 +531,12 @@ public class IntersceneMatchParameters {
		return new double [][] {new double[3],
		{-pimu_gyr_offs[2], -pimu_gyr_offs[1], pimu_gyr_offs[0]}}; // {XYZ,ATR}
		}

		public void set_pimu_omegas(double [] atr) {
		pimu_gyr_offs[2] = - atr[0];
		pimu_gyr_offs[1] = - atr[1];
		pimu_gyr_offs[0] = atr[2];
		}
		public void dialogQuestions(GenericJTabbedDialog gd) {
		// gd.addMessage ("Scene parameters selection");
		// gd.addTab ("Inter-Match", "Parameters for full-resolution (no decimation/macrotiles) scene matching");
		@@ -640,6 +655,12 @@ public class IntersceneMatchParameters {
		"Minimal required number of re-calculations of the interscene-accumulated DSI.");
		gd.addCheckbox ("Adjust IMU orientation", this.adjust_imu_orient,
		"Adjust IMU misalignment to the camera.");
		gd.addCheckbox ("Adjust IMU orientation", this.apply_imu_orient,
		"Apply IMU misalignment to the camera if adjusted.");
		gd.addCheckbox ("Adjust gyro offsets", this.adjust_gyro,
		"Adjust qyro omegas offsets.");
		gd.addCheckbox ("Apply gyro offsets", this.apply_gyro,
		"Apply adjusted qyro omegas offsets.");
		gd.addCheckbox ("Calculate IMS orientation correction", this.calc_quat_corr,
		"Calculate camera orientation correction from predicted by IMS .");
		gd.addCheckbox ("Apply IMS orientation correction ", this.apply_quat_corr,
		@@ -1275,6 +1296,14 @@ public class IntersceneMatchParameters {
		gd.addNumericField("Regularization weight", this.quat_reg_w, 3,5,"",
		"Regularization weight [0..1) weight of q0^2+q1^2+q1^2+q3^2 -1.");

		gd.addNumericField("Maximal derivatives by axes ratio", this.quat_max_ratio, 3,5,"x",
		"If difference is larger, add regularization to reduce it.");
		gd.addNumericField("Maximal correction angles change", this.quat_max_change, 3,5,"rad",
		"Do not update corrections if any angle is above this limit. ");
		gd.addNumericField("Minimal translation for mount calibration", this.quat_min_transl, 3,5,"m",
		"Do not use translation for IMS mount adjustment if it is too small.");
		gd.addNumericField("Minimal rotation for mount calibration", this.quat_min_rot, 3,5,"rad",
		"Do not use rotations for IMS mount adjustment if it is too small.");

		gd.addTab("Stereo/Video","Parameters for stereo video generation");
		gd.addMessage ("Stereo");
		@@ -1425,6 +1454,9 @@ public class IntersceneMatchParameters {
		this.min_num_orient = (int) gd.getNextNumber(); if (min_num_orient < 1) min_num_orient = 1;
		this.min_num_interscene = (int) gd.getNextNumber(); if (min_num_interscene < 1) min_num_interscene = 1;
		this.adjust_imu_orient = gd.getNextBoolean();
		this.apply_imu_orient = gd.getNextBoolean();
		this.adjust_gyro = gd.getNextBoolean();
		this.apply_gyro = gd.getNextBoolean();
		this.calc_quat_corr = gd.getNextBoolean();
		this.apply_quat_corr = gd.getNextBoolean();
		this.use_quat_corr = gd.getNextBoolean();
		@@ -1716,6 +1748,11 @@ public class IntersceneMatchParameters {
		this.quat_num_iter = (int) gd.getNextNumber();
		this.quat_reg_w = gd.getNextNumber();

		this.quat_max_ratio = gd.getNextNumber();
		this.quat_max_change = gd.getNextNumber();
		this.quat_min_transl = gd.getNextNumber();
		this.quat_min_rot = gd.getNextNumber();

		if (stereo_views.length > 0) {
		int i = gd.getNextChoiceIndex();
		if (i > 0) {
		@@ -1856,6 +1893,9 @@ public class IntersceneMatchParameters {
		properties.setProperty(prefix+"min_num_orient", this.min_num_orient+""); // int
		properties.setProperty(prefix+"min_num_interscene", this.min_num_interscene+""); // int
		properties.setProperty(prefix+"adjust_imu_orient", this.adjust_imu_orient+""); // boolean
		properties.setProperty(prefix+"apply_imu_orient", this.apply_imu_orient+""); // boolean
		properties.setProperty(prefix+"adjust_gyro", this.adjust_gyro+""); // boolean
		properties.setProperty(prefix+"apply_gyro", this.apply_gyro+""); // boolean
		properties.setProperty(prefix+"calc_quat_corr", this.calc_quat_corr+""); // boolean
		properties.setProperty(prefix+"apply_quat_corr", this.apply_quat_corr+""); // boolean
		properties.setProperty(prefix+"use_quat_corr", this.use_quat_corr+""); // boolean
		@@ -2157,6 +2197,11 @@ public class IntersceneMatchParameters {
		properties.setProperty(prefix+"quat_num_iter", this.quat_num_iter+""); // int
		properties.setProperty(prefix+"quat_reg_w", this.quat_reg_w+""); // double

		properties.setProperty(prefix+"quat_max_ratio", this.quat_max_ratio+""); // double
		properties.setProperty(prefix+"quat_max_change", this.quat_max_change+""); // double
		properties.setProperty(prefix+"quat_min_transl", this.quat_min_transl+""); // double
		properties.setProperty(prefix+"quat_min_rot", this.quat_min_rot+""); // double

		properties.setProperty(prefix+"stereo_merge", this.stereo_merge+""); // boolean
		properties.setProperty(prefix+"stereo_gap", this.stereo_gap+""); // int
		properties.setProperty(prefix+"stereo_intereye", this.stereo_intereye+""); // double
		@@ -2252,6 +2297,9 @@ public class IntersceneMatchParameters {
		if (properties.getProperty(prefix+"min_num_orient")!=null) this.min_num_orient=Integer.parseInt(properties.getProperty(prefix+"min_num_orient"));
		if (properties.getProperty(prefix+"min_num_interscene")!=null) this.min_num_interscene=Integer.parseInt(properties.getProperty(prefix+"min_num_interscene"));
		if (properties.getProperty(prefix+"adjust_imu_orient")!=null) this.adjust_imu_orient=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_imu_orient"));
		if (properties.getProperty(prefix+"apply_imu_orient")!=null) this.apply_imu_orient=Boolean.parseBoolean(properties.getProperty(prefix+"apply_imu_orient"));
		if (properties.getProperty(prefix+"adjust_gyro")!=null) this.adjust_gyro=Boolean.parseBoolean(properties.getProperty(prefix+"adjust_gyro"));
		if (properties.getProperty(prefix+"apply_gyro")!=null) this.apply_gyro=Boolean.parseBoolean(properties.getProperty(prefix+"apply_gyro"));
		if (properties.getProperty(prefix+"calc_quat_corr")!=null) this.calc_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"calc_quat_corr"));
		if (properties.getProperty(prefix+"apply_quat_corr")!=null) this.apply_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"apply_quat_corr"));
		if (properties.getProperty(prefix+"use_quat_corr")!=null) this.use_quat_corr=Boolean.parseBoolean(properties.getProperty(prefix+"use_quat_corr"));
		@@ -2560,6 +2608,11 @@ public class IntersceneMatchParameters {
		if (properties.getProperty(prefix+"quat_num_iter")!=null) this.quat_num_iter=Integer.parseInt(properties.getProperty(prefix+"quat_num_iter"));
		if (properties.getProperty(prefix+"quat_reg_w")!=null) this.quat_reg_w=Double.parseDouble(properties.getProperty(prefix+"quat_reg_w"));

		if (properties.getProperty(prefix+"quat_max_ratio")!=null) this.quat_max_ratio=Double.parseDouble(properties.getProperty(prefix+"quat_max_ratio"));
		if (properties.getProperty(prefix+"quat_max_change")!=null) this.quat_max_change=Double.parseDouble(properties.getProperty(prefix+"quat_max_change"));
		if (properties.getProperty(prefix+"quat_min_transl")!=null) this.quat_min_transl=Double.parseDouble(properties.getProperty(prefix+"quat_min_transl"));
		if (properties.getProperty(prefix+"quat_min_rot")!=null) this.quat_min_rot=Double.parseDouble(properties.getProperty(prefix+"quat_min_rot"));

		if (properties.getProperty(prefix+"stereo_merge")!=null) this.stereo_merge=Boolean.parseBoolean(properties.getProperty(prefix+"stereo_merge"));
		if (properties.getProperty(prefix+"stereo_gap")!=null) this.stereo_gap=Integer.parseInt(properties.getProperty(prefix+"stereo_gap"));
		if (properties.getProperty(prefix+"stereo_intereye")!=null) this.stereo_intereye=Double.parseDouble(properties.getProperty(prefix+"stereo_intereye"));
		@@ -2675,6 +2728,9 @@ public class IntersceneMatchParameters {
		imp.min_num_orient = this.min_num_orient;
		imp.min_num_interscene = this.min_num_interscene;
		imp.adjust_imu_orient = this.adjust_imu_orient;
		imp.apply_imu_orient = this.apply_imu_orient;
		imp.adjust_gyro = this.adjust_gyro;
		imp.apply_gyro = this.apply_gyro;
		imp.calc_quat_corr = this.calc_quat_corr;
		imp.apply_quat_corr = this.apply_quat_corr;
		imp.use_quat_corr = this.use_quat_corr;
		@@ -2972,6 +3028,11 @@ public class IntersceneMatchParameters {
		imp.quat_num_iter = this.quat_num_iter;
		imp.quat_reg_w = this.quat_reg_w;

		imp.quat_max_ratio = this.quat_max_ratio;
		imp.quat_max_change = this.quat_max_change;
		imp.quat_min_transl = this.quat_min_transl;
		imp.quat_min_rot = this.quat_min_rot;

		imp.stereo_merge = this.stereo_merge;
		imp.stereo_gap = this.stereo_gap;
		imp.stereo_intereye = this.stereo_intereye;

src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java

+3 −0

Original line number	Diff line number	Diff line
		@@ -4659,7 +4659,10 @@ public class OpticalFlow {
		// skip completely if no color or mono, tiff or video

		boolean adjust_imu_orient = clt_parameters.imp.adjust_imu_orient; // calculate camera orientation correction from predicted by IMS

		boolean calc_quat_corr = clt_parameters.imp.calc_quat_corr; // calculate camera orientation correction from predicted by IMS


		// apply_quat_corr used inside getGroundIns() - used when generating output
		// boolean apply_quat_corr = clt_parameters.imp.apply_quat_corr; // apply camera orientation correction from predicted by IMS
		boolean use_ims_rotation = clt_parameters.imp.use_quat_corr; // use internally (probably deprecated

src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java

+141 −14

Original line number	Diff line number	Diff line
		@@ -266,6 +266,8 @@ public class QuadCLTCPU {
		int num_iter = clt_parameters.imp.quat_num_iter; // 20;
		boolean last_run = false;
		double reg_w = clt_parameters.imp.quat_reg_w; // 0.25;
		double quat_max_ratio = clt_parameters.imp.quat_max_ratio; // 0.25;

		double [] quat0 = use3? new double[3] : new double [] {1.0, 0.0, 0.0, 0.0}; // identity
		QuaternionLma quaternionLma = new QuaternionLma();
		double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR
		@@ -291,6 +293,22 @@ public class QuadCLTCPU {
		} else if ((quat_lma_mode == QuaternionLma.MODE_XYZQ) \|\|
		(quat_lma_mode == QuaternionLma.MODE_XYZQ_LOCAL) \|\|
		(quat_lma_mode == QuaternionLma.MODE_XYZ4Q3)) {
		quaternionLma.prepareLMA(
		quat_lma_mode, // int mode,
		vect_x, // double [][][] vect_x,
		vect_y_scaled, // vect_y, // double [][][] vect_y,
		null, // double [] vect_w, all same weight
		translation_weight, // double translation_weight, // 0.0 ... 1.0;
		0.0, // reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
		quat0, // double [] quat0,
		debugLevel); // int debug_level)
		double [] mn_mx_diag = quaternionLma.getMinMaxDiag(debugLevel);
		if (mn_mx_diag[0]quat_max_ratioquat_max_ratio < mn_mx_diag[1]) {
		double a = mn_mx_diag[1]; // Math.sqrt(mn_mx_diag[1]);
		reg_w = a/(a + quat_max_ratio*quat_max_ratio/quat0.length);
		if (debugLevel > -1) {
		System.out.println("==== Calculated reg_w = "+reg_w);
		}
		quaternionLma.prepareLMA(
		quat_lma_mode, // int mode,
		vect_x, // double [][][] vect_x,
		@@ -300,6 +318,7 @@ public class QuadCLTCPU {
		reg_w, // double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
		quat0, // double [] quat0,
		debugLevel); // int debug_level)
		}
		} else {
		/*
		double [][] vect_y = new double [quadCLTs.length][]; // camera XYZ
		@@ -394,9 +413,6 @@ public class QuadCLTCPU {
		xyzatr[nscene][1][0], xyzatr[nscene][1][1], xyzatr[nscene][1][2]);
		Rotation ims_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
		ims_xyzatr[nscene][1][0], ims_xyzatr[nscene][1][1], ims_xyzatr[nscene][1][2]);
		// Rotation rotation_atr = rot.applyTo(scene_atr);
		// Rotation rotation_atr = rot.applyTo(ims_atr);
		// Rotation rotation_atr2 = rotation_atr.applyTo(rot_invert); // applyInverseTo?
		Rotation rotation_atr,rotation_atr2;
		if (use_inv) {
		rotation_atr = rot_invert.applyTo(ims_atr);
		@@ -407,7 +423,6 @@ public class QuadCLTCPU {
		}
		rotated_xyzatr[nscene] = new double [][] {rotated_xyz.clone(),
		rotation_atr2.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
		/// rotation_atr.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV)};
		}
		if (debugLevel > -1) {
		double [] angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
		@@ -449,6 +464,9 @@ public class QuadCLTCPU {
		int last_index,
		int debugLevel
		) {
		boolean apply_imu_orient = clt_parameters.imp.apply_imu_orient; // apply IMU misalignment to the camera if adjusted
		boolean adjust_gyro = clt_parameters.imp.adjust_gyro; // adjust qyro omegas offsets
		boolean apply_gyro = clt_parameters.imp.apply_gyro; // apply adjusted qyro omegas offsets
		double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
		clt_parameters, // final CLTParameters clt_parameters,
		quadCLTs, // final QuadCLT[] quadCLTs,
		@@ -500,20 +518,129 @@ public class QuadCLTCPU {
		double [] new_ims_mount_atr = Imx5.adjustMountAtrByQuat(
		ims_mount_atr, // double [] ims_atr,
		quat); // double [] corr_q)
		if (apply_imu_orient) {
		clt_parameters.imp.setImsMountATR(new_ims_mount_atr);
		if (debugLevel > -3) {
		System.out.println ("* IMU mount angles updated, need to save the main configuration file for persistent storage *");
		}
		} else {
		System.out.println ("* IMU mount angles calculated, but not applied *");
		}
		double [] new_atr = clt_parameters.imp.getImsMountATR(); // converts to radians
		double [] degrees = new double[3];
		for (int i = 0; i < 3; i++) degrees[i]=new_atr[i]*180/Math.PI;
		System.out.println("New ATR(rad)=["+new_atr[0]+", "+new_atr[1]+", "+new_atr[2]+"]");
		System.out.println("New ATR(deg)=["+degrees[0]+", "+degrees[1]+", "+degrees[2]+"]");
		System.out.println(String.format(
		"Using ATR(rad)=[%9f, %9f, %9f]", new_atr[0], new_atr[1], new_atr[2]));
		System.out.println(String.format(
		"Using ATR(deg)=[%9f, %9f, %9f]", degrees[0], degrees[1], degrees[2]));
		double [] omega_corr = null;
		if (adjust_gyro) {
		omega_corr = getOmegaCorrections(
		clt_parameters, // CLTParameters clt_parameters, // CLTParameters clt_parameters,
		rotated_xyzatr, // double [][][] rotated_xyzatr,
		quadCLTs, // QuadCLT[] quadCLTs,
		ref_index, // int ref_index,
		earliest_scene, // int earliest_scene,
		last_index, // int last_index,
		debugLevel); // int debugLevel
		if (omega_corr != null) {
		double [] used_omegas = clt_parameters.imp.get_pimu_offs()[1]; // returns in atr order
		double [] new_omegas = new double[3];
		for (int i = 0; i < 3; i++) {
		new_omegas[i] = used_omegas[i] - omega_corr[i];
		}
		if (debugLevel > -1) {
		System.out.println(String.format(
		"Used omegas (ATR, rad/s) = [%9f, %9f, %9f]", used_omegas[0],used_omegas[1],used_omegas[2]));
		System.out.println(String.format(
		"Diff.omegas (ATR, rad/s) = [%9f, %9f, %9f]", omega_corr[0], omega_corr[1], omega_corr[2]));
		System.out.println(String.format(
		"New omegas (ATR, rad/s) = [%9f, %9f, %9f]", new_omegas[0], new_omegas[1], new_omegas[2]));
		}
		if (apply_gyro) {
		clt_parameters.imp.set_pimu_omegas(new_omegas);
		if (debugLevel > -3) {
		System.out.println(String.format(
		"Applied new gyro omegas (ATR, rad/s) = [%9f, %9f, %9f]", new_omegas[0], new_omegas[1], new_omegas[2]));
		System.out.println ("* Need to save the main configuration file *");
		}
		} else {
		if (debugLevel > -3) {
		System.out.println(String.format(
		"New gyro omegas (ATR, rad/s) are not applied = [%9f, %9f, %9f]", new_omegas[0], new_omegas[1], new_omegas[2]));
		}
		}
		} else {
		System.out.println("* Adjustment of the gyro omegas failed *");

		}
		}
		} else {
		System.out.println ("* Failed to calculate IMS mount correction! *");
		}
		}

		public static double [] getOmegaCorrections(
		CLTParameters clt_parameters, // CLTParameters clt_parameters,
		double [][][] rotated_xyzatr,
		QuadCLT[] quadCLTs,
		int ref_index,
		int early_index,
		int last_index,
		int debugLevel
		) {
		double [][][] xyzatr = new double [quadCLTs.length][][];
		ErsCorrection ers_ref = quadCLTs[ref_index].getErsCorrection();
		double [] timestamps = new double [quadCLTs.length];
		double [][][] data = new double[3][last_index-early_index+1][2];
		double ts_ref = quadCLTs[ref_index].getTimeStamp();
		for (int nscene = early_index; nscene <= last_index; nscene++) {
		timestamps[nscene] = quadCLTs[nscene].getTimeStamp();
		xyzatr[nscene] = ers_ref.getSceneXYZATR(quadCLTs[nscene].getImageName());
		Rotation scene_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
		xyzatr[nscene][1][0], xyzatr[nscene][1][1], xyzatr[nscene][1][2]);
		Rotation imu_atr = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
		rotated_xyzatr[nscene][1][0],
		rotated_xyzatr[nscene][1][1],
		rotated_xyzatr[nscene][1][2]);
		Rotation diff_rot = imu_atr.applyInverseTo(scene_atr); // left rotate IMU to match scene
		double rel_ts = timestamps[nscene] - ts_ref;
		double [] diff_atr = diff_rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
		for (int i = 0; i < 3; i++) {
		data[i][nscene-early_index][0] = rel_ts; // for all xyz the same
		data[i][nscene-early_index][1] = diff_atr[i];
		}
		}
		PolynomialApproximation pa= new PolynomialApproximation(-1); // debugLevel);
		double [][] coeffs = new double [3][2];
		for (int i = 0; i < 3; i++) {
		coeffs[i] = pa.polynomialApproximation1d(data[i], 1); // linear
		}
		if (debugLevel > -1) {
		System.out.println("Azimuth omega correction: " +coeffs[0][1]+" ("+coeffs[0][0]+")");
		System.out.println(" Tilt omega correction: " +coeffs[1][1]+" ("+coeffs[1][0]+")");
		System.out.println(" Roll omega correction: " +coeffs[2][1]+" ("+coeffs[2][0]+")");
		if (debugLevel > 0) {
		System.out.println(String.format("%3s"+
		"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s",
		"N","rt","dA","dT","dR","lA","lT","lR"));
		for (int nscene = early_index; nscene <= last_index; nscene++) {
		System.out.println(String.format("%3d"+
		"\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f\t%9.5f",
		nscene, data[0][nscene-early_index][0], // timestamps[nscene]-timestamps[ref_index],
		data[0][nscene-early_index][1],
		data[1][nscene-early_index][1],
		data[2][nscene-early_index][1],
		coeffs[0][0]+coeffs[0][1] * data[0][nscene-early_index][0],
		coeffs[1][0]+coeffs[1][1] * data[1][nscene-early_index][0],
		coeffs[2][0]+coeffs[2][1] * data[2][nscene-early_index][0]));
		}
		}
		}
		return new double [] {coeffs[0][1],coeffs[1][1],coeffs[2][1]};
		}


		/**
		* Refine scene poses (now only linear) from currently adjusted poses
		* (adjusted using IMS orientations) and IMS velocities (currently

src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java

+37 −5

Original line number	Diff line number	Diff line
		@@ -221,12 +221,13 @@ public class QuaternionLma {
		debug_level); // final int debug_level);
		return;
		}
		//MODE_XYZQ
		N = vect_x.length;
		this.mode = mode;
		samples = 3+ quat0.length;
		samples_x = 7;
		pure_weight = 1.0 - reg_w;
		int extra_samples = 0; // (quat0.length < 4)? 0:REGLEN;
		int extra_samples = (reg_w > 0) ? quat0.length:0; // (quat0.length < 4)? 0:REGLEN;
		x_vector = new double [samples_x * N];
		y_vector = new double [samples * N + extra_samples];
		weights = new double [samples * N + extra_samples];
		@@ -286,8 +287,11 @@ public class QuaternionLma {
		double k = (pure_weight)/sw;
		for (int i = 0; i < weights.length; i++) weights[i] *= k;
		if (extra_samples > 0) {
		weights [samples * N] = 1.0 - pure_weight;
		y_vector[samples * N] = 1.0;
		double w = (1.0 - pure_weight)/parameters_vector.length;
		for (int i = 0; i < parameters_vector.length; i++) {
		weights [samples * N + i] = w;
		y_vector[samples * N] = 0.0; // or target value
		}
		}
		last_jt = new double [parameters_vector.length][];
		if (debug_level > 0) {
		@@ -387,7 +391,7 @@ public class QuaternionLma {
		samples = 7;
		samples_x = 7;
		pure_weight = 1.0 - reg_w;
		int extra_samples = 0; // (quat0.length < 4)? 0:REGLEN;
		int extra_samples = 0; // (reg_w > 0)? quat0.length:0; // (quat0.length < 4)? 0:REGLEN;
		x_vector = new double [samples * N];
		y_vector = new double [samples * N + extra_samples];
		y_inv_vector = new double [samples_x * N + extra_samples];
		@@ -1130,6 +1134,14 @@ public class QuaternionLma {
		}
		}
		}
		if (weights.length > N*samples) {
		for (int i = 0; i < vector3.length; i++) {
		fx[samples*N + i] = vector3[i];
		if (jt != null) {
		jt[i][samples*N + i] = 1.0;
		}
		}
		}
		return fx;
		}

		@@ -1563,6 +1575,26 @@ public class QuaternionLma {
		return rslt[0]? iter : -1;
		}

		public double [] getMinMaxDiag(
		int debug_level){
		double [][] jt = new double [parameters_vector.length][];
		double [] fx = getFxDerivs(
		parameters_vector, // double [] vector,
		jt, // final double [][] jt, // should be null or initialized with [vector.length][]
		debug_level); // final int debug_level)
		Matrix wjtjlambda = new Matrix(getWJtJlambda(
		0, // *10, // temporary
		jt)); // double [][] jt)
		double [] mn_mx= {Double.NaN,Double.NaN};
		for (int i = 0; i < parameters_vector.length; i++) {
		double d = wjtjlambda.get(i,i);
		if (!(d > mn_mx[0])) mn_mx[0] = d;
		if (!(d < mn_mx[1])) mn_mx[1] = d;

		}
		return mn_mx;

		}
		private boolean [] lmaStep(
		double lambda,
		double rms_diff,

Admin message