Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
wolf
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Container Registry
Model registry
Operate
Environments
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
mobile_robotics
wolf_projects
wolf_lib
wolf
Merge requests
!178
Add three_D_tools.h
Code
Review changes
Check out branch
Download
Patches
Plain diff
Merged
Add three_D_tools.h
newfile
into
master
Overview
0
Commits
1
Pipelines
1
Changes
1
Merged
Joan Solà Ortega
requested to merge
newfile
into
master
7 years ago
Overview
0
Commits
1
Pipelines
1
Changes
1
Expand
0
0
Merge request reports
Compare
master
master (base)
and
latest version
latest version
7b0f0402
1 commit,
7 years ago
1 file
+
383
−
0
Inline
Compare changes
Side-by-side
Inline
Show whitespace changes
Show one file at a time
src/three_D_tools.h
0 → 100644
+
383
−
0
Options
/*
* three_D_tools.h
*
* Created on: Mar 15, 2018
* Author: jsola
*/
#ifndef THREE_D_TOOLS_H_
#define THREE_D_TOOLS_H_
#include
"wolf.h"
#include
"rotations.h"
/*
* Most functions in this file are explained in the document:
*
* Joan Sola, "IMU pre-integration", 2015-2017 IRI-CSIC
*
* They relate manipulations of Delta motion magnitudes used for IMU pre-integration.
*
* The Delta is defined as
* Delta = [Dp, Dq, Dv]
* with
* Dp : position delta
* Dq : quaternion delta
* Dv : velocity delta
*
* They are listed below:
*
* - identity: I = Delta at the origin, with Dp = [0,0,0]; Dq = [0,0,0,1], Dv = [0,0,0]
* - inverse: so that D (+) D.inv = I
* - compose: Dc = D1 (+) D2
* - between: Db = D2 (-) D1, so that D2 = D1 (+) Db
* - composeOverState: x2 = x1 (+) D
* - betweenStates: D = x2 (-) x1, so that x2 = x1 (+) D
* - lift: got from Delta manifold to tangent space (equivalent to log() in rotations)
* - retract: go from tangent space to delta manifold (equivalent to exp() in rotations)
* - plus: D2 = D1 (+) retract(d)
* - diff: d = lift( D2 (-) D1 )
* - body2delta: construct a delta from body magnitudes of linAcc and angVel
*/
namespace
wolf
{
namespace
three_D
{
using
namespace
Eigen
;
template
<
typename
D1
,
typename
D2
>
inline
void
identity
(
MatrixBase
<
D1
>&
p
,
QuaternionBase
<
D2
>&
q
)
{
p
=
MatrixBase
<
D1
>::
Zero
(
3
,
1
);
q
=
QuaternionBase
<
D2
>::
Identity
();
}
template
<
typename
D1
,
typename
D2
>
inline
void
identity
(
MatrixBase
<
D1
>&
p
,
MatrixBase
<
D2
>&
q
)
{
typedef
typename
D1
::
Scalar
T1
;
typedef
typename
D2
::
Scalar
T2
;
p
<<
T1
(
0
),
T1
(
0
),
T1
(
0
);
q
<<
T2
(
0
),
T2
(
0
),
T2
(
0
),
T2
(
1
);
}
template
<
typename
T
=
wolf
::
Scalar
>
inline
Matrix
<
T
,
7
,
1
>
identity
()
{
Matrix
<
T
,
7
,
1
>
ret
;
ret
<<
T
(
0
),
T
(
0
),
T
(
0
),
T
(
0
),
T
(
0
),
T
(
0
),
T
(
1
);
return
ret
;
}
template
<
typename
D1
,
typename
D2
,
typename
D4
,
typename
D5
>
inline
void
inverse
(
const
MatrixBase
<
D1
>&
dp
,
const
QuaternionBase
<
D2
>&
dq
,
MatrixBase
<
D4
>&
idp
,
QuaternionBase
<
D5
>&
idq
)
{
MatrixSizeCheck
<
3
,
1
>::
check
(
dp
);
MatrixSizeCheck
<
3
,
1
>::
check
(
idp
);
idp
=
-
(
dq
.
conjugate
()
*
dp
);
idq
=
dq
.
conjugate
();
}
template
<
typename
D1
,
typename
D2
>
inline
void
inverse
(
const
MatrixBase
<
D1
>&
d
,
MatrixBase
<
D2
>&
id
)
{
MatrixSizeCheck
<
7
,
1
>::
check
(
d
);
MatrixSizeCheck
<
7
,
1
>::
check
(
id
);
Map
<
const
Matrix
<
typename
D1
::
Scalar
,
3
,
1
>
>
dp
(
&
d
(
0
)
);
Map
<
const
Quaternion
<
typename
D1
::
Scalar
>
>
dq
(
&
d
(
3
)
);
Map
<
Matrix
<
typename
D2
::
Scalar
,
3
,
1
>
>
idp
(
&
id
(
0
)
);
Map
<
Quaternion
<
typename
D2
::
Scalar
>
>
idq
(
&
id
(
3
)
);
inverse
(
dp
,
dq
,
idp
,
idq
);
}
template
<
typename
D
>
inline
Matrix
<
typename
D
::
Scalar
,
7
,
1
>
inverse
(
const
MatrixBase
<
D
>&
d
)
{
Matrix
<
typename
D
::
Scalar
,
7
,
1
>
id
;
inverse
(
d
,
id
);
return
id
;
}
template
<
typename
D1
,
typename
D2
,
typename
D4
,
typename
D5
,
typename
D7
,
typename
D8
,
typename
D6
>
inline
void
compose
(
const
MatrixBase
<
D1
>&
dp1
,
const
QuaternionBase
<
D2
>&
dq1
,
const
MatrixBase
<
D4
>&
dp2
,
const
QuaternionBase
<
D5
>&
dq2
,
MatrixBase
<
D7
>&
sum_p
,
QuaternionBase
<
D8
>&
sum_q
)
{
MatrixSizeCheck
<
3
,
1
>::
check
(
dp1
);
MatrixSizeCheck
<
3
,
1
>::
check
(
dp2
);
MatrixSizeCheck
<
3
,
1
>::
check
(
sum_p
);
sum_p
=
dp1
+
dq1
*
dp2
;
sum_q
=
dq1
*
dq2
;
// dq here to avoid possible aliasing between d1 and sum
}
template
<
typename
D1
,
typename
D2
,
typename
D3
>
inline
void
compose
(
const
MatrixBase
<
D1
>&
d1
,
const
MatrixBase
<
D2
>&
d2
,
MatrixBase
<
D3
>&
sum
)
{
MatrixSizeCheck
<
7
,
1
>::
check
(
d1
);
MatrixSizeCheck
<
7
,
1
>::
check
(
d2
);
MatrixSizeCheck
<
7
,
1
>::
check
(
sum
);
Map
<
const
Matrix
<
typename
D1
::
Scalar
,
3
,
1
>
>
dp1
(
&
d1
(
0
)
);
Map
<
const
Quaternion
<
typename
D1
::
Scalar
>
>
dq1
(
&
d1
(
3
)
);
Map
<
const
Matrix
<
typename
D2
::
Scalar
,
3
,
1
>
>
dp2
(
&
d2
(
0
)
);
Map
<
const
Quaternion
<
typename
D2
::
Scalar
>
>
dq2
(
&
d2
(
3
)
);
Map
<
Matrix
<
typename
D3
::
Scalar
,
3
,
1
>
>
sum_p
(
&
sum
(
0
)
);
Map
<
Quaternion
<
typename
D3
::
Scalar
>
>
sum_q
(
&
sum
(
3
)
);
compose
(
dp1
,
dq1
,
dp2
,
dq2
,
sum_p
,
sum_q
);
}
template
<
typename
D1
,
typename
D2
>
inline
Matrix
<
typename
D1
::
Scalar
,
7
,
1
>
compose
(
const
MatrixBase
<
D1
>&
d1
,
const
MatrixBase
<
D2
>&
d2
)
{
Matrix
<
typename
D1
::
Scalar
,
7
,
1
>
ret
;
compose
(
d1
,
d2
,
ret
);
return
ret
;
}
template
<
typename
D1
,
typename
D2
,
typename
D3
,
typename
D4
,
typename
D5
>
inline
void
compose
(
const
MatrixBase
<
D1
>&
d1
,
const
MatrixBase
<
D2
>&
d2
,
MatrixBase
<
D3
>&
sum
,
MatrixBase
<
D4
>&
J_sum_d1
,
MatrixBase
<
D5
>&
J_sum_d2
)
{
MatrixSizeCheck
<
7
,
1
>::
check
(
d1
);
MatrixSizeCheck
<
7
,
1
>::
check
(
d2
);
MatrixSizeCheck
<
7
,
1
>::
check
(
sum
);
MatrixSizeCheck
<
6
,
6
>::
check
(
J_sum_d1
);
MatrixSizeCheck
<
6
,
6
>::
check
(
J_sum_d2
);
// Some useful temporaries
Matrix
<
typename
D1
::
Scalar
,
3
,
3
>
dR1
=
q2R
(
d1
.
segment
(
3
,
4
));
//dq1.matrix(); // First Delta, DR
Matrix
<
typename
D2
::
Scalar
,
3
,
3
>
dR2
=
q2R
(
d2
.
segment
(
3
,
4
));
//dq2.matrix(); // Second delta, dR
// Jac wrt first delta
J_sum_d1
.
setIdentity
();
// dDp'/dDp = dDv'/dDv = I
J_sum_d1
.
block
(
0
,
3
,
3
,
3
).
noalias
()
=
-
dR1
*
skew
(
d2
.
head
(
3
))
;
// dDp'/dDo
J_sum_d1
.
block
(
3
,
3
,
3
,
3
)
=
dR2
.
transpose
();
// dDo'/dDo
// Jac wrt second delta
J_sum_d2
.
setIdentity
();
//
J_sum_d2
.
block
(
0
,
0
,
3
,
3
)
=
dR1
;
// dDp'/ddp
// J_sum_d2.block(3,3,3,3) = Matrix3s::Identity(); // dDo'/ddo = I
// compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
compose
(
d1
,
d2
,
sum
);
}
template
<
typename
D1
,
typename
D2
,
typename
D4
,
typename
D5
,
typename
D7
,
typename
D8
,
typename
D6
>
inline
void
between
(
const
MatrixBase
<
D1
>&
dp1
,
const
QuaternionBase
<
D2
>&
dq1
,
const
MatrixBase
<
D4
>&
dp2
,
const
QuaternionBase
<
D5
>&
dq2
,
MatrixBase
<
D7
>&
diff_p
,
QuaternionBase
<
D8
>&
diff_q
)
{
MatrixSizeCheck
<
3
,
1
>::
check
(
dp1
);
MatrixSizeCheck
<
3
,
1
>::
check
(
dp2
);
MatrixSizeCheck
<
3
,
1
>::
check
(
diff_p
);
diff_p
=
dq1
.
conjugate
()
*
(
dp2
-
dp1
);
diff_q
=
dq1
.
conjugate
()
*
dq2
;
}
template
<
typename
D1
,
typename
D2
,
typename
D3
>
inline
void
between
(
const
MatrixBase
<
D1
>&
d1
,
const
MatrixBase
<
D2
>&
d2
,
MatrixBase
<
D3
>&
d2_minus_d1
)
{
MatrixSizeCheck
<
7
,
1
>::
check
(
d1
);
MatrixSizeCheck
<
7
,
1
>::
check
(
d2
);
MatrixSizeCheck
<
7
,
1
>::
check
(
d2_minus_d1
);
Map
<
const
Matrix
<
typename
D1
::
Scalar
,
3
,
1
>
>
dp1
(
&
d1
(
0
)
);
Map
<
const
Quaternion
<
typename
D1
::
Scalar
>
>
dq1
(
&
d1
(
3
)
);
Map
<
const
Matrix
<
typename
D2
::
Scalar
,
3
,
1
>
>
dp2
(
&
d2
(
0
)
);
Map
<
const
Quaternion
<
typename
D2
::
Scalar
>
>
dq2
(
&
d2
(
3
)
);
Map
<
Matrix
<
typename
D3
::
Scalar
,
3
,
1
>
>
diff_p
(
&
d2_minus_d1
(
0
)
);
Map
<
Quaternion
<
typename
D3
::
Scalar
>
>
diff_q
(
&
d2_minus_d1
(
3
)
);
between
(
dp1
,
dq1
,
dp2
,
dq2
,
diff_p
,
diff_q
);
}
template
<
typename
D1
,
typename
D2
>
inline
Matrix
<
typename
D1
::
Scalar
,
7
,
1
>
between
(
const
MatrixBase
<
D1
>&
d1
,
const
MatrixBase
<
D2
>&
d2
)
{
Matrix
<
typename
D1
::
Scalar
,
7
,
1
>
diff
;
between
(
d1
,
d2
,
diff
);
return
diff
;
}
template
<
typename
Derived
>
Matrix
<
typename
Derived
::
Scalar
,
6
,
1
>
lift
(
const
MatrixBase
<
Derived
>&
delta_in
)
{
MatrixSizeCheck
<
7
,
1
>::
check
(
delta_in
);
Matrix
<
typename
Derived
::
Scalar
,
6
,
1
>
ret
;
Map
<
const
Matrix
<
typename
Derived
::
Scalar
,
3
,
1
>
>
dp_in
(
&
delta_in
(
0
)
);
Map
<
const
Quaternion
<
typename
Derived
::
Scalar
>
>
dq_in
(
&
delta_in
(
3
)
);
Map
<
Matrix
<
typename
Derived
::
Scalar
,
3
,
1
>
>
dp_ret
(
&
ret
(
0
)
);
Map
<
Matrix
<
typename
Derived
::
Scalar
,
3
,
1
>
>
do_ret
(
&
ret
(
3
)
);
dp_ret
=
dp_in
;
do_ret
=
log_q
(
dq_in
);
return
ret
;
}
template
<
typename
Derived
>
Matrix
<
typename
Derived
::
Scalar
,
7
,
1
>
retract
(
const
MatrixBase
<
Derived
>&
d_in
)
{
MatrixSizeCheck
<
6
,
1
>::
check
(
d_in
);
Matrix
<
typename
Derived
::
Scalar
,
7
,
1
>
ret
;
Map
<
const
Matrix
<
typename
Derived
::
Scalar
,
3
,
1
>
>
dp_in
(
&
d_in
(
0
)
);
Map
<
const
Matrix
<
typename
Derived
::
Scalar
,
3
,
1
>
>
do_in
(
&
d_in
(
3
)
);
Map
<
Matrix
<
typename
Derived
::
Scalar
,
3
,
1
>
>
dp
(
&
ret
(
0
)
);
Map
<
Quaternion
<
typename
Derived
::
Scalar
>
>
dq
(
&
ret
(
3
)
);
dp
=
dp_in
;
dq
=
exp_q
(
do_in
);
return
ret
;
}
template
<
typename
D1
,
typename
D2
,
typename
D4
,
typename
D5
,
typename
D7
,
typename
D8
,
typename
D6
>
inline
void
plus
(
const
MatrixBase
<
D1
>&
dp1
,
const
QuaternionBase
<
D2
>&
dq1
,
const
MatrixBase
<
D4
>&
dp2
,
const
MatrixBase
<
D5
>&
do2
,
MatrixBase
<
D7
>&
plus_p
,
QuaternionBase
<
D8
>&
plus_q
)
{
plus_p
=
dp1
+
dp2
;
plus_q
=
dq1
*
exp_q
(
do2
);
}
template
<
typename
D1
,
typename
D2
,
typename
D3
>
inline
void
plus
(
const
MatrixBase
<
D1
>&
d1
,
const
MatrixBase
<
D2
>&
d2
,
MatrixBase
<
D3
>&
d_pert
)
{
Map
<
const
Matrix
<
typename
D1
::
Scalar
,
3
,
1
>
>
dp1
(
&
d1
(
0
)
);
Map
<
const
Quaternion
<
typename
D1
::
Scalar
>
>
dq1
(
&
d1
(
3
)
);
Map
<
const
Matrix
<
typename
D2
::
Scalar
,
3
,
1
>
>
dp2
(
&
d2
(
0
)
);
Map
<
const
Matrix
<
typename
D2
::
Scalar
,
3
,
1
>
>
do2
(
&
d2
(
3
)
);
Map
<
Matrix
<
typename
D3
::
Scalar
,
3
,
1
>
>
dp_p
(
&
d_pert
(
0
)
);
Map
<
Quaternion
<
typename
D3
::
Scalar
>
>
dq_p
(
&
d_pert
(
3
)
);
plus
(
dp1
,
dq1
,
dp2
,
do2
,
dp_p
,
dq_p
);
}
template
<
typename
D1
,
typename
D2
>
inline
Matrix
<
typename
D1
::
Scalar
,
7
,
1
>
plus
(
const
MatrixBase
<
D1
>&
d1
,
const
MatrixBase
<
D2
>&
d2
)
{
Matrix
<
typename
D1
::
Scalar
,
7
,
1
>
ret
;
plus
(
d1
,
d2
,
ret
);
return
ret
;
}
template
<
typename
D1
,
typename
D2
,
typename
D4
,
typename
D5
,
typename
D7
,
typename
D8
>
inline
void
diff
(
const
MatrixBase
<
D1
>&
dp1
,
const
QuaternionBase
<
D2
>&
dq1
,
const
MatrixBase
<
D4
>&
dp2
,
const
QuaternionBase
<
D5
>&
dq2
,
MatrixBase
<
D7
>&
diff_p
,
MatrixBase
<
D8
>&
diff_o
)
{
diff_p
=
dp2
-
dp1
;
diff_o
=
log_q
(
dq1
.
conjugate
()
*
dq2
);
}
template
<
typename
D1
,
typename
D2
,
typename
D4
,
typename
D5
,
typename
D6
,
typename
D7
,
typename
D8
,
typename
D9
>
inline
void
diff
(
const
MatrixBase
<
D1
>&
dp1
,
const
QuaternionBase
<
D2
>&
dq1
,
const
MatrixBase
<
D4
>&
dp2
,
const
QuaternionBase
<
D5
>&
dq2
,
MatrixBase
<
D6
>&
diff_p
,
MatrixBase
<
D7
>&
diff_o
,
MatrixBase
<
D8
>&
J_do_dq1
,
MatrixBase
<
D9
>&
J_do_dq2
)
{
diff
(
dp1
,
dq1
,
dp2
,
dq2
,
diff_p
,
diff_o
);
J_do_dq1
=
-
jac_SO3_left_inv
(
diff_o
);
J_do_dq2
=
jac_SO3_right_inv
(
diff_o
);
}
template
<
typename
D1
,
typename
D2
,
typename
D3
>
inline
void
diff
(
const
MatrixBase
<
D1
>&
d1
,
const
MatrixBase
<
D2
>&
d2
,
MatrixBase
<
D3
>&
err
)
{
Map
<
const
Matrix
<
typename
D1
::
Scalar
,
3
,
1
>
>
dp1
(
&
d1
(
0
)
);
Map
<
const
Quaternion
<
typename
D1
::
Scalar
>
>
dq1
(
&
d1
(
3
)
);
Map
<
const
Matrix
<
typename
D2
::
Scalar
,
3
,
1
>
>
dp2
(
&
d2
(
0
)
);
Map
<
const
Quaternion
<
typename
D2
::
Scalar
>
>
dq2
(
&
d2
(
3
)
);
Map
<
Matrix
<
typename
D3
::
Scalar
,
3
,
1
>
>
diff_p
(
&
err
(
0
)
);
Map
<
Matrix
<
typename
D3
::
Scalar
,
3
,
1
>
>
diff_o
(
&
err
(
3
)
);
diff
(
dp1
,
dq1
,
dp2
,
dq2
,
diff_p
,
diff_o
);
}
template
<
typename
D1
,
typename
D2
,
typename
D3
,
typename
D4
,
typename
D5
>
inline
void
diff
(
const
MatrixBase
<
D1
>&
d1
,
const
MatrixBase
<
D2
>&
d2
,
MatrixBase
<
D3
>&
dif
,
MatrixBase
<
D4
>&
J_diff_d1
,
MatrixBase
<
D5
>&
J_diff_d2
)
{
Map
<
const
Matrix
<
typename
D1
::
Scalar
,
3
,
1
>
>
dp1
(
&
d1
(
0
)
);
Map
<
const
Quaternion
<
typename
D1
::
Scalar
>
>
dq1
(
&
d1
(
3
)
);
Map
<
const
Matrix
<
typename
D1
::
Scalar
,
3
,
1
>
>
dv1
(
&
d1
(
7
)
);
Map
<
const
Matrix
<
typename
D2
::
Scalar
,
3
,
1
>
>
dp2
(
&
d2
(
0
)
);
Map
<
const
Quaternion
<
typename
D2
::
Scalar
>
>
dq2
(
&
d2
(
3
)
);
Map
<
const
Matrix
<
typename
D2
::
Scalar
,
3
,
1
>
>
dv2
(
&
d2
(
7
)
);
Map
<
Matrix
<
typename
D3
::
Scalar
,
3
,
1
>
>
diff_p
(
&
dif
(
0
)
);
Map
<
Matrix
<
typename
D3
::
Scalar
,
3
,
1
>
>
diff_o
(
&
dif
(
3
)
);
Map
<
Matrix
<
typename
D3
::
Scalar
,
3
,
1
>
>
diff_v
(
&
dif
(
6
)
);
Matrix
<
typename
D4
::
Scalar
,
3
,
3
>
J_do_dq1
,
J_do_dq2
;
diff
(
dp1
,
dq1
,
dv1
,
dp2
,
dq2
,
dv2
,
diff_p
,
diff_o
,
diff_v
,
J_do_dq1
,
J_do_dq2
);
/* d = diff(d1, d2) is
* dp = dp2 - dp1
* do = Log(dq1.conj * dq2)
* dv = dv2 - dv1
*
* With trivial Jacobians for dp and dv, and:
* J_do_dq1 = - J_l_inv(theta)
* J_do_dq2 = J_r_inv(theta)
*/
J_diff_d1
=
-
Matrix
<
typename
D4
::
Scalar
,
6
,
6
>::
Identity
();
// d(p2 - p1) / d(p1) = - Identity
J_diff_d1
.
block
(
3
,
3
,
3
,
3
)
=
J_do_dq1
;
// d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
J_diff_d2
.
setIdentity
();
// d(R1.tr*R2) / d(R2) = Identity
J_diff_d2
.
block
(
3
,
3
,
3
,
3
)
=
J_do_dq2
;
// d(R1.tr*R2) / d(R1) = J_r_inv(theta)
}
template
<
typename
D1
,
typename
D2
>
inline
Matrix
<
typename
D1
::
Scalar
,
6
,
1
>
diff
(
const
MatrixBase
<
D1
>&
d1
,
const
MatrixBase
<
D2
>&
d2
)
{
Matrix
<
typename
D1
::
Scalar
,
6
,
1
>
ret
;
diff
(
d1
,
d2
,
ret
);
return
ret
;
}
}
// namespace three_d
}
// namespace wolf
#endif
/* THREE_D_TOOLS_H_ */
Loading