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
e9aa9780
Commit
e9aa9780
authored
Nov 30, 2023
by
Andrey Filippov
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Initial ERS from IMS
parent
99fcbcc4
Changes
6
Show whitespace changes
Inline
Side-by-side
Showing
6 changed files
with
209 additions
and
507 deletions
+209
-507
Imx5.java
src/main/java/com/elphel/imagej/ims/Imx5.java
+12
-77
ErsCorrection.java
...n/java/com/elphel/imagej/tileprocessor/ErsCorrection.java
+7
-0
Interscene.java
...main/java/com/elphel/imagej/tileprocessor/Interscene.java
+102
-427
IntersceneMatchParameters.java
...lphel/imagej/tileprocessor/IntersceneMatchParameters.java
+16
-0
OpticalFlow.java
...ain/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
+21
-3
QuadCLTCPU.java
...main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
+51
-0
No files found.
src/main/java/com/elphel/imagej/ims/Imx5.java
View file @
e9aa9780
...
...
@@ -187,25 +187,11 @@ public class Imx5 {
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
);
public
static
double
[]
quaternionImsToCam
(
double
[]
quat
,
// ims_to_ned
double
[]
ims_atr
,
// -> mount_to_cam
double
[]
quat_ort
)
{
// -> ims_to_mount_ortho
Rotation
ims_to_mount_ortho
=
new
Rotation
(
quat_ort
[
0
],
quat_ort
[
1
],
quat_ort
[
2
],
quat_ort
[
3
],
true
);
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
]);
...
...
@@ -214,75 +200,24 @@ public class Imx5 {
return
new
double
[]
{
cam_quat
.
getQ0
(),
cam_quat
.
getQ1
(),
cam_quat
.
getQ2
(),
cam_quat
.
getQ3
()};
}
// from ims frame to camera frame
public
static
double
[]
quaternionImsToCam
(
double
[]
quat
,
double
[]
ims_atr
,
double
[]
quat_ort
)
{
double
[]
ims_atr
,
// -> mount_to_cam
double
[]
quat_ort
)
{
// -> ims_to_mount_ortho
Rotation
ims_to_mount_ortho
=
new
Rotation
(
quat_ort
[
0
],
quat_ort
[
1
],
quat_ort
[
2
],
quat_ort
[
3
],
true
);
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
);
Rotation
cam_quat
=
mount_to_cam
.
applyTo
(
ims_to_mount_ortho
);
return
new
double
[]
{
cam_quat
.
getQ0
(),
cam_quat
.
getQ1
(),
cam_quat
.
getQ2
(),
cam_quat
.
getQ3
()};
}
public
static
double
[]
quatToCamAtr
(
double
[]
quat
)
{
Rotation
rot
=
new
Rotation
(
quat
[
0
],
quat
[
1
],
quat
[
2
],
quat
[
3
],
true
);
return
rot
.
getAngles
(
RotationOrder
.
YXZ
,
ErsCorrection
.
ROT_CONV
);
}
public
static
double
[]
imsToCamRotations
(
double
[]
ims_theta
,
int
ord
,
boolean
rev_order
,
boolean
rev_matrix
)
{
RotationConvention
rc
=
RotationConvention
.
FRAME_TRANSFORM
;
// RotationOrder ro = zyx ? RotationOrder.ZYX : RotationOrder.XYZ; // XYZ; // ZYX;
RotationOrder
ro
=
RotationOrder
.
XYZ
;
switch
(
ord
)
{
case
0
:
ro
=
RotationOrder
.
XYZ
;
break
;
case
1
:
ro
=
RotationOrder
.
XZY
;
break
;
case
2
:
ro
=
RotationOrder
.
YXZ
;
break
;
case
3
:
ro
=
RotationOrder
.
YZX
;
break
;
case
4
:
ro
=
RotationOrder
.
ZXY
;
break
;
case
5
:
ro
=
RotationOrder
.
ZYX
;
break
;
}
RotationOrder
ro1
=
ro
;
// ? RotationOrder.ZYX : RotationOrder.XYZ; //.XYZ;
Rotation
cam_to_ims0
=
new
Rotation
(
new
double
[][]
{{
0
,
0
,
1
},{-
1
,
0
,
0
},{
0
,-
1
,
0
}},
1
E
-
8
);
// Rotation cam_to_ims1 = new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2);
Rotation
cam_to_ims
=
rev_matrix
?
(
new
Rotation
(
ro
,
rc
,
-
Math
.
PI
/
2
,
0
,
-
Math
.
PI
/
2
)):(
new
Rotation
(
ro
,
rc
,
Math
.
PI
/
2
,
0
,
Math
.
PI
/
2
));
double
[]
angles1
=
cam_to_ims
.
getAngles
(
ro1
,
rc
);
Rotation
ims_rot
=
new
Rotation
(
ro
,
rc
,
ims_theta
[
0
],
ims_theta
[
1
],
ims_theta
[
2
]);
//Rotation r1= new Rotation(RotationOrder.YXZ, RC, ro.aux_azimuth, ro.aux_tilt, ro.aux_roll)
Rotation
comb_rot
=
rev_order
?
ims_rot
.
applyTo
(
cam_to_ims
):
cam_to_ims
.
applyTo
(
ims_rot
);
double
[]
angles
=
comb_rot
.
getAngles
(
ro1
,
rc
);
return
angles
;
}
public
static
double
[]
imsQToCamRotations
(
double
[]
ims_qn2b
,
int
ord
,
boolean
rev_order
,
boolean
rev_matrix
)
{
RotationConvention
rc
=
RotationConvention
.
FRAME_TRANSFORM
;
// RotationOrder ro = zyx ? RotationOrder.ZYX : RotationOrder.XYZ; // XYZ; // ZYX;
RotationOrder
ro
=
RotationOrder
.
XYZ
;
switch
(
ord
)
{
case
0
:
ro
=
RotationOrder
.
XYZ
;
break
;
case
1
:
ro
=
RotationOrder
.
XZY
;
break
;
case
2
:
ro
=
RotationOrder
.
YXZ
;
break
;
case
3
:
ro
=
RotationOrder
.
YZX
;
break
;
case
4
:
ro
=
RotationOrder
.
ZXY
;
break
;
case
5
:
ro
=
RotationOrder
.
ZYX
;
break
;
}
RotationOrder
ro1
=
ro
;
// ? RotationOrder.ZYX : RotationOrder.XYZ; //.XYZ;
Rotation
cam_to_ims0
=
new
Rotation
(
new
double
[][]
{{
0
,
0
,
1
},{-
1
,
0
,
0
},{
0
,-
1
,
0
}},
1
E
-
8
);
// Rotation cam_to_ims1 = new Rotation(ro, rc, -Math.PI/2, 0, -Math.PI/2);
Rotation
cam_to_ims
=
rev_matrix
?
(
new
Rotation
(
ro
,
rc
,
-
Math
.
PI
/
2
,
0
,
-
Math
.
PI
/
2
)):(
new
Rotation
(
ro
,
rc
,
Math
.
PI
/
2
,
0
,
Math
.
PI
/
2
));
double
[]
angles1
=
cam_to_ims
.
getAngles
(
ro1
,
rc
);
// Rotation ims_rot = new Rotation(ro, rc, ims_theta[0], ims_theta[1], ims_theta[2]);
// public Rotation(double q0, double q1, double q2, double q3, boolean needsNormalization) {
Rotation
ims_rot
=
new
Rotation
(
ims_qn2b
[
0
],
ims_qn2b
[
1
],
ims_qn2b
[
2
],
ims_qn2b
[
3
],
true
);
//Rotation r1= new Rotation(RotationOrder.YXZ, RC, ro.aux_azimuth, ro.aux_tilt, ro.aux_roll)
Rotation
comb_rot
=
rev_order
?
ims_rot
.
applyTo
(
cam_to_ims
):
cam_to_ims
.
applyTo
(
ims_rot
);
double
[]
angles
=
comb_rot
.
getAngles
(
ro1
,
rc
);
return
angles
;
}
public
static
double
[]
nedFromLla
(
double
[]
lla
,
double
[]
lla_ref
)
{
double
[]
ned
=
new
double
[]
{
EARTH_RADIUS
*
(
lla
[
0
]-
lla_ref
[
0
])
*
Math
.
PI
/
180
,
...
...
src/main/java/com/elphel/imagej/tileprocessor/ErsCorrection.java
View file @
e9aa9780
...
...
@@ -260,6 +260,13 @@ public class ErsCorrection extends GeometryCorrection {
this
.
ers_wxyz_center_dt
=
ers_xyz_dt
;
this
.
ers_watr_center_dt
=
ers_atr_dt
;
}
public
void
setErsDt
(
double
[][]
ers_xyzatr_dt
)
{
this
.
ers_wxyz_center_dt
=
ers_xyzatr_dt
[
0
];
this
.
ers_watr_center_dt
=
ers_xyzatr_dt
[
1
];
}
public
void
setErsDt_test
(
double
[]
ers_xyz_dt
,
double
[]
ers_atr_dt
)
{
...
...
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @
e9aa9780
...
...
@@ -277,6 +277,7 @@ public class Interscene {
return
earliest_scene2
;
}
public
static
int
setInitialOrientationsIms
(
final
CLTParameters
clt_parameters
,
int
min_num_scenes
,
...
...
@@ -290,6 +291,7 @@ public class Interscene {
final
int
threadsMax
,
// int threadsMax,
final
boolean
updateStatus
,
final
int
debugLevel
)
{
// double [][] dbg_scale_dt = {clt_parameters.ilp.ilma_scale_xyz, clt_parameters.ilp.ilma_scale_atr};
double
maximal_series_rms
=
0.0
;
double
min_ref_str
=
clt_parameters
.
imp
.
min_ref_str
;
boolean
ref_need_lma
=
clt_parameters
.
imp
.
ref_need_lma
;
...
...
@@ -300,7 +302,10 @@ public class Interscene {
boolean
ims_use
=
clt_parameters
.
imp
.
ims_use
;
double
[]
ims_ortho
=
clt_parameters
.
imp
.
ims_ortho
;
double
[]
ims_mount_atr
=
clt_parameters
.
imp
.
getImsMountATR
();
// converts to radians
double
[]
ims_mount_xyz
=
clt_parameters
.
imp
.
ims_mount_xyz
;
// not yet used
// double [] ims_mount_xyz = clt_parameters.imp.ims_mount_xyz; // not yet used
// double [] quat_ims_cam = Imx5.quaternionImsToCam(
// ims_mount_atr, // new double[] {0, 0.13, 0},
// ims_ortho);
double
boost_max_short
=
2.0
;
//
double
boost_zoom_short
=
1.5
;
//
...
...
@@ -380,7 +385,7 @@ public class Interscene {
boolean
after_spiral
=
false
;
boolean
got_spiral
=
false
;
// int search_rad = clt_parameters.imp.search_rad; // 10;
boolean
scale_ims_velocities
=
true
;
Did_ins_2
d2_ref
=
quadCLTs
[
ref_index
].
did_ins_2
;
// apply correction to orientation
double
[]
cam_quat_ref_enu
=
Imx5
.
quaternionImsToCam
(
d2_ref
.
getQEnu
()
,
...
...
@@ -389,11 +394,19 @@ public class Interscene {
double
[]
ref_abs_atr_enu
=
Imx5
.
quatToCamAtr
(
cam_quat_ref_enu
);
double
[][]
ims_ref_xyzatr_enu
=
{
ZERO3
,
ref_abs_atr_enu
};
double
[][]
last_corr_xyzatr
=
{
ZERO3
,
ZERO3
};
double
[][]
cam_dxyzatr_ref
=
quadCLTs
[
ref_index
].
getDxyzatrIms
(
clt_parameters
,
scale_ims_velocities
);
// scale IMS velocities
ers_reference
.
addScene
(
quadCLTs
[
ref_index
].
getImageName
(),
// add reference scene (itself) too
scenes_xyzatr
[
ref_index
][
0
],
scenes_xyzatr
[
ref_index
][
1
],
ZERO3
,
// ers_scene.getErsXYZ_dt(),
ZERO3
);
// ers_scene.getErsATR_dt()
cam_dxyzatr_ref
[
0
],
// ZERO3, // ers_scene.getErsXYZ_dt(),
cam_dxyzatr_ref
[
1
]);
// ZERO3);// ers_scene.getErsATR_dt()
// Will be used in prepareLMA()
quadCLTs
[
ref_index
].
getErsCorrection
().
setErsDt
(
// set for ref also (should be set before non-ref!)
QuadCLTCPU
.
scaleDtToErs
(
clt_parameters
,
cam_dxyzatr_ref
));
for
(
int
scene_index
=
ref_index
-
1
;
scene_index
>=
earliest_scene
;
scene_index
--)
{
if
((
ref_index
-
scene_index
)
>=
max_num_scenes
){
earliest_scene
=
scene_index
+
1
;
...
...
@@ -462,6 +475,15 @@ public class Interscene {
" pixels"
);
}
}
double
[][]
cam_dxyzatr
=
quadCLTs
[
scene_index
].
getDxyzatrIms
(
clt_parameters
,
scale_ims_velocities
);
// scale IMS velocities
// Will be used in prepareLMA()
quadCLTs
[
scene_index
].
getErsCorrection
().
setErsDt
(
// set for ref also (should be set before non-ref!)
QuadCLTCPU
.
scaleDtToErs
(
clt_parameters
,
cam_dxyzatr
));
// Does not use motion blur for reference scene here!
scenes_xyzatr
[
scene_index
]
=
adjustPairsLMAInterscene
(
clt_parameters
,
// CLTParameters clt_parameters,
...
...
@@ -562,12 +584,15 @@ public class Interscene {
}
}
// TODO: Maybe after testing high-ers scenes - restore velocities from before/after scenes
double
[][]
adjusted_xyzatr_dt
=
QuadCLTCPU
.
scaleDtFromErs
(
clt_parameters
,
quadCLTs
[
scene_index
].
getErsCorrection
().
getErsXYZATR_dt
(
));
ers_reference
.
addScene
(
scene_QuadClt
.
getImageName
(),
scenes_xyzatr
[
scene_index
][
0
],
scenes_xyzatr
[
scene_index
][
1
],
ZERO3
,
// ers_scene.getErsXYZ_dt(),
ZERO3
// ers_scene.getErsATR_dt()
adjusted_xyzatr_dt
[
0
],
//
ZERO3, // ers_scene.getErsXYZ_dt(),
adjusted_xyzatr_dt
[
1
]
//
ZERO3 // ers_scene.getErsATR_dt()
);
if
(
lma_rms
[
0
]
>
maximal_series_rms
)
{
maximal_series_rms
=
lma_rms
[
0
];
...
...
@@ -1103,6 +1128,7 @@ public class Interscene {
ErsCorrection
ers_reference
=
quadCLTs
[
ref_index
].
getErsCorrection
();
double
[][][]
scenes_xyzatr
=
new
double
[
quadCLTs
.
length
][][];
// previous scene relative to the next one
double
[][][]
dxyzatr_dt
=
new
double
[
quadCLTs
.
length
][][];
scenes_xyzatr
[
ref_index
]
=
new
double
[
2
][
3
];
// all zeros
// should have at least next or previous non-null
...
...
@@ -1132,6 +1158,7 @@ public class Interscene {
}
scenes_xyzatr
[
nscene
]
=
new
double
[][]
{
ers_reference
.
getSceneXYZ
(
ts
),
ers_reference
.
getSceneATR
(
ts
)};
}
dxyzatr_dt
[
nscene
]
=
ers_reference
.
getSceneErsXYZATR_dt
(
quadCLTs
[
nscene
].
getImageName
());
if
(
debug_ers
)
{
System
.
out
.
println
(
String
.
format
(
"%3d xyz: %9.5f %9.5f %9.5f atr: %9.5f %9.5f %9.5f"
,
nscene
,
...
...
@@ -1139,7 +1166,6 @@ public class Interscene {
scenes_xyzatr
[
nscene
][
1
][
0
],
scenes_xyzatr
[
nscene
][
1
][
1
],
scenes_xyzatr
[
nscene
][
1
][
2
]));
}
}
double
[][][]
dxyzatr_dt
;
switch
(
ers_mode
)
{
case
1
:
dxyzatr_dt
=
OpticalFlow
.
getVelocitiesFromScenes
(
...
...
@@ -1150,13 +1176,13 @@ public class Interscene {
scenes_xyzatr
,
// double [][][] scenes_xyzatr,
half_run_range
);
// double half_run_range
break
;
case
2
:
// read from ims
default
:
dxyzatr_dt
=
new
double
[
quadCLTs
.
length
][][];
for
(
int
nscene
=
last_scene
;
nscene
>=
earliest_scene
;
nscene
--)
{
// read from map (ts)
// divide by dbg_scale_dt, so saved in map will match ERS, not *_dt (currently they are the same)
}
//
case 2: // read from ims
default
:
// do nothing - already read
//
dxyzatr_dt = new double [quadCLTs.length][][];
//
for (int nscene = last_scene; nscene >= earliest_scene; nscene--) {
//
// read from map (ts)
//
// divide by dbg_scale_dt, so saved in map will match ERS, not *_dt (currently they are the same)
//
}
}
if
(
debug_ers
)
{
...
...
@@ -1214,7 +1240,7 @@ public class Interscene {
double
[]
scene_xyz_pre
=
ZERO3
;
double
[]
scene_atr_pre
=
ZERO3
;
// find correct signs when setting. dxyzatr_dt[][] is also used for motion blur (correctly)
/*
double [][] scaled_dxyzatr_dt = new double[2][3];
for (int i = 0; i < scaled_dxyzatr_dt.length; i++) {
for (int j = 0; j < scaled_dxyzatr_dt[i].length; j++) {
...
...
@@ -1226,6 +1252,12 @@ public class Interscene {
quadCLTs[nscene].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
scaled_dxyzatr_dt[0], // (ers_use_xyz? dxyzatr_dt[nscene][0]: ZERO3), //, // dxyzatr_dt[nscene][0], // double [] ers_xyz_dt,
scaled_dxyzatr_dt[1]); // dxyzatr_dt[nscene][1]); // double [] ers_atr_dt)(ers_scene_original_xyz_dt);
quadCLTs[nscene].getErsCorrection().setErsDt( // set for ref also (should be set before non-ref!)
QuadCLTCPU.scaleDtToErs(
clt_parameters,
dxyzatr_dt[nscene]));
*/
if
(
dbg_mb_img
!=
null
)
{
boolean
show_corrected
=
false
;
if
(
nscene
==
debug_scene
)
{
...
...
@@ -1304,15 +1336,14 @@ public class Interscene {
// double [][] dxyzatr_dt_ref = getVelocities( // was already set
// quadCLTs, // QuadCLT [] quadCLTs,
// ref_index); // int nscene)
double
[][]
dxyzatr_dt_ref
=
dxyzatr_dt
[
nscene
];
mb_vectors_ref
=
OpticalFlow
.
getMotionBlur
(
quadCLTs
[
ref_index
],
// QuadCLT ref_scene,
quadCLTs
[
ref_index
],
// QuadCLT scene, // can be the same as ref_scene
ref_pXpYD
,
// double [][] ref_pXpYD, // here it is scene, not reference!
ZERO3
,
// double [] camera_xyz,
ZERO3
,
// double [] camera_atr,
dxyzatr_dt
_ref
[
0
],
// double [] camera_xyz_dt,
dxyzatr_dt
_ref
[
1
],
// double [] camera_atr_dt,
dxyzatr_dt
[
nscene
]
[
0
],
// double [] camera_xyz_dt,
dxyzatr_dt
[
nscene
]
[
1
],
// double [] camera_atr_dt,
0
,
// int shrink_gaps, // will gaps, but not more that grow by this
debugLevel
);
// int debug_level)
}
...
...
@@ -1477,12 +1508,24 @@ public class Interscene {
}
}
// overwrite old data
// undo velocities->ers scaling
double
[][]
adjusted_xyzatr_dt
=
QuadCLTCPU
.
scaleDtFromErs
(
clt_parameters
,
quadCLTs
[
nscene
].
getErsCorrection
().
getErsXYZATR_dt
(
));
// ers_reference.addScene(ts,
// scenes_xyzatr[nscene][0],
// scenes_xyzatr[nscene][1],
// quadCLTs[nscene].getErsCorrection().getErsXYZ_dt(), // same as dxyzatr_dt[nscene][0], just keep for future adjustments?
// quadCLTs[nscene].getErsCorrection().getErsATR_dt() // same as dxyzatr_dt[nscene][1], just keep for future adjustments?
// );
ers_reference
.
addScene
(
ts
,
scenes_xyzatr
[
nscene
][
0
],
scenes_xyzatr
[
nscene
][
1
],
quadCLTs
[
nscene
].
getErsCorrection
().
getErsXYZ_dt
(),
// same as dxyzatr_dt[nscene][0], just keep for future adjustments?
quadCLTs
[
nscene
].
getErsCorrection
().
getErsATR_dt
()
// same as dxyzatr_dt[nscene][1], just keep for future adjustments?
adjusted_xyzatr_dt
[
0
],
adjusted_xyzatr_dt
[
1
]
);
if
(
lma_rms
[
0
]
>
maximal_series_rms
)
{
maximal_series_rms
=
lma_rms
[
0
];
}
...
...
@@ -1717,12 +1760,15 @@ public class Interscene {
for
(
int
i:
ErsCorrection
.
DP_ERS_INDICES
)
{
param_select_mod
[
i
]
=
false
;
}
// TODO Removed on 11/23/2023 for use with IMS. Set reasonable data if IMS is not used
/*
// now just copy ers from the reference
ErsCorrection ers_ref = reference_QuadClt.getErsCorrection();
ErsCorrection ers_scene = scene_QuadClt.getErsCorrection();
// when disabled - scene ers from the same as reference one
ers_scene.ers_watr_center_dt = ers_ref.ers_watr_center_dt.clone();
ers_scene.ers_wxyz_center_dt = ers_ref.ers_wxyz_center_dt.clone();
*/
}
// TODO: save ers_scene.ers_watr_center_dt and ers_scene.ers_wxyz_center_dt
intersceneLma
.
prepareLMA
(
...
...
@@ -3727,339 +3773,6 @@ public class Interscene {
ref_scene
.
saveQuadClt
();
// to re-load new set of Bayer images to the GPU (do nothing for CPU) and Geometry
return
tp_tasks_ref
;
}
@Deprecated
public
static
void
generateEgomotionTable0
(
CLTParameters
clt_parameters
,
QuadCLT
[]
quadCLTs
,
int
ref_index
,
int
earliest_scene
,
String
path
,
String
comment
)
{
boolean
use_euler
=
true
;
boolean
use_qn2b
=
true
;
double
[]
ims_ortho
=
clt_parameters
.
imp
.
ims_ortho
;
double
[]
ims_mount_atr
=
clt_parameters
.
imp
.
getImsMountATR
();
// converts to radians
// double [] quat_ortho = {0.5, 0.5, -0.5, 0.5}; // approximate IMU orientation
String
header_img
=
"#\ttimestamp\tx(m)\ty(m)\tz(m)\ta(rad)\ttilt(rad)\troll(rad)"
+
"\tVx(m/s)\tVy(m/s)\tVz(m/s)\tVa(rad/s)\tVt(rad/s)\tVr(rad/s)"
;
String
header_ins1
=
"\ttow\ttheta0\ttheta1\ttheta2\tu(m/s)\tv(m/s)\tw(m/s)"
+
"\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_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"
+
"\tabs_A\tabs_T\tabs_R\trel_A\trel_T\trel_R"
+
"\tu_dir\tv_dir\tw_dir\tu_inv\tv_inv\tw_inv"
;
String
header_pimu
=
"\to0\to1\to2\ta0\ta1\ta2"
;
String
header_cam000
=
"\ttheta0-000\ttheta1-000\ttheta2-000"
;
String
header_cam001
=
"\ttheta0-001\ttheta1-001\ttheta2-001"
;
String
header_cam010
=
"\ttheta0-010\ttheta1-010\ttheta2-010"
;
String
header_cam011
=
"\ttheta0-011\ttheta1-011\ttheta2-011"
;
String
header_cam100
=
"\ttheta0-100\ttheta1-100\ttheta2-100"
;
String
header_cam101
=
"\ttheta0-101\ttheta1-101\ttheta2-101"
;
String
header_cam110
=
"\ttheta0-110\ttheta1-110\ttheta2-110"
;
String
header_cam111
=
"\ttheta0-111\ttheta1-111\ttheta2-111"
;
String
header_cam200
=
"\ttheta0-200\ttheta1-200\ttheta2-200"
;
String
header_cam201
=
"\ttheta0-201\ttheta1-201\ttheta2-201"
;
String
header_cam210
=
"\ttheta0-210\ttheta1-210\ttheta2-210"
;
String
header_cam211
=
"\ttheta0-211\ttheta1-211\ttheta2-211"
;
String
header_cam300
=
"\ttheta0-300\ttheta1-300\ttheta2-300"
;
String
header_cam301
=
"\ttheta0-301\ttheta1-301\ttheta2-301"
;
String
header_cam310
=
"\ttheta0-310\ttheta1-310\ttheta2-310"
;
String
header_cam311
=
"\ttheta0-311\ttheta1-311\ttheta2-311"
;
String
header_cam400
=
"\ttheta0-400\ttheta1-400\ttheta2-400"
;
String
header_cam401
=
"\ttheta0-401\ttheta1-401\ttheta2-401"
;
String
header_cam410
=
"\ttheta0-410\ttheta1-410\ttheta2-410"
;
String
header_cam411
=
"\ttheta0-411\ttheta1-411\ttheta2-411"
;
String
header_cam500
=
"\ttheta0-500\ttheta1-500\ttheta2-500"
;
String
header_cam501
=
"\ttheta0-501\ttheta1-501\ttheta2-501"
;
String
header_cam510
=
"\ttheta0-510\ttheta1-510\ttheta2-510"
;
String
header_cam511
=
"\ttheta0-511\ttheta1-511\ttheta2-511"
;
String
header_qn2b000
=
"\tqn2b0-000\tqn2b1-000\tqn2b2-000"
;
String
header_qn2b001
=
"\tqn2b0-001\tqn2b1-001\tqn2b2-001"
;
String
header_qn2b010
=
"\tqn2b0-010\tqn2b1-010\tqn2b2-010"
;
String
header_qn2b011
=
"\tqn2b0-011\tqn2b1-011\tqn2b2-011"
;
String
header_qn2b100
=
"\tqn2b0-100\tqn2b1-100\tqn2b2-100"
;
String
header_qn2b101
=
"\tqn2b0-101\tqn2b1-101\tqn2b2-101"
;
String
header_qn2b110
=
"\tqn2b0-110\tqn2b1-110\tqn2b2-110"
;
String
header_qn2b111
=
"\tqn2b0-111\tqn2b1-111\tqn2b2-111"
;
String
header_qn2b200
=
"\tqn2b0-200\tqn2b1-200\tqn2b2-200"
;
String
header_qn2b201
=
"\tqn2b0-201\tqn2b1-201\tqn2b2-201"
;
String
header_qn2b210
=
"\tqn2b0-210\tqn2b1-210\tqn2b2-210"
;
String
header_qn2b211
=
"\tqn2b0-211\tqn2b1-211\tqn2b2-211"
;
String
header_qn2b300
=
"\tqn2b0-300\tqn2b1-300\tqn2b2-300"
;
String
header_qn2b301
=
"\tqn2b0-301\tqn2b1-301\tqn2b2-301"
;
String
header_qn2b310
=
"\tqn2b0-310\tqn2b1-310\tqn2b2-310"
;
String
header_qn2b311
=
"\tqn2b0-311\tqn2b1-311\tqn2b2-311"
;
String
header_qn2b400
=
"\tqn2b0-400\tqn2b1-400\tqn2b2-400"
;
String
header_qn2b401
=
"\tqn2b0-401\tqn2b1-401\tqn2b2-401"
;
String
header_qn2b410
=
"\tqn2b0-410\tqn2b1-410\tqn2b2-410"
;
String
header_qn2b411
=
"\tqn2b0-411\tqn2b1-411\tqn2b2-411"
;
String
header_qn2b500
=
"\tqn2b0-500\tqn2b1-500\tqn2b2-500"
;
String
header_qn2b501
=
"\tqn2b0-501\tqn2b1-501\tqn2b2-501"
;
String
header_qn2b510
=
"\tqn2b0-510\tqn2b1-510\tqn2b2-510"
;
String
header_qn2b511
=
"\tqn2b0-511\tqn2b1-511\tqn2b2-511"
;
String
header
=
header_img
+
header_ins1
+
header_ins2
+
header_ins2_extra
+
header_pimu
;
if
(
use_qn2b
)
{
header
+=
header_qn2b000
+
header_qn2b001
+
header_qn2b010
+
header_qn2b011
+
header_qn2b100
+
header_qn2b101
+
header_qn2b110
+
header_qn2b111
+
header_qn2b200
+
header_qn2b201
+
header_qn2b210
+
header_qn2b211
+
header_qn2b300
+
header_qn2b301
+
header_qn2b310
+
header_qn2b311
+
header_qn2b400
+
header_qn2b401
+
header_qn2b410
+
header_qn2b411
+
header_qn2b500
+
header_qn2b501
+
header_qn2b510
+
header_qn2b511
;
}
if
(
use_euler
)
{
header
+=
header_cam000
+
header_cam001
+
header_cam010
+
header_cam011
+
header_cam100
+
header_cam101
+
header_cam110
+
header_cam111
+
header_cam200
+
header_cam201
+
header_cam210
+
header_cam211
+
header_cam300
+
header_cam301
+
header_cam310
+
header_cam311
+
header_cam400
+
header_cam401
+
header_cam410
+
header_cam411
+
header_cam500
+
header_cam501
+
header_cam510
+
header_cam511
;
}
StringBuffer
sb
=
new
StringBuffer
();
QuadCLT
ref_scene
=
quadCLTs
[
ref_index
];
ErsCorrection
ers_reference
=
ref_scene
.
getErsCorrection
();
double
[][][]
scenes_xyzatr
=
new
double
[
quadCLTs
.
length
][][];
for
(
int
nscene
=
earliest_scene
;
nscene
<
quadCLTs
.
length
;
nscene
++)
{
QuadCLT
scene
=
quadCLTs
[
nscene
];
if
(
nscene
==
ref_index
)
{
scenes_xyzatr
[
nscene
]
=
new
double
[
2
][
3
];
}
else
{
String
ts
=
scene
.
getImageName
();
scenes_xyzatr
[
nscene
]
=
new
double
[][]
{
ers_reference
.
getSceneXYZ
(
ts
),
ers_reference
.
getSceneATR
(
ts
)};
}
}
double
[][][]
dxyzatr_dt
=
OpticalFlow
.
getVelocitiesFromScenes
(
quadCLTs
,
// QuadCLT [] scenes, // ordered by increasing timestamps
ref_index
,
earliest_scene
,
// int start_scene,
quadCLTs
.
length
-
1
,
// int end1_scene,
scenes_xyzatr
,
// double [][][] scenes_xyzatr,
clt_parameters
.
ofp
.
lpf_series
);
// half_run_range); // double half_run_range
Did_ins_2
d2_ref
=
quadCLTs
[
ref_index
].
did_ins_2
;
double
[]
cam_quat_ref
=
Imx5
.
quaternionImsToCam
(
d2_ref
.
getQn2b
()
,
ims_mount_atr
,
// new double[] {0, 0.13, 0},
ims_ortho
);
double
[]
ref_abs_atr
=
Imx5
.
quatToCamAtr
(
cam_quat_ref
);
double
[][]
ims_ref_xyzatr
=
{
ZERO3
,
ref_abs_atr
};
for
(
int
nscene
=
earliest_scene
;
nscene
<
quadCLTs
.
length
;
nscene
++)
{
QuadCLT
scene
=
quadCLTs
[
nscene
];
if
(
scene
==
null
)
{
for
(
int
i
=
0
;
i
<
26
;
i
++)
{
sb
.
append
(
"\t---"
);
}
sb
.
append
(
"\n"
);
continue
;
}
double
timestamp
=
scene
.
getTimeStamp
();
Did_ins_1
d1
=
scene
.
did_ins_1
;
Did_ins_2
d2
=
scene
.
did_ins_2
;
Did_pimu
d3
=
scene
.
did_pimu
;
sb
.
append
(
nscene
+
"\t"
+
timestamp
);
// TODO: try saved, not calculated velocities!
sb
.
append
(
"\t"
+
scenes_xyzatr
[
nscene
][
0
][
0
]+
"\t"
+
scenes_xyzatr
[
nscene
][
0
][
1
]+
"\t"
+
scenes_xyzatr
[
nscene
][
0
][
2
]);
sb
.
append
(
"\t"
+
scenes_xyzatr
[
nscene
][
1
][
0
]+
"\t"
+
scenes_xyzatr
[
nscene
][
1
][
1
]+
"\t"
+
scenes_xyzatr
[
nscene
][
1
][
2
]);
sb
.
append
(
"\t"
+
dxyzatr_dt
[
nscene
][
0
][
0
]+
"\t"
+
dxyzatr_dt
[
nscene
][
0
][
1
]+
"\t"
+
dxyzatr_dt
[
nscene
][
0
][
2
]);
sb
.
append
(
"\t"
+
dxyzatr_dt
[
nscene
][
1
][
0
]+
"\t"
+
dxyzatr_dt
[
nscene
][
1
][
1
]+
"\t"
+
dxyzatr_dt
[
nscene
][
1
][
2
]);
// String header_ins1="\ttow\ttheta0\ttheta1\ttheta2\tu(m/s)\tv(m/s)\tw(m/s)"+
// "\tlong\tlat\talt\tned0\tned1\tned2";
sb
.
append
(
"\t"
+
d1
.
timeOfWeek
);
sb
.
append
(
"\t"
+
d1
.
theta
[
0
]+
"\t"
+
d1
.
theta
[
1
]+
"\t"
+
d1
.
theta
[
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
.
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)\tlat\tlong\talt";
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
.
uvw
[
0
]+
"\t"
+
d2
.
uvw
[
1
]+
"\t"
+
d2
.
uvw
[
2
]);
sb
.
append
(
"\t"
+
d2
.
lla
[
0
]+
"\t"
+
d2
.
lla
[
1
]+
"\t"
+
d2
.
lla
[
2
]);
double
[]
double_theta
=
d1
.
getTheta
();
//imsToCamRotations(double [] ims_theta, boolean rev_order, boolean rev_matrix )
double
[]
double_qn2b
=
d2
.
getQn2b
();
double
[]
double_uvw
=
d2
.
getUvw
();
double
[]
uvw_dir
=
Imx5
.
applyQuternionTo
(
double_qn2b
,
double_uvw
,
false
);
// bad
double
[]
uvw_inv
=
Imx5
.
applyQuternionTo
(
double_qn2b
,
double_uvw
,
true
);
// good
//Converting from local uvw to NED: (qn2b).applyInverseTo(uvw,ned) results in [vNorth, vEast, vUp]
double
[]
ned
=
Imx5
.
nedFromLla
(
d2
.
lla
,
d2_ref
.
lla
);
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"
+
ims_xyz
[
0
]+
"\t"
+
ims_xyz
[
1
]+
"\t"
+
ims_xyz
[
2
]);
// imu axes
double
[]
cam_quat1
=
Imx5
.
quaternionImsToCam
(
double_qn2b
,
new
double
[
3
],
ims_ortho
);
double
[]
cam_quat2
=
Imx5
.
quaternionImsToCam
(
double_qn2b
,
ims_mount_atr
,
// new double[] {0, 0.13, 0},
ims_ortho
);
double
[]
cam_xyz1
=
Imx5
.
applyQuternionTo
(
cam_quat1
,
ned
,
false
);
double
[]
cam_xyz2
=
Imx5
.
applyQuternionTo
(
cam_quat2
,
ned
,
false
);
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
]);
//
double
[]
scene_abs_atr
=
Imx5
.
quatToCamAtr
(
cam_quat2
);
double
[][]
ims_scene_xyzatr
=
{
ZERO3
,
scene_abs_atr
};
double
[]
scene_rel_atr
=
ErsCorrection
.
combineXYZATR
(
ims_scene_xyzatr
,
ErsCorrection
.
invertXYZATR
(
ims_ref_xyzatr
))[
1
];
sb
.
append
(
"\t"
+
scene_abs_atr
[
0
]+
"\t"
+
scene_abs_atr
[
1
]+
"\t"
+
scene_abs_atr
[
2
]);
//
sb
.
append
(
"\t"
+
scene_rel_atr
[
0
]+
"\t"
+
scene_rel_atr
[
1
]+
"\t"
+
scene_rel_atr
[
2
]);
//
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
// 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
.
vel
[
0
]/
d3
.
dt
)+
"\t"
+(
d3
.
vel
[
1
]/
d3
.
dt
)+
"\t"
+(
d3
.
vel
[
2
]/
d3
.
dt
));
// String header_cam= "\ttheta0\ttheta1\ttheta2\troll\tpitch\tyaw";
if
(
use_qn2b
)
{
double
[]
cam_qn2b_000
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
0
,
false
,
false
);
double
[]
cam_qn2b_001
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
0
,
false
,
true
);
double
[]
cam_qn2b_010
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
0
,
true
,
false
);
double
[]
cam_qn2b_011
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
0
,
true
,
true
);
double
[]
cam_qn2b_100
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
1
,
false
,
false
);
double
[]
cam_qn2b_101
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
1
,
false
,
true
);
double
[]
cam_qn2b_110
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
1
,
true
,
false
);
double
[]
cam_qn2b_111
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
1
,
true
,
true
);
double
[]
cam_qn2b_200
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
2
,
false
,
false
);
double
[]
cam_qn2b_201
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
2
,
false
,
true
);
double
[]
cam_qn2b_210
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
2
,
true
,
false
);
double
[]
cam_qn2b_211
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
2
,
true
,
true
);
double
[]
cam_qn2b_300
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
3
,
false
,
false
);
double
[]
cam_qn2b_301
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
3
,
false
,
true
);
double
[]
cam_qn2b_310
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
3
,
true
,
false
);
double
[]
cam_qn2b_311
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
3
,
true
,
true
);
double
[]
cam_qn2b_400
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
4
,
false
,
false
);
double
[]
cam_qn2b_401
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
4
,
false
,
true
);
double
[]
cam_qn2b_410
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
4
,
true
,
false
);
double
[]
cam_qn2b_411
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
4
,
true
,
true
);
double
[]
cam_qn2b_500
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
5
,
false
,
false
);
double
[]
cam_qn2b_501
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
5
,
false
,
true
);
double
[]
cam_qn2b_510
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
5
,
true
,
false
);
double
[]
cam_qn2b_511
=
Imx5
.
imsQToCamRotations
(
double_qn2b
,
5
,
true
,
true
);
sb
.
append
(
"\t"
+
cam_qn2b_000
[
0
]+
"\t"
+
cam_qn2b_000
[
1
]+
"\t"
+
cam_qn2b_000
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_001
[
0
]+
"\t"
+
cam_qn2b_001
[
1
]+
"\t"
+
cam_qn2b_001
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_010
[
0
]+
"\t"
+
cam_qn2b_010
[
1
]+
"\t"
+
cam_qn2b_010
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_011
[
0
]+
"\t"
+
cam_qn2b_011
[
1
]+
"\t"
+
cam_qn2b_011
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_100
[
0
]+
"\t"
+
cam_qn2b_100
[
1
]+
"\t"
+
cam_qn2b_100
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_101
[
0
]+
"\t"
+
cam_qn2b_101
[
1
]+
"\t"
+
cam_qn2b_101
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_110
[
0
]+
"\t"
+
cam_qn2b_110
[
1
]+
"\t"
+
cam_qn2b_110
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_111
[
0
]+
"\t"
+
cam_qn2b_111
[
1
]+
"\t"
+
cam_qn2b_111
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_200
[
0
]+
"\t"
+
cam_qn2b_200
[
1
]+
"\t"
+
cam_qn2b_200
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_201
[
0
]+
"\t"
+
cam_qn2b_201
[
1
]+
"\t"
+
cam_qn2b_201
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_210
[
0
]+
"\t"
+
cam_qn2b_210
[
1
]+
"\t"
+
cam_qn2b_210
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_211
[
0
]+
"\t"
+
cam_qn2b_211
[
1
]+
"\t"
+
cam_qn2b_211
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_300
[
0
]+
"\t"
+
cam_qn2b_300
[
1
]+
"\t"
+
cam_qn2b_300
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_301
[
0
]+
"\t"
+
cam_qn2b_301
[
1
]+
"\t"
+
cam_qn2b_301
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_310
[
0
]+
"\t"
+
cam_qn2b_310
[
1
]+
"\t"
+
cam_qn2b_310
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_311
[
0
]+
"\t"
+
cam_qn2b_311
[
1
]+
"\t"
+
cam_qn2b_311
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_400
[
0
]+
"\t"
+
cam_qn2b_400
[
1
]+
"\t"
+
cam_qn2b_400
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_401
[
0
]+
"\t"
+
cam_qn2b_401
[
1
]+
"\t"
+
cam_qn2b_401
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_410
[
0
]+
"\t"
+
cam_qn2b_410
[
1
]+
"\t"
+
cam_qn2b_410
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_411
[
0
]+
"\t"
+
cam_qn2b_411
[
1
]+
"\t"
+
cam_qn2b_411
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_500
[
0
]+
"\t"
+
cam_qn2b_500
[
1
]+
"\t"
+
cam_qn2b_500
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_501
[
0
]+
"\t"
+
cam_qn2b_501
[
1
]+
"\t"
+
cam_qn2b_501
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_510
[
0
]+
"\t"
+
cam_qn2b_510
[
1
]+
"\t"
+
cam_qn2b_510
[
2
]);
sb
.
append
(
"\t"
+
cam_qn2b_511
[
0
]+
"\t"
+
cam_qn2b_511
[
1
]+
"\t"
+
cam_qn2b_511
[
2
]);
}
if
(
use_euler
)
{
double
[]
cam_theta_000
=
Imx5
.
imsToCamRotations
(
double_theta
,
0
,
false
,
false
);
double
[]
cam_theta_001
=
Imx5
.
imsToCamRotations
(
double_theta
,
0
,
false
,
true
);
double
[]
cam_theta_010
=
Imx5
.
imsToCamRotations
(
double_theta
,
0
,
true
,
false
);
double
[]
cam_theta_011
=
Imx5
.
imsToCamRotations
(
double_theta
,
0
,
true
,
true
);
double
[]
cam_theta_100
=
Imx5
.
imsToCamRotations
(
double_theta
,
1
,
false
,
false
);
double
[]
cam_theta_101
=
Imx5
.
imsToCamRotations
(
double_theta
,
1
,
false
,
true
);
double
[]
cam_theta_110
=
Imx5
.
imsToCamRotations
(
double_theta
,
1
,
true
,
false
);
double
[]
cam_theta_111
=
Imx5
.
imsToCamRotations
(
double_theta
,
1
,
true
,
true
);
double
[]
cam_theta_200
=
Imx5
.
imsToCamRotations
(
double_theta
,
2
,
false
,
false
);
double
[]
cam_theta_201
=
Imx5
.
imsToCamRotations
(
double_theta
,
2
,
false
,
true
);
double
[]
cam_theta_210
=
Imx5
.
imsToCamRotations
(
double_theta
,
2
,
true
,
false
);
double
[]
cam_theta_211
=
Imx5
.
imsToCamRotations
(
double_theta
,
2
,
true
,
true
);
double
[]
cam_theta_300
=
Imx5
.
imsToCamRotations
(
double_theta
,
3
,
false
,
false
);
double
[]
cam_theta_301
=
Imx5
.
imsToCamRotations
(
double_theta
,
3
,
false
,
true
);
double
[]
cam_theta_310
=
Imx5
.
imsToCamRotations
(
double_theta
,
3
,
true
,
false
);
double
[]
cam_theta_311
=
Imx5
.
imsToCamRotations
(
double_theta
,
3
,
true
,
true
);
double
[]
cam_theta_400
=
Imx5
.
imsToCamRotations
(
double_theta
,
4
,
false
,
false
);
double
[]
cam_theta_401
=
Imx5
.
imsToCamRotations
(
double_theta
,
4
,
false
,
true
);
double
[]
cam_theta_410
=
Imx5
.
imsToCamRotations
(
double_theta
,
4
,
true
,
false
);
double
[]
cam_theta_411
=
Imx5
.
imsToCamRotations
(
double_theta
,
4
,
true
,
true
);
double
[]
cam_theta_500
=
Imx5
.
imsToCamRotations
(
double_theta
,
5
,
false
,
false
);
double
[]
cam_theta_501
=
Imx5
.
imsToCamRotations
(
double_theta
,
5
,
false
,
true
);
double
[]
cam_theta_510
=
Imx5
.
imsToCamRotations
(
double_theta
,
5
,
true
,
false
);
double
[]
cam_theta_511
=
Imx5
.
imsToCamRotations
(
double_theta
,
5
,
true
,
true
);
sb
.
append
(
"\t"
+
cam_theta_000
[
0
]+
"\t"
+
cam_theta_000
[
1
]+
"\t"
+
cam_theta_000
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_001
[
0
]+
"\t"
+
cam_theta_001
[
1
]+
"\t"
+
cam_theta_001
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_010
[
0
]+
"\t"
+
cam_theta_010
[
1
]+
"\t"
+
cam_theta_010
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_011
[
0
]+
"\t"
+
cam_theta_011
[
1
]+
"\t"
+
cam_theta_011
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_100
[
0
]+
"\t"
+
cam_theta_100
[
1
]+
"\t"
+
cam_theta_100
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_101
[
0
]+
"\t"
+
cam_theta_101
[
1
]+
"\t"
+
cam_theta_101
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_110
[
0
]+
"\t"
+
cam_theta_110
[
1
]+
"\t"
+
cam_theta_110
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_111
[
0
]+
"\t"
+
cam_theta_111
[
1
]+
"\t"
+
cam_theta_111
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_200
[
0
]+
"\t"
+
cam_theta_200
[
1
]+
"\t"
+
cam_theta_200
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_201
[
0
]+
"\t"
+
cam_theta_201
[
1
]+
"\t"
+
cam_theta_201
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_210
[
0
]+
"\t"
+
cam_theta_210
[
1
]+
"\t"
+
cam_theta_210
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_211
[
0
]+
"\t"
+
cam_theta_211
[
1
]+
"\t"
+
cam_theta_211
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_300
[
0
]+
"\t"
+
cam_theta_300
[
1
]+
"\t"
+
cam_theta_300
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_301
[
0
]+
"\t"
+
cam_theta_301
[
1
]+
"\t"
+
cam_theta_301
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_310
[
0
]+
"\t"
+
cam_theta_310
[
1
]+
"\t"
+
cam_theta_310
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_311
[
0
]+
"\t"
+
cam_theta_311
[
1
]+
"\t"
+
cam_theta_311
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_400
[
0
]+
"\t"
+
cam_theta_400
[
1
]+
"\t"
+
cam_theta_400
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_401
[
0
]+
"\t"
+
cam_theta_401
[
1
]+
"\t"
+
cam_theta_401
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_410
[
0
]+
"\t"
+
cam_theta_410
[
1
]+
"\t"
+
cam_theta_410
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_411
[
0
]+
"\t"
+
cam_theta_411
[
1
]+
"\t"
+
cam_theta_411
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_500
[
0
]+
"\t"
+
cam_theta_500
[
1
]+
"\t"
+
cam_theta_500
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_501
[
0
]+
"\t"
+
cam_theta_501
[
1
]+
"\t"
+
cam_theta_501
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_510
[
0
]+
"\t"
+
cam_theta_510
[
1
]+
"\t"
+
cam_theta_510
[
2
]);
sb
.
append
(
"\t"
+
cam_theta_511
[
0
]+
"\t"
+
cam_theta_511
[
1
]+
"\t"
+
cam_theta_511
[
2
]);
}
/*
sb.append("\t"+
cam_theta[0]*180/Math.PI+"\t"+
cam_theta[1]*180/Math.PI+"\t"+
cam_theta[2]*180/Math.PI);
*/
sb
.
append
(
"\n"
);
}
if
(
path
!=
null
)
{
String
footer
=(
comment
!=
null
)
?
(
"Comment: "
+
comment
):
""
;
CalibrationFileManagement
.
saveStringToFile
(
path
,
header
+
"\n"
+
sb
.
toString
()+
"\n"
+
footer
);
}
else
{
new
TextWindow
(
"Sharpness History"
,
header
,
sb
.
toString
(),
1000
,
900
);
}
}
public
static
void
generateEgomotionTable
(
...
...
@@ -4110,8 +3823,10 @@ public class Interscene {
String
header_pimu
=
"\to0\to1\to2\ta0\ta1\ta2"
;
String
header_camv
=
"\tcam_vx\tcam_vy\tcam_vz\tcam_va\tcam_vt\tcam_vr"
;
String
header
=
header_ts
+(
use_processed
?
header_img:
""
)+
header_ins1
+
header_ins2
+
header_ins2_extra
+
header_pimu
;
String
header
=
header_ts
+(
use_processed
?
header_img:
""
)+
header_ins1
+
header_ins2
+
header_ins2_extra
+
header_pimu
+
header_camv
;
StringBuffer
sb
=
new
StringBuffer
();
double
[][][]
dxyzatr_dt
=
null
;
...
...
@@ -4124,6 +3839,11 @@ public class Interscene {
scenes_xyzatr
,
// double [][][] scenes_xyzatr,
clt_parameters
.
ofp
.
lpf_series
);
// half_run_range); // double half_run_range
}
double
[]
quat_ims_cam
=
Imx5
.
quaternionImsToCam
(
ims_mount_atr
,
// new double[] {0, 0.13, 0},
ims_ortho
);
Did_ins_2
d2_ref
=
quadCLTs
[
ref_index
].
did_ins_2
;
double
[]
cam_quat_ref
=
Imx5
.
quaternionImsToCam
(
d2_ref
.
getQn2b
()
,
ims_mount_atr
,
// new double[] {0, 0.13, 0},
...
...
@@ -4212,11 +3932,20 @@ public class Interscene {
d2_ref
.
getQn2b
(),
// double[] quat_ned,
ims_mount_atr
,
// double [] ims_mount_atr,
ims_ortho
);
//double [] ims_ortho)
double
[]
cam_xyz_enu
=
test_xyz_enu
(
/*
double [] cam_xyz_enu0 = test_xyz_enu(
Imx5.enuFromLla (d2.lla, d2_ref.lla), //double [] enu,
d2_ref.getQEnu(), // double[] quat_enu,
ims_mount_atr, // double [] ims_mount_atr,
ims_ortho); //double [] ims_ortho)
*/
double
[]
cam_xyz_enu
=
Imx5
.
applyQuternionTo
(
Imx5
.
quaternionImsToCam
(
d2_ref
.
getQEnu
(),
// double[] quat_enu,
ims_mount_atr
,
ims_ortho
),
enu
,
false
);
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
]);
//
...
...
@@ -4246,9 +3975,22 @@ public class Interscene {
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
// 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
.
vel
[
0
]/
d3
.
dt
)+
"\t"
+(
d3
.
vel
[
1
]/
d3
.
dt
)+
"\t"
+(
d3
.
vel
[
2
]/
d3
.
dt
));
/*
double [] omegas=new double[] {d3.theta[0]/d3.dt, d3.theta[1]/d3.dt, d3.theta[2]/d3.dt};
double [] cam_dxyz = Imx5.applyQuternionTo(quat_ims_cam, double_uvw, false);
double [] cam_datr = Imx5.applyQuternionTo(quat_ims_cam, omegas, false);
sb.append("\t"+cam_dxyz[0]+ "\t"+cam_dxyz[1]+ "\t"+cam_dxyz[2]);
// a (around Y),t (around X), r (around Z)
sb.append("\t"+cam_datr[1]+ "\t"+cam_datr[0]+ "\t"+cam_datr[2]);
*/
double
[][]
cam_dxyzatr
=
scene
.
getDxyzatrIms
(
clt_parameters
,
false
);
// raw, do not scale
sb
.
append
(
"\t"
+
cam_dxyzatr
[
0
][
0
]+
"\t"
+
cam_dxyzatr
[
0
][
1
]+
"\t"
+
cam_dxyzatr
[
0
][
2
]);
sb
.
append
(
"\t"
+
cam_dxyzatr
[
1
][
0
]+
"\t"
+
cam_dxyzatr
[
1
][
1
]+
"\t"
+
cam_dxyzatr
[
1
][
2
]);
// add lpf
sb
.
append
(
"\n"
);
}
...
...
@@ -4287,71 +4029,4 @@ public class Interscene {
}
static
double
[]
test_ned0
(
double
[]
ned
,
double
[]
quat
,
double
[]
ims_mount_atr
,
double
[]
ims_ortho
)
{
double
[][]
enu_ned
=
{{
0
,
1
,
0
},{
1
,
0
,
0
},{
0
,
0
,
-
1
}};
Rotation
rot_enu_ned
=
new
Rotation
(
enu_ned
,
1
E
-
8
);
// double [] cam_xyzs = Imx5.applyQuternionTo(cam_quats, enu, false);
Rotation
quat_rot
=
new
Rotation
(
quat
[
0
],
quat
[
1
],
quat
[
2
],
quat
[
3
],
true
);
double
[]
enu
=
new
double
[]
{
ned
[
1
],
ned
[
0
],
-
ned
[
2
]};
double
[]
rslt
=
new
double
[
3
];
// same as ned
rot_enu_ned
.
applyTo
(
enu
,
rslt
);
double
[]
rslt1
=
new
double
[
3
];
double
[]
rslt2
=
new
double
[
3
];
quat_rot
.
applyTo
(
ned
,
rslt1
);
// Rotation qr = rot_enu_ned.applyTo(quat_rot);
Rotation
qr
=
quat_rot
.
applyTo
(
rot_enu_ned
);
qr
.
applyTo
(
enu
,
rslt2
);
double
[]
cam_quat2
=
Imx5
.
quaternionImsToCam
(
quat
,
ims_mount_atr
,
ims_ortho
);
double
[]
cam_xyz2
=
Imx5
.
applyQuternionTo
(
cam_quat2
,
ned
,
false
);
double
[][]
reord
=
new
double
[
16
][
4
];
for
(
int
i
=
0
;
i
<
reord
.
length
;
i
++)
{
for
(
int
j
=
0
;
j
<
4
;
j
++)
{
boolean
neg
=
((
i
>>
j
)
&
1
)
>
0
;
reord
[
i
][
j
]
=
0.5
*
(
neg
?
-
1
:
1
);
}
}
System
.
out
.
println
(
"cam_xyz2: "
+
cam_xyz2
[
0
]+
", "
+
cam_xyz2
[
1
]+
", "
+
cam_xyz2
[
2
]);
Rotation
rot_reord
;
Rotation
quat_enu_rot
;
double
[]
quat_enu
;
double
[]
cam_quats
;
for
(
int
i
=
0
;
i
<
reord
.
length
;
i
++)
{
rot_reord
=
new
Rotation
(
reord
[
i
][
0
],
reord
[
i
][
1
],
reord
[
i
][
2
],
reord
[
i
][
3
],
true
);
// quat_enu_rot = rot_reord.applyTo(quat_rot);
quat_enu_rot
=
quat_rot
.
applyTo
(
rot_reord
);
quat_enu
=
new
double
[]
{
quat_enu_rot
.
getQ0
(),
quat_enu_rot
.
getQ1
(),
quat_enu_rot
.
getQ2
(),
quat_enu_rot
.
getQ3
()};
cam_quats
=
Imx5
.
quaternionImsToCam
(
quat_enu
,
ims_mount_atr
,
ims_ortho
);
double
[]
cam_xyzs
=
Imx5
.
applyQuternionTo
(
cam_quats
,
enu
,
false
);
System
.
out
.
println
(
i
+
": "
+
cam_xyzs
[
0
]+
", "
+
cam_xyzs
[
1
]+
", "
+
cam_xyzs
[
2
]);
}
return
cam_xyz2
;
}
}
src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java
View file @
e9aa9780
...
...
@@ -41,6 +41,9 @@ public class IntersceneMatchParameters {
public
double
[]
ims_ortho
=
{
0.5
,
-
0.5
,
0.5
,
-
0.5
};
// approximate (90-deg) IMS to camera
public
double
[]
ims_mount_atr
=
{
0
,
0
,
0
};
// IMS mount fine correction (A,T,R around camera axes)
public
double
[]
ims_mount_xyz
=
{
0
,
0
,
0
};
// IMS center in camera coordinates
public
double
[]
ims_scale_xyz
=
{
1.1
,
1.1
,
1.1
};
public
double
[]
ims_scale_atr
=
{
1.1
,
1.1
,
1.1
};
public
boolean
sfm_use
=
true
;
// use SfM to improve depth map
public
double
sfm_min_base
=
2.0
;
// use SfM if baseline exceeds this
...
...
@@ -462,6 +465,11 @@ public class IntersceneMatchParameters {
gd
.
addStringField
(
"IMS mount XYZ correction (m)"
,
IntersceneMatchParameters
.
doublesToString
(
ims_mount_xyz
),
80
,
"MS center (X,Y,Z m) in camera coordinates."
);
gd
.
addStringField
(
"scale XYZ from IMS"
,
IntersceneMatchParameters
.
doublesToString
(
ims_scale_xyz
),
80
,
"Scales for X, Y, and Z velocities when converting from IMS data, such as '1.1 1.1 1.1'."
);
gd
.
addStringField
(
"scale ATR from IMS"
,
IntersceneMatchParameters
.
doublesToString
(
ims_scale_atr
),
80
,
"Scales for Azimuth, Tilt, and Roll velocities when converting from IMS data, such as '1.1 1.1 1.1'."
);
gd
.
addTab
(
"SfM"
,
"Structure from Motion to improve depth map for the lateral views"
);
gd
.
addCheckbox
(
"Use SfM"
,
this
.
sfm_use
,
"Use SfM for the depth map enhancement for laterally moving camera."
);
...
...
@@ -1186,6 +1194,8 @@ public class IntersceneMatchParameters {
this
.
ims_ortho
=
IntersceneMatchParameters
.
StringToDoubles
(
gd
.
getNextString
(),
4
);
this
.
ims_mount_atr
=
IntersceneMatchParameters
.
StringToDoubles
(
gd
.
getNextString
(),
3
);
this
.
ims_mount_xyz
=
IntersceneMatchParameters
.
StringToDoubles
(
gd
.
getNextString
(),
3
);
this
.
ims_scale_xyz
=
IntersceneMatchParameters
.
StringToDoubles
(
gd
.
getNextString
(),
3
);
this
.
ims_scale_atr
=
IntersceneMatchParameters
.
StringToDoubles
(
gd
.
getNextString
(),
3
);
this
.
sfm_use
=
gd
.
getNextBoolean
();
this
.
sfm_min_base
=
gd
.
getNextNumber
();
this
.
sfm_min_gain
=
gd
.
getNextNumber
();
...
...
@@ -1566,6 +1576,8 @@ public class IntersceneMatchParameters {
properties
.
setProperty
(
prefix
+
"ims_ortho"
,
IntersceneMatchParameters
.
doublesToString
(
this
.
ims_ortho
));
properties
.
setProperty
(
prefix
+
"ims_mount_atr"
,
IntersceneMatchParameters
.
doublesToString
(
this
.
ims_mount_atr
));
properties
.
setProperty
(
prefix
+
"ims_mount_xyz"
,
IntersceneMatchParameters
.
doublesToString
(
this
.
ims_mount_xyz
));
properties
.
setProperty
(
prefix
+
"ims_scale_xyz"
,
IntersceneMatchParameters
.
doublesToString
(
this
.
ims_scale_xyz
));
properties
.
setProperty
(
prefix
+
"ims_scale_atr"
,
IntersceneMatchParameters
.
doublesToString
(
this
.
ims_scale_atr
));
properties
.
setProperty
(
prefix
+
"sfm_use"
,
this
.
sfm_use
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"sfm_min_base"
,
this
.
sfm_min_base
+
""
);
// double
properties
.
setProperty
(
prefix
+
"sfm_min_gain"
,
this
.
sfm_min_gain
+
""
);
// double
...
...
@@ -1906,6 +1918,8 @@ public class IntersceneMatchParameters {
if
(
properties
.
getProperty
(
prefix
+
"ims_ortho"
)!=
null
)
this
.
ims_ortho
=
IntersceneMatchParameters
.
StringToDoubles
(
properties
.
getProperty
(
prefix
+
"ims_ortho"
),
4
);
if
(
properties
.
getProperty
(
prefix
+
"ims_mount_atr"
)!=
null
)
this
.
ims_mount_atr
=
IntersceneMatchParameters
.
StringToDoubles
(
properties
.
getProperty
(
prefix
+
"ims_mount_atr"
),
3
);
if
(
properties
.
getProperty
(
prefix
+
"ims_mount_xyz"
)!=
null
)
this
.
ims_mount_xyz
=
IntersceneMatchParameters
.
StringToDoubles
(
properties
.
getProperty
(
prefix
+
"ims_mount_xyz"
),
3
);
if
(
properties
.
getProperty
(
prefix
+
"ims_scale_xyz"
)!=
null
)
this
.
ims_scale_xyz
=
IntersceneMatchParameters
.
StringToDoubles
(
properties
.
getProperty
(
prefix
+
"ims_scale_xyz"
),
3
);
if
(
properties
.
getProperty
(
prefix
+
"ims_scale_atr"
)!=
null
)
this
.
ims_scale_atr
=
IntersceneMatchParameters
.
StringToDoubles
(
properties
.
getProperty
(
prefix
+
"ims_scale_atr"
),
3
);
if
(
properties
.
getProperty
(
prefix
+
"sfm_use"
)!=
null
)
this
.
sfm_use
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"sfm_use"
));
if
(
properties
.
getProperty
(
prefix
+
"sfm_min_base"
)!=
null
)
this
.
sfm_min_base
=
Double
.
parseDouble
(
properties
.
getProperty
(
prefix
+
"sfm_min_base"
));
if
(
properties
.
getProperty
(
prefix
+
"sfm_min_gain"
)!=
null
)
this
.
sfm_min_gain
=
Double
.
parseDouble
(
properties
.
getProperty
(
prefix
+
"sfm_min_gain"
));
...
...
@@ -2273,6 +2287,8 @@ public class IntersceneMatchParameters {
imp
.
ims_ortho
=
this
.
ims_ortho
.
clone
();
imp
.
ims_mount_atr
=
this
.
ims_mount_atr
.
clone
();
imp
.
ims_mount_xyz
=
this
.
ims_mount_xyz
.
clone
();
imp
.
ims_scale_xyz
=
this
.
ims_scale_xyz
.
clone
();
imp
.
ims_scale_atr
=
this
.
ims_scale_atr
.
clone
();
imp
.
sfm_use
=
this
.
sfm_use
;
imp
.
sfm_min_base
=
this
.
sfm_min_base
;
imp
.
sfm_min_gain
=
this
.
sfm_min_gain
;
...
...
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @
e9aa9780
...
...
@@ -4773,6 +4773,20 @@ public class OpticalFlow {
int
center_index
=
quadCLTs
[
last_index
].
getReferenceIndex
(
null
);
if
(
center_index
==
-
1
)
{
build_ref_dsi
=
true
;
}
else
{
QuadCLT
try_ref_scene
=
(
QuadCLT
)
quadCLT_main
.
spawnQuadCLT
(
// will conditionImageSet
quadCLTs
[
last_index
].
getReferenceTimestamp
(),
// set_channels[last_index].set_name,
clt_parameters
,
colorProcParameters
,
//
threadsMax
,
debugLevel
-
2
);
if
((
try_ref_scene
==
null
)
||
(
try_ref_scene
.
getDLS
()
==
null
))
{
if
(
debugLevel
>-
2
)
{
System
.
out
.
println
(
"DSI data for scene "
+
quadCLTs
[
last_index
].
getReferenceTimestamp
()+
" does not exist, forcing initial orientation."
);
}
force_initial_orientations
=
true
;
}
}
}
else
{
if
((
quadCLTs
[
last_index
]
==
null
)
||
!
quadCLTs
[
last_index
].
dsiExists
())
{
...
...
@@ -4995,7 +5009,7 @@ public class OpticalFlow {
ref_index
=
quadCLTs
[
last_index
].
getReferenceIndex
(
quadCLTs
);
quadCLTs
[
ref_index
].
restoreInterProperties
(
null
,
false
,
debugLevel
);
//null
int
[]
first_last_index
=
quadCLTs
[
ref_index
].
getFirstLastIndex
(
quadCLTs
);
earliest_scene
=
first_last_index
[
0
];
earliest_scene
=
first_last_index
[
0
];
// null pointer if does not exist
last_index
=
first_last_index
[
1
];
}
// TODO: 10.17.2023 - verify OK to remove this - next should be already tested during initial orientation
...
...
@@ -5188,8 +5202,12 @@ public class OpticalFlow {
}
boolean
disable_ers
=
(
quadCLTs
[
ref_index
].
getNumOrient
()
<
2
);
// first orient - no ERS!
boolean
ers_from_ims
=
false
;
// change later
int
ers_mode
=
(
quadCLTs
[
ref_index
].
getNumOrient
()
<
2
)
?
(
ers_from_ims
?
2
:
1
):
0
;
ers_mode
=
1
;
// TODO: remove later!
// int ers_mode = (quadCLTs[ref_index].getNumOrient() < 2) ? (ers_from_ims? 2 : 1):0;
int
ers_mode
=
0
;
// keep
// with IMS it is already set during initial orientation. In non-IMS mode
if
(!
ers_from_ims
&&
(
quadCLTs
[
ref_index
].
getNumOrient
()
<
2
))
{
ers_mode
=
1
;
// calculate velocity
}
earliest_scene
=
Interscene
.
reAdjustPairsLMAInterscene
(
// after combo dsi is available and preliminary poses are known
clt_parameters
,
// CLTParameters clt_parameters,
mb_max_gain
,
// double mb_max_gain,
...
...
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @
e9aa9780
...
...
@@ -196,6 +196,57 @@ public class QuadCLTCPU {
return
timestamp_reference
;
}
public
double
[][]
getDxyzatrIms
(
CLTParameters
clt_parameters
,
boolean
scale
)
{
double
[]
ims_ortho
=
clt_parameters
.
imp
.
ims_ortho
;
double
[]
ims_mount_atr
=
clt_parameters
.
imp
.
getImsMountATR
();
// converts to radians
double
[]
quat_ims_cam
=
Imx5
.
quaternionImsToCam
(
ims_mount_atr
,
// new double[] {0, 0.13, 0},
ims_ortho
);
double
[]
double_uvw
=
did_ins_2
.
getUvw
();
double
[]
omegas
=
new
double
[]
{
did_pimu
.
theta
[
0
]/
did_pimu
.
dt
,
did_pimu
.
theta
[
1
]/
did_pimu
.
dt
,
did_pimu
.
theta
[
2
]/
did_pimu
.
dt
};
double
[]
cam_dxyz
=
Imx5
.
applyQuternionTo
(
quat_ims_cam
,
double_uvw
,
false
);
double
[]
cam_datr
=
Imx5
.
applyQuternionTo
(
quat_ims_cam
,
omegas
,
false
);
// a (around Y),t (around X), r (around Z)
double
[][]
dxyzatyr
=
new
double
[][]
{
cam_dxyz
,{
cam_datr
[
1
],
cam_datr
[
0
],
cam_datr
[
2
]}};
if
(
scale
)
{
for
(
int
i
=
0
;
i
<
3
;
i
++)
{
dxyzatyr
[
0
][
i
]*=
clt_parameters
.
imp
.
ims_scale_xyz
[
i
];
dxyzatyr
[
1
][
i
]*=
clt_parameters
.
imp
.
ims_scale_atr
[
i
];
}
}
return
dxyzatyr
;
}
public
static
double
[][]
scaleDtToErs
(
CLTParameters
clt_parameters
,
double
[][]
cam_dxyzatr
){
double
[][]
dbg_scale_dt
=
{
clt_parameters
.
ilp
.
ilma_scale_xyz
,
clt_parameters
.
ilp
.
ilma_scale_atr
};
double
[][]
scaled_dxyzatr
=
new
double
[
2
][
3
];
for
(
int
i
=
0
;
i
<
scaled_dxyzatr
.
length
;
i
++)
{
for
(
int
j
=
0
;
j
<
scaled_dxyzatr
[
i
].
length
;
j
++)
{
scaled_dxyzatr
[
i
][
j
]
=
cam_dxyzatr
[
i
][
j
]*
dbg_scale_dt
[
i
][
j
];
}
}
return
scaled_dxyzatr
;
}
public
static
double
[][]
scaleDtFromErs
(
CLTParameters
clt_parameters
,
double
[][]
scaled_dxyzatr
){
double
[][]
dbg_scale_dt
=
{
clt_parameters
.
ilp
.
ilma_scale_xyz
,
clt_parameters
.
ilp
.
ilma_scale_atr
};
double
[][]
cam_dxyzatr
=
new
double
[
2
][
3
];
for
(
int
i
=
0
;
i
<
scaled_dxyzatr
.
length
;
i
++)
{
for
(
int
j
=
0
;
j
<
scaled_dxyzatr
[
i
].
length
;
j
++)
{
cam_dxyzatr
[
i
][
j
]
=
scaled_dxyzatr
[
i
][
j
]
/
dbg_scale_dt
[
i
][
j
];
}
}
return
cam_dxyzatr
;
}
/**
* If this scene has a reference to one of the scenes array, return its index in the array
* otherwise return -1 if there is no pointer to reference scene and -2 if pointer is not
...
...
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