Commit 77381e24 authored by Andrey Filippov's avatar Andrey Filippov

Unfinished mods

parent 2014ae08
...@@ -9,6 +9,7 @@ import java.nio.ByteOrder; ...@@ -9,6 +9,7 @@ import java.nio.ByteOrder;
import org.apache.commons.math3.geometry.euclidean.threed.Rotation; import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention; import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder; import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
import com.elphel.imagej.tileprocessor.ErsCorrection; import com.elphel.imagej.tileprocessor.ErsCorrection;
...@@ -207,6 +208,18 @@ public class Imx5 { ...@@ -207,6 +208,18 @@ public class Imx5 {
rslt_rot.getQ3()}; rslt_rot.getQ3()};
} }
/**
* Get up (for IMS) vector relative to the camera reference frame
* @param ims_atr mount to camera correction
* @return {x,y,z} of the IMS-up relative to the camera frame (reference scene)
*/
public static double [] getUpAxis(
double [] ims_atr ) {// -> mount_to_cam
Rotation mount_to_cam = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
ims_atr[0], ims_atr[1], ims_atr[2]);
return mount_to_cam.applyInverseTo(new Vector3D(0, 0, 1)).toArray();
}
public static double [] quaternionImsToCam( public static double [] quaternionImsToCam(
double [] quat, // ims_to_ned double [] quat, // ims_to_ned
......
...@@ -5032,7 +5032,9 @@ public class OpticalFlow { ...@@ -5032,7 +5032,9 @@ public class OpticalFlow {
ref_index,// ref_indx, ref_index,// ref_indx,
earliest_scene, // int earliest_scene, earliest_scene, // int earliest_scene,
ego_path, // String path, ego_path, // String path,
ego_comment); // String comment); ego_comment, // String comment);
debugLevel); // int debugLevel);
if (debugLevel> -3) { if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path); System.out.println("Egomotion table saved to "+ego_path);
} }
...@@ -5386,7 +5388,8 @@ public class OpticalFlow { ...@@ -5386,7 +5388,8 @@ public class OpticalFlow {
ref_index,// ref_indx, ref_index,// ref_indx,
earliest_scene, // int earliest_scene, earliest_scene, // int earliest_scene,
ego_path, // String path, ego_path, // String path,
ego_comment); // String comment); ego_comment, // String comment);
debugLevel); // int debugLevel)
if (debugLevel> -3) { if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path); System.out.println("Egomotion table saved to "+ego_path);
} }
...@@ -5438,7 +5441,7 @@ public class OpticalFlow { ...@@ -5438,7 +5441,7 @@ public class OpticalFlow {
} }
double [] rms = new double[5]; double [] rms = new double[5];
double [] quat = new double[4]; double [] quat = new double[4];
int quat_lma_mode = 4; // 3; // 2; // 1; int quat_lma_mode = 03; // 4; // 3; // 2; // 1;
int debug_lev = debugLevel; // 3; int debug_lev = debugLevel; // 3;
double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters double avg_z = quadCLTs[ref_index].getAverageZ(true); // in meters
double translation_weight = 1.0 / (avg_z + 1.0); double translation_weight = 1.0 / (avg_z + 1.0);
...@@ -5583,7 +5586,8 @@ public class OpticalFlow { ...@@ -5583,7 +5586,8 @@ public class OpticalFlow {
quadCLTs, // QuadCLT [] quadCLTs, quadCLTs, // QuadCLT [] quadCLTs,
ref_index, // int ref_index, ref_index, // int ref_index,
earliest_scene, // int earliest_scene, earliest_scene, // int earliest_scene,
quat_rms); // double [] rms // null or double[2]; quat_rms, // double [] rms // null or double[2];
debugLevel); // int debugLevel
if (quatCorr != null) { if (quatCorr != null) {
int num_iter = (int) quat_rms[4]; int num_iter = (int) quat_rms[4];
if (debugLevel> -3) { if (debugLevel> -3) {
...@@ -5596,6 +5600,16 @@ public class OpticalFlow { ...@@ -5596,6 +5600,16 @@ public class OpticalFlow {
quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...) quadCLTs[ref_index].saveInterProperties( // save properties for interscene processing (extrinsics, ers, ...)
null, // String path, // full name with extension or w/o path to use x3d directory null, // String path, // full name with extension or w/o path to use x3d directory
debugLevel+1); debugLevel+1);
if (debugLevel > -3) {
Rotation rot = new Rotation(quatCorr[0],quatCorr[1],quatCorr[2],quatCorr[3], false); // no normalization - see if can be scaled
System.out.println("Applying correction ot the IMS to world orientation (rotating around IMS vertical):");
double [] corr_angles = rot.getAngles(RotationOrder.YXZ, ErsCorrection.ROT_CONV);
double [] corr_degrees = new double[3];
for (int i = 0; i < 3; i++) corr_degrees[i]=corr_angles[i]*180/Math.PI;
System.out.println("quatCorr=["+quatCorr[0]+", "+quatCorr[1]+", "+quatCorr[2]+", "+quatCorr[3]+"]");
System.out.println("ATR(rad)=["+corr_angles[0]+", "+corr_angles[1]+", "+corr_angles[2]+"]");
System.out.println("ATR(deg)=["+corr_degrees[0]+", "+corr_degrees[1]+", "+corr_degrees[2]+"]");
}
} else { } else {
if (debugLevel> -3) { if (debugLevel> -3) {
System.out.println("Failed to perform attitude correction with QuaternionLma."); System.out.println("Failed to perform attitude correction with QuaternionLma.");
...@@ -5616,7 +5630,9 @@ public class OpticalFlow { ...@@ -5616,7 +5630,9 @@ public class OpticalFlow {
ref_index,// ref_indx, ref_index,// ref_indx,
earliest_scene, // int earliest_scene, earliest_scene, // int earliest_scene,
ego_path, // String path, ego_path, // String path,
ego_comment); // String comment); ego_comment, // String comment);
debugLevel); // int debugLevel)
if (debugLevel> -3) { if (debugLevel> -3) {
System.out.println("Egomotion table saved to "+ego_path); System.out.println("Egomotion table saved to "+ego_path);
} }
...@@ -5628,7 +5644,8 @@ public class OpticalFlow { ...@@ -5628,7 +5644,8 @@ public class OpticalFlow {
ref_index,// ref_indx, ref_index,// ref_indx,
earliest_scene, // int earliest_scene, earliest_scene, // int earliest_scene,
ego_path, // String path, ego_path, // String path,
ego_comment); // String comment); ego_comment, // String comment);
debugLevel); // int debugLevel)
} }
} }
boolean test_ground = false; // true; boolean test_ground = false; // true;
...@@ -8256,7 +8273,8 @@ public class OpticalFlow { ...@@ -8256,7 +8273,8 @@ public class OpticalFlow {
int start_scene, int start_scene,
int end_scene, int end_scene,
double [][][] scenes_xyzatr, // <=0 use +/-1 or +0 if other are not available double [][][] scenes_xyzatr, // <=0 use +/-1 or +0 if other are not available
double half_run_range double half_run_range,
int debugLevel
){ ){
double [][][] ers_xyzatr = new double [scenes_xyzatr.length][][]; double [][][] ers_xyzatr = new double [scenes_xyzatr.length][][];
if (half_run_range <=0 ) { if (half_run_range <=0 ) {
...@@ -8314,9 +8332,13 @@ public class OpticalFlow { ...@@ -8314,9 +8332,13 @@ public class OpticalFlow {
for (int m = 0; m < sy.length; m++) { for (int m = 0; m < sy.length; m++) {
for (int d = 0; d < sy[m].length; d++) { for (int d = 0; d < sy[m].length; d++) {
if (scenes_xyzatr[ns]== null ) { if (scenes_xyzatr[ns]== null ) {
if (debugLevel > -1) {
System.out.println("getVelocitiesFromScenes():scenes_xyzatr["+ns+"]== null"); System.out.println("getVelocitiesFromScenes():scenes_xyzatr["+ns+"]== null");
}
}else if (scenes_xyzatr[ns][m]== null ) { }else if (scenes_xyzatr[ns][m]== null ) {
if (debugLevel > -1) {
System.out.println("getVelocitiesFromScenes():scenes_xyzatr["+ns+"]["+m+"] == null"); System.out.println("getVelocitiesFromScenes():scenes_xyzatr["+ns+"]["+m+"] == null");
}
} else { } else {
double y = scenes_xyzatr[ns][m][d]; double y = scenes_xyzatr[ns][m][d];
sy [m][d] += w * y; sy [m][d] += w * y;
......
...@@ -256,6 +256,7 @@ public class QuadCLTCPU { ...@@ -256,6 +256,7 @@ public class QuadCLTCPU {
double [] rms, // null or double[5]; double [] rms, // null or double[5];
int debugLevel int debugLevel
) { ) {
final boolean use3 = (quat_lma_mode == 3); // true;
double lambda = clt_parameters.imp.quat_lambda; // 0.1; double lambda = clt_parameters.imp.quat_lambda; // 0.1;
double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5; double lambda_scale_good = clt_parameters.imp.quat_lambda_scale_good; // 0.5;
double lambda_scale_bad = clt_parameters.imp.quat_lambda_scale_bad; // 8.0; double lambda_scale_bad = clt_parameters.imp.quat_lambda_scale_bad; // 8.0;
...@@ -264,7 +265,7 @@ public class QuadCLTCPU { ...@@ -264,7 +265,7 @@ public class QuadCLTCPU {
int num_iter = clt_parameters.imp.quat_num_iter; // 20; int num_iter = clt_parameters.imp.quat_num_iter; // 20;
boolean last_run = false; boolean last_run = false;
double reg_w = clt_parameters.imp.quat_reg_w; // 0.25; double reg_w = clt_parameters.imp.quat_reg_w; // 0.25;
double [] quat0 = new double [] {1.0, 0.0, 0.0, 0.0}; // identity double [] quat0 = use3? new double[3] : new double [] {1.0, 0.0, 0.0, 0.0}; // identity
QuaternionLma quaternionLma = new QuaternionLma(); QuaternionLma quaternionLma = new QuaternionLma();
if ((quat_lma_mode == 2) || (quat_lma_mode == 4)) { if ((quat_lma_mode == 2) || (quat_lma_mode == 4)) {
double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR double [][][] vect_y = new double [quadCLTs.length][][]; // camera XYZATR
......
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