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
ab8d3318
Commit
ab8d3318
authored
Jan 30, 2024
by
Andrey Filippov
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
Bug fixing to make run for many scenes. Started LPF for the ground w/o
SfM
parent
3f84a242
Changes
8
Hide whitespace changes
Inline
Side-by-side
Showing
8 changed files
with
774 additions
and
138 deletions
+774
-138
Eyesis_Correction.java
.../java/com/elphel/imagej/correction/Eyesis_Correction.java
+1
-1
ElphelTiffWriter.java
...main/java/com/elphel/imagej/readers/ElphelTiffWriter.java
+1
-1
Interscene.java
...main/java/com/elphel/imagej/tileprocessor/Interscene.java
+180
-9
IntersceneMatchParameters.java
...lphel/imagej/tileprocessor/IntersceneMatchParameters.java
+17
-2
OpticalFlow.java
...ain/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
+37
-15
QuadCLTCPU.java
...main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
+480
-82
QuaternionLma.java
...n/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
+56
-27
TexturedModel.java
...n/java/com/elphel/imagej/tileprocessor/TexturedModel.java
+2
-1
No files found.
src/main/java/com/elphel/imagej/correction/Eyesis_Correction.java
View file @
ab8d3318
...
@@ -5090,7 +5090,7 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
...
@@ -5090,7 +5090,7 @@ public class Eyesis_Correction implements PlugIn, ActionListener {
System
.
out
.
println
(
"=== IMS ==="
);
System
.
out
.
println
(
"=== IMS ==="
);
QUAD_CLT_AUX
.
showQuatCorr
();
QUAD_CLT_AUX
.
showQuatCorr
();
System
.
out
.
println
(
"=== IMU ==="
);
System
.
out
.
println
(
"=== IMU ==="
);
QUAD_CLT_AUX
.
showPimuOffsets
(
);
QUAD_CLT_AUX
.
showPimuOffsets
(
CLT_PARAMETERS
);
//
@SuppressWarnings
(
"unused"
)
@SuppressWarnings
(
"unused"
)
QuadCLT
dbg_QUAD_CLT
=
QUAD_CLT
;
QuadCLT
dbg_QUAD_CLT
=
QUAD_CLT
;
@SuppressWarnings
(
"unused"
)
@SuppressWarnings
(
"unused"
)
...
...
src/main/java/com/elphel/imagej/readers/ElphelTiffWriter.java
View file @
ab8d3318
...
@@ -295,7 +295,7 @@ public class ElphelTiffWriter {
...
@@ -295,7 +295,7 @@ public class ElphelTiffWriter {
private
static
IIOMetadataNode
createTimeStamp
(
LocalDateTime
dt
,
int
digits_after
)
{
// 3
private
static
IIOMetadataNode
createTimeStamp
(
LocalDateTime
dt
,
int
digits_after
)
{
// 3
int
denom
=
1
;
int
denom
=
1
;
for
(
int
i
=
0
;
i
<
digits_after
;
i
++)
denom
*=
10
;
for
(
int
i
=
0
;
i
<
digits_after
;
i
++)
denom
*=
10
;
int
fsec
=
dt
.
getSecond
()*
denom
+((
int
)
Math
.
round
(
denom
*
dt
.
getNano
()*
1
E
-
9
));
int
fsec
=
dt
.
getSecond
()*
denom
+((
int
)
Math
.
round
(
denom
*
(
dt
.
getNano
()*
1
E
-
9
)
));
IIOMetadataNode
node_rationals
=
new
IIOMetadataNode
(
TIFF_RATIONALS_TAG
);
IIOMetadataNode
node_rationals
=
new
IIOMetadataNode
(
TIFF_RATIONALS_TAG
);
IIOMetadataNode
node_hrs
=
new
IIOMetadataNode
(
TIFF_RATIONAL_TAG
);
IIOMetadataNode
node_hrs
=
new
IIOMetadataNode
(
TIFF_RATIONAL_TAG
);
...
...
src/main/java/com/elphel/imagej/tileprocessor/Interscene.java
View file @
ab8d3318
...
@@ -23,8 +23,10 @@
...
@@ -23,8 +23,10 @@
*/
*/
package
com
.
elphel
.
imagej
.
tileprocessor
;
package
com
.
elphel
.
imagej
.
tileprocessor
;
import
java.text.SimpleDateFormat
;
import
java.util.ArrayList
;
import
java.util.ArrayList
;
import
java.util.Arrays
;
import
java.util.Arrays
;
import
java.util.Calendar
;
import
java.util.concurrent.atomic.AtomicInteger
;
import
java.util.concurrent.atomic.AtomicInteger
;
import
org.apache.commons.math3.geometry.euclidean.threed.Rotation
;
import
org.apache.commons.math3.geometry.euclidean.threed.Rotation
;
...
@@ -508,6 +510,11 @@ public class Interscene {
...
@@ -508,6 +510,11 @@ public class Interscene {
double
sfm_fracall
=
clt_parameters
.
imp
.
sfm_fracall
;
// 0.3; // minimal relative area of the SfM-e
double
sfm_fracall
=
clt_parameters
.
imp
.
sfm_fracall
;
// 0.3; // minimal relative area of the SfM-e
boolean
fmg_initial_en
=
clt_parameters
.
imp
.
fmg_initial_en
;
// enable IMS-based FPN mitigation for initial orientation
boolean
fmg_initial_en
=
clt_parameters
.
imp
.
fmg_initial_en
;
// enable IMS-based FPN mitigation for initial orientation
double
fmg_distance
=
clt_parameters
.
imp
.
fmg_distance
;
// try to find other reference scene not closer than this pixels
double
fmg_distance
=
clt_parameters
.
imp
.
fmg_distance
;
// try to find other reference scene not closer than this pixels
// todo: improve to use "preferred" distance
if
(
fmg_distance
<
(
min_offset
+
2
))
{
fmg_distance
=
min_offset
+
2
;
}
double
fmg_max_quad
=
clt_parameters
.
imp
.
fmg_max_quad
;
// estimate offset by 4 points (rooll-aware, 25% from center) if center
double
fmg_max_quad
=
clt_parameters
.
imp
.
fmg_max_quad
;
// estimate offset by 4 points (rooll-aware, 25% from center) if center
// offset is too small
// offset is too small
boolean
fmg_rectilinear
=
clt_parameters
.
imp
.
fmg_rectilinear
;
// use rectilinear model for scene offset estimation
boolean
fmg_rectilinear
=
clt_parameters
.
imp
.
fmg_rectilinear
;
// use rectilinear model for scene offset estimation
...
@@ -680,6 +687,10 @@ public class Interscene {
...
@@ -680,6 +687,10 @@ public class Interscene {
if
(
fmg_initial_en
&&
!
fpn_list
.
isEmpty
())
{
if
(
fmg_initial_en
&&
!
fpn_list
.
isEmpty
())
{
// here max_offset is not critical, min_offset can be 0 too
// here max_offset is not critical, min_offset can be 0 too
//double [] min_max = {min_offset, max_offset, 0.0} ; // {min, max, actual rms)
//double [] min_max = {min_offset, max_offset, 0.0} ; // {min, max, actual rms)
if
(
fmg_distance
<
(
min_max
[
0
]
+
2
))
{
fmg_distance
=
min_max
[
0
]
+
2
;
}
int
[][]
fpn_pairs
=
getFPNPairs
(
int
[][]
fpn_pairs
=
getFPNPairs
(
fpn_list
,
// ArrayList<Integer> fpn_list,
fpn_list
,
// ArrayList<Integer> fpn_list,
fmg_distance
,
// double fpn_mitigate_dist,
fmg_distance
,
// double fpn_mitigate_dist,
...
@@ -689,6 +700,15 @@ public class Interscene {
...
@@ -689,6 +700,15 @@ public class Interscene {
avg_z
,
// double avg_z,
avg_z
,
// double avg_z,
last_index
,
// ref_index, // int ref_index, // >= earliest_scene
last_index
,
// ref_index, // int ref_index, // >= earliest_scene
earliest_index
);
// int earliest_scene)
earliest_index
);
// int earliest_scene)
// mitigating problem, that in the process of adjusting offset can fall below
// the minimum and coordinates will be NaN:
/*
Mitigating FPN for scene 361 being too close to reference 344, using scene 367 as a reference
interCorrPair(): avg_offs = 10.755562139763402 <= 256.0, sw = 4217.0 iter=3, RMS=0.5364197453455559 (Pure RMS=0.5364197453455559)
interCorrPair(): avg_offs = 7.795390182218127 < 8.0, sw = 4219.0
adjustPairsLMAInterscene() returned null
*/
double
[]
min_max_fpn
=
{
0
,
min_max
[
1
]};
for
(
int
ipair
=
0
;
ipair
<
fpn_pairs
.
length
;
ipair
++)
if
(
fpn_pairs
[
ipair
][
1
]
>=
0
)
{
for
(
int
ipair
=
0
;
ipair
<
fpn_pairs
.
length
;
ipair
++)
if
(
fpn_pairs
[
ipair
][
1
]
>=
0
)
{
if
(
debugLevel
>
-
4
)
{
if
(
debugLevel
>
-
4
)
{
System
.
out
.
println
(
"Mitigating FPN for scene "
+
fpn_pairs
[
ipair
][
0
]+
System
.
out
.
println
(
"Mitigating FPN for scene "
+
fpn_pairs
[
ipair
][
0
]+
...
@@ -701,7 +721,7 @@ public class Interscene {
...
@@ -701,7 +721,7 @@ public class Interscene {
use_lma_dsi
,
// clt_parameters.imp.use_lma_dsi,
use_lma_dsi
,
// clt_parameters.imp.use_lma_dsi,
false
,
// boolean fpn_disable, // disable fpn filter if images are known to be too close
false
,
// boolean fpn_disable, // disable fpn filter if images are known to be too close
true
,
// boolean disable_ers,
true
,
// boolean disable_ers,
min_max
,
// double []
min_max, // null or pair of minimal and maximal offsets
min_max
_fpn
,
// min_max,// double []
min_max, // null or pair of minimal and maximal offsets
fail_reason
,
// int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
fail_reason
,
// int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
quadCLTs
,
// QuadCLT [] quadCLTs,
quadCLTs
,
// QuadCLT [] quadCLTs,
ref_index
,
// int ref_index,
ref_index
,
// int ref_index,
...
@@ -755,6 +775,23 @@ public class Interscene {
...
@@ -755,6 +775,23 @@ public class Interscene {
}
}
}
}
}
}
// Add to log
StringBuffer
sb
=
new
StringBuffer
();
sb
.
append
(
new
SimpleDateFormat
(
"yyyy/MM/dd HH:mm:ss"
).
format
(
Calendar
.
getInstance
().
getTime
())+
"\n"
);
sb
.
append
(
"Finished invertInitialOrientation():\n"
);
sb
.
append
(
"getNumOrient()= "
+
quadCLTs
[
ref_index
].
getNumOrient
()+
" of "
+
clt_parameters
.
imp
.
min_num_orient
+
"\n"
);
sb
.
append
(
"getNumAccum()= "
+
quadCLTs
[
ref_index
].
getNumAccum
()+
" of "
+
clt_parameters
.
imp
.
min_num_interscene
+
"\n"
);
sb
.
append
(
"earliest_scene= "
+
earliest_index
+
"\n"
);
sb
.
append
(
"ref_index= "
+
ref_index
+
"\n"
);
sb
.
append
(
"last_index= "
+
last_index
+
"\n"
);
sb
.
append
(
"old_ref_index= "
+
ref_old
+
"\n"
);
sb
.
append
(
"Maximal RMSE= "
+
maximal_series_rms
+
"\n"
);
sb
.
append
(
"------------------------\n\n"
);
quadCLTs
[
ref_index
].
saveStringInModelDirectory
(
sb
.
toString
(),
QuadCLT
.
ORIENTATION_LOGS_SUFFIX
);
// String suffix)
}
}
return
0
;
return
0
;
}
}
...
@@ -1161,6 +1198,9 @@ public class Interscene {
...
@@ -1161,6 +1198,9 @@ public class Interscene {
if
(
fmg_initial_en
&&
!
fpn_list
.
isEmpty
())
{
if
(
fmg_initial_en
&&
!
fpn_list
.
isEmpty
())
{
// here max_offset is not critical, min_offset can be 0 too
// here max_offset is not critical, min_offset can be 0 too
double
[]
min_max
=
{
min_offset
,
max_offset
,
0.0
}
;
// {min, max, actual rms)
double
[]
min_max
=
{
min_offset
,
max_offset
,
0.0
}
;
// {min, max, actual rms)
if
(
fmg_distance
<
(
min_max
[
0
]
+
2
))
{
fmg_distance
=
min_max
[
0
]
+
2
;
}
int
[][]
fpn_pairs
=
getFPNPairs
(
int
[][]
fpn_pairs
=
getFPNPairs
(
fpn_list
,
// ArrayList<Integer> fpn_list,
fpn_list
,
// ArrayList<Integer> fpn_list,
fmg_distance
,
// double fpn_mitigate_dist,
fmg_distance
,
// double fpn_mitigate_dist,
...
@@ -1170,6 +1210,15 @@ public class Interscene {
...
@@ -1170,6 +1210,15 @@ public class Interscene {
avg_z
,
// double avg_z,
avg_z
,
// double avg_z,
ref_index
,
// int ref_index, // >= earliest_scene
ref_index
,
// int ref_index, // >= earliest_scene
earliest_scene
);
// int earliest_scene)
earliest_scene
);
// int earliest_scene)
// mitigating problem, that in the process of adjusting offset can fall below
// the minimum and coordinates will be NaN:
/*
Mitigating FPN for scene 361 being too close to reference 344, using scene 367 as a reference
interCorrPair(): avg_offs = 10.755562139763402 <= 256.0, sw = 4217.0 iter=3, RMS=0.5364197453455559 (Pure RMS=0.5364197453455559)
interCorrPair(): avg_offs = 7.795390182218127 < 8.0, sw = 4219.0
adjustPairsLMAInterscene() returned null
*/
double
[]
min_max_fpn
=
{
0
,
min_max
[
1
]};
for
(
int
ipair
=
0
;
ipair
<
fpn_pairs
.
length
;
ipair
++)
if
(
fpn_pairs
[
ipair
][
1
]
>=
0
)
{
for
(
int
ipair
=
0
;
ipair
<
fpn_pairs
.
length
;
ipair
++)
if
(
fpn_pairs
[
ipair
][
1
]
>=
0
)
{
if
(
debugLevel
>
-
4
)
{
if
(
debugLevel
>
-
4
)
{
System
.
out
.
println
(
"Mitigating FPN for scene "
+
fpn_pairs
[
ipair
][
0
]+
System
.
out
.
println
(
"Mitigating FPN for scene "
+
fpn_pairs
[
ipair
][
0
]+
...
@@ -1182,7 +1231,7 @@ public class Interscene {
...
@@ -1182,7 +1231,7 @@ public class Interscene {
use_lma_dsi
,
// clt_parameters.imp.use_lma_dsi,
use_lma_dsi
,
// clt_parameters.imp.use_lma_dsi,
false
,
// boolean fpn_disable, // disable fpn filter if images are known to be too close
false
,
// boolean fpn_disable, // disable fpn filter if images are known to be too close
true
,
// boolean disable_ers,
true
,
// boolean disable_ers,
min_max
,
// double []
min_max, // null or pair of minimal and maximal offsets
min_max
_fpn
,
// min_max,// double []
min_max, // null or pair of minimal and maximal offsets
fail_reason
,
// int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
fail_reason
,
// int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
quadCLTs
,
// QuadCLT [] quadCLTs,
quadCLTs
,
// QuadCLT [] quadCLTs,
ref_index
,
// int ref_index,
ref_index
,
// int ref_index,
...
@@ -1249,6 +1298,17 @@ public class Interscene {
...
@@ -1249,6 +1298,17 @@ public class Interscene {
quadCLTs
[
ref_index
].
saveInterProperties
(
// save properties for interscene processing (extrinsics, ers, ...) // null pointer
quadCLTs
[
ref_index
].
saveInterProperties
(
// save properties for interscene processing (extrinsics, ers, ...) // null pointer
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
);
// Add to log
StringBuffer
sb
=
new
StringBuffer
();
sb
.
append
(
new
SimpleDateFormat
(
"yyyy/MM/dd HH:mm:ss"
).
format
(
Calendar
.
getInstance
().
getTime
())+
"\n"
);
sb
.
append
(
"Finished setInitialOrientationsIms():\n"
);
sb
.
append
(
"getNumOrient()= "
+
quadCLTs
[
ref_index
].
getNumOrient
()+
" of "
+
clt_parameters
.
imp
.
min_num_orient
+
"\n"
);
sb
.
append
(
"getNumAccum()= "
+
quadCLTs
[
ref_index
].
getNumAccum
()+
" of "
+
clt_parameters
.
imp
.
min_num_interscene
+
"\n"
);
sb
.
append
(
"earliest_scene= "
+
earliest_scene
+
"\n"
);
sb
.
append
(
"ref_index= "
+
ref_index
+
"\n"
);
sb
.
append
(
"Maximal RMSE= "
+
maximal_series_rms
+
"\n"
);
sb
.
append
(
"------------------------\n\n"
);
quadCLTs
[
ref_index
].
saveStringInModelDirectory
(
sb
.
toString
(),
QuadCLT
.
ORIENTATION_LOGS_SUFFIX
);
// String suffix)
return
earliest_scene
;
return
earliest_scene
;
}
}
...
@@ -1919,6 +1979,7 @@ public class Interscene {
...
@@ -1919,6 +1979,7 @@ public class Interscene {
double
fmg_distance
=
clt_parameters
.
imp
.
fmg_distance
;
// try to find other reference scene not closer than this pixels
double
fmg_distance
=
clt_parameters
.
imp
.
fmg_distance
;
// try to find other reference scene not closer than this pixels
double
fmg_max_quad
=
clt_parameters
.
imp
.
fmg_max_quad
;
// estimate offset by 4 points (rooll-aware, 25% from center) if center
double
fmg_max_quad
=
clt_parameters
.
imp
.
fmg_max_quad
;
// estimate offset by 4 points (rooll-aware, 25% from center) if center
boolean
fmg_rectilinear
=
clt_parameters
.
imp
.
fmg_rectilinear
;
// use rectilinear model for scene offset estimation
boolean
fmg_rectilinear
=
clt_parameters
.
imp
.
fmg_rectilinear
;
// use rectilinear model for scene offset estimation
boolean
use_precomp
=
clt_parameters
.
imp
.
use_precomp
;
// try to predict initial error from previous scenes
// int avg_len = clt_parameters.imp.avg_len;
// int avg_len = clt_parameters.imp.avg_len;
// Set up velocities from known coordinates, use averaging
// Set up velocities from known coordinates, use averaging
...
@@ -2091,6 +2152,28 @@ public class Interscene {
...
@@ -2091,6 +2152,28 @@ public class Interscene {
last_scene
);
// int last_index)
last_scene
);
// int last_index)
// } else if (readjust_xy_ims && (reg_weight_xy > 0.0)) {
// } else if (readjust_xy_ims && (reg_weight_xy > 0.0)) {
}
else
if
(
readjust_xy_ims
)
{
}
else
if
(
readjust_xy_ims
)
{
// optionally run adjustment here (QuadCLT.rotateImsToCameraXYZ()? Or only
scenes_xyzatr_pull
=
QuadCLT
.
refineXYZFromIMU
(
clt_parameters
,
// CLTParameters clt_parameters,
false
,
// true, // false, // boolean common_scale_only // false - individual by direction
use_Z
,
// boolean keepZ, // if adjusting Z, qill use its old value
quadCLTs
,
// QuadCLT[] quadCLTs,
scenes_xyzatr
,
// double [][][] xyzatr,
null
,
// double [][][] pimu_xyzatr, // if null - will be recalculated
ref_index
,
// int ref_index,
earliest_scene
,
// int early_index,
last_scene
,
// int last_index,
debugLevel
);
//int debugLevel)
for
(
int
nscene
=
earliest_scene
;
nscene
<=
last_scene
;
nscene
++)
if
(
scenes_xyzatr
[
nscene
]
!=
null
)
{
scenes_xyzatr_pull
[
nscene
]
=
modifyATRtoXYZ
(
scenes_xyzatr
[
nscene
],
// double [][] cur_xyzatr, // careful with Z - using the new one
scenes_xyzatr_pull
[
nscene
][
0
],
// double [] new_xyz,
avg_z
);
// double avg_z
}
/*
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
double [][][] pimu_xyzatr = QuadCLT.integratePIMU(
clt_parameters, // final CLTParameters clt_parameters,
clt_parameters, // final CLTParameters clt_parameters,
quadCLTs, // final QuadCLT[] quadCLTs,
quadCLTs, // final QuadCLT[] quadCLTs,
...
@@ -2146,6 +2229,7 @@ public class Interscene {
...
@@ -2146,6 +2229,7 @@ public class Interscene {
avg_z); // double avg_z
avg_z); // double avg_z
}
}
}
}
*/
// old version
// old version
/*
/*
scenes_xyzatr_pull = QuadCLT.refineFromImsVelocities(
scenes_xyzatr_pull = QuadCLT.refineFromImsVelocities(
...
@@ -2287,6 +2371,18 @@ public class Interscene {
...
@@ -2287,6 +2371,18 @@ public class Interscene {
System
.
out
.
println
(
"test_adjust = "
+
test_adjust
);
System
.
out
.
println
(
"test_adjust = "
+
test_adjust
);
}
}
// Main cycle - first goes down from the center, then up, and finally processes "fpn" scenes (close to reference)
double
[][]
precomp_xyzatr
=
new
double
[
2
][
3
];
int
last_processed_scene
=
-
2
;
// none
/*
* There is still a problem with a large mismatch at start position for far off-reference scenes,
* especiall those that were not refined after inversion - reversing order of earlier processed "later"
* half (after the reference in the center) earlier processed as the bottom half of the other sub-sequence.
*
* For mitigation we'll use that both half-sequences start from the center with small mismatches. The
* precomp_xyzatr will be used to store how much LMA corrected from the original estimation and add it
* to the next scene (only when it is the next), similar to the initial orientation.
*/
for
(
int
nscene:
scene_seq
)
{
for
(
int
nscene:
scene_seq
)
{
if
(
nscene
==
debug_scene
)
{
if
(
nscene
==
debug_scene
)
{
...
@@ -2351,6 +2447,26 @@ public class Interscene {
...
@@ -2351,6 +2447,26 @@ public class Interscene {
if
(
est_shift
<
min_max
[
0
])
{
if
(
est_shift
<
min_max
[
0
])
{
fail_reason
[
0
]=
FAIL_REASON_MIN
;
fail_reason
[
0
]=
FAIL_REASON_MIN
;
}
else
{
}
else
{
double
[][]
corr_xyzatr_pull
=
new
double
[][]
{
scenes_xyzatr_pull
[
nscene
][
0
].
clone
(),
scenes_xyzatr_pull
[
nscene
][
1
].
clone
()};
/*
* scenes_xyzatr[ref_index],// double [][] scene0_xyzatr, - scene to compare to
* scenes_xyzatr[nscene], // double [][] scene1_xyzatr - previous known pose?
* scenes_xyzatr_pull[nscene], // double [][] scene1_xyzatr_pull, - now not a pull,
* but a rigid target set as initial approximation
*/
boolean
applied_precomp
=
false
;
if
(
use_precomp
&&
(
last_processed_scene
>=
nscene
-
1
)
&&
(
last_processed_scene
<=
nscene
+
1
))
{
corr_xyzatr_pull
=
ErsCorrection
.
combineXYZATR
(
scenes_xyzatr_pull
[
nscene
],
precomp_xyzatr
);
applied_precomp
=
true
;
if
(
debugLevel
>
-
2
)
{
System
.
out
.
println
(
String
.
format
(
"Applied precompensation: [%9.6f, %9.6f, %9.6f] [%9.6f, %9.6f, %9.6f]"
,
precomp_xyzatr
[
0
][
0
],
precomp_xyzatr
[
0
][
1
],
precomp_xyzatr
[
0
][
2
],
precomp_xyzatr
[
1
][
0
],
precomp_xyzatr
[
1
][
1
],
precomp_xyzatr
[
1
][
2
]));
}
}
scenes_xyzatr
[
nscene
]
=
adjustDiffPairsLMAInterscene
(
// compare two scenes, first may be reference, use motion blur
scenes_xyzatr
[
nscene
]
=
adjustDiffPairsLMAInterscene
(
// compare two scenes, first may be reference, use motion blur
clt_parameters
,
//CLTParameters clt_parameters,
clt_parameters
,
//CLTParameters clt_parameters,
use_lma_dsi
,
//,boolean use_lma_dsi,
use_lma_dsi
,
//,boolean use_lma_dsi,
...
@@ -2371,7 +2487,7 @@ public class Interscene {
...
@@ -2371,7 +2487,7 @@ public class Interscene {
scenes_xyzatr
[
ref_index
],
// double [][] scene0_xyzatr,
scenes_xyzatr
[
ref_index
],
// double [][] scene0_xyzatr,
scenes_xyzatr
[
nscene
],
// double [][] scene1_xyzatr,
scenes_xyzatr
[
nscene
],
// double [][] scene1_xyzatr,
avg_z
,
// double average_z,
avg_z
,
// double average_z,
scenes_xyzatr_pull
[
nscene
],
// double [][] scene1_xyzatr_pull,
corr_xyzatr_pull
,
//
scenes_xyzatr_pull[nscene], // double [][] scene1_xyzatr_pull,
param_select
,
// boolean[] param_select,
param_select
,
// boolean[] param_select,
param_regweights
,
// double [] param_regweights,
param_regweights
,
// double [] param_regweights,
lma_rms
,
// double [] rms_out, // null or double [2]
lma_rms
,
// double [] rms_out, // null or double [2]
...
@@ -2381,6 +2497,22 @@ public class Interscene {
...
@@ -2381,6 +2497,22 @@ public class Interscene {
mb_max_gain
,
// double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
mb_max_gain
,
// double mb_max_gain, // 5.0; // motion blur maximal gain (if more - move second point more than a pixel
clt_parameters
.
imp
.
debug_level
);
// int debugLevel)
clt_parameters
.
imp
.
debug_level
);
// int debugLevel)
adjust_OK
=
scenes_xyzatr
[
nscene
]
!=
null
;
adjust_OK
=
scenes_xyzatr
[
nscene
]
!=
null
;
if
(!
adjust_OK
)
{
last_processed_scene
=
-
2
;
}
else
if
(
use_precomp
)
{
double
[][]
last_corr_xyzatr
=
ErsCorrection
.
combineXYZATR
(
scenes_xyzatr
[
nscene
],
ErsCorrection
.
invertXYZATR
(
corr_xyzatr_pull
));
// add precomp_xyzatr if it was applied:
if
(
applied_precomp
)
{
precomp_xyzatr
=
ErsCorrection
.
combineXYZATR
(
precomp_xyzatr
,
last_corr_xyzatr
);
}
else
{
// just use difference
precomp_xyzatr
=
last_corr_xyzatr
;
}
last_processed_scene
=
nscene
;
}
}
}
if
(
adjust_OK
&&
fail_on_zoom_roll
)
{
// check only for initial orientation, do not check on readjustments
if
(
adjust_OK
&&
fail_on_zoom_roll
)
{
// check only for initial orientation, do not check on readjustments
if
(
Math
.
abs
(
scenes_xyzatr
[
nscene
][
1
][
2
])
>
max_roll
)
{
if
(
Math
.
abs
(
scenes_xyzatr
[
nscene
][
1
][
2
])
>
max_roll
)
{
...
@@ -2460,6 +2592,9 @@ public class Interscene {
...
@@ -2460,6 +2592,9 @@ public class Interscene {
System
.
out
.
println
(
"num_fpn_mitigate= "
+
fpn_list
.
size
());
System
.
out
.
println
(
"num_fpn_mitigate= "
+
fpn_list
.
size
());
}
}
if
(
fmg_reorient_en
&&
!
fpn_list
.
isEmpty
())
{
if
(
fmg_reorient_en
&&
!
fpn_list
.
isEmpty
())
{
if
(
fmg_distance
<
(
min_max
[
0
]
+
2
))
{
fmg_distance
=
min_max
[
0
]
+
2
;
}
int
[][]
fpn_pairs
=
getFPNPairs
(
int
[][]
fpn_pairs
=
getFPNPairs
(
fpn_list
,
// ArrayList<Integer> fpn_list,
fpn_list
,
// ArrayList<Integer> fpn_list,
fmg_distance
,
// double fpn_mitigate_dist,
fmg_distance
,
// double fpn_mitigate_dist,
...
@@ -2469,6 +2604,15 @@ public class Interscene {
...
@@ -2469,6 +2604,15 @@ public class Interscene {
avg_z
,
// double avg_z,
avg_z
,
// double avg_z,
last_scene
,
// latest_scene, // int ref_index, // >= earliest_scene
last_scene
,
// latest_scene, // int ref_index, // >= earliest_scene
earliest_scene
);
// int earliest_scene)
earliest_scene
);
// int earliest_scene)
// mitigating problem, that in the process of adjusting offset can fall below
// the minimum and coordinates will be NaN:
/*
Mitigating FPN for scene 361 being too close to reference 344, using scene 367 as a reference
interCorrPair(): avg_offs = 10.755562139763402 <= 256.0, sw = 4217.0 iter=3, RMS=0.5364197453455559 (Pure RMS=0.5364197453455559)
interCorrPair(): avg_offs = 7.795390182218127 < 8.0, sw = 4219.0
adjustPairsLMAInterscene() returned null
*/
double
[]
min_max_fpn
=
{
0
,
min_max
[
1
]};
boolean
test_adjust1
=
debugLevel
>
1000
;
boolean
test_adjust1
=
debugLevel
>
1000
;
if
(
test_adjust1
)
{
if
(
test_adjust1
)
{
int
[][]
fpn_pairs_dbg
=
new
int
[
fpn_pairs
.
length
+
3
][
2
];
int
[][]
fpn_pairs_dbg
=
new
int
[
fpn_pairs
.
length
+
3
][
2
];
...
@@ -2503,7 +2647,7 @@ public class Interscene {
...
@@ -2503,7 +2647,7 @@ public class Interscene {
use_lma_dsi
,
//,boolean use_lma_dsi,
use_lma_dsi
,
//,boolean use_lma_dsi,
fpn_disable
,
// boolean fpn_disable, // disable fpn filter if images are known to be too close
fpn_disable
,
// boolean fpn_disable, // disable fpn filter if images are known to be too close
disable_ers
,
// boolean disable_ers,
disable_ers
,
// boolean disable_ers,
min_max
,
// double [] min_max, // null or pair of minimal and maximal offsets
min_max
_fpn
,
// min_max,
// double [] min_max, // null or pair of minimal and maximal offsets
fail_reason
,
// int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
fail_reason
,
// int [] fail_reason, // null or int[1]: 0 - OK, 1 - LMA, 2 - min, 3 - max
quadCLTs
,
// QuadCLT [] quadCLTs,
quadCLTs
,
// QuadCLT [] quadCLTs,
ref_index
,
// int ref_index,
ref_index
,
// int ref_index,
...
@@ -2624,6 +2768,31 @@ public class Interscene {
...
@@ -2624,6 +2768,31 @@ public class Interscene {
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
);
// Add to log
StringBuffer
sb
=
new
StringBuffer
();
sb
.
append
(
new
SimpleDateFormat
(
"yyyy/MM/dd HH:mm:ss"
).
format
(
Calendar
.
getInstance
().
getTime
())+
"\n"
);
sb
.
append
(
"Finished reAdjustPairsLMAInterscene():\n"
);
sb
.
append
(
"getNumOrient()= "
+
quadCLTs
[
ref_index
].
getNumOrient
()+
" of "
+
clt_parameters
.
imp
.
min_num_orient
+
"\n"
);
sb
.
append
(
"getNumAccum()= "
+
quadCLTs
[
ref_index
].
getNumAccum
()+
" of "
+
clt_parameters
.
imp
.
min_num_interscene
+
"\n"
);
sb
.
append
(
"earliest_scene= "
+
range
[
0
]+
"\n"
);
sb
.
append
(
"ref_index= "
+
ref_index
+
"\n"
);
sb
.
append
(
"last_index= "
+
range
[
1
]+
"\n"
);
sb
.
append
(
"Maximal RMSE= "
+
maximal_series_rms
+
"\n"
);
sb
.
append
(
"lpf_xy= "
+
lpf_xy
+
"\n"
);
sb
.
append
(
"readjust_xy_ims="
+
readjust_xy_ims
+
"\n"
);
sb
.
append
(
"lma_xyzatr= "
+
lma_xyzatr
+
"\n"
);
sb
.
append
(
"use_R= "
+
use_R
+
"\n"
);
sb
.
append
(
"use_R= "
+
use_R
+
"\n"
);
sb
.
append
(
"mb_max_gain= "
+
mb_max_gain
+
"\n"
);
sb
.
append
(
"avg_z= "
+
avg_z
+
"m\n"
);
sb
.
append
(
"reg_weight_xy= "
+
reg_weight_xy
+
"\n"
);
sb
.
append
(
"disable_ers= "
+
disable_ers
+
"\n"
);
sb
.
append
(
"disable_ers_y= "
+
disable_ers_y
+
"\n"
);
sb
.
append
(
"disable_ers_r= "
+
disable_ers_r
+
"\n"
);
sb
.
append
(
"use_precomp= "
+
use_precomp
+
"\n"
);
sb
.
append
(
"------------------------\n\n"
);
quadCLTs
[
ref_index
].
saveStringInModelDirectory
(
sb
.
toString
(),
QuadCLT
.
ORIENTATION_LOGS_SUFFIX
);
// String suffix)
return
earliest_scene
;
return
earliest_scene
;
}
}
...
@@ -2644,11 +2813,13 @@ public class Interscene {
...
@@ -2644,11 +2813,13 @@ public class Interscene {
){
){
double
dx
=
new_xyz
[
0
]
-
cur_xyzatr
[
0
][
0
];
double
dx
=
new_xyz
[
0
]
-
cur_xyzatr
[
0
][
0
];
double
dy
=
new_xyz
[
1
]
-
cur_xyzatr
[
0
][
1
];
double
dy
=
new_xyz
[
1
]
-
cur_xyzatr
[
0
][
1
];
// System.out.println(String.format("modifyATRtoXYZ(): dx=%7.3f, dy=%7.3f", dx, dy));
double
[][]
diff_xyzatr
=
new
double
[][]
{{
dx
,
dy
,
0
},{
dx
/
avg_z
,
-
dy
/
avg_z
,
0
}};
double
[][]
diff_xyzatr
=
new
double
[][]
{{
dx
,
dy
,
0
},{
dx
/
avg_z
,
-
dy
/
avg_z
,
0
}};
double
[][]
new_xyzatr
=
ErsCorrection
.
combineXYZATR
(
cur_xyzatr
,
diff_xyzatr
);
double
[][]
new_xyzatr
=
ErsCorrection
.
combineXYZATR
(
cur_xyzatr
,
diff_xyzatr
);
return
new_xyzatr
;
return
new_xyzatr
;
}
}
// Make it the only entry point
// Make it the only entry point
...
@@ -5186,9 +5357,9 @@ public class Interscene {
...
@@ -5186,9 +5357,9 @@ public class Interscene {
double
[]
corr_angles
=
rot
.
getAngles
(
RotationOrder
.
YXZ
,
ErsCorrection
.
ROT_CONV
);
double
[]
corr_angles
=
rot
.
getAngles
(
RotationOrder
.
YXZ
,
ErsCorrection
.
ROT_CONV
);
double
[]
corr_degrees
=
new
double
[
3
];
double
[]
corr_degrees
=
new
double
[
3
];
for
(
int
i
=
0
;
i
<
3
;
i
++)
corr_degrees
[
i
]=
corr_angles
[
i
]*
180
/
Math
.
PI
;
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
(
"
generateEgomotionTable():
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
(
"
generateEgomotionTable():
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
]+
"]"
);
System
.
out
.
println
(
"
generateEgomotionTable():
ATR(deg)=["
+
corr_degrees
[
0
]+
", "
+
corr_degrees
[
1
]+
", "
+
corr_degrees
[
2
]+
"]"
);
}
}
for
(
int
nscene
=
earliest_scene
;
nscene
<
quadCLTs
.
length
;
nscene
++)
{
for
(
int
nscene
=
earliest_scene
;
nscene
<
quadCLTs
.
length
;
nscene
++)
{
...
@@ -5544,7 +5715,7 @@ public class Interscene {
...
@@ -5544,7 +5715,7 @@ public class Interscene {
}
}
}
}
// relative to the GPS/compass
public
static
double
[]
getQuaternionCorrection
(
public
static
double
[]
getQuaternionCorrection
(
CLTParameters
clt_parameters
,
CLTParameters
clt_parameters
,
QuadCLT
[]
quadCLTs
,
QuadCLT
[]
quadCLTs
,
...
...
src/main/java/com/elphel/imagej/tileprocessor/IntersceneMatchParameters.java
View file @
ab8d3318
...
@@ -118,6 +118,7 @@ public class IntersceneMatchParameters {
...
@@ -118,6 +118,7 @@ public class IntersceneMatchParameters {
public
boolean
apply_imu_orient
=
true
;
// apply IMU misalignment to the camera if adjusted
public
boolean
apply_imu_orient
=
true
;
// apply IMU misalignment to the camera if adjusted
public
boolean
orient_by_move
=
false
;
// use translation data to adjust IMU orientation
public
boolean
orient_by_move
=
false
;
// use translation data to adjust IMU orientation
public
boolean
orient_by_rot
=
true
;
// use rotation data to adjust IMU orientation
public
boolean
orient_by_rot
=
true
;
// use rotation data to adjust IMU orientation
public
boolean
orient_combo
=
true
;
// use combined rotation+orientation for IMU/camera matching
public
boolean
adjust_gyro
=
false
;
// adjust qyro omegas offsets
public
boolean
adjust_gyro
=
false
;
// adjust qyro omegas offsets
public
boolean
apply_gyro
=
true
;
// apply adjusted qyro omegas offsets
public
boolean
apply_gyro
=
true
;
// apply adjusted qyro omegas offsets
public
boolean
adjust_accl
=
false
;
// adjust IMU velocities scales
public
boolean
adjust_accl
=
false
;
// adjust IMU velocities scales
...
@@ -386,7 +387,9 @@ public class IntersceneMatchParameters {
...
@@ -386,7 +387,9 @@ public class IntersceneMatchParameters {
public
double
max_zoom_diff
=
0
;
// for down-views when changing altitude (0 - ignore)
public
double
max_zoom_diff
=
0
;
// for down-views when changing altitude (0 - ignore)
public
boolean
fpn_skip
=
true
;
// skip too close scenes (false - abort, previous behavior)
public
boolean
fpn_skip
=
true
;
// skip too close scenes (false - abort, previous behavior)
public
boolean
fpn_rematch
=
true
;
// match fpn-failed scenes to later scenes with larger difference
public
boolean
fpn_rematch
=
true
;
// match fpn-failed scenes to later scenes with larger difference
public
boolean
refine_invert
=
false
;
// Refine with LMA while inverting relative poses from other reference
// still not clear why it sometimes fails without refine_invert (too large initial mismatch)
public
boolean
refine_invert
=
true
;
// Refine with LMA while inverting relative poses from other reference
public
boolean
use_precomp
=
false
;
// try to predict initial error from previous scenes
// Remove moving objects (goal is not to detect slightest movement, but to improve pose matching
// Remove moving objects (goal is not to detect slightest movement, but to improve pose matching
public
boolean
mov_en
=
true
;
// enable detection/removal of the moving objects during pose matching
public
boolean
mov_en
=
true
;
// enable detection/removal of the moving objects during pose matching
...
@@ -673,12 +676,14 @@ public class IntersceneMatchParameters {
...
@@ -673,12 +676,14 @@ public class IntersceneMatchParameters {
"Minimal required number of re-calculations of the interscene-accumulated DSI."
);
"Minimal required number of re-calculations of the interscene-accumulated DSI."
);
gd
.
addCheckbox
(
"Adjust IMU orientation"
,
this
.
adjust_imu_orient
,
gd
.
addCheckbox
(
"Adjust IMU orientation"
,
this
.
adjust_imu_orient
,
"Adjust IMU misalignment to the camera."
);
"Adjust IMU misalignment to the camera."
);
gd
.
addCheckbox
(
"A
djust IMU orientation"
,
this
.
apply_imu_orient
,
gd
.
addCheckbox
(
"A
pply IMU orientation"
,
this
.
apply_imu_orient
,
"Apply IMU misalignment to the camera if adjusted."
);
"Apply IMU misalignment to the camera if adjusted."
);
gd
.
addCheckbox
(
"Use translation for IMU orientation"
,
this
.
orient_by_move
,
gd
.
addCheckbox
(
"Use translation for IMU orientation"
,
this
.
orient_by_move
,
"Use translation data to adjust IMU orientation ."
);
"Use translation data to adjust IMU orientation ."
);
gd
.
addCheckbox
(
"Use rotation for IMU orientation"
,
this
.
orient_by_rot
,
gd
.
addCheckbox
(
"Use rotation for IMU orientation"
,
this
.
orient_by_rot
,
"Use rotation data to adjust IMU orientation."
);
"Use rotation data to adjust IMU orientation."
);
gd
.
addCheckbox
(
"Use combo mode IMU orientation"
,
this
.
orient_combo
,
"Use combined Z/h, R, A-X/h, T+Y/h for IMU mount-to-camera orientation correction. False - use X,Y,Z,A,T,R"
);
gd
.
addCheckbox
(
"Adjust gyro offsets"
,
this
.
adjust_gyro
,
gd
.
addCheckbox
(
"Adjust gyro offsets"
,
this
.
adjust_gyro
,
"Adjust qyro omegas offsets."
);
"Adjust qyro omegas offsets."
);
gd
.
addCheckbox
(
"Apply gyro offsets"
,
this
.
apply_gyro
,
gd
.
addCheckbox
(
"Apply gyro offsets"
,
this
.
apply_gyro
,
...
@@ -1180,6 +1185,8 @@ public class IntersceneMatchParameters {
...
@@ -1180,6 +1185,8 @@ public class IntersceneMatchParameters {
"Match fpn-failed scenes to later scenes with larger difference."
);
"Match fpn-failed scenes to later scenes with larger difference."
);
gd
.
addCheckbox
(
"Refine inversion"
,
this
.
refine_invert
,
gd
.
addCheckbox
(
"Refine inversion"
,
this
.
refine_invert
,
"Refine with LMA while inverting relative poses from other reference."
);
"Refine with LMA while inverting relative poses from other reference."
);
gd
.
addCheckbox
(
"Precompensate orientation readjustment"
,
this
.
use_precomp
,
"Guess needed initial precompensation from the previously processed scenes."
);
gd
.
addMessage
(
"Detect and remove moving objects from pose matching"
);
gd
.
addMessage
(
"Detect and remove moving objects from pose matching"
);
gd
.
addCheckbox
(
"Enable movement detection/elimination"
,
this
.
mov_en
,
gd
.
addCheckbox
(
"Enable movement detection/elimination"
,
this
.
mov_en
,
...
@@ -1488,6 +1495,7 @@ public class IntersceneMatchParameters {
...
@@ -1488,6 +1495,7 @@ public class IntersceneMatchParameters {
this
.
apply_imu_orient
=
gd
.
getNextBoolean
();
this
.
apply_imu_orient
=
gd
.
getNextBoolean
();
this
.
orient_by_move
=
gd
.
getNextBoolean
();
this
.
orient_by_move
=
gd
.
getNextBoolean
();
this
.
orient_by_rot
=
gd
.
getNextBoolean
();
this
.
orient_by_rot
=
gd
.
getNextBoolean
();
this
.
orient_combo
=
gd
.
getNextBoolean
();
this
.
adjust_gyro
=
gd
.
getNextBoolean
();
this
.
adjust_gyro
=
gd
.
getNextBoolean
();
this
.
apply_gyro
=
gd
.
getNextBoolean
();
this
.
apply_gyro
=
gd
.
getNextBoolean
();
this
.
adjust_accl
=
gd
.
getNextBoolean
();
this
.
adjust_accl
=
gd
.
getNextBoolean
();
...
@@ -1716,6 +1724,7 @@ public class IntersceneMatchParameters {
...
@@ -1716,6 +1724,7 @@ public class IntersceneMatchParameters {
this
.
fpn_skip
=
gd
.
getNextBoolean
();
this
.
fpn_skip
=
gd
.
getNextBoolean
();
this
.
fpn_rematch
=
gd
.
getNextBoolean
();
this
.
fpn_rematch
=
gd
.
getNextBoolean
();
this
.
refine_invert
=
gd
.
getNextBoolean
();
this
.
refine_invert
=
gd
.
getNextBoolean
();
this
.
use_precomp
=
gd
.
getNextBoolean
();
this
.
mov_en
=
gd
.
getNextBoolean
();
this
.
mov_en
=
gd
.
getNextBoolean
();
this
.
mov_sigma
=
gd
.
getNextNumber
();
this
.
mov_sigma
=
gd
.
getNextNumber
();
...
@@ -1935,6 +1944,7 @@ public class IntersceneMatchParameters {
...
@@ -1935,6 +1944,7 @@ public class IntersceneMatchParameters {
properties
.
setProperty
(
prefix
+
"apply_imu_orient"
,
this
.
apply_imu_orient
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"apply_imu_orient"
,
this
.
apply_imu_orient
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"orient_by_move"
,
this
.
orient_by_move
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"orient_by_move"
,
this
.
orient_by_move
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"orient_by_rot"
,
this
.
orient_by_rot
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"orient_by_rot"
,
this
.
orient_by_rot
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"orient_combo"
,
this
.
orient_combo
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"adjust_gyro"
,
this
.
adjust_gyro
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"adjust_gyro"
,
this
.
adjust_gyro
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"apply_gyro"
,
this
.
apply_gyro
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"apply_gyro"
,
this
.
apply_gyro
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"adjust_accl"
,
this
.
adjust_accl
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"adjust_accl"
,
this
.
adjust_accl
+
""
);
// boolean
...
@@ -2172,6 +2182,7 @@ public class IntersceneMatchParameters {
...
@@ -2172,6 +2182,7 @@ public class IntersceneMatchParameters {
properties
.
setProperty
(
prefix
+
"fpn_skip"
,
this
.
fpn_skip
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"fpn_skip"
,
this
.
fpn_skip
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"fpn_rematch"
,
this
.
fpn_rematch
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"fpn_rematch"
,
this
.
fpn_rematch
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"refine_invert"
,
this
.
refine_invert
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"refine_invert"
,
this
.
refine_invert
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"use_precomp"
,
this
.
use_precomp
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"mov_en"
,
this
.
mov_en
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"mov_en"
,
this
.
mov_en
+
""
);
// boolean
properties
.
setProperty
(
prefix
+
"mov_sigma"
,
this
.
mov_sigma
+
""
);
// double
properties
.
setProperty
(
prefix
+
"mov_sigma"
,
this
.
mov_sigma
+
""
);
// double
...
@@ -2345,6 +2356,7 @@ public class IntersceneMatchParameters {
...
@@ -2345,6 +2356,7 @@ public class IntersceneMatchParameters {
if
(
properties
.
getProperty
(
prefix
+
"apply_imu_orient"
)!=
null
)
this
.
apply_imu_orient
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"apply_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
+
"orient_by_move"
)!=
null
)
this
.
orient_by_move
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"orient_by_move"
));
if
(
properties
.
getProperty
(
prefix
+
"orient_by_move"
)!=
null
)
this
.
orient_by_move
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"orient_by_move"
));
if
(
properties
.
getProperty
(
prefix
+
"orient_by_rot"
)!=
null
)
this
.
orient_by_rot
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"orient_by_rot"
));
if
(
properties
.
getProperty
(
prefix
+
"orient_by_rot"
)!=
null
)
this
.
orient_by_rot
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"orient_by_rot"
));
if
(
properties
.
getProperty
(
prefix
+
"orient_combo"
)!=
null
)
this
.
orient_combo
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"orient_combo"
));
if
(
properties
.
getProperty
(
prefix
+
"adjust_gyro"
)!=
null
)
this
.
adjust_gyro
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"adjust_gyro"
));
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
+
"apply_gyro"
)!=
null
)
this
.
apply_gyro
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"apply_gyro"
));
if
(
properties
.
getProperty
(
prefix
+
"adjust_accl"
)!=
null
)
this
.
adjust_accl
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"adjust_accl"
));
if
(
properties
.
getProperty
(
prefix
+
"adjust_accl"
)!=
null
)
this
.
adjust_accl
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"adjust_accl"
));
...
@@ -2588,6 +2600,7 @@ public class IntersceneMatchParameters {
...
@@ -2588,6 +2600,7 @@ public class IntersceneMatchParameters {
if
(
properties
.
getProperty
(
prefix
+
"fpn_skip"
)!=
null
)
this
.
fpn_skip
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"fpn_skip"
));
if
(
properties
.
getProperty
(
prefix
+
"fpn_skip"
)!=
null
)
this
.
fpn_skip
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"fpn_skip"
));
if
(
properties
.
getProperty
(
prefix
+
"fpn_rematch"
)!=
null
)
this
.
fpn_rematch
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"fpn_rematch"
));
if
(
properties
.
getProperty
(
prefix
+
"fpn_rematch"
)!=
null
)
this
.
fpn_rematch
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"fpn_rematch"
));
if
(
properties
.
getProperty
(
prefix
+
"refine_invert"
)!=
null
)
this
.
refine_invert
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"refine_invert"
));
if
(
properties
.
getProperty
(
prefix
+
"refine_invert"
)!=
null
)
this
.
refine_invert
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"refine_invert"
));
if
(
properties
.
getProperty
(
prefix
+
"use_precomp"
)!=
null
)
this
.
use_precomp
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"use_precomp"
));
if
(
properties
.
getProperty
(
prefix
+
"mov_en"
)!=
null
)
this
.
mov_en
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"mov_en"
));
if
(
properties
.
getProperty
(
prefix
+
"mov_en"
)!=
null
)
this
.
mov_en
=
Boolean
.
parseBoolean
(
properties
.
getProperty
(
prefix
+
"mov_en"
));
if
(
properties
.
getProperty
(
prefix
+
"mov_sigma"
)!=
null
)
this
.
mov_sigma
=
Double
.
parseDouble
(
properties
.
getProperty
(
prefix
+
"mov_sigma"
));
if
(
properties
.
getProperty
(
prefix
+
"mov_sigma"
)!=
null
)
this
.
mov_sigma
=
Double
.
parseDouble
(
properties
.
getProperty
(
prefix
+
"mov_sigma"
));
...
@@ -2782,6 +2795,7 @@ public class IntersceneMatchParameters {
...
@@ -2782,6 +2795,7 @@ public class IntersceneMatchParameters {
imp
.
apply_imu_orient
=
this
.
apply_imu_orient
;
imp
.
apply_imu_orient
=
this
.
apply_imu_orient
;
imp
.
orient_by_move
=
this
.
orient_by_move
;
imp
.
orient_by_move
=
this
.
orient_by_move
;
imp
.
orient_by_rot
=
this
.
orient_by_rot
;
imp
.
orient_by_rot
=
this
.
orient_by_rot
;
imp
.
orient_combo
=
this
.
orient_combo
;
imp
.
adjust_gyro
=
this
.
adjust_gyro
;
imp
.
adjust_gyro
=
this
.
adjust_gyro
;
imp
.
apply_gyro
=
this
.
apply_gyro
;
imp
.
apply_gyro
=
this
.
apply_gyro
;
imp
.
adjust_accl
=
this
.
adjust_accl
;
imp
.
adjust_accl
=
this
.
adjust_accl
;
...
@@ -3016,6 +3030,7 @@ public class IntersceneMatchParameters {
...
@@ -3016,6 +3030,7 @@ public class IntersceneMatchParameters {
imp
.
fpn_skip
=
this
.
fpn_skip
;
imp
.
fpn_skip
=
this
.
fpn_skip
;
imp
.
fpn_rematch
=
this
.
fpn_rematch
;
imp
.
fpn_rematch
=
this
.
fpn_rematch
;
imp
.
refine_invert
=
this
.
refine_invert
;
imp
.
refine_invert
=
this
.
refine_invert
;
imp
.
use_precomp
=
this
.
use_precomp
;
imp
.
mov_en
=
this
.
mov_en
;
imp
.
mov_en
=
this
.
mov_en
;
imp
.
mov_sigma
=
this
.
mov_sigma
;
imp
.
mov_sigma
=
this
.
mov_sigma
;
...
...
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @
ab8d3318
...
@@ -29,8 +29,10 @@ import java.awt.Rectangle;
...
@@ -29,8 +29,10 @@ import java.awt.Rectangle;
import
java.io.File
;
import
java.io.File
;
import
java.io.IOException
;
import
java.io.IOException
;
import
java.security.NoSuchAlgorithmException
;
import
java.security.NoSuchAlgorithmException
;
import
java.text.SimpleDateFormat
;
import
java.util.ArrayList
;
import
java.util.ArrayList
;
import
java.util.Arrays
;
import
java.util.Arrays
;
import
java.util.Calendar
;
import
java.util.Collections
;
import
java.util.Collections
;
import
java.util.Comparator
;
import
java.util.Comparator
;
import
java.util.List
;
import
java.util.List
;
...
@@ -5426,15 +5428,30 @@ public class OpticalFlow {
...
@@ -5426,15 +5428,30 @@ public class OpticalFlow {
}
}
}
}
}
}
// quadCLTs[ref_index].getSmoothGround(clt_parameters);
// later move to the right place
// later move to the right place
if
(
adjust_imu_orient
)
{
// (quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {
if
(
adjust_imu_orient
)
{
// (quadCLTs[ref_index].getNumOrient() >= clt_parameters.imp.mb_all_index)) {
QuadCLT
.
adjustImuOrient
(
boolean
orient_combo
=
clt_parameters
.
imp
.
orient_combo
;
// use combined rotation+orientation for IMU/camera matching
clt_parameters
,
//CLTParameters clt_parameters, // CLTParameters clt_parameters,
QuadCLT
.
adjustImuOrient
(
quadCLTs
,
// QuadCLT[] quadCLTs,
clt_parameters
,
//CLTParameters clt_parameters, // CLTParameters clt_parameters,
ref_index
,
// int ref_index,
orient_combo
,
// boolean orient_combo = clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
earliest_scene
,
// int earliest_scene,
quadCLTs
,
// QuadCLT[] quadCLTs,
last_index
,
// int last_index,
ref_index
,
// int ref_index,
debugLevel
);
// int debugLevel
earliest_scene
,
// int earliest_scene,
last_index
,
// int last_index,
debugLevel
);
// int debugLevel
// Try both orient_combo/!orient_combo for the log!
QuadCLT
.
adjustImuOrient
(
clt_parameters
,
//CLTParameters clt_parameters, // CLTParameters clt_parameters,
!
orient_combo
,
// boolean orient_combo = clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
quadCLTs
,
// QuadCLT[] quadCLTs,
ref_index
,
// int ref_index,
earliest_scene
,
// int earliest_scene,
last_index
,
// int last_index,
debugLevel
);
// int debugLevel
}
}
if
(
run_ly
)
{
if
(
run_ly
)
{
if
(
debugLevel
>
-
3
)
{
if
(
debugLevel
>
-
3
)
{
...
@@ -5548,15 +5565,20 @@ public class OpticalFlow {
...
@@ -5548,15 +5565,20 @@ 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
);
StringBuffer
sb
=
new
StringBuffer
();
sb
.
append
(
new
SimpleDateFormat
(
"yyyy/MM/dd HH:mm:ss"
).
format
(
Calendar
.
getInstance
().
getTime
())+
"\n"
);
Rotation
rot
=
new
Rotation
(
quatCorr
[
0
],
quatCorr
[
1
],
quatCorr
[
2
],
quatCorr
[
3
],
false
);
// no normalization - see if can be scaled
sb
.
append
(
"Applying correction to the IMS to world orientation (rotating around IMS vertical):\n"
);
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
;
sb
.
append
(
"compass: quatCorr=["
+
quatCorr
[
0
]+
", "
+
quatCorr
[
1
]+
", "
+
quatCorr
[
2
]+
", "
+
quatCorr
[
3
]+
"]\n"
);
sb
.
append
(
"compass: ATR(rad)=["
+
corr_angles
[
0
]+
", "
+
corr_angles
[
1
]+
", "
+
corr_angles
[
2
]+
"]\n"
);
sb
.
append
(
"compass: ATR(deg)=["
+
corr_degrees
[
0
]+
", "
+
corr_degrees
[
1
]+
", "
+
corr_degrees
[
2
]+
"]\n"
);
sb
.
append
(
"------------------------\n\n"
);
quadCLTs
[
ref_index
].
saveStringInModelDirectory
(
sb
.
toString
(),
QuadCLT
.
IMU_CALIB_LOGS_SUFFIX
);
// String suffix)
if
(
debugLevel
>
-
3
)
{
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
.
print
(
sb
.
toString
());
System
.
out
.
println
(
"Applying correction to 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
)
{
...
...
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @
ab8d3318
...
@@ -27,28 +27,25 @@ package com.elphel.imagej.tileprocessor;
...
@@ -27,28 +27,25 @@ package com.elphel.imagej.tileprocessor;
//import java.awt.Polygon;
//import java.awt.Polygon;
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.RotationOrder
;
import
org.apache.commons.math3.geometry.euclidean.threed.RotationOrder
;
import
java.awt.Rectangle
;
import
java.awt.Rectangle
;
import
java.io.BufferedWriter
;
import
java.io.File
;
import
java.io.File
;
import
java.io.FileInputStream
;
import
java.io.FileInputStream
;
import
java.io.FileNotFoundException
;
import
java.io.FileNotFoundException
;
import
java.io.FileOutputStream
;
import
java.io.FileOutputStream
;
import
java.io.FileWriter
;
import
java.io.IOException
;
import
java.io.IOException
;
import
java.io.InputStream
;
import
java.io.InputStream
;
import
java.io.OutputStream
;
import
java.io.OutputStream
;
import
java.nio.charset.Charset
;
import
java.text.SimpleDateFormat
;
import
java.nio.file.Files
;
import
java.nio.file.Path
;
import
java.nio.file.Paths
;
import
java.nio.file.attribute.PosixFilePermission
;
import
java.time.LocalDateTime
;
import
java.time.LocalDateTime
;
import
java.util.ArrayList
;
import
java.util.ArrayList
;
import
java.util.Arrays
;
import
java.util.Arrays
;
import
java.util.Calendar
;
import
java.util.Collections
;
import
java.util.Collections
;
import
java.util.Comparator
;
import
java.util.Comparator
;
import
java.util.Enumeration
;
import
java.util.Enumeration
;
import
java.util.List
;
import
java.util.Properties
;
import
java.util.Properties
;
import
java.util.Random
;
import
java.util.Random
;
import
java.util.Set
;
import
java.util.Set
;
...
@@ -99,6 +96,9 @@ import loci.formats.FormatException;
...
@@ -99,6 +96,9 @@ import loci.formats.FormatException;
public
class
QuadCLTCPU
{
public
class
QuadCLTCPU
{
public
static
final
String
IMU_CALIB_LOGS_SUFFIX
=
"-IMU_CALIB.log"
;
public
static
final
String
ORIENTATION_LOGS_SUFFIX
=
"-ORIENTATION.log"
;
public
static
final
String
[]
DSI_SUFFIXES
=
{
"-INTER-INTRA-LMA"
,
"-INTER-INTRA"
,
"-DSI_MAIN"
};
public
static
final
String
[]
DSI_SUFFIXES
=
{
"-INTER-INTRA-LMA"
,
"-INTER-INTRA"
,
"-DSI_MAIN"
};
public
static
int
INDEX_INTER_LMA
=
0
;
public
static
int
INDEX_INTER_LMA
=
0
;
public
static
int
INDEX_INTER
=
1
;
public
static
int
INDEX_INTER
=
1
;
...
@@ -193,6 +193,7 @@ public class QuadCLTCPU {
...
@@ -193,6 +193,7 @@ public class QuadCLTCPU {
public
Did_gps_pos
did_gps1_ubx_pos
=
null
;
public
Did_gps_pos
did_gps1_ubx_pos
=
null
;
public
String
ims_last_path
=
null
;
public
String
ims_last_path
=
null
;
public
double
[]
quat_corr
=
null
;
// correction for IMS camera frame to actual camera frame (for reference frames)
public
double
[]
quat_corr
=
null
;
// correction for IMS camera frame to actual camera frame (for reference frames)
@Deprecated
public
double
[][]
pimu_offsets
=
new
double
[
2
][
3
];
// linear and angular velocities offsets to DID_PIMU outputs (subtract from IMU data)
public
double
[][]
pimu_offsets
=
new
double
[
2
][
3
];
// linear and angular velocities offsets to DID_PIMU outputs (subtract from IMU data)
// public boolean quat_corr_active = false; // correction for IMS camera frame to actual camera frame (for reference frames)
// public boolean quat_corr_active = false; // correction for IMS camera frame to actual camera frame (for reference frames)
...
@@ -212,11 +213,11 @@ public class QuadCLTCPU {
...
@@ -212,11 +213,11 @@ public class QuadCLTCPU {
public
void
setQuatCorr
(
double
[]
quat
)
{
public
void
setQuatCorr
(
double
[]
quat
)
{
quat_corr
=
quat
;
quat_corr
=
quat
;
}
}
@Deprecated
public
double
[][]
getPimuOffsets
()
{
public
double
[][]
getPimuOffsets
()
{
return
pimu_offsets
;
return
pimu_offsets
;
}
}
@Deprecated
public
void
setPimuOffsets
(
double
[][]
offsets
)
{
// never
public
void
setPimuOffsets
(
double
[][]
offsets
)
{
// never
pimu_offsets
=
offsets
;
pimu_offsets
=
offsets
;
}
}
...
@@ -475,11 +476,12 @@ public class QuadCLTCPU {
...
@@ -475,11 +476,12 @@ public class QuadCLTCPU {
public
static
void
adjustImuOrient
(
public
static
void
adjustImuOrient
(
CLTParameters
clt_parameters
,
// CLTParameters clt_parameters,
CLTParameters
clt_parameters
,
// CLTParameters clt_parameters,
boolean
orient_combo
,
QuadCLT
[]
quadCLTs
,
QuadCLT
[]
quadCLTs
,
int
ref_index
,
int
ref_index
,
int
earliest_scene
,
int
earliest_scene
,
int
last_index
,
int
last_index
,
int
debugLevel
int
debugLevel
)
{
)
{
boolean
orient_by_move
=
clt_parameters
.
imp
.
orient_by_move
;
// use translation data to adjust IMU orientation
boolean
orient_by_move
=
clt_parameters
.
imp
.
orient_by_move
;
// use translation data to adjust IMU orientation
boolean
orient_by_rot
=
clt_parameters
.
imp
.
orient_by_rot
;
// use rotation data to adjust IMU orientation
boolean
orient_by_rot
=
clt_parameters
.
imp
.
orient_by_rot
;
// use rotation data to adjust IMU orientation
...
@@ -487,13 +489,14 @@ public class QuadCLTCPU {
...
@@ -487,13 +489,14 @@ public class QuadCLTCPU {
System
.
out
.
println
(
"Neither translation nor rotation data are enabled to adjust IMU orientation relative to the camera"
);
System
.
out
.
println
(
"Neither translation nor rotation data are enabled to adjust IMU orientation relative to the camera"
);
return
;
return
;
}
}
// boolean orient_combo = clt_parameters.imp.orient_combo; // use combined rotation+orientation for IMU/camera matching
boolean
apply_imu_orient
=
clt_parameters
.
imp
.
apply_imu_orient
;
// apply IMU misalignment to the camera if adjusted
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
adjust_gyro
=
clt_parameters
.
imp
.
adjust_gyro
;
// adjust qyro omegas offsets
boolean
apply_gyro
=
clt_parameters
.
imp
.
apply_gyro
;
// apply adjusted qyro omegas offsets
boolean
apply_gyro
=
clt_parameters
.
imp
.
apply_gyro
;
// apply adjusted qyro omegas offsets
boolean
adjust_accl
=
clt_parameters
.
imp
.
adjust_accl
;
// adjust IMU velocities scales
boolean
adjust_accl
=
clt_parameters
.
imp
.
adjust_accl
;
// adjust IMU velocities scales
boolean
apply_accl
=
clt_parameters
.
imp
.
apply_accl
;
// apply IMU velocities scales
boolean
apply_accl
=
clt_parameters
.
imp
.
apply_accl
;
// apply IMU velocities scales
double
quat_max_change
=
clt_parameters
.
imp
.
quat_max_change
;
// do not apply if any component of the result exceeds
double
quat_max_change
=
clt_parameters
.
imp
.
quat_max_change
;
// do not apply if any component of the result exceeds
double
quat_min_lin
=
clt_parameters
.
imp
.
quat_min_lin
;
// meters, minimal distance per axis to adjust IMS velocity scale
double
quat_min_lin
=
clt_parameters
.
imp
.
quat_min_lin
;
// meters, minimal distance per axis to adjust IMS velocity scale
double
[][][]
pimu_xyzatr
=
QuadCLT
.
integratePIMU
(
double
[][][]
pimu_xyzatr
=
QuadCLT
.
integratePIMU
(
clt_parameters
,
// final CLTParameters clt_parameters,
clt_parameters
,
// final CLTParameters clt_parameters,
quadCLTs
,
// final QuadCLT[] quadCLTs,
quadCLTs
,
// final QuadCLT[] quadCLTs,
...
@@ -510,8 +513,7 @@ public class QuadCLTCPU {
...
@@ -510,8 +513,7 @@ public class QuadCLTCPU {
}
}
double
[]
rms
=
new
double
[
5
];
double
[]
rms
=
new
double
[
5
];
double
[]
quat
=
new
double
[
4
];
double
[]
quat
=
new
double
[
4
];
// int quat_lma_mode = QuaternionLma.MODE_XYZQ; // MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
int
quat_lma_mode
=
orient_combo
?
QuaternionLma
.
MODE_COMBO_LOCAL
:
QuaternionLma
.
MODE_XYZQ
;
// MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 4; // 3; // 2; // 1;
int
quat_lma_mode
=
QuaternionLma
.
MODE_COMBO_LOCAL
;
// MODE_XYZ4Q3; // MODE_XYZQ; // MODE_XYZQ_LOCAL; // 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
);
...
@@ -536,47 +538,59 @@ public class QuadCLTCPU {
...
@@ -536,47 +538,59 @@ public class QuadCLTCPU {
debug_lev
);
// int debugLevel
debug_lev
);
// int debugLevel
if
(
rotated_xyzatr
!=
null
)
{
if
(
rotated_xyzatr
!=
null
)
{
Rotation
rot
=
new
Rotation
(
quat
[
0
],
quat
[
1
],
quat
[
2
],
quat
[
3
],
false
);
// no normalization - see if can be scaled
Rotation
rot
=
new
Rotation
(
quat
[
0
],
quat
[
1
],
quat
[
2
],
quat
[
3
],
false
);
// no normalization - see if can be scaled
if
(
debugLevel
>
-
3
)
{
System
.
out
.
println
(
"Applying correction to the IMS mount orientation:"
);
double
[]
corr_angles
=
rot
.
getAngles
(
RotationOrder
.
YXZ
,
ErsCorrection
.
ROT_CONV
);
double
[]
corr_degrees
=
new
double
[
3
];
double
quat_scale
=
Math
.
sqrt
(
quat
[
0
]*
quat
[
0
]+
quat
[
1
]*
quat
[
1
]+
quat
[
2
]*
quat
[
2
]+
quat
[
3
]*
quat
[
3
]);
for
(
int
i
=
0
;
i
<
3
;
i
++)
corr_degrees
[
i
]=
corr_angles
[
i
]*
180
/
Math
.
PI
;
System
.
out
.
println
(
"quat=["
+
quat
[
0
]+
", "
+
quat
[
1
]+
", "
+
quat
[
2
]+
", "
+
quat
[
3
]+
"]"
);
System
.
out
.
println
(
"scale="
+
quat_scale
);
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
]+
"]"
);
}
double
[]
ims_mount_atr
=
clt_parameters
.
imp
.
getImsMountATR
();
// converts to radians
double
[]
ims_mount_atr
=
clt_parameters
.
imp
.
getImsMountATR
();
// converts to radians
double
[]
new_ims_mount_atr
=
Imx5
.
adjustMountAtrByQuat
(
double
[]
new_ims_mount_atr
=
Imx5
.
adjustMountAtrByQuat
(
ims_mount_atr
,
// double [] ims_atr,
ims_mount_atr
,
// double [] ims_atr,
quat
);
// double [] corr_q)
quat
);
// double [] corr_q)
/*
saveStringInModelDirectory(
sb.tiString, // String string,
IMU_CALIB_LOGS_SUFFIX); // String suffix)
*/
StringBuffer
sb
=
new
StringBuffer
();
sb
.
append
(
new
SimpleDateFormat
(
"yyyy/MM/dd HH:mm:ss"
).
format
(
Calendar
.
getInstance
().
getTime
())+
"\n"
);
sb
.
append
(
"Applying correction to the IMS mount orientation:\n"
);
sb
.
append
(
"getNumOrient()="
+
quadCLTs
[
ref_index
].
getNumOrient
()+
" of "
+
clt_parameters
.
imp
.
min_num_orient
+
"\n"
);
sb
.
append
(
"getNumAccum()= "
+
quadCLTs
[
ref_index
].
getNumAccum
()+
" of "
+
clt_parameters
.
imp
.
min_num_interscene
+
"\n"
);
sb
.
append
(
"avg_z= "
+
avg_z
+
" m\n"
);
sb
.
append
(
"translation_weight= "
+
translation_weight
+
"\n"
);
sb
.
append
(
"quat_lma_mode= "
+
quat_lma_mode
+
"\n"
);
double
[]
corr_angles
=
rot
.
getAngles
(
RotationOrder
.
YXZ
,
ErsCorrection
.
ROT_CONV
);
double
[]
corr_degrees
=
new
double
[
3
];
double
quat_scale
=
Math
.
sqrt
(
quat
[
0
]*
quat
[
0
]+
quat
[
1
]*
quat
[
1
]+
quat
[
2
]*
quat
[
2
]+
quat
[
3
]*
quat
[
3
]);
for
(
int
i
=
0
;
i
<
3
;
i
++)
corr_degrees
[
i
]=
corr_angles
[
i
]*
180
/
Math
.
PI
;
double
[]
new_degrees
=
new
double
[
3
];
for
(
int
i
=
0
;
i
<
3
;
i
++)
new_degrees
[
i
]=
new_ims_mount_atr
[
i
]*
180
/
Math
.
PI
;
sb
.
append
(
"quat=["
+
quat
[
0
]+
", "
+
quat
[
1
]+
", "
+
quat
[
2
]+
", "
+
quat
[
3
]+
"]"
);
sb
.
append
(
"scale="
+
quat_scale
+
"\n"
);
sb
.
append
(
"delta ATR(rad)=["
+
corr_angles
[
0
]+
", "
+
corr_angles
[
1
]+
", "
+
corr_angles
[
2
]+
"]\n"
);
sb
.
append
(
"delta ATR(deg)=["
+
corr_degrees
[
0
]+
", "
+
corr_degrees
[
1
]+
", "
+
corr_degrees
[
2
]+
"]\n"
);
sb
.
append
(
" new ATR(rad)=["
+
new_ims_mount_atr
[
0
]+
", "
+
new_ims_mount_atr
[
1
]+
", "
+
new_ims_mount_atr
[
2
]+
"]\n"
);
sb
.
append
(
" new ATR(deg)=["
+
new_degrees
[
0
]+
", "
+
new_degrees
[
1
]+
", "
+
new_degrees
[
2
]+
"]\n"
);
if
(
apply_imu_orient
)
{
if
(
apply_imu_orient
)
{
for
(
int
i
=
0
;
i
<
new_ims_mount_atr
.
length
;
i
++)
{
for
(
int
i
=
0
;
i
<
new_ims_mount_atr
.
length
;
i
++)
{
if
(
Math
.
abs
(
new_ims_mount_atr
[
i
])
>
quat_max_change
)
{
if
(
Math
.
abs
(
new_ims_mount_atr
[
i
])
>
quat_max_change
)
{
apply_imu_orient
=
false
;
apply_imu_orient
=
false
;
if
(
debugLevel
>
-
3
)
{
sb
.
append
(
"*** IMU mount angle ["
+
i
+
"]=="
+
new_ims_mount_atr
[
i
]+
System
.
out
.
println
(
"*** IMU mount angle ["
+
i
+
"]=="
+
new_ims_mount_atr
[
i
]+
" exceeds the specified limit ("
+
quat_max_change
+
")"
);
" exceeds the specified limit ("
+
quat_max_change
+
")\n"
);
System
.
out
.
println
(
"*** Orientation update is disabled."
);
sb
.
append
(
"*** Orientation update is disabled.\n"
);
}
}
}
}
}
}
}
if
(
apply_imu_orient
)
{
if
(
apply_imu_orient
)
{
clt_parameters
.
imp
.
setImsMountATR
(
new_ims_mount_atr
);
clt_parameters
.
imp
.
setImsMountATR
(
new_ims_mount_atr
);
if
(
debugLevel
>
-
3
)
{
sb
.
append
(
"*** IMU mount angles updated, need to save the main configuration file for persistent storage ***\n"
);
System
.
out
.
println
(
"*** IMU mount angles updated, need to save the main configuration file for persistent storage ***"
);
}
}
else
{
}
else
{
System
.
out
.
println
(
"*** IMU mount angles calculated, but not applied ***
"
);
sb
.
append
(
"*** IMU mount angles calculated, but not applied ***\n
"
);
}
}
double
[]
new_atr
=
clt_parameters
.
imp
.
getImsMountATR
();
// converts to radians
double
[]
new_atr
=
clt_parameters
.
imp
.
getImsMountATR
();
// converts to radians
double
[]
degrees
=
new
double
[
3
];
double
[]
degrees
=
new
double
[
3
];
for
(
int
i
=
0
;
i
<
3
;
i
++)
degrees
[
i
]=
new_atr
[
i
]*
180
/
Math
.
PI
;
for
(
int
i
=
0
;
i
<
3
;
i
++)
degrees
[
i
]=
new_atr
[
i
]*
180
/
Math
.
PI
;
System
.
out
.
println
(
String
.
format
(
sb
.
append
(
String
.
format
(
"Using ATR(rad)=[%9f, %9f, %9f]"
,
new_atr
[
0
],
new_atr
[
1
],
new_atr
[
2
]));
"Using ATR(rad)=[%9f, %9f, %9f]\n"
,
new_atr
[
0
],
new_atr
[
1
],
new_atr
[
2
]));
System
.
out
.
println
(
String
.
format
(
sb
.
append
(
String
.
format
(
"Using ATR(deg)=[%9f, %9f, %9f]"
,
degrees
[
0
],
degrees
[
1
],
degrees
[
2
]));
"Using ATR(deg)=[%9f, %9f, %9f]\n"
,
degrees
[
0
],
degrees
[
1
],
degrees
[
2
]));
double
[]
omega_corr
=
null
;
double
[]
omega_corr
=
null
;
if
(
adjust_gyro
)
{
if
(
adjust_gyro
)
{
omega_corr
=
getOmegaCorrections
(
omega_corr
=
getOmegaCorrections
(
...
@@ -593,30 +607,28 @@ public class QuadCLTCPU {
...
@@ -593,30 +607,28 @@ public class QuadCLTCPU {
for
(
int
i
=
0
;
i
<
3
;
i
++)
{
for
(
int
i
=
0
;
i
<
3
;
i
++)
{
new_omegas
[
i
]
=
used_omegas
[
i
]
-
omega_corr
[
i
];
new_omegas
[
i
]
=
used_omegas
[
i
]
-
omega_corr
[
i
];
}
}
if
(
debugLevel
>
-
1
)
{
sb
.
append
(
String
.
format
(
System
.
out
.
println
(
String
.
format
(
"Used omegas (ATR, rad/s) = [%9f, %9f, %9f]\n"
,
"Used omegas (ATR, rad/s) = [%9f, %9f, %9f]"
,
used_omegas
[
0
],
used_omegas
[
1
],
used_omegas
[
2
]));
used_omegas
[
0
],
used_omegas
[
1
],
used_omegas
[
2
]));
System
.
out
.
println
(
String
.
format
(
sb
.
append
(
String
.
format
(
"Diff.omegas (ATR, rad/s) = [%9f, %9f, %9f]"
,
omega_corr
[
0
],
omega_corr
[
1
],
omega_corr
[
2
]));
"Diff.omegas (ATR, rad/s) = [%9f, %9f, %9f]\n"
,
System
.
out
.
println
(
String
.
format
(
omega_corr
[
0
],
omega_corr
[
1
],
omega_corr
[
2
]));
"New omegas (ATR, rad/s) = [%9f, %9f, %9f]"
,
new_omegas
[
0
],
new_omegas
[
1
],
new_omegas
[
2
]));
sb
.
append
(
String
.
format
(
}
"New omegas (ATR, rad/s) = [%9f, %9f, %9f]\n"
,
new_omegas
[
0
],
new_omegas
[
1
],
new_omegas
[
2
]));
if
(
apply_gyro
)
{
if
(
apply_gyro
)
{
clt_parameters
.
imp
.
set_pimu_omegas
(
new_omegas
);
clt_parameters
.
imp
.
set_pimu_omegas
(
new_omegas
);
if
(
debugLevel
>
-
3
)
{
sb
.
append
(
String
.
format
(
System
.
out
.
println
(
String
.
format
(
"Applied new gyro omegas (ATR, rad/s) = [%9f, %9f, %9f]\n"
,
"Applied new gyro omegas (ATR, rad/s) = [%9f, %9f, %9f]"
,
new_omegas
[
0
],
new_omegas
[
1
],
new_omegas
[
2
]));
new_omegas
[
0
],
new_omegas
[
1
],
new_omegas
[
2
]));
System
.
out
.
println
(
"*** Need to save the main configuration file ***"
);
sb
.
append
(
"*** Need to save the main configuration file ***\n"
);
}
}
else
{
}
else
{
if
(
debugLevel
>
-
3
)
{
sb
.
append
(
String
.
format
(
System
.
out
.
println
(
String
.
format
(
"New gyro omegas (ATR, rad/s) are not applied = [%9f, %9f, %9f]\n"
,
"New gyro omegas (ATR, rad/s) are not applied = [%9f, %9f, %9f]"
,
new_omegas
[
0
],
new_omegas
[
1
],
new_omegas
[
2
]));
new_omegas
[
0
],
new_omegas
[
1
],
new_omegas
[
2
]));
}
}
}
}
else
{
}
else
{
System
.
out
.
println
(
"*** Adjustment of the gyro omegas failed ***"
);
sb
.
append
(
"*** Adjustment of the gyro omegas failed ***\n"
);
}
}
}
}
double
[]
acc_corr
=
null
;
double
[]
acc_corr
=
null
;
...
@@ -639,35 +651,41 @@ public class QuadCLTCPU {
...
@@ -639,35 +651,41 @@ public class QuadCLTCPU {
new_accl_corr
[
i
]
=
used_accl_corr
[
i
]
*
acc_corr
[
i
];
new_accl_corr
[
i
]
=
used_accl_corr
[
i
]
*
acc_corr
[
i
];
num_corr
++;
num_corr
++;
}
}
if
(
debugLevel
>
-
1
)
{
sb
.
append
(
String
.
format
(
System
.
out
.
println
(
String
.
format
(
"Used velocities scales = [%9f, %9f, %9f]\n"
,
"Used velocities scales = [%9f, %9f, %9f]"
,
used_accl_corr
[
0
],
used_accl_corr
[
1
],
used_accl_corr
[
2
]));
used_accl_corr
[
0
],
used_accl_corr
[
1
],
used_accl_corr
[
2
]));
System
.
out
.
println
(
String
.
format
(
sb
.
append
(
String
.
format
(
"Diff.velocities scales = [%9f, %9f, %9f]"
,
acc_corr
[
0
],
acc_corr
[
1
],
acc_corr
[
2
]));
"Diff.velocities scales = [%9f, %9f, %9f]\n"
,
System
.
out
.
println
(
String
.
format
(
acc_corr
[
0
],
acc_corr
[
1
],
acc_corr
[
2
]));
"New velocities scales = [%9f, %9f, %9f]"
,
new_accl_corr
[
0
],
new_accl_corr
[
1
],
new_accl_corr
[
2
]));
sb
.
append
(
String
.
format
(
}
"New velocities scales = [%9f, %9f, %9f]\n"
,
new_accl_corr
[
0
],
new_accl_corr
[
1
],
new_accl_corr
[
2
]));
if
(
apply_accl
&&
(
num_corr
>
0
))
{
if
(
apply_accl
&&
(
num_corr
>
0
))
{
clt_parameters
.
imp
.
set_pimu_velocities_scales
(
new_accl_corr
);
clt_parameters
.
imp
.
set_pimu_velocities_scales
(
new_accl_corr
);
if
(
debugLevel
>
-
3
)
{
sb
.
append
(
String
.
format
(
System
.
out
.
println
(
String
.
format
(
"Applied new velocities scales = [%9f, %9f, %9f]\n"
,
"Applied new velocities scales = [%9f, %9f, %9f]"
,
new_accl_corr
[
0
],
new_accl_corr
[
1
],
new_accl_corr
[
2
]));
new_accl_corr
[
0
],
new_accl_corr
[
1
],
new_accl_corr
[
2
]));
System
.
out
.
println
(
"*** Need to save the main configuration file ***"
);
sb
.
append
(
"*** Need to save the main configuration file ***\n"
);
}
}
else
{
}
else
{
if
(
debugLevel
>
-
3
)
{
sb
.
append
(
String
.
format
(
System
.
out
.
println
(
String
.
format
(
"New velocities scales are not applied = [%9f, %9f, %9f]\n"
,
"New velocities scales are not applied = [%9f, %9f, %9f]"
,
new_accl_corr
[
0
],
new_accl_corr
[
1
],
new_accl_corr
[
2
]));
new_accl_corr
[
0
],
new_accl_corr
[
1
],
new_accl_corr
[
2
]));
}
}
}
}
else
{
}
else
{
System
.
out
.
println
(
"*** Adjustment of the gyro omegas failed ***"
);
sb
.
append
(
"*** Adjustment of the gyro omegas failed ***\n"
);
}
}
//
}
sb
.
append
(
"------------------------\n\n"
);
quadCLTs
[
ref_index
].
saveStringInModelDirectory
(
sb
.
toString
(),
IMU_CALIB_LOGS_SUFFIX
);
// String suffix)
if
(
debugLevel
>
-
3
)
{
System
.
out
.
print
(
sb
.
toString
());
}
}
}
else
{
}
else
{
System
.
out
.
println
(
"*** Failed to calculate IMS mount correction! ***"
);
System
.
out
.
println
(
"*** Failed to calculate IMS mount correction! ***"
);
}
}
}
}
public
static
double
[]
getVelocitiesCorrections
(
public
static
double
[]
getVelocitiesCorrections
(
...
@@ -806,6 +824,93 @@ public class QuadCLTCPU {
...
@@ -806,6 +824,93 @@ public class QuadCLTCPU {
return
new
double
[]
{
coeffs
[
0
][
1
],
coeffs
[
1
][
1
],
coeffs
[
2
][
1
]};
return
new
double
[]
{
coeffs
[
0
][
1
],
coeffs
[
1
][
1
],
coeffs
[
2
][
1
]};
}
}
public
static
double
[][][]
refineXYZFromIMU
(
CLTParameters
clt_parameters
,
boolean
common_scale_only
,
boolean
keepZ
,
QuadCLT
[]
quadCLTs
,
double
[][][]
xyzatr
,
double
[][][]
pimu_xyzatr
,
// if null - will be recalculated
int
ref_index
,
int
early_index
,
int
last_index
,
int
debugLevel
){
if
(
pimu_xyzatr
==
null
)
{
pimu_xyzatr
=
QuadCLT
.
integratePIMU
(
// recalculate to updated if calibration was changed
clt_parameters
,
// final CLTParameters clt_parameters,
quadCLTs
,
// final QuadCLT[] quadCLTs,
ref_index
,
// final int ref_index,
null
,
// double [][][] dxyzatr,
early_index
,
// final int early_index,
last_index
);
//(quadCLTs.length -1) // int last_index,
}
double
[][][]
xyzatr_pull
=
new
double
[
xyzatr
.
length
][][];
double
[]
sxyz1
=
new
double
[
3
],
sxyz2
=
new
double
[
3
];
for
(
int
nscene
=
early_index
;
nscene
<=
last_index
;
nscene
++)
{
for
(
int
i
=
0
;
i
<
3
;
i
++)
{
double
x
=
xyzatr
[
nscene
][
0
][
i
];
double
px
=
pimu_xyzatr
[
nscene
][
0
][
i
];
// NULL pointer
sxyz1
[
i
]
+=
px
*
px
;
sxyz2
[
i
]
+=
px
*
x
;
}
}
double
[]
k
=
new
double
[
3
];
if
(
common_scale_only
)
{
double
s1
=
0
,
s2
=
0
;
for
(
int
i
=
0
;
i
<
3
;
i
++)
{
s1
+=
sxyz1
[
i
];
s2
+=
sxyz2
[
i
];
}
for
(
int
i
=
0
;
i
<
3
;
i
++)
{
k
[
i
]
=
s2
/
s1
;
}
}
else
{
for
(
int
i
=
0
;
i
<
3
;
i
++)
{
k
[
i
]
=
sxyz2
[
i
]/
sxyz1
[
i
];
}
}
for
(
int
nscene
=
early_index
;
nscene
<=
last_index
;
nscene
++)
{
xyzatr_pull
[
nscene
]
=
new
double
[][]
{
pimu_xyzatr
[
nscene
][
0
].
clone
(),
xyzatr
[
nscene
][
1
].
clone
()};
for
(
int
i
=
0
;
i
<
2
;
i
++)
{
xyzatr_pull
[
nscene
][
0
][
i
]
*=
k
[
i
];
}
if
(
keepZ
)
{
xyzatr_pull
[
nscene
][
0
][
2
]
=
xyzatr
[
nscene
][
0
][
2
];
}
else
{
xyzatr_pull
[
nscene
][
0
][
2
]
*=
k
[
2
];
}
}
if
(
debugLevel
>
-
1
)
{
if
(
common_scale_only
)
{
System
.
out
.
println
(
"refineXYZFromIMU(): Scaled IMU X,Y,Z by "
+
k
[
0
]+
"(common) to use as camera target values."
);
}
else
{
System
.
out
.
println
(
"refineXYZFromIMU(): Scaled IMU X,Y,Z by ["
+
k
[
0
]+
", "
+
k
[
1
]+
", "
+
k
[
2
]+
"] to use as camera target values."
);
}
}
if
(
debugLevel
>
0
)
{
System
.
out
.
println
(
String
.
format
(
"%3s"
+
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"
+
//x,y,z, ix, iy, iz
"\t%9s\t%9s\t%9s"
,
// px, py, pz
"N"
,
"X"
,
"Y"
,
"Z"
,
"iX"
,
"iY"
,
"iZ2"
,
"pX"
,
"pY"
,
"pZ"
));
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\t%9.5f\t%9.5f"
,
nscene
,
xyzatr
[
nscene
][
0
][
0
],
xyzatr
[
nscene
][
0
][
1
],
xyzatr
[
nscene
][
0
][
2
],
pimu_xyzatr
[
nscene
][
0
][
0
],
pimu_xyzatr
[
nscene
][
0
][
1
],
pimu_xyzatr
[
nscene
][
0
][
2
],
xyzatr_pull
[
nscene
][
0
][
0
],
xyzatr_pull
[
nscene
][
0
][
1
],
xyzatr_pull
[
nscene
][
0
][
2
]));
}
}
return
xyzatr_pull
;
}
/**
/**
* Refine scene poses (now only linear) from currently adjusted poses
* Refine scene poses (now only linear) from currently adjusted poses
...
@@ -1576,10 +1681,285 @@ public class QuadCLTCPU {
...
@@ -1576,10 +1681,285 @@ public class QuadCLTCPU {
whs
,
// initialize to int[3] to return {width, height, scale reduction}
whs
,
// initialize to int[3] to return {width, height, scale reduction}
debug_level
);
debug_level
);
}
}
}
public
double
[]
getSmoothGround
(
final
CLTParameters
clt_parameters
)
{
}
final
boolean
use_lma
=
true
;
// false, or there will be too many gaps?
public
double
[][]
getGroundNoIms
(
final
double
discard_low
=
0.1
;
// fraction of all pixels
final
double
discard_high
=
0.5
;
// fraction of all pixels
// final double discard_adisp, // discard above/below this fraction of average height
// final double discard_rdisp, // discard above/below this fraction of average height
final
int
stile_hstep
=
4
;
final
double
stile_radius
=
8.0
;
//
final
double
sigma
=
2.0
;
final
int
radius_levels
=
4
;
final
int
min_non_empty
=
10
;
// minimal number of non-empty tiles for fitting planes
final
int
debugLevel
=
3
;
return
getSmoothGround
(
clt_parameters
,
// final CLTParameters clt_parameters,
use_lma
,
// final boolean use_lma, // false, or there will be too many gaps?
discard_low
,
// final double discard_low, // fraction of all pixels
discard_high
,
// final double discard_high, // fraction of all pixels
stile_hstep
,
// final int stile_hstep,
stile_radius
,
// final double stile_radius,
sigma
,
// final double sigma,
radius_levels
,
// final int radius_levels,
min_non_empty
,
// final int min_non_empty, // minimal number of non-empty tiles for fitting planes
debugLevel
);
// final int debugLevel);
}
public
double
[]
getSmoothGround
(
final
CLTParameters
clt_parameters
,
final
boolean
use_lma
,
// false, or there will be too many gaps?
final
double
discard_low
,
// fraction of all pixels
final
double
discard_high
,
// fraction of all pixels
// final double discard_adisp, // discard above/below this fraction of average height
// final double discard_rdisp, // discard above/below this fraction of average height
final
int
stile_hstep
,
final
double
stile_radius
,
final
double
sigma
,
final
int
radius_levels
,
final
int
min_non_empty
,
// minimal number of non-empty tiles for fitting planes
final
int
debugLevel
)
{
final
int
stile_step
=
2
*
stile_hstep
;
final
int
num_bins
=
1000
;
final
double
normal_damping
=
0.001
;
// pull to horizontal if not enough data
final
double
hist_rlow
=
0.5
;
final
double
hist_rhigh
=
2.0
;
// allow strong/high, but not near the edges
final
int
tilesX
=
tp
.
getTilesX
();
final
int
tilesY
=
tp
.
getTilesY
();
double
[]
smooth_disparity
=
new
double
[
tilesX
*
tilesY
];
int
stilesX
=
(
tilesX
-
1
)/
stile_step
+
1
;
int
stilesY
=
(
tilesY
-
1
)/
stile_step
+
1
;
final
double
[][]
stiles_coeff
=
new
double
[
stilesX
*
stilesY
][
3
];
final
double
[][][]
wnd
=
new
double
[
radius_levels
][][];
// ][irad][irad];
final
double
[]
rad
=
new
double
[
radius_levels
];
final
int
[]
irada
=
new
int
[
radius_levels
];
for
(
int
nlev
=
0
;
nlev
<
radius_levels
;
nlev
++
)
{
if
(
nlev
==
0
)
{
rad
[
nlev
]
=
stile_radius
;
}
else
{
rad
[
nlev
]
=
rad
[
nlev
-
1
]
*
2
;
}
irada
[
nlev
]
=
(
int
)
Math
.
ceil
(
rad
[
nlev
]);
wnd
[
nlev
]=
new
double
[
irada
[
nlev
]][
irada
[
nlev
]];
double
ir2
=
irada
[
nlev
]*
irada
[
nlev
];
for
(
int
i
=
0
;
i
<
irada
[
nlev
];
i
++)
{
for
(
int
j
=
0
;
j
<
irada
[
nlev
];
j
++)
{
double
r2
=
(
i
*
i
+
j
*
j
)/
ir2
;
if
(
r2
<=
1.0
)
{
wnd
[
nlev
][
i
][
j
]
=
Math
.
cos
(
Math
.
PI
*
Math
.
sqrt
(
r2
)/
2
);
}
}
}
}
final
double
[][]
dls
=
getDLS
();
if
(
dls
==
null
)
{
return
null
;
}
final
double
[][]
ds
=
new
double
[][]
{
dls
[
use_lma
?
1
:
0
].
clone
(),
dls
[
2
]};
double
sw
=
0
,
swd
=
0
;
for
(
int
i
=
0
;
i
<
ds
[
0
].
length
;
i
++)
if
(!
Double
.
isNaN
(
ds
[
0
][
i
])){
sw
+=
ds
[
1
][
i
];
swd
+=
ds
[
0
][
i
]
*
ds
[
1
][
i
];
}
double
disp_avg
=
swd
/
sw
;
final
double
hist_low
=
hist_rlow
*
disp_avg
;
final
double
hist_high
=
hist_rhigh
*
disp_avg
;
double
k
=
num_bins
/
(
hist_high
-
hist_low
);
final
Thread
[]
threads
=
ImageDtt
.
newThreadArray
(
THREADS_MAX
);
final
AtomicInteger
ai
=
new
AtomicInteger
(
0
);
for
(
int
ithread
=
0
;
ithread
<
threads
.
length
;
ithread
++)
{
threads
[
ithread
]
=
new
Thread
()
{
public
void
run
()
{
TileNeibs
tn
=
new
TileNeibs
(
tilesX
,
tilesY
);
PolynomialApproximation
pa
=
new
PolynomialApproximation
();
double
[]
damping
=
new
double
[]
{
normal_damping
,
normal_damping
};
double
[]
hist
=
new
double
[
num_bins
];
for
(
int
nstile
=
ai
.
getAndIncrement
();
nstile
<
stiles_coeff
.
length
;
nstile
=
ai
.
getAndIncrement
())
{
int
stileX
=
nstile
%
stilesX
;
int
stileY
=
nstile
/
stilesX
;
int
nlev
=
0
;
int
num_non_empty
;
// fill data arrays and the histogram
int
tileXC
=
stile_hstep
+
stile_step
*
stileX
;
// center tiles
int
tileYC
=
stile_hstep
+
stile_step
*
stileY
;
int
ntileC
=
tileXC
+
tilesX
*
tileYC
;
for
(;
nlev
<
radius_levels
;
nlev
++)
{
int
irad
=
irada
[
nlev
];
int
idia
=
2
*
irad
-
1
;
double
[]
stile_d
=
new
double
[
idia
*
idia
];
double
[]
stile_w
=
new
double
[
idia
*
idia
];
// Arrays.fill(stile_w, 0.0); // not all elements may be filled
Arrays
.
fill
(
hist
,
0.0
);
double
sw
=
0
;
// , swd=0;
num_non_empty
=
0
;
// fill data arrays and the histogram
// later make to use lma first, then non-lma if nothing exists. Or increase size? (use several ones?
for
(
int
i
=
0
;
i
<
idia
;
i
++)
{
int
dy
=
i
-
irad
+
1
;
int
ady
=
Math
.
abs
(
dy
);
for
(
int
j
=
0
;
j
<
idia
;
j
++)
{
int
dx
=
j
-
irad
+
1
;
int
adx
=
Math
.
abs
(
dx
);
int
tindx
=
tn
.
getNeibIndex
(
ntileC
,
dx
,
dy
);
if
(
tindx
>=
0
)
{
int
ltile
=
i
*
idia
+
j
;
double
d
=
ds
[
0
][
tindx
];
double
w
=
ds
[
1
][
tindx
]*
wnd
[
nlev
][
ady
][
adx
];
int
bin
=
(
int
)
(
k
*
(
d
-
hist_low
));
if
((
w
>
0
)
&&
(
bin
>=
0
)
&&
(
bin
<
num_bins
))
{
stile_d
[
ltile
]
=
d
;
stile_w
[
ltile
]
=
w
;
sw
+=
w
;
// swd += w * d;
hist
[
bin
]
+=
w
;
num_non_empty
++;
}
}
}
}
if
((
num_non_empty
<
min_non_empty
)
||
(
sw
==
0
))
{
continue
;
// go to the next radius level
}
// try using histogram to remove outliers
double
dl
=
discard_low
*
sw
;
double
dh
=
discard_high
*
sw
;
double
sh
=
0.0
;
int
indx
=
0
;
for
(;
indx
<
num_bins
;
indx
++)
{
sh
+=
hist
[
indx
];
if
(
sh
>=
dl
)
break
;
}
double
shp
=
sh
-
hist
[
indx
];
// step back from the overshoot indx
double
thr_low
=
hist_low
+
(
indx
-
(
sh
-
dl
)/(
sh
-
shp
))/
num_bins
*(
hist_high
-
hist_low
);
indx
=
num_bins
-
1
;
sh
=
0.0
;
for
(;
indx
>=
0
;
indx
--)
{
sh
+=
hist
[
indx
];
if
(
sh
>=
dh
)
{
break
;
}
}
shp
=
sh
-
hist
[
indx
];
double
thr_high
=
hist_low
+
(
indx
+
(
sh
-
dh
)/(
sh
-
shp
))/
num_bins
*(
hist_high
-
hist_low
);
int
numgood
=
0
;
// boolean [] good = new boolean[ds[0].length];
sw
=
0.0
;
// swd = 0.0;
for
(
int
i
=
0
;
i
<
idia
;
i
++)
{
for
(
int
j
=
0
;
j
<
idia
;
j
++)
{
int
ltile
=
i
*
idia
+
j
;
if
(
stile_w
[
ltile
]
>
0
)
{
if
((
stile_d
[
ltile
]
>=
thr_low
)
&&
(
stile_d
[
ltile
]
<=
thr_high
))
{
numgood
++;
}
else
{
stile_w
[
ltile
]
=
0
;
}
}
}
}
if
(
numgood
<
min_non_empty
)
{
continue
;
// go to the next radius level
}
// fit plane
double
[][][]
mdata
=
new
double
[
numgood
][][];
int
mindx
=
0
;
for
(
int
i
=
0
;
i
<
idia
;
i
++)
{
int
dy
=
i
-
irad
+
1
;
for
(
int
j
=
0
;
j
<
idia
;
j
++)
{
int
dx
=
j
-
irad
+
1
;
int
ltile
=
i
*
idia
+
j
;
if
(
stile_w
[
ltile
]
>
0
)
{
mdata
[
mindx
]
=
new
double
[
3
][];
mdata
[
mindx
][
0
]
=
new
double
[
2
];
mdata
[
mindx
][
0
][
0
]
=
dx
;
mdata
[
mindx
][
0
][
1
]
=
dy
;
mdata
[
mindx
][
1
]
=
new
double
[
1
];
mdata
[
mindx
][
1
][
0
]
=
stile_d
[
ltile
];
mdata
[
mindx
][
2
]
=
new
double
[
1
];
mdata
[
mindx
][
2
][
0
]
=
stile_w
[
ltile
];
mindx
++;
}
}
}
double
[][]
approx2d
=
pa
.
quadraticApproximation
(
mdata
,
true
,
// boolean forceLinear, // use linear approximation
damping
,
// double [] damping, null OK
-
1
);
// debug level
stiles_coeff
[
nstile
][
0
]
=
approx2d
[
0
][
2
];
// offset
stiles_coeff
[
nstile
][
1
]
=
approx2d
[
0
][
0
];
// tiltX
stiles_coeff
[
nstile
][
2
]
=
approx2d
[
0
][
1
];
// tiltY
}
}
}
};
}
ImageDtt
.
startAndJoin
(
threads
);
// Fill planes in each stile
ai
.
set
(
0
);
for
(
int
ithread
=
0
;
ithread
<
threads
.
length
;
ithread
++)
{
threads
[
ithread
]
=
new
Thread
()
{
public
void
run
()
{
TileNeibs
tn
=
new
TileNeibs
(
tilesX
,
tilesY
);
for
(
int
nstile
=
ai
.
getAndIncrement
();
nstile
<
stiles_coeff
.
length
;
nstile
=
ai
.
getAndIncrement
())
{
int
stileX
=
nstile
%
stilesX
;
int
stileY
=
nstile
/
stilesX
;
int
tileXC
=
stile_hstep
+
stile_step
*
stileX
;
// center tiles
int
tileYC
=
stile_hstep
+
stile_step
*
stileY
;
int
ntileC
=
tileXC
+
tilesX
*
tileYC
;
for
(
int
dy
=
-
stile_hstep
;
dy
<
stile_hstep
;
dy
++)
{
for
(
int
dx
=
-
stile_hstep
;
dx
<
stile_hstep
;
dx
++)
{
int
tindx
=
tn
.
getNeibIndex
(
ntileC
,
dx
,
dy
);
if
(
tindx
>=
0
)
{
double
d
=
stiles_coeff
[
nstile
][
0
]
+
stiles_coeff
[
nstile
][
1
]
*
dx
+
stiles_coeff
[
nstile
][
2
]
*
dy
;
smooth_disparity
[
tindx
]
=
d
;
}
}
}
}
}
};
}
ImageDtt
.
startAndJoin
(
threads
);
// Maybe use Gaussian? to additionally smooth "seams"
double
[][]
dbg_img
=
null
;
if
(
debugLevel
>
0
)
{
dbg_img
=
new
double
[
5
][];
dbg_img
[
0
]
=
dls
[
2
];
// strt
dbg_img
[
1
]
=
dls
[
0
];
// disp
dbg_img
[
2
]
=
dls
[
1
];
// lma
dbg_img
[
3
]
=
smooth_disparity
.
clone
();
}
if
(
sigma
>
0
)
{
(
new
DoubleGaussianBlur
()).
blurDouble
(
smooth_disparity
,
tilesX
,
tilesY
,
sigma
,
sigma
,
0.01
);
}
if
(
dbg_img
!=
null
)
{
dbg_img
[
4
]
=
smooth_disparity
;
String
[]
dbg_titles
=
{
"str"
,
"disp"
,
"lma"
,
"smooth"
,
"blur"
};
ShowDoubleFloatArrays
.
showArrays
(
dbg_img
,
tp
.
getTilesX
(),
tp
.
getTilesY
(),
true
,
getImageName
()+
"-smooth-ground.tiff "
,
dbg_titles
);
}
// replace strong high enough objects?
return
smooth_disparity
;
}
public
double
[][]
getGroundNoIms
(
CLTParameters
clt_parameters
,
CLTParameters
clt_parameters
,
boolean
use_lma
,
boolean
use_lma
,
boolean
use_parallel_proj
,
boolean
use_parallel_proj
,
...
@@ -4383,6 +4763,24 @@ public class QuadCLTCPU {
...
@@ -4383,6 +4763,24 @@ public class QuadCLTCPU {
return
imp
;
return
imp
;
}
}
public
boolean
saveStringInModelDirectory
(
String
string
,
String
suffix
)
{
// includes extension
String
x3d_path
=
getX3dDirectory
();
String
file_name
=
image_name
+
suffix
;
String
file_path
=
x3d_path
+
Prefs
.
getFileSeparator
()
+
file_name
;
try
{
BufferedWriter
out
=
new
BufferedWriter
(
new
FileWriter
(
file_path
,
true
));
out
.
write
(
string
);
out
.
close
();
return
true
;
}
catch
(
IOException
e
)
{
// Display message when exception occurs
System
.
out
.
println
(
"exception occurred"
+
e
);
return
false
;
}
}
public
ImagePlus
saveDoubleArrayInModelDirectory
(
public
ImagePlus
saveDoubleArrayInModelDirectory
(
String
suffix
,
String
suffix
,
...
@@ -10712,15 +11110,15 @@ public class QuadCLTCPU {
...
@@ -10712,15 +11110,15 @@ public class QuadCLTCPU {
System
.
out
.
println
(
"No IMS orientation correction is defined"
);
System
.
out
.
println
(
"No IMS orientation correction is defined"
);
}
}
}
}
public
void
showPimuOffsets
(
)
{
public
static
void
showPimuOffsets
(
CLTParameters
clt_parameters
)
{
showPimuOffsets
(
getPimuOffset
s
());
showPimuOffsets
(
clt_parameters
.
imp
.
get_pimu_off
s
());
}
}
public
static
void
showPimuOffsets
(
double
[][]
pimu_offsets
)
{
public
static
void
showPimuOffsets
(
double
[][]
pimu_offsets
)
{
if
(
pimu_offsets
!=
null
)
{
if
(
pimu_offsets
!=
null
)
{
System
.
out
.
println
(
"Linear velocities
offsets (m/s) =
["
+
pimu_offsets
[
0
][
0
]+
", "
+
System
.
out
.
println
(
"Linear velocities
scales (from 1.0) =
["
+
pimu_offsets
[
0
][
0
]+
", "
+
pimu_offsets
[
0
][
1
]+
", "
+
pimu_offsets
[
0
][
2
]+
"]"
);
pimu_offsets
[
0
][
1
]+
", "
+
pimu_offsets
[
0
][
2
]+
"]"
);
System
.
out
.
println
(
"Angular velocities offsets (rad/s) = ["
+
System
.
out
.
println
(
"Angular velocities offsets (rad/s) =
["
+
pimu_offsets
[
1
][
0
]+
", "
+
pimu_offsets
[
1
][
1
]+
", "
+
+
pimu_offsets
[
1
][
2
]+
"]"
);
pimu_offsets
[
1
][
0
]+
", "
+
pimu_offsets
[
1
][
1
]+
", "
+
+
pimu_offsets
[
1
][
2
]+
"]"
);
}
else
{
}
else
{
System
.
out
.
println
(
"No PIMU offsets are defined"
);
System
.
out
.
println
(
"No PIMU offsets are defined"
);
...
...
src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
View file @
ab8d3318
...
@@ -35,10 +35,10 @@ import Jama.Matrix;
...
@@ -35,10 +35,10 @@ import Jama.Matrix;
public
class
QuaternionLma
{
public
class
QuaternionLma
{
private
final
static
int
REGLEN
=
1
;
// number of extra (regularization) samples
private
final
static
int
REGLEN
=
1
;
// number of extra (regularization) samples
public
static
final
int
MODE_XYZ
=
0
;
public
static
final
int
MODE_XYZ
=
0
;
public
static
final
int
MODE_XYZQ
=
1
;
public
static
final
int
MODE_XYZQ
=
1
;
// OK with [3]
public
static
final
int
MODE_COMBO
=
2
;
public
static
final
int
MODE_COMBO
=
2
;
public
static
final
int
MODE_XYZQ_LOCAL
=
3
;
public
static
final
int
MODE_XYZQ_LOCAL
=
3
;
public
static
final
int
MODE_COMBO_LOCAL
=
4
;
public
static
final
int
MODE_COMBO_LOCAL
=
4
;
// OK with [3]
public
static
final
int
MODE_COMPASS
=
5
;
public
static
final
int
MODE_COMPASS
=
5
;
public
static
final
int
MODE_XYZ4Q3
=
6
;
// Q0-Q3 for tranlation (with scale), Q1-Q3 - for rotation
public
static
final
int
MODE_XYZ4Q3
=
6
;
// Q0-Q3 for tranlation (with scale), Q1-Q3 - for rotation
...
@@ -67,6 +67,7 @@ public class QuaternionLma {
...
@@ -67,6 +67,7 @@ public class QuaternionLma {
private
double
[]
last_ymfx
=
null
;
private
double
[]
last_ymfx
=
null
;
private
double
[][]
last_jt
=
null
;
private
double
[][]
last_jt
=
null
;
private
double
[]
axis
=
null
;
private
double
[]
axis
=
null
;
private
double
[]
dbg_data
;
public
double
[]
getQuaternion
()
{
public
double
[]
getQuaternion
()
{
if
(
parameters_vector
.
length
==
3
)
{
if
(
parameters_vector
.
length
==
3
)
{
...
@@ -673,10 +674,10 @@ public class QuaternionLma {
...
@@ -673,10 +674,10 @@ public class QuaternionLma {
jt
,
// final double [][] jt, // should be null or initialized with [vector.length][]
jt
,
// final double [][] jt, // should be null or initialized with [vector.length][]
debug_level
);
// final int debug_level)
debug_level
);
// final int debug_level)
}
else
{
}
else
{
return
getFxDerivs6Dof
(
return
getFxDerivs6Dof
(
vector
,
// double [] vector,
vector
,
// double [] vector,
jt
,
// final double [][] jt, // should be null or initialized with [vector.length][]
jt
,
// final double [][] jt, // should be null or initialized with [vector.length][]
debug_level
);
// final int debug_level)
debug_level
);
// final int debug_level)
}
}
case
MODE_COMBO:
return
getFxDerivsVisual
(
// fill change
case
MODE_COMBO:
return
getFxDerivsVisual
(
// fill change
vector
,
// double [] vector,
vector
,
// double [] vector,
...
@@ -748,7 +749,7 @@ public class QuaternionLma {
...
@@ -748,7 +749,7 @@ public class QuaternionLma {
double
c
=
Math
.
cos
(
vector
[
0
]/
2
),
s
=
Math
.
sin
(
vector
[
0
]/
2
);
double
c
=
Math
.
cos
(
vector
[
0
]/
2
),
s
=
Math
.
sin
(
vector
[
0
]/
2
);
//axis
//axis
double
[]
fx
=
new
double
[
weights
.
length
];
double
[]
fx
=
new
double
[
weights
.
length
];
final
double
[]
q
=
new
double
[]
{
c
/
2
,
s
*
axis
[
0
]/
2
,
s
*
axis
[
1
]/
2
,
s
*
axis
[
2
]/
2
};
final
double
[]
q
=
new
double
[]
{
c
,
s
*
axis
[
0
],
s
*
axis
[
1
],
s
*
axis
[
2
]
};
double
[]
dq_dv
=
new
double
[]
{-
s
/
2
,
c
*
axis
[
0
]/
2
,
c
*
axis
[
1
]/
2
,
c
*
axis
[
2
]/
2
};
double
[]
dq_dv
=
new
double
[]
{-
s
/
2
,
c
*
axis
[
0
]/
2
,
c
*
axis
[
1
]/
2
,
c
*
axis
[
2
]/
2
};
if
(
jt
!=
null
)
{
if
(
jt
!=
null
)
{
for
(
int
i
=
0
;
i
<
vector
.
length
;
i
++)
{
for
(
int
i
=
0
;
i
<
vector
.
length
;
i
++)
{
...
@@ -759,6 +760,14 @@ public class QuaternionLma {
...
@@ -759,6 +760,14 @@ public class QuaternionLma {
double
[][]
xyz_dq
;
double
[][]
xyz_dq
;
for
(
int
i
=
0
;
i
<
N
;
i
++)
{
for
(
int
i
=
0
;
i
<
N
;
i
++)
{
int
i3
=
3
*
i
;
int
i3
=
3
*
i
;
has_data:
{
for
(
int
j
=
0
;
j
<
samples
;
j
++)
{
if
(
weights
[
i3
+
j
]
>
0
)
{
break
has_data
;
}
}
continue
;
// nothing to process for this scene
}
final
double
[]
xyz
=
new
double
[]
{
x_vector
[
i3
+
0
],
x_vector
[
i3
+
1
],
x_vector
[
i3
+
2
]};
final
double
[]
xyz
=
new
double
[]
{
x_vector
[
i3
+
0
],
x_vector
[
i3
+
1
],
x_vector
[
i3
+
2
]};
xyz_rot
=
applyTo
(
q
,
xyz
);
xyz_rot
=
applyTo
(
q
,
xyz
);
System
.
arraycopy
(
xyz_rot
,
0
,
fx
,
i3
,
3
);
System
.
arraycopy
(
xyz_rot
,
0
,
fx
,
i3
,
3
);
...
@@ -922,7 +931,7 @@ public class QuaternionLma {
...
@@ -922,7 +931,7 @@ public class QuaternionLma {
}
}
return
fx
;
return
fx
;
}
}
private
double
[]
getFxDerivs6DofMode33
(
private
double
[]
getFxDerivs6DofMode33
(
// MODE_XYZQ_LOCAL = 3; // OK with [3]
double
[]
vector
,
//
double
[]
vector
,
//
final
double
[][]
jt
,
// should be null or initialized with [vector.length][]
final
double
[][]
jt
,
// should be null or initialized with [vector.length][]
final
int
debug_level
)
{
final
int
debug_level
)
{
...
@@ -933,6 +942,7 @@ public class QuaternionLma {
...
@@ -933,6 +942,7 @@ public class QuaternionLma {
final
double
q0
=
getQ0
(
vector
);
final
double
q0
=
getQ0
(
vector
);
// final double [] vector_r = normSign(new double[] { q0,q1,q2,q3});
// final double [] vector_r = normSign(new double[] { q0,q1,q2,q3});
final
double
[]
vector_r
=
new
double
[]
{
q0
,
q1
,
q2
,
q3
};
final
double
[]
vector_r
=
new
double
[]
{
q0
,
q1
,
q2
,
q3
};
if
(
jt
!=
null
)
{
if
(
jt
!=
null
)
{
for
(
int
i
=
0
;
i
<
vector
.
length
;
i
++)
{
for
(
int
i
=
0
;
i
<
vector
.
length
;
i
++)
{
jt
[
i
]
=
new
double
[
weights
.
length
];
jt
[
i
]
=
new
double
[
weights
.
length
];
...
@@ -1084,7 +1094,7 @@ public class QuaternionLma {
...
@@ -1084,7 +1094,7 @@ public class QuaternionLma {
return
fx
;
return
fx
;
}
}
private
double
[]
getFxDerivs6Dof33
(
// vector[3], but only 3 for rotations
private
double
[]
getFxDerivs6Dof33
(
// vector[3], but only 3 for rotations
MODE_XYZQ tested
double
[]
vector3
,
//
double
[]
vector3
,
//
final
double
[][]
jt
,
// should be null or initialized with [vector.length][]
final
double
[][]
jt
,
// should be null or initialized with [vector.length][]
final
int
debug_level
)
{
final
int
debug_level
)
{
...
@@ -1428,10 +1438,14 @@ public class QuaternionLma {
...
@@ -1428,10 +1438,14 @@ public class QuaternionLma {
return
fx
;
return
fx
;
}
}
private
double
[]
getFxDerivsVisualMode43
(
private
double
[]
getFxDerivsVisualMode43
(
// tested MODE_COMBO_LOCAL = 4; // OK with [3]
double
[]
vector3
,
double
[]
vector3
,
final
double
[][]
jt
,
// should be null or initialized with [vector.length][]
final
double
[][]
jt
,
// should be null or initialized with [vector.length][]
final
int
debug_level
)
{
final
int
debug_level
)
{
boolean
dbg_out
=
debug_level
>
2
;
if
(
dbg_out
)
{
dbg_data
=
new
double
[
N
*
samples_x
];
}
double
[]
vector
=
new
double
[]
{
getQ0
(
vector3
),
vector3
[
0
],
vector3
[
1
],
vector3
[
2
]};
double
[]
vector
=
new
double
[]
{
getQ0
(
vector3
),
vector3
[
0
],
vector3
[
1
],
vector3
[
2
]};
double
[]
fx
=
new
double
[
weights
.
length
];
double
[]
fx
=
new
double
[
weights
.
length
];
double
[]
qn
=
new
double
[
4
];
double
[]
qn
=
new
double
[
4
];
...
@@ -1476,12 +1490,15 @@ public class QuaternionLma {
...
@@ -1476,12 +1490,15 @@ public class QuaternionLma {
fx
[
i4
+
1
]
=
2
*
comb_y
[
1
][
3
];
// quat_rot[3]; // 2 * Q3
fx
[
i4
+
1
]
=
2
*
comb_y
[
1
][
3
];
// quat_rot[3]; // 2 * Q3
fx
[
i4
+
2
]
=
2
*
comb_y
[
1
][
2
]
-
comb_y
[
0
][
0
]/
height
;
// quat_rot[2] - xyz_rot[0] / height; // 2 * Q2 - X / height
fx
[
i4
+
2
]
=
2
*
comb_y
[
1
][
2
]
-
comb_y
[
0
][
0
]/
height
;
// quat_rot[2] - xyz_rot[0] / height; // 2 * Q2 - X / height
fx
[
i4
+
3
]
=
2
*
comb_y
[
1
][
1
]
+
comb_y
[
0
][
1
]/
height
;
// quat_rot[1] + xyz_rot[1] / height; // 2 * Q1 + Y / height
fx
[
i4
+
3
]
=
2
*
comb_y
[
1
][
1
]
+
comb_y
[
0
][
1
]/
height
;
// quat_rot[1] + xyz_rot[1] / height; // 2 * Q1 + Y / height
/*
if
(
dbg_out
)
{
fx[i4 + 0] = comb_y[0][2]/ height; // xyz_rot[2] / height; // Z
dbg_data
[
i7
+
0
]
=
comb_y
[
0
][
0
]/
height
;
fx[i4 + 1] = -2 * comb_y[1][3]; // quat_rot[3]; // 2 * Q3
dbg_data
[
i7
+
1
]
=
comb_y
[
0
][
1
]/
height
;
fx[i4 + 2] = -2 * comb_y[1][2] - comb_y[0][0]/ height; // quat_rot[2] - xyz_rot[0] / height; // 2 * Q2 - X / height
dbg_data
[
i7
+
2
]
=
comb_y
[
0
][
2
]/
height
;
fx[i4 + 3] = -2 * comb_y[1][1] + comb_y[0][1]/ height; // quat_rot[1] + xyz_rot[1] / height; // 2 * Q1 + Y / height
dbg_data
[
i7
+
3
]
=
comb_y
[
1
][
0
];
*/
dbg_data
[
i7
+
4
]
=
comb_y
[
1
][
1
];
dbg_data
[
i7
+
5
]
=
comb_y
[
1
][
2
];
dbg_data
[
i7
+
6
]
=
comb_y
[
1
][
3
];
}
if
(
jt
!=
null
)
{
if
(
jt
!=
null
)
{
xyz_dq
=
applyToDQ
(
vector
,
xyz
);
xyz_dq
=
applyToDQ
(
vector
,
xyz
);
double
[][]
xyz_dq_local
=
new
double
[
xyz_dq
.
length
][];
double
[][]
xyz_dq_local
=
new
double
[
xyz_dq
.
length
][];
...
@@ -1719,17 +1736,21 @@ public class QuaternionLma {
...
@@ -1719,17 +1736,21 @@ public class QuaternionLma {
parameters_vector
,
// double [] vector,
parameters_vector
,
// double [] vector,
null
,
// final double [][] jt, // should be null or initialized with [vector.length][]
null
,
// final double [][] jt, // should be null or initialized with [vector.length][]
debug_level
);
// final int debug_level)
debug_level
);
// final int debug_level)
debugYfX
(
"fx-"
,
// String pfx,
debugYfX
(
"fx-"
,
// String pfx,
fx
);
// double [] data)
fx
);
// double [] data)
if
(
debug_level
>
1
)
{
if
(
debug_level
>
2
)
{
double
delta
=
1
E
-
5
;
debugYfX
(
"ffx-"
,
// String pfx,
System
.
out
.
println
(
"\n\n"
);
dbg_data
);
// double [] data)
double
err
=
compareJT
(
}
parameters_vector
,
// double [] vector,
if
(
debug_level
>
1
)
{
delta
);
// double delta);
double
delta
=
1
E
-
5
;
System
.
out
.
println
(
"Maximal error = "
+
err
);
System
.
out
.
println
(
"\n\n"
);
}
double
err
=
compareJT
(
parameters_vector
,
// double [] vector,
delta
);
// double delta);
System
.
out
.
println
(
"Maximal error = "
+
err
);
}
}
}
return
rslt
[
0
]?
iter
:
-
1
;
return
rslt
[
0
]?
iter
:
-
1
;
}
}
...
@@ -1773,6 +1794,11 @@ public class QuaternionLma {
...
@@ -1773,6 +1794,11 @@ public class QuaternionLma {
debugYfX
(
"fx0-"
,
// String pfx,
debugYfX
(
"fx0-"
,
// String pfx,
fx
);
// double [] data)
fx
);
// double [] data)
}
}
if
(
debug_level
>
2
)
{
debugYfX
(
"ffx0-"
,
// String pfx,
dbg_data
);
// double [] data)
}
if
(
debug_level
>
1
)
{
if
(
debug_level
>
1
)
{
double
delta
=
1
E
-
5
;
double
delta
=
1
E
-
5
;
System
.
out
.
println
(
"\n\n"
);
System
.
out
.
println
(
"\n\n"
);
...
@@ -1919,6 +1945,9 @@ public class QuaternionLma {
...
@@ -1919,6 +1945,9 @@ public class QuaternionLma {
public
void
debugYfX
(
public
void
debugYfX
(
String
pfx
,
String
pfx
,
double
[]
data
)
{
double
[]
data
)
{
if
(
data
==
null
)
{
return
;
}
// if ((mode == 1) || ((mode == 2) && (data.length >= x_vector.length))) { // different data size data[3*nscene+...]
// if ((mode == 1) || ((mode == 2) && (data.length >= x_vector.length))) { // different data size data[3*nscene+...]
if
((
mode
==
MODE_XYZ
)
||
((
mode
==
MODE_COMPASS
)))
{
if
((
mode
==
MODE_XYZ
)
||
((
mode
==
MODE_COMPASS
)))
{
System
.
out
.
println
(
String
.
format
(
"%3s"
+
System
.
out
.
println
(
String
.
format
(
"%3s"
+
...
...
src/main/java/com/elphel/imagej/tileprocessor/TexturedModel.java
View file @
ab8d3318
...
@@ -2576,6 +2576,7 @@ public class TexturedModel {
...
@@ -2576,6 +2576,7 @@ public class TexturedModel {
double
[]
sfm_gain
=
(
min_sfm_gain
>
0.0
)
?
combo_dsn_final
[
OpticalFlow
.
COMBO_DSN_INDX_SFM_GAIN
]
:
null
;
double
[]
sfm_gain
=
(
min_sfm_gain
>
0.0
)
?
combo_dsn_final
[
OpticalFlow
.
COMBO_DSN_INDX_SFM_GAIN
]
:
null
;
// currently conditionInitialDS() zeroes disparity for blue_sky. TODO: allow some FG over blue_sky?
// currently conditionInitialDS() zeroes disparity for blue_sky. TODO: allow some FG over blue_sky?
// gets Blue Sky from scene.dsi , not from the file!
// gets Blue Sky from scene.dsi , not from the file!
double
[][]
ds_fg
=
OpticalFlow
.
conditionInitialDS
(
double
[][]
ds_fg
=
OpticalFlow
.
conditionInitialDS
(
true
,
// boolean use_conf, // use configuration parameters, false - use following
true
,
// boolean use_conf, // use configuration parameters, false - use following
clt_parameters
,
// CLTParameters clt_parameters,
clt_parameters
,
// CLTParameters clt_parameters,
...
@@ -3165,7 +3166,7 @@ public class TexturedModel {
...
@@ -3165,7 +3166,7 @@ public class TexturedModel {
hdr_whs[0], // int width, // int tilesX,
hdr_whs[0], // int width, // int tilesX,
hdr_whs[1]); // int height, // int tilesY,
hdr_whs[1]); // int height, // int tilesY,
*/
*/
scenes
[
ref_index
].
writeLwirGeoTiff32
(
scenes
[
ref_index
].
writeLwirGeoTiff32
(
// Negative value supplied for TIFF_RATIONAL
clt_parameters
,
// final CLTParameters clt_parameters,
clt_parameters
,
// final CLTParameters clt_parameters,
cropped_z
[
0
],
// double [] data,
cropped_z
[
0
],
// double [] data,
top_left_lla
,
// double [] lla, // latitude, longitude, altitude (or null)
top_left_lla
,
// double [] lla, // latitude, longitude, altitude (or null)
...
...
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