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
1d564879
Commit
1d564879
authored
Jan 13, 2024
by
Andrey Filippov
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
working snapshot
parent
7de7fe06
Changes
3
Show whitespace changes
Inline
Side-by-side
Showing
3 changed files
with
281 additions
and
17 deletions
+281
-17
OpticalFlow.java
...ain/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
+1
-1
QuadCLTCPU.java
...main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
+3
-1
QuaternionLma.java
...n/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
+277
-15
No files found.
src/main/java/com/elphel/imagej/tileprocessor/OpticalFlow.java
View file @
1d564879
...
@@ -5437,7 +5437,7 @@ public class OpticalFlow {
...
@@ -5437,7 +5437,7 @@ public class OpticalFlow {
}
}
double
[]
rms
=
new
double
[
5
];
double
[]
rms
=
new
double
[
5
];
double
[]
quat
=
new
double
[
4
];
double
[]
quat
=
new
double
[
4
];
int
quat_lma_mode
=
2
;
// 1;
int
quat_lma_mode
=
3
;
//
2; // 1;
int
debug_lev
=
3
;
int
debug_lev
=
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
);
...
...
src/main/java/com/elphel/imagej/tileprocessor/QuadCLTCPU.java
View file @
1d564879
...
@@ -274,6 +274,7 @@ public class QuadCLTCPU {
...
@@ -274,6 +274,7 @@ public class QuadCLTCPU {
vect_y
[
nscene
]
=
xyzatr
[
nscene
];
vect_y
[
nscene
]
=
xyzatr
[
nscene
];
}
}
quaternionLma
.
prepareLMA
(
quaternionLma
.
prepareLMA
(
quat_lma_mode
,
// int mode,
avg_height
,
// double avg_height,
avg_height
,
// double avg_height,
vect_x
,
// double [][][] vect_x,
vect_x
,
// double [][][] vect_x,
vect_y
,
// double [][][] vect_y,
vect_y
,
// double [][][] vect_y,
...
@@ -281,7 +282,7 @@ public class QuadCLTCPU {
...
@@ -281,7 +282,7 @@ public class QuadCLTCPU {
translation_weight
,
// double translation_weight, // 0.0 ... 1.0;
translation_weight
,
// double translation_weight, // 0.0 ... 1.0;
quat0
,
// double [] quat0,
quat0
,
// double [] quat0,
debugLevel
);
// int debug_level)
debugLevel
);
// int debug_level)
}
else
if
(
quat_lma_mode
==
1
)
{
}
else
if
(
(
quat_lma_mode
==
1
)
||
(
quat_lma_mode
==
3
)
)
{
double
[][][]
vect_y
=
new
double
[
quadCLTs
.
length
][][];
// camera XYZATR
double
[][][]
vect_y
=
new
double
[
quadCLTs
.
length
][][];
// camera XYZATR
double
[][][]
vect_x
=
new
double
[
quadCLTs
.
length
][][];
// IMS XYZATR
double
[][][]
vect_x
=
new
double
[
quadCLTs
.
length
][][];
// IMS XYZATR
for
(
int
nscene
=
early_index
;
nscene
<=
last_index
;
nscene
++)
{
for
(
int
nscene
=
early_index
;
nscene
<=
last_index
;
nscene
++)
{
...
@@ -289,6 +290,7 @@ public class QuadCLTCPU {
...
@@ -289,6 +290,7 @@ public class QuadCLTCPU {
vect_y
[
nscene
]
=
xyzatr
[
nscene
];
vect_y
[
nscene
]
=
xyzatr
[
nscene
];
}
}
quaternionLma
.
prepareLMA
(
quaternionLma
.
prepareLMA
(
quat_lma_mode
,
// int mode,
vect_x
,
// double [][][] vect_x,
vect_x
,
// double [][][] vect_x,
vect_y
,
// double [][][] vect_y,
vect_y
,
// double [][][] vect_y,
null
,
// double [] vect_w, all same weight
null
,
// double [] vect_w, all same weight
...
...
src/main/java/com/elphel/imagej/tileprocessor/QuaternionLma.java
View file @
1d564879
...
@@ -47,6 +47,7 @@ public class QuaternionLma {
...
@@ -47,6 +47,7 @@ public class QuaternionLma {
private
double
[]
parameters_vector
=
null
;
private
double
[]
parameters_vector
=
null
;
private
double
[]
x_vector
=
null
;
private
double
[]
x_vector
=
null
;
private
double
[]
y_vector
=
null
;
private
double
[]
y_vector
=
null
;
private
double
[]
y_inv_vector
=
null
;
private
double
[]
weights
;
// normalized so sum is 1.0 for all - samples and extra regularization
private
double
[]
weights
;
// normalized so sum is 1.0 for all - samples and extra regularization
private
double
pure_weight
;
// weight of samples only
private
double
pure_weight
;
// weight of samples only
private
double
[]
last_ymfx
=
null
;
private
double
[]
last_ymfx
=
null
;
...
@@ -106,6 +107,7 @@ public class QuaternionLma {
...
@@ -106,6 +107,7 @@ public class QuaternionLma {
public
void
prepareLMA
(
public
void
prepareLMA
(
int
mode
,
double
[][][]
vect_x
,
// []{{x,y,z},{a,t,r}}
double
[][][]
vect_x
,
// []{{x,y,z},{a,t,r}}
double
[][][]
vect_y
,
// []{{x,y,z},{a,t,r}}
double
[][][]
vect_y
,
// []{{x,y,z},{a,t,r}}
double
[]
vect_w
,
// one per scene
double
[]
vect_w
,
// one per scene
...
@@ -113,8 +115,20 @@ public class QuaternionLma {
...
@@ -113,8 +115,20 @@ public class QuaternionLma {
double
reg_w
,
// regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
double
reg_w
,
// regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
double
[]
quat0
,
double
[]
quat0
,
final
int
debug_level
)
{
final
int
debug_level
)
{
if
(
mode
!=
1
)
{
prepareLMAMode3
(
mode
,
// int mode,
vect_x
,
// double [][][] vect_x, // []{{x,y,z},{a,t,r}}
vect_y
,
// double [][][] vect_y, // []{{x,y,z},{a,t,r}}
vect_w
,
// double [] vect_w, // one per scene
translation_weight
,
// double translation_weight, // 0.0 ... 1.0;
reg_w
,
// double reg_w, // regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
quat0
,
// double [] quat0,
debug_level
);
// final int debug_level);
return
;
}
N
=
vect_x
.
length
;
N
=
vect_x
.
length
;
mode
=
1
;
this
.
mode
=
mode
;
samples
=
7
;
samples
=
7
;
samples_x
=
7
;
samples_x
=
7
;
pure_weight
=
1.0
-
reg_w
;
pure_weight
=
1.0
-
reg_w
;
...
@@ -122,8 +136,6 @@ public class QuaternionLma {
...
@@ -122,8 +136,6 @@ public class QuaternionLma {
y_vector
=
new
double
[
samples
*
N
+
REGLEN
];
y_vector
=
new
double
[
samples
*
N
+
REGLEN
];
weights
=
new
double
[
samples
*
N
+
REGLEN
];
weights
=
new
double
[
samples
*
N
+
REGLEN
];
parameters_vector
=
quat0
.
clone
();
parameters_vector
=
quat0
.
clone
();
double
[]
tr_w
=
new
double
[]
{
translation_weight
,
1.0
-
translation_weight
};
double
sw
=
0
;
double
sw
=
0
;
for
(
int
i
=
0
;
i
<
N
;
i
++)
{
for
(
int
i
=
0
;
i
<
N
;
i
++)
{
if
((
vect_x
[
i
]==
null
)
||
(
vect_y
[
i
]==
null
))
{
if
((
vect_x
[
i
]==
null
)
||
(
vect_y
[
i
]==
null
))
{
...
@@ -135,17 +147,18 @@ public class QuaternionLma {
...
@@ -135,17 +147,18 @@ public class QuaternionLma {
}
else
{
}
else
{
// xyz
// xyz
double
w
=
translation_weight
*((
vect_w
!=
null
)?
vect_w
[
i
]
:
1.0
);
double
w
=
translation_weight
*((
vect_w
!=
null
)?
vect_w
[
i
]
:
1.0
);
// quaternions
Rotation
rot_x
=
new
Rotation
(
RotationOrder
.
YXZ
,
ErsCorrection
.
ROT_CONV
,
vect_x
[
i
][
1
][
0
],
vect_x
[
i
][
1
][
1
],
vect_x
[
i
][
1
][
2
]);
Rotation
rot_y
=
new
Rotation
(
RotationOrder
.
YXZ
,
ErsCorrection
.
ROT_CONV
,
vect_y
[
i
][
1
][
0
],
vect_y
[
i
][
1
][
1
],
vect_y
[
i
][
1
][
2
]);
for
(
int
j
=
0
;
j
<
3
;
j
++)
{
for
(
int
j
=
0
;
j
<
3
;
j
++)
{
x_vector
[
samples
*
i
+
j
]
=
vect_x
[
i
][
0
][
j
];
x_vector
[
samples
*
i
+
j
]
=
vect_x
[
i
][
0
][
j
];
y_vector
[
samples
*
i
+
j
]
=
vect_y
[
i
][
0
][
j
];
y_vector
[
samples
*
i
+
j
]
=
vect_y
[
i
][
0
][
j
];
weights
[
samples
*
i
+
j
]
=
w
;
weights
[
samples
*
i
+
j
]
=
w
;
sw
+=
w
;
sw
+=
w
;
}
}
// quaternions
Rotation
rot_x
=
new
Rotation
(
RotationOrder
.
YXZ
,
ErsCorrection
.
ROT_CONV
,
vect_x
[
i
][
1
][
0
],
vect_x
[
i
][
1
][
1
],
vect_x
[
i
][
1
][
2
]);
Rotation
rot_y
=
new
Rotation
(
RotationOrder
.
YXZ
,
ErsCorrection
.
ROT_CONV
,
vect_y
[
i
][
1
][
0
],
vect_y
[
i
][
1
][
1
],
vect_y
[
i
][
1
][
2
]);
x_vector
[
samples
*
i
+
3
]
=
rot_x
.
getQ0
();
x_vector
[
samples
*
i
+
3
]
=
rot_x
.
getQ0
();
x_vector
[
samples
*
i
+
4
]
=
rot_x
.
getQ1
();
x_vector
[
samples
*
i
+
4
]
=
rot_x
.
getQ1
();
x_vector
[
samples
*
i
+
5
]
=
rot_x
.
getQ2
();
x_vector
[
samples
*
i
+
5
]
=
rot_x
.
getQ2
();
...
@@ -175,7 +188,100 @@ public class QuaternionLma {
...
@@ -175,7 +188,100 @@ public class QuaternionLma {
}
}
public
void
prepareLMAMode3
(
int
mode
,
double
[][][]
vect_x
,
// []{{x,y,z},{a,t,r}}
double
[][][]
vect_y
,
// []{{x,y,z},{a,t,r}}
double
[]
vect_w
,
// one per scene
double
translation_weight
,
// 0.0 ... 1.0;
double
reg_w
,
// regularization weight [0..1) weight of q0^2+q1^2+q3^2 -1
double
[]
quat0
,
final
int
debug_level
)
{
N
=
vect_x
.
length
;
this
.
mode
=
mode
;
samples
=
7
;
samples_x
=
7
;
pure_weight
=
1.0
-
reg_w
;
x_vector
=
new
double
[
samples
*
N
];
y_vector
=
new
double
[
samples
*
N
+
REGLEN
];
y_inv_vector
=
new
double
[
samples_x
*
N
+
REGLEN
];
weights
=
new
double
[
samples
*
N
+
REGLEN
];
parameters_vector
=
quat0
.
clone
();
double
[]
tr_w
=
new
double
[]
{
translation_weight
,
1.0
-
translation_weight
};
double
sw
=
0
;
for
(
int
i
=
0
;
i
<
N
;
i
++)
{
if
((
vect_x
[
i
]==
null
)
||
(
vect_y
[
i
]==
null
))
{
for
(
int
j
=
0
;
j
<
samples
;
j
++)
{
x_vector
[
samples
*
i
+
j
]
=
0.0
;
y_inv_vector
[
samples
*
i
+
j
]
=
0.0
;
weights
[
samples
*
i
+
j
]
=
0.0
;
}
}
else
{
// xyz
for
(
int
j
=
0
;
j
<
3
;
j
++)
{
x_vector
[
samples_x
*
i
+
j
]
=
vect_x
[
i
][
0
][
j
];
}
// quaternions
Rotation
rot_x
=
new
Rotation
(
RotationOrder
.
YXZ
,
ErsCorrection
.
ROT_CONV
,
vect_x
[
i
][
1
][
0
],
vect_x
[
i
][
1
][
1
],
vect_x
[
i
][
1
][
2
]);
x_vector
[
samples_x
*
i
+
3
]
=
rot_x
.
getQ0
();
x_vector
[
samples_x
*
i
+
4
]
=
rot_x
.
getQ1
();
x_vector
[
samples_x
*
i
+
5
]
=
rot_x
.
getQ2
();
x_vector
[
samples_x
*
i
+
6
]
=
rot_x
.
getQ3
();
Rotation
rot_y
=
new
Rotation
(
RotationOrder
.
YXZ
,
ErsCorrection
.
ROT_CONV
,
vect_y
[
i
][
1
][
0
],
vect_y
[
i
][
1
][
1
],
vect_y
[
i
][
1
][
2
]);
// just testing - y should be all 0
double
[]
xyz_y
=
new
double
[]
{
vect_y
[
i
][
0
][
0
],
vect_y
[
i
][
0
][
1
],
vect_y
[
i
][
0
][
2
]};
double
[]
quat_y
=
new
double
[]
{
rot_y
.
getQ0
(),
rot_y
.
getQ1
(),
rot_y
.
getQ2
(),
rot_y
.
getQ3
()};
double
[][]
inv_y
=
invertTransRot
(
xyz_y
,
// double [] xyz_src, // transformation to apply to (was reference_xyz)
quat_y
);
// double [] quat_src);
double
[][]
comb_y
=
combineTransRot
(
// just to verify it is 0;
inv_y
[
0
],
// double [] xyz_src, // transformation to apply to (was reference_xyz)
inv_y
[
1
],
// double [] quat_src, // transformation to apply to (was reference_atr)
xyz_y
,
// double [] xyz_target, // to which is applied (was scene_xyz)
quat_y
);
// double [] quat_target // to which is applied (was scene_atr)
System
.
arraycopy
(
inv_y
[
0
],
0
,
y_inv_vector
,
samples_x
*
i
,
3
);
System
.
arraycopy
(
inv_y
[
1
],
0
,
y_inv_vector
,
samples_x
*
i
+
3
,
4
);
double
wt
=
translation_weight
*((
vect_w
!=
null
)?
vect_w
[
i
]
:
1.0
);
double
wr
=
0.75
*(
1.0
-
translation_weight
)*((
vect_w
!=
null
)?
vect_w
[
i
]
:
1.0
);
for
(
int
j
=
0
;
j
<
3
;
j
++)
{
weights
[
samples
*
i
+
j
]
=
wt
;
sw
+=
wt
;
}
for
(
int
j
=
0
;
j
<
4
;
j
++)
{
weights
[
samples
*
i
+
3
+
j
]
=
wr
;
sw
+=
wr
;
}
}
}
double
k
=
(
pure_weight
)/
sw
;
for
(
int
i
=
0
;
i
<
weights
.
length
;
i
++)
weights
[
i
]
*=
k
;
weights
[
samples
*
N
]
=
1.0
-
pure_weight
;
y_inv_vector
[
samples
*
N
]
=
1.0
;
last_jt
=
new
double
[
parameters_vector
.
length
][];
if
(
debug_level
>
0
)
{
debugYfX
(
"Y-INV-"
,
// String pfx,
y_inv_vector
);
// double [] data)
debugYfX
(
"PIMU-"
,
// String pfx,
x_vector
);
// double [] data)
}
}
public
void
prepareLMA
(
public
void
prepareLMA
(
int
mode
,
double
avg_height
,
//
double
avg_height
,
//
double
[][][]
vect_x
,
// []{{x,y,z},{a,t,r}}
double
[][][]
vect_x
,
// []{{x,y,z},{a,t,r}}
double
[][][]
vect_y
,
// []{{x,y,z},{a,t,r}}
double
[][][]
vect_y
,
// []{{x,y,z},{a,t,r}}
...
@@ -184,7 +290,7 @@ public class QuaternionLma {
...
@@ -184,7 +290,7 @@ public class QuaternionLma {
double
[]
quat0
,
double
[]
quat0
,
final
int
debug_level
)
{
final
int
debug_level
)
{
N
=
vect_x
.
length
;
N
=
vect_x
.
length
;
mode
=
2
;
this
.
mode
=
mode
;
//
2;
samples
=
4
;
samples
=
4
;
samples_x
=
7
;
samples_x
=
7
;
height
=
avg_height
;
height
=
avg_height
;
...
@@ -560,6 +666,10 @@ public class QuaternionLma {
...
@@ -560,6 +666,10 @@ public class QuaternionLma {
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
3
:
return
getFxDerivs6DofMode3
(
vector
,
// double [] vector,
jt
,
// final double [][] jt, // should be null or initialized with [vector.length][]
debug_level
);
// final int debug_level)
}
}
double
[]
fx
=
new
double
[
weights
.
length
];
double
[]
fx
=
new
double
[
weights
.
length
];
final
double
q0
=
vector
[
0
];
final
double
q0
=
vector
[
0
];
...
@@ -599,7 +709,6 @@ public class QuaternionLma {
...
@@ -599,7 +709,6 @@ public class QuaternionLma {
final
double
q1
=
vector
[
1
];
final
double
q1
=
vector
[
1
];
final
double
q2
=
vector
[
2
];
final
double
q2
=
vector
[
2
];
final
double
q3
=
vector
[
3
];
final
double
q3
=
vector
[
3
];
// final double [] vector_r = normSign(new double[] {-q0,q1,q2,q3}); // seems better with reversal
final
double
[]
vector_r
=
normSign
(
new
double
[]
{
q0
,
q1
,
q2
,
q3
});
final
double
[]
vector_r
=
normSign
(
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
++)
{
...
@@ -635,11 +744,9 @@ public class QuaternionLma {
...
@@ -635,11 +744,9 @@ public class QuaternionLma {
}
}
// rotations
// rotations
final
double
[]
quat_r
=
{
x_vector
[
i7
+
3
],
x_vector
[
i7
+
4
],
x_vector
[
i7
+
5
],
x_vector
[
i7
+
6
]};
final
double
[]
quat_r
=
{
x_vector
[
i7
+
3
],
x_vector
[
i7
+
4
],
x_vector
[
i7
+
5
],
x_vector
[
i7
+
6
]};
// quat_rot = compose(vector, quat_r);
quat_rot
=
composeQR_Q
(
vector_r
,
quat_r
);
quat_rot
=
composeQR_Q
(
vector_r
,
quat_r
);
System
.
arraycopy
(
quat_rot
,
0
,
fx
,
i7
+
3
,
4
);
System
.
arraycopy
(
quat_rot
,
0
,
fx
,
i7
+
3
,
4
);
if
(
jt
!=
null
)
{
if
(
jt
!=
null
)
{
// quat_dq = composeDQ(quat_r);
quat_dq
=
composeQR_QdQ
(
vector_r
,
quat_r
);
quat_dq
=
composeQR_QdQ
(
vector_r
,
quat_r
);
for
(
int
j
=
0
;
j
<
4
;
j
++)
{
for
(
int
j
=
0
;
j
<
4
;
j
++)
{
System
.
arraycopy
(
quat_dq
[
j
],
0
,
jt
[
j
],
i7
+
3
,
4
);
System
.
arraycopy
(
quat_dq
[
j
],
0
,
jt
[
j
],
i7
+
3
,
4
);
...
@@ -649,6 +756,92 @@ public class QuaternionLma {
...
@@ -649,6 +756,92 @@ public class QuaternionLma {
return
fx
;
return
fx
;
}
}
private
double
[]
getFxDerivs6DofMode3
(
double
[]
vector
,
final
double
[][]
jt
,
// should be null or initialized with [vector.length][]
final
int
debug_level
)
{
double
[]
fx
=
new
double
[
weights
.
length
];
final
double
q0
=
vector
[
0
];
final
double
q1
=
vector
[
1
];
final
double
q2
=
vector
[
2
];
final
double
q3
=
vector
[
3
];
final
double
[]
vector_r
=
normSign
(
new
double
[]
{
q0
,
q1
,
q2
,
q3
});
if
(
jt
!=
null
)
{
for
(
int
i
=
0
;
i
<
vector
.
length
;
i
++)
{
jt
[
i
]
=
new
double
[
weights
.
length
];
jt
[
i
][
samples
*
N
]
=
2
*
vector
[
i
];
}
}
fx
[
samples
*
N
]
=
q0
*
q0
+
q1
*
q1
+
q2
*
q2
+
q3
*
q3
;
double
[]
xyz_rot
;
double
[]
quat_rot
;
double
[][]
xyz_dq
;
double
[][]
quat_dq
;
double
[][]
inv_y
=
new
double
[][]
{
new
double
[
3
],
new
double
[
4
]};
for
(
int
i
=
0
;
i
<
N
;
i
++)
{
int
i7
=
samples
*
i
;
has_data:
{
for
(
int
j
=
0
;
j
<
samples
;
j
++)
{
if
(
weights
[
i7
+
j
]
>
0
)
{
break
has_data
;
}
}
continue
;
// nothing to process for this scene
}
// translations
final
double
[]
xyz
=
new
double
[]
{
x_vector
[
i7
+
0
],
x_vector
[
i7
+
1
],
x_vector
[
i7
+
2
]};
// rotations
final
double
[]
quat_r
=
{
x_vector
[
i7
+
3
],
x_vector
[
i7
+
4
],
x_vector
[
i7
+
5
],
x_vector
[
i7
+
6
]};
xyz_rot
=
applyTo
(
vector
,
xyz
);
quat_rot
=
composeQR_Q
(
vector_r
,
quat_r
);
System
.
arraycopy
(
y_inv_vector
,
i7
,
inv_y
[
0
],
0
,
3
);
System
.
arraycopy
(
y_inv_vector
,
i7
+
3
,
inv_y
[
1
],
0
,
4
);
double
[][]
comb_y
=
combineTransRot
(
//
inv_y
[
0
],
// double [] xyz_src, // transformation to apply to (was reference_xyz)
inv_y
[
1
],
// double [] quat_src, // transformation to apply to (was reference_atr)
xyz_rot
,
// double [] xyz_target, // to which is applied (was scene_xyz)
quat_rot
);
// double [] quat_target // to which is applied (was scene_atr)
System
.
arraycopy
(
comb_y
[
0
],
0
,
fx
,
i7
,
3
);
System
.
arraycopy
(
comb_y
[
1
],
0
,
fx
,
i7
+
3
,
4
);
if
(
jt
!=
null
)
{
xyz_dq
=
applyToDQ
(
vector
,
xyz
);
double
[][]
xyz_dq_local
=
new
double
[
xyz_dq
.
length
][];
for
(
int
j
=
0
;
j
<
xyz_dq
.
length
;
j
++)
{
xyz_dq_local
[
j
]
=
combineTransRot
(
null
,
// double [] xyz_src, // transformation to apply to (was reference_xyz)
inv_y
[
1
],
// double [] quat_src, // transformation to apply to (was reference_atr)
xyz_dq
[
j
],
// double [] xyz_target, // to which is applied (was scene_xyz)
null
)[
0
];
// double [] quat_target // to which is applied (was scene_atr)
}
for
(
int
j
=
0
;
j
<
4
;
j
++)
{
System
.
arraycopy
(
xyz_dq_local
[
j
],
0
,
jt
[
j
],
i7
,
3
);
}
quat_dq
=
composeQR_QdQ
(
vector_r
,
quat_r
);
double
[][]
invy_mat
=
qMat
(
inv_y
[
1
]);
double
[][]
quat_dq_local
=
mulMat
(
invy_mat
,
quat_dq
);
double
[][]
dcomp_dsecond
=
composeDR
(
inv_y
[
1
]);
double
[][]
quat_dq_local1
=
new
double
[
4
][];
for
(
int
j
=
0
;
j
<
4
;
j
++)
{
// quat_dq_local1[j] = mulMat(dcomp_dsecond, quat_dq[j]);
quat_dq_local1
[
j
]
=
mulMat
(
invy_mat
,
quat_dq
[
j
]);
}
for
(
int
j
=
0
;
j
<
4
;
j
++)
{
System
.
arraycopy
(
quat_dq_local1
[
j
],
0
,
jt
[
j
],
i7
+
3
,
4
);
}
}
}
return
fx
;
}
private
double
compareJT
(
private
double
compareJT
(
double
[]
vector
,
double
[]
vector
,
double
delta
)
{
double
delta
)
{
...
@@ -665,7 +858,7 @@ public class QuaternionLma {
...
@@ -665,7 +858,7 @@ public class QuaternionLma {
-
1
);
// final int debug_level)
-
1
);
// final int debug_level)
for
(
int
n
=
0
;
n
<
weights
.
length
;
n
++)
if
(
weights
[
n
]
>
0
)
{
for
(
int
n
=
0
;
n
<
weights
.
length
;
n
++)
if
(
weights
[
n
]
>
0
)
{
System
.
out
.
println
(
String
.
format
(
System
.
out
.
println
(
String
.
format
(
"%3d\t%1
0.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7f\t%10.7
f"
,
"%3d\t%1
2.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9f\t%12.9
f"
,
n
,
jt
[
0
][
n
],
jt
[
1
][
n
],
jt
[
2
][
n
],
jt
[
3
][
n
],
n
,
jt
[
0
][
n
],
jt
[
1
][
n
],
jt
[
2
][
n
],
jt
[
3
][
n
],
jt_delta
[
0
][
n
],
jt_delta
[
1
][
n
],
jt_delta
[
2
][
n
],
jt_delta
[
3
][
n
],
jt_delta
[
0
][
n
],
jt_delta
[
1
][
n
],
jt_delta
[
2
][
n
],
jt_delta
[
3
][
n
],
jt
[
0
][
n
]-
jt_delta
[
0
][
n
],
jt
[
1
][
n
]-
jt_delta
[
1
][
n
],
jt
[
2
][
n
]-
jt_delta
[
2
][
n
],
jt
[
3
][
n
]-
jt_delta
[
3
][
n
]));
jt
[
0
][
n
]-
jt_delta
[
0
][
n
],
jt
[
1
][
n
]-
jt_delta
[
1
][
n
],
jt
[
2
][
n
]-
jt_delta
[
2
][
n
],
jt
[
3
][
n
]-
jt_delta
[
3
][
n
]));
...
@@ -674,7 +867,7 @@ public class QuaternionLma {
...
@@ -674,7 +867,7 @@ public class QuaternionLma {
}
}
}
}
System
.
out
.
println
(
String
.
format
(
System
.
out
.
println
(
String
.
format
(
"-\t-\t-\t-\t-\t-\t-\t-\t-\t%1
0.7f\t%10.7f\t%10.7f\t%10.7
f"
,
"-\t-\t-\t-\t-\t-\t-\t-\t-\t%1
2.9f\t%12.9f\t%12.9f\t%12.9
f"
,
errors
[
0
],
errors
[
1
],
errors
[
2
],
errors
[
3
]));
errors
[
0
],
errors
[
1
],
errors
[
2
],
errors
[
3
]));
double
err
=
0
;
double
err
=
0
;
for
(
int
i
=
0
;
i
<
vector
.
length
;
i
++)
{
for
(
int
i
=
0
;
i
<
vector
.
length
;
i
++)
{
...
@@ -1127,7 +1320,8 @@ public class QuaternionLma {
...
@@ -1127,7 +1320,8 @@ public class QuaternionLma {
public
void
debugYfX
(
public
void
debugYfX
(
String
pfx
,
String
pfx
,
double
[]
data
)
{
double
[]
data
)
{
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
==
1
)
||
(
data
.
length
>=
x_vector
.
length
))
{
// different data size data[3*nscene+...]
System
.
out
.
println
(
String
.
format
(
"%3s"
+
System
.
out
.
println
(
String
.
format
(
"%3s"
+
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"
+
//x,y,z, q0,q1,q2,q3,a,t,r
"\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s\t%9s"
+
//x,y,z, q0,q1,q2,q3,a,t,r
"\t%9s\t%9s\t%9s"
,
"\t%9s\t%9s\t%9s"
,
...
@@ -1166,6 +1360,27 @@ public class QuaternionLma {
...
@@ -1166,6 +1360,27 @@ public class QuaternionLma {
return
new
String
[]
{};
return
new
String
[]
{};
}
}
public
static
double
[][]
combineTransRot
(
double
[]
xyz_src
,
// transformation to apply to (was reference_xyz)
double
[]
quat_src
,
// transformation to apply to (was reference_atr)
double
[]
xyz_target
,
// to which is applied (was scene_xyz)
double
[]
quat_target
// to which is applied (was scene_atr)
)
{
double
[]
offs
=
applyTo
(
quat_src
,
xyz_target
);
double
[]
xyz
=
(
xyz_src
==
null
)?
offs
:
addTo
(
xyz_src
,
offs
);
double
[]
quat
=
(
quat_target
==
null
)?
null
:
compose
(
quat_src
,
quat_target
);
// includes normSign()
return
new
double
[][]
{
xyz
,
quat
};
}
public
static
double
[][]
invertTransRot
(
double
[]
xyz_src
,
// transformation to apply to (was reference_xyz)
double
[]
quat_src
){
// transformation to apply to (was reference_atr)
double
[]
quat
=
normSign
(
new
double
[]
{-
quat_src
[
0
],
quat_src
[
1
],
quat_src
[
2
],
quat_src
[
3
]});
double
[]
xyz
=
applyTo
(
quat
,
new
double
[]
{-
xyz_src
[
0
],-
xyz_src
[
1
],-
xyz_src
[
2
]});
return
new
double
[][]
{
xyz
,
quat
};
}
/**
/**
* Apply quaternion q to quaternion r
* Apply quaternion q to quaternion r
* @param q - 4 components (scalar, vector) of the quaternion to apply to the other one
* @param q - 4 components (scalar, vector) of the quaternion to apply to the other one
...
@@ -1359,6 +1574,16 @@ public class QuaternionLma {
...
@@ -1359,6 +1574,16 @@ public class QuaternionLma {
{-
q
[
3
],-
q
[
2
],
q
[
1
],
q
[
0
]}};
{-
q
[
3
],-
q
[
2
],
q
[
1
],
q
[
0
]}};
}
}
public
static
double
[]
addTo
(
double
[]
offs
,
double
[]
xyz
)
{
return
new
double
[]
{
offs
[
0
]+
xyz
[
0
],
offs
[
1
]+
xyz
[
1
],
offs
[
2
]+
xyz
[
2
]};
}
/**
/**
* Apply quaternion to a 3D vector
* Apply quaternion to a 3D vector
* @param q 4 components (scalar, vector) of the quaternion being applied
* @param q 4 components (scalar, vector) of the quaternion being applied
...
@@ -1407,4 +1632,41 @@ public class QuaternionLma {
...
@@ -1407,4 +1632,41 @@ public class QuaternionLma {
{-
2
*
xyz
[
2
]*
q
[
0
]+
2
*
xyz
[
1
]*
q
[
1
],
2
*
s
+
2
*
xyz
[
1
]*
q
[
2
],
2
*
xyz
[
0
]*
q
[
0
]+
2
*
xyz
[
1
]*
q
[
3
]},
{-
2
*
xyz
[
2
]*
q
[
0
]+
2
*
xyz
[
1
]*
q
[
1
],
2
*
s
+
2
*
xyz
[
1
]*
q
[
2
],
2
*
xyz
[
0
]*
q
[
0
]+
2
*
xyz
[
1
]*
q
[
3
]},
{
2
*
xyz
[
1
]*
q
[
0
]+
2
*
xyz
[
2
]*
q
[
1
],
-
2
*
xyz
[
0
]*
q
[
0
]+
2
*
xyz
[
2
]*
q
[
2
],
2
*
s
+
2
*
xyz
[
2
]*
q
[
3
]}};
{
2
*
xyz
[
1
]*
q
[
0
]+
2
*
xyz
[
2
]*
q
[
1
],
-
2
*
xyz
[
0
]*
q
[
0
]+
2
*
xyz
[
2
]*
q
[
2
],
2
*
s
+
2
*
xyz
[
2
]*
q
[
3
]}};
}
}
public
static
double
[]
mulMat
(
double
[][]
mat
,
double
[]
vect
)
{
double
[]
rslt
=
new
double
[
mat
.
length
];
for
(
int
i
=
0
;
i
<
mat
.
length
;
i
++)
{
for
(
int
j
=
0
;
j
<
vect
.
length
;
j
++)
{
rslt
[
i
]+=
mat
[
i
][
j
]*
vect
[
j
];
}
}
return
rslt
;
}
public
static
double
[][]
mulMat
(
double
[][]
mat
,
double
[][]
mat1
)
{
double
[][]
rslt
=
new
double
[
mat
.
length
][
mat1
[
0
].
length
];
for
(
int
i
=
0
;
i
<
rslt
.
length
;
i
++)
{
for
(
int
j
=
0
;
j
<
rslt
[
0
].
length
;
j
++)
{
for
(
int
k
=
0
;
k
<
mat
[
0
].
length
;
k
++)
{
rslt
[
i
][
j
]
+=
mat
[
i
][
k
]*
mat1
[
k
][
j
];
}
}
}
return
rslt
;
}
public
static
double
[][]
qMat
(
double
[]
q
){
return
new
double
[][]
{
{
q
[
0
],-
q
[
1
],-
q
[
2
],-
q
[
3
]},
{
q
[
1
],
q
[
0
],
q
[
3
],-
q
[
2
]},
{
q
[
2
],-
q
[
3
],
q
[
0
],
q
[
1
]},
{
q
[
3
],
q
[
2
],-
q
[
1
],
q
[
0
]}};
}
}
}
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