Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
I
imagej-elphel
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
3
Issues
3
List
Board
Labels
Milestones
Wiki
Wiki
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Commits
Issue Boards
Open sidebar
Elphel
imagej-elphel
Commits
21b8905c
Commit
21b8905c
authored
Oct 08, 2023
by
Andrey Filippov
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Before interscene refactoring
parent
b9a32b13
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
with
64 additions
and
16 deletions
+64
-16
Imx5.java
src/main/java/com/elphel/imagej/ims/Imx5.java
+32
-1
ErsCorrection.java
...n/java/com/elphel/imagej/tileprocessor/ErsCorrection.java
+1
-1
OpticalFlow.java
...ain/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
+31
-14
No files found.
src/main/java/com/elphel/imagej/ims/Imx5.java
View file @
21b8905c
...
@@ -10,6 +10,8 @@ import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
...
@@ -10,6 +10,8 @@ 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
com.elphel.imagej.tileprocessor.ErsCorrection
;
public
class
Imx5
{
public
class
Imx5
{
...
@@ -173,6 +175,35 @@ public class Imx5 {
...
@@ -173,6 +175,35 @@ public class Imx5 {
return
rslt
;
return
rslt
;
}
}
public
static
double
[]
quaternionImsToCam
(
double
[]
quat
)
{
double
[][]
ort
=
{{
0
,-
1
,
0
},{
0
,
0
,-
1
},{
1
,
0
,
0
}};
// multiply by camera xyz, get imu xyz
Rotation
cam_to_ims0
=
new
Rotation
(
ort
,
1
E
-
8
);
Rotation
ims_rot
=
new
Rotation
(
quat
[
0
],
quat
[
1
],
quat
[
2
],
quat
[
3
],
true
);
Rotation
cam_quat
=
cam_to_ims0
.
applyTo
(
ims_rot
);
return
new
double
[]
{
cam_quat
.
getQ0
(),
cam_quat
.
getQ1
(),
cam_quat
.
getQ2
(),
cam_quat
.
getQ3
()};
}
/**
* Get quaternion from IMS data, fixed ortho rotation for IMU to camera and small
* additional correction of IMU mount to the camera using camera axes (Y- up, X - right,
* z - back from the target)
* @param quat IMU-measured quaternion
* @param ims_atr azimuth, tilt, roll of the IMU mount relative to the camera axes
* @return quaternion to apply to NED to get camera-referenced coordinates
*/
public
static
double
[]
quaternionImsToCam
(
double
[]
quat
,
double
[]
ims_atr
)
{
double
[][]
ort
=
{{
0
,-
1
,
0
},{
0
,
0
,-
1
},{
1
,
0
,
0
}};
// multiply by camera xyz, get imu xyz
Rotation
ims_to_mount_ortho
=
new
Rotation
(
ort
,
1
E
-
8
);
Rotation
ims_to_ned
=
new
Rotation
(
quat
[
0
],
quat
[
1
],
quat
[
2
],
quat
[
3
],
true
);
Rotation
mount_to_cam
=
new
Rotation
(
RotationOrder
.
YXZ
,
ErsCorrection
.
ROT_CONV
,
ims_atr
[
0
],
ims_atr
[
1
],
ims_atr
[
2
]);
Rotation
mount_to_ned
=
ims_to_mount_ortho
.
applyTo
(
ims_to_ned
);
Rotation
cam_quat
=
mount_to_cam
.
applyTo
(
mount_to_ned
);
return
new
double
[]
{
cam_quat
.
getQ0
(),
cam_quat
.
getQ1
(),
cam_quat
.
getQ2
(),
cam_quat
.
getQ3
()};
}
public
static
double
[]
imsToCamRotations
(
double
[]
ims_theta
,
int
ord
,
boolean
rev_order
,
boolean
rev_matrix
)
{
public
static
double
[]
imsToCamRotations
(
double
[]
ims_theta
,
int
ord
,
boolean
rev_order
,
boolean
rev_matrix
)
{
RotationConvention
rc
=
RotationConvention
.
FRAME_TRANSFORM
;
RotationConvention
rc
=
RotationConvention
.
FRAME_TRANSFORM
;
// RotationOrder ro = zyx ? RotationOrder.ZYX : RotationOrder.XYZ; // XYZ; // ZYX;
// RotationOrder ro = zyx ? RotationOrder.ZYX : RotationOrder.XYZ; // XYZ; // ZYX;
...
@@ -226,8 +257,8 @@ public class Imx5 {
...
@@ -226,8 +257,8 @@ public class Imx5 {
public
static
double
[]
nedFromLla
(
double
[]
lla
,
double
[]
lla_ref
)
{
public
static
double
[]
nedFromLla
(
double
[]
lla
,
double
[]
lla_ref
)
{
double
[]
ned
=
new
double
[]
{
double
[]
ned
=
new
double
[]
{
EARTH_RADIUS
*
Math
.
cos
(
lla
[
0
]
*
Math
.
PI
/
180
)*(
lla
[
1
]
-
lla_ref
[
1
])
*
Math
.
PI
/
180.0
,
EARTH_RADIUS
*
(
lla
[
0
]-
lla_ref
[
0
])
*
Math
.
PI
/
180
,
EARTH_RADIUS
*
(
lla
[
0
]-
lla_ref
[
0
])
*
Math
.
PI
/
180
,
EARTH_RADIUS
*
Math
.
cos
(
lla
[
0
]
*
Math
.
PI
/
180
)*(
lla
[
1
]
-
lla_ref
[
1
])
*
Math
.
PI
/
180.0
,
-(
lla
[
2
]
-
lla_ref
[
2
])
-(
lla
[
2
]
-
lla_ref
[
2
])
};
};
return
ned
;
return
ned
;
...
...
src/main/java/com/elphel/imagej/tileprocessor/ErsCorrection.java
View file @
21b8905c
...
@@ -187,7 +187,7 @@ public class ErsCorrection extends GeometryCorrection {
...
@@ -187,7 +187,7 @@ public class ErsCorrection extends GeometryCorrection {
DP_DVX
,
DP_DVY
,
DP_DVZ
,
DP_DVX
,
DP_DVY
,
DP_DVZ
,
DP_DSVAZ
,
DP_DSVTL
,
DP_DSVRL
,
DP_DSVAZ
,
DP_DSVTL
,
DP_DSVRL
,
DP_DSVX
,
DP_DSVY
,
DP_DSVZ
};
DP_DSVX
,
DP_DSVY
,
DP_DSVZ
};
static
final
RotationConvention
ROT_CONV
=
RotationConvention
.
FRAME_TRANSFORM
;
public
static
final
RotationConvention
ROT_CONV
=
RotationConvention
.
FRAME_TRANSFORM
;
static
final
double
THRESHOLD
=
1
E
-
10
;
static
final
double
THRESHOLD
=
1
E
-
10
;
static
final
double
LINE_ERR
=
0.001
;
// line accuracy for ERS when converting from world to pixels.
static
final
double
LINE_ERR
=
0.001
;
// line accuracy for ERS when converting from world to pixels.
// parameters for the ERS distortion calculation
// parameters for the ERS distortion calculation
...
...
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @
21b8905c
...
@@ -26,12 +26,8 @@ package com.elphel.imagej.tileprocessor;
...
@@ -26,12 +26,8 @@ package com.elphel.imagej.tileprocessor;
import java.awt.Color;
import java.awt.Color;
import java.awt.Font;
import java.awt.Font;
import java.awt.Rectangle;
import java.awt.Rectangle;
import java.io.ByteArrayOutputStream;
import java.io.File;
import java.io.File;
import java.io.IOException;
import java.io.IOException;
import java.io.ObjectOutputStream;
import java.io.Serializable;
import java.security.MessageDigest;
import java.security.NoSuchAlgorithmException;
import java.security.NoSuchAlgorithmException;
import java.util.ArrayList;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Arrays;
...
@@ -42,9 +38,6 @@ import java.util.Properties;
...
@@ -42,9 +38,6 @@ import java.util.Properties;
import java.util.concurrent.atomic.AtomicBoolean;
import java.util.concurrent.atomic.AtomicBoolean;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.atomic.DoubleAccumulator;
import java.util.concurrent.atomic.DoubleAccumulator;
import javax.xml.bind.DatatypeConverter;
import org.apache.commons.math3.complex.Quaternion;
import java.util.concurrent.ThreadLocalRandom;
import java.util.concurrent.ThreadLocalRandom;
...
@@ -55,14 +48,11 @@ import com.elphel.imagej.cameras.EyesisCorrectionParameters;
...
@@ -55,14 +48,11 @@ import com.elphel.imagej.cameras.EyesisCorrectionParameters;
import com.elphel.imagej.common.DoubleGaussianBlur;
import com.elphel.imagej.common.DoubleGaussianBlur;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.common.ShowDoubleFloatArrays;
import com.elphel.imagej.correction.CorrectionColorProc;
import com.elphel.imagej.correction.CorrectionColorProc;
import com.elphel.imagej.correction.Eyesis_Correction;
import com.elphel.imagej.gpu.GpuQuad;
import com.elphel.imagej.gpu.GpuQuad;
import com.elphel.imagej.gpu.TpTask;
import com.elphel.imagej.gpu.TpTask;
import com.elphel.imagej.ims.Did_gps_pos;
import com.elphel.imagej.ims.Did_ins_1;
import com.elphel.imagej.ims.Did_ins_1;
import com.elphel.imagej.ims.Did_ins_2;
import com.elphel.imagej.ims.Did_ins_2;
import com.elphel.imagej.ims.Did_pimu;
import com.elphel.imagej.ims.Did_pimu;
import com.elphel.imagej.ims.EventLogger;
import com.elphel.imagej.ims.Imx5;
import com.elphel.imagej.ims.Imx5;
import com.elphel.imagej.jp4.JP46_Reader_camera;
import com.elphel.imagej.jp4.JP46_Reader_camera;
...
@@ -76,7 +66,6 @@ import ij.process.FloatProcessor;
...
@@ -76,7 +66,6 @@ import ij.process.FloatProcessor;
import ij.process.ImageConverter;
import ij.process.ImageConverter;
import ij.process.ImageProcessor;
import ij.process.ImageProcessor;
import ij.text.TextWindow;
import ij.text.TextWindow;
import ij.plugin.filter.AVI_Writer;
import ij.plugin.filter.GaussianBlur;
import ij.plugin.filter.GaussianBlur;
public class OpticalFlow {
public class OpticalFlow {
...
@@ -134,7 +123,7 @@ public class OpticalFlow {
...
@@ -134,7 +123,7 @@ public class OpticalFlow {
{W,W,1,W,W,W,1,W},
{W,W,1,W,W,W,1,W},
{W,W,W,1,W,W,W,1},
{W,W,W,1,W,W,W,1},
{W,1,W,W,W,1,W,W}};
{W,1,W,W,W,1,W,W}};
// Same for a large
d
kernel (5x5)
// Same for a large
r
kernel (5x5)
public static int [][] KERN_FG2 = {
public static int [][] KERN_FG2 = {
// 0 1 2 3 4 5 6 7 8 91011121314151617181920212223
// 0 1 2 3 4 5 6 7 8 91011121314151617181920212223
{1,1,3,2,2,2,3,1,1,1,0,0,3,0,0,2,2,2,0,0,3,0,0,1},
{1,1,3,2,2,2,3,1,1,1,0,0,3,0,0,2,2,2,0,0,3,0,0,1},
...
@@ -6251,7 +6240,9 @@ public class OpticalFlow {
...
@@ -6251,7 +6240,9 @@ public class OpticalFlow {
"\tlat\tlong\talt\tned0\tned1\tned2";
"\tlat\tlong\talt\tned0\tned1\tned2";
String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlat\tlong\talt";
String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlat\tlong\talt";
String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
"\tcam_X1\tcam_Y1\tcam_Z1\tcam_X2\tcam_Y2\tcam_Z2"+
"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
String header_cam000= "\ttheta0-000\ttheta1-000\ttheta2-000";
String header_cam000= "\ttheta0-000\ttheta1-000\ttheta2-000";
...
@@ -6377,7 +6368,7 @@ public class OpticalFlow {
...
@@ -6377,7 +6368,7 @@ public class OpticalFlow {
sb.append("\t"+d1.uvw[0]+ "\t"+d1.uvw[1]+ "\t"+d1.uvw[2]);
sb.append("\t"+d1.uvw[0]+ "\t"+d1.uvw[1]+ "\t"+d1.uvw[2]);
sb.append("\t"+d1.lla[0]+ "\t"+d1.lla[1]+ "\t"+d1.lla[2]);
sb.append("\t"+d1.lla[0]+ "\t"+d1.lla[1]+ "\t"+d1.lla[2]);
sb.append("\t"+d1.ned[0]+ "\t"+d1.ned[1]+ "\t"+d1.ned[2]);
sb.append("\t"+d1.ned[0]+ "\t"+d1.ned[1]+ "\t"+d1.ned[2]);
//
String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlong\tlat
\talt";
//
String header_ins2="\ttow\tqn2b0\tqn2b1\tqn2b2\tqn2b3\tu(m/s)\tv(m/s)\tw(m/s)\tlat\tlong
\talt";
sb.append("\t"+d2.timeOfWeek);
sb.append("\t"+d2.timeOfWeek);
sb.append("\t"+d2.qn2b[0]+"\t"+d2.qn2b[1]+"\t"+d2.qn2b[2]+"\t"+d2.qn2b[3]);
sb.append("\t"+d2.qn2b[0]+"\t"+d2.qn2b[1]+"\t"+d2.qn2b[2]+"\t"+d2.qn2b[3]);
sb.append("\t"+d2.uvw[0]+ "\t"+d2.uvw[1]+ "\t"+d2.uvw[2]);
sb.append("\t"+d2.uvw[0]+ "\t"+d2.uvw[1]+ "\t"+d2.uvw[2]);
...
@@ -6392,14 +6383,40 @@ public class OpticalFlow {
...
@@ -6392,14 +6383,40 @@ public class OpticalFlow {
double [] ned = Imx5.nedFromLla (d2.lla, d2_ref.lla);
double [] ned = Imx5.nedFromLla (d2.lla, d2_ref.lla);
double [] ims_xyz = Imx5.applyQuternionTo(double_qn2b, ned, false);
double [] ims_xyz = Imx5.applyQuternionTo(double_qn2b, ned, false);
// String header_ins2_extra="\tned_N\tned_E\tned_D\timu_X\timu_Y\timu_Z"+
// "\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv";
sb.append("\t"+ned[0]+ "\t"+ned[1]+ "\t"+ned[2]); // global axes
sb.append("\t"+ned[0]+ "\t"+ned[1]+ "\t"+ned[2]); // global axes
sb.append("\t"+ims_xyz[0]+ "\t"+ims_xyz[1]+ "\t"+ims_xyz[2]); // imu axes
sb.append("\t"+ims_xyz[0]+ "\t"+ims_xyz[1]+ "\t"+ims_xyz[2]); // imu axes
// double [] quaternionImsToCam(double[]quat, boolean rvrs)
double [] cam_quat1 =Imx5.quaternionImsToCam(double_qn2b);
double [] cam_quat2 =Imx5.quaternionImsToCam(double_qn2b,
// new double[] {Math.PI/4,0,0});
new double[] {0, 0.13, 0});
double [] cam_xyz1 = Imx5.applyQuternionTo(cam_quat1, ned, false);
double [] cam_xyz2 = Imx5.applyQuternionTo(cam_quat2, ned, false);
/*
public static double [] quaternionImsToCam(double[]quat, double [] ims_atr) {
double [][] ort = {{0,-1,0},{0, 0,-1},{1, 0,0}}; // multiply by camera xyz, get imu xyz
Rotation ims_to_mount_ortho = new Rotation(ort, 1E-8);
Rotation ims_to_ned = new Rotation(quat[0],quat[1],quat[2],quat[3],true);
Rotation mount_to_cam = new Rotation(RotationOrder.YXZ, ErsCorrection.ROT_CONV,
ims_atr[0], ims_atr[1], ims_atr[2]);
Rotation mount_to_ned = ims_to_mount_ortho.applyTo(ims_to_ned);
Rotation cam_quat = mount_to_cam.applyTo(mount_to_ned);
return new double [] {cam_quat.getQ0(),cam_quat.getQ1(),cam_quat.getQ2(),cam_quat.getQ3()};
}
*/
sb.append("\t"+cam_xyz1[0]+ "\t"+cam_xyz1[1]+ "\t"+cam_xyz1[2]); //
sb.append("\t"+cam_xyz2[0]+ "\t"+cam_xyz2[1]+ "\t"+cam_xyz2[2]); //
sb.append("\t"+uvw_dir[0]+ "\t"+uvw_dir[1]+ "\t"+uvw_dir[2]); // wrong
sb.append("\t"+uvw_dir[0]+ "\t"+uvw_dir[1]+ "\t"+uvw_dir[2]); // wrong
sb.append("\t"+uvw_inv[0]+ "\t"+uvw_inv[1]+ "\t"+uvw_inv[2]); // correct
sb.append("\t"+uvw_inv[0]+ "\t"+uvw_inv[1]+ "\t"+uvw_inv[2]); // correct
// String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
// String header_pimu="\to0\to1\to2\ta0\ta1\ta2";
sb.append("\t"+(d3.theta[0]/d3.dt)+"\t"+(d3.theta[1]/d3.dt)+"\t"+(d3.theta[2]/d3.dt));
sb.append("\t"+(d3.theta[0]/d3.dt)+"\t"+(d3.theta[1]/d3.dt)+"\t"+(d3.theta[2]/d3.dt));
sb.append("\t"+(d3.vel[0]/d3.dt)+"\t"+(d3.vel[1]/d3.dt)+"\t"+(d3.vel[2]/d3.dt));
sb.append("\t"+(d3.vel[0]/d3.dt)+"\t"+(d3.vel[1]/d3.dt)+"\t"+(d3.vel[2]/d3.dt));
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment