Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
I
imu
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Deploy
Releases
Model registry
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor 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
plugins
imu
Commits
40f18270
Commit
40f18270
authored
5 years ago
by
Médéric Fourmy
Browse files
Options
Downloads
Patches
Plain Diff
Added necessary files, adapting the functions in the API one by one, WIP
parent
41af6a35
No related branches found
No related tags found
No related merge requests found
Changes
4
Hide whitespace changes
Inline
Side-by-side
Showing
4 changed files
CMakeLists.txt
+6
-0
6 additions, 0 deletions
CMakeLists.txt
include/IMU/math/IMU_tools_Lie.h
+702
-0
702 additions, 0 deletions
include/IMU/math/IMU_tools_Lie.h
test/CMakeLists.txt
+3
-0
3 additions, 0 deletions
test/CMakeLists.txt
test/gtest_IMU_tools_Lie.cpp
+636
-0
636 additions, 0 deletions
test/gtest_IMU_tools_Lie.cpp
with
1347 additions
and
0 deletions
CMakeLists.txt
+
6
−
0
View file @
40f18270
...
...
@@ -129,6 +129,9 @@ SET(HDRS_COMMON
SET
(
HDRS_MATH
include/IMU/math/IMU_tools.h
)
SET
(
HDRS_MATH
include/IMU/math/IMU_tools_Lie.h
)
SET
(
HDRS_UTILS
)
SET
(
HDRS_CAPTURE
...
...
@@ -160,6 +163,9 @@ SET(SRCS_COMMON
SET
(
SRCS_MATH
include/IMU/math/IMU_tools.h
)
SET
(
SRCS_MATH
include/IMU/math/IMU_tools_Lie.h
)
SET
(
SRCS_UTILS
)
...
...
This diff is collapsed.
Click to expand it.
include/IMU/math/IMU_tools_Lie.h
0 → 100644
+
702
−
0
View file @
40f18270
/*
* imu_tools_Lie.h
*
* Created on: Oct 7, 2019
* Author: mfourmy
*/
#ifndef IMU_TOOLS_LIE_H_
#define IMU_TOOLS_LIE_H_
#include
"core/common/wolf.h"
#include
"core/math/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
* - log: got from Delta manifold to tangent space (equivalent to log() in rotations)
* - exp_IMU: go from tangent space to delta manifold (equivalent to exp() in rotations)
* - plus: D2 = D1 (+) exp_IMU(d)
* - diff: d = log_IMU( D2 (-) D1 )
* - body2delta: construct a delta from body magnitudes of linAcc and angVel
*/
namespace
wolf
{
namespace
imu_with_dt
{
using
namespace
Eigen
;
template
<
typename
D1
,
typename
D2
,
typename
D3
,
typename
T
>
inline
void
identity
(
MatrixBase
<
D1
>&
p
,
QuaternionBase
<
D2
>&
q
,
MatrixBase
<
D3
>&
v
,
T
&
dt
)
{
// Same as imu_tools.h with dt added to the state
p
=
MatrixBase
<
D1
>::
Zero
(
3
,
1
);
q
=
QuaternionBase
<
D2
>::
Identity
();
v
=
MatrixBase
<
D3
>::
Zero
(
3
,
1
);
dt
=
T
(
0
);
}
template
<
typename
D1
,
typename
D2
,
typename
D3
,
typename
T
>
inline
void
identity
(
MatrixBase
<
D1
>&
p
,
MatrixBase
<
D2
>&
q
,
MatrixBase
<
D3
>&
v
,
T
&
dt
)
{
typedef
typename
D1
::
Scalar
T1
;
typedef
typename
D2
::
Scalar
T2
;
typedef
typename
D3
::
Scalar
T3
;
p
<<
T1
(
0
),
T1
(
0
),
T1
(
0
);
q
<<
T2
(
0
),
T2
(
0
),
T2
(
0
),
T2
(
1
);
v
<<
T3
(
0
),
T3
(
0
),
T3
(
0
);
dt
=
T
(
0
);
}
template
<
typename
T
=
Scalar
>
inline
Matrix
<
T
,
11
,
1
>
identity
()
{
Matrix
<
T
,
11
,
1
>
ret
;
ret
<<
T
(
0
),
T
(
0
),
T
(
0
),
T
(
0
),
T
(
0
),
T
(
0
),
T
(
1
),
T
(
0
),
T
(
0
),
T
(
0
),
T
(
0
);
return
ret
;
}
template
<
typename
D1
,
typename
D2
>
inline
void
adjoint
(
const
MatrixBase
<
D1
>
d
,
MatrixBase
<
D1
>
adjd
)
{
// Adjoint matrix associated to the adjoint operator
Matrix
<
typename
D1
::
Scalar
,
3
,
1
>
dp
=
d
.
segment
<
3
>
(
0
);
Matrix
<
typename
D1
::
Scalar
,
3
,
3
>
dR
=
q2R
(
d
.
segment
<
4
>
(
3
));
Matrix
<
typename
D1
::
Scalar
,
3
,
1
>
dv
=
d
.
segment
<
3
>
(
7
);
typename
D1
::
Scalar
dt
=
d
(
10
);
adjd
.
setIdentity
();
adjd
<
3
,
3
>
(
0
,
0
)
=
dR
;
adjd
<
3
,
3
>
(
0
,
3
)
=
skew
(
dp
-
dv
*
dt
)
*
dR
;
adjd
<
3
,
3
>
(
0
,
6
)
=
-
dR
*
dt
;
adjd
<
3
,
1
>
(
0
,
9
)
=
dv
;
adjd
<
3
,
3
>
(
3
,
3
)
=
dR
;
adjd
<
3
,
3
>
(
6
,
3
)
=
skew
(
dv
)
*
dR
;
adjd
<
3
,
3
>
(
6
,
6
)
=
dR
;
}
template
<
typename
D
>
inline
Matrix
<
typename
D
::
Scalar
,
10
,
10
>
adjoint
(
const
MatrixBase
<
D
>
d
){
Matrix
<
typename
D
::
Scalar
,
10
,
10
>
adjd
;
adjoint
(
d
,
adjd
);
return
adjd
;
}
// template<typename D1, typename D2>
// inline Matrix<T, 11, 1> smalladjoint(const MatrixBase<D1> d, MatrixBase<D1> sadjd)
// {
// // Adjoint matrix associated to the adjoint operator
// TODO
// return ret;
// }
template
<
typename
D1
,
typename
D2
,
typename
D3
,
typename
D4
,
typename
D5
,
typename
D6
,
typename
T
>
inline
void
inverse
(
const
MatrixBase
<
D1
>&
dp
,
const
QuaternionBase
<
D2
>&
dq
,
const
MatrixBase
<
D3
>&
dv
,
const
T
&
dt
,
MatrixBase
<
D4
>&
idp
,
QuaternionBase
<
D5
>&
idq
,
MatrixBase
<
D6
>&
idv
,
T
&
idt
)
{
// Same as imu_tools.h
MatrixSizeCheck
<
3
,
1
>::
check
(
dp
);
MatrixSizeCheck
<
3
,
1
>::
check
(
dv
);
MatrixSizeCheck
<
3
,
1
>::
check
(
idp
);
MatrixSizeCheck
<
3
,
1
>::
check
(
idv
);
idp
=
-
(
dq
.
conjugate
()
*
(
dp
-
dv
*
typename
D3
::
Scalar
(
dt
)
)
);
idq
=
dq
.
conjugate
();
idv
=
-
(
dq
.
conjugate
()
*
dv
);
idt
=
-
dt
;
}
template
<
typename
D1
,
typename
D2
>
inline
void
inverse
(
const
MatrixBase
<
D1
>&
d
,
MatrixBase
<
D2
>&
id
)
{
MatrixSizeCheck
<
11
,
1
>::
check
(
d
);
MatrixSizeCheck
<
11
,
1
>::
check
(
id
);
typedef
typename
D1
::
Scalar
T
;
Map
<
const
Matrix
<
typename
D1
::
Scalar
,
3
,
1
>
>
dp
(
&
d
(
0
)
);
Map
<
const
Quaternion
<
typename
D1
::
Scalar
>
>
dq
(
&
d
(
3
)
);
Map
<
const
Matrix
<
typename
D1
::
Scalar
,
3
,
1
>
>
dv
(
&
d
(
7
)
);
const
T
&
dt
=
d
(
10
)
;
Map
<
Matrix
<
typename
D2
::
Scalar
,
3
,
1
>
>
idp
(
&
id
(
0
)
);
Map
<
Quaternion
<
typename
D2
::
Scalar
>
>
idq
(
&
id
(
3
)
);
Map
<
Matrix
<
typename
D2
::
Scalar
,
3
,
1
>
>
idv
(
&
id
(
7
)
);
T
&
idt
=
id
(
10
)
;
inverse
(
dp
,
dq
,
dv
,
dt
,
idp
,
idq
,
idv
,
idt
);
}
template
<
typename
D
>
inline
Matrix
<
typename
D
::
Scalar
,
11
,
1
>
inverse
(
const
MatrixBase
<
D
>&
d
)
{
Matrix
<
typename
D
::
Scalar
,
11
,
1
>
id
;
inverse
(
d
,
id
);
return
id
;
}
template
<
typename
D1
,
typename
D2
,
typename
D3
,
typename
D4
,
typename
D5
,
typename
D6
,
typename
D7
,
typename
D8
,
typename
D9
,
typename
T
>
inline
void
compose
(
const
MatrixBase
<
D1
>&
dp1
,
const
QuaternionBase
<
D2
>&
dq1
,
const
MatrixBase
<
D3
>&
dv1
,
const
T
&
dt1
,
const
MatrixBase
<
D4
>&
dp2
,
const
QuaternionBase
<
D5
>&
dq2
,
const
MatrixBase
<
D6
>&
dv2
,
const
T
&
dt2
,
MatrixBase
<
D7
>&
sum_p
,
QuaternionBase
<
D8
>&
sum_q
,
MatrixBase
<
D9
>&
sum_v
,
T
&
sum_dt
)
{
// Same as imu_tools.h
MatrixSizeCheck
<
3
,
1
>::
check
(
dp1
);
MatrixSizeCheck
<
3
,
1
>::
check
(
dv1
);
MatrixSizeCheck
<
3
,
1
>::
check
(
dp2
);
MatrixSizeCheck
<
3
,
1
>::
check
(
dv2
);
MatrixSizeCheck
<
3
,
1
>::
check
(
sum_p
);
MatrixSizeCheck
<
3
,
1
>::
check
(
sum_v
);
sum_p
=
dp1
+
dv1
*
dt2
+
dq1
*
dp2
;
sum_v
=
dv1
+
dq1
*
dv2
;
sum_q
=
dq1
*
dq2
;
sum_dt
=
dt1
+
dt2
;
}
template
<
typename
D1
,
typename
D2
,
typename
D3
>
inline
void
compose
(
const
MatrixBase
<
D1
>&
d1
,
const
MatrixBase
<
D2
>&
d2
,
MatrixBase
<
D3
>&
sum
)
{
MatrixSizeCheck
<
11
,
1
>::
check
(
d1
);
MatrixSizeCheck
<
11
,
1
>::
check
(
d2
);
MatrixSizeCheck
<
11
,
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
D1
::
Scalar
,
3
,
1
>
>
dv1
(
&
d1
(
7
)
);
const
typename
D1
::
Scalar
&
dt1
=
d1
(
10
);
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
)
);
const
typename
D2
::
Scalar
&
dt2
=
d2
(
10
);
Map
<
Matrix
<
typename
D3
::
Scalar
,
3
,
1
>
>
sum_p
(
&
sum
(
0
)
);
Map
<
Quaternion
<
typename
D3
::
Scalar
>
>
sum_q
(
&
sum
(
3
)
);
Map
<
Matrix
<
typename
D3
::
Scalar
,
3
,
1
>
>
sum_v
(
&
sum
(
7
)
);
typename
D3
::
Scalar
&
sum_dt
=
sum
(
10
);
compose
(
dp1
,
dq1
,
dv1
,
dt1
,
dp2
,
dq2
,
dv2
,
dt2
,
sum_p
,
sum_q
,
sum_v
,
sum_dt
);
}
template
<
typename
D1
,
typename
D2
>
inline
Matrix
<
typename
D1
::
Scalar
,
11
,
1
>
compose
(
const
MatrixBase
<
D1
>&
d1
,
const
MatrixBase
<
D2
>&
d2
)
{
Matrix
<
typename
D1
::
Scalar
,
11
,
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
)
{
// Same as imu_tools.h
MatrixSizeCheck
<
11
,
1
>::
check
(
d1
);
MatrixSizeCheck
<
11
,
1
>::
check
(
d2
);
MatrixSizeCheck
<
11
,
1
>::
check
(
sum
);
MatrixSizeCheck
<
10
,
10
>::
check
(
J_sum_d1
);
MatrixSizeCheck
<
10
,
10
>::
check
(
J_sum_d2
);
// Jac wrt first delta -> adjoint inverse
J_sum_d1
=
adjoint
(
inverse
(
d1
));
// Jac wrt second delta -> indentity
J_sum_d2
.
setIdentity
();
//
// 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
D3
,
typename
D4
,
typename
D5
,
typename
D6
,
typename
D7
,
typename
D8
,
typename
D9
,
typename
T
>
inline
void
between
(
const
MatrixBase
<
D1
>&
dp1
,
const
QuaternionBase
<
D2
>&
dq1
,
const
MatrixBase
<
D3
>&
dv1
,
const
T
dt1
,
const
MatrixBase
<
D4
>&
dp2
,
const
QuaternionBase
<
D5
>&
dq2
,
const
MatrixBase
<
D6
>&
dv2
,
const
T
dt2
,
MatrixBase
<
D7
>&
diff_p
,
QuaternionBase
<
D8
>&
diff_q
,
MatrixBase
<
D9
>&
diff_v
,
T
&
diff_dt
)
{
// Different from imu_tools.h
MatrixSizeCheck
<
3
,
1
>::
check
(
dp1
);
MatrixSizeCheck
<
3
,
1
>::
check
(
dv1
);
MatrixSizeCheck
<
3
,
1
>::
check
(
dp2
);
MatrixSizeCheck
<
3
,
1
>::
check
(
dv2
);
MatrixSizeCheck
<
3
,
1
>::
check
(
diff_p
);
MatrixSizeCheck
<
3
,
1
>::
check
(
diff_v
);
diff_p
=
dq1
.
conjugate
()
*
(
dp2
-
dp1
-
dv1
*
(
dt2
-
dt1
));
// very different!
diff_v
=
dq1
.
conjugate
()
*
(
dv2
-
dv1
);
diff_q
=
dq1
.
conjugate
()
*
dq2
;
diff_dt
=
dt2
-
dt1
;
}
template
<
typename
D1
,
typename
D2
,
typename
D3
>
inline
void
between
(
const
MatrixBase
<
D1
>&
d1
,
const
MatrixBase
<
D2
>&
d2
,
MatrixBase
<
D3
>&
d2_minus_d1
)
{
MatrixSizeCheck
<
11
,
1
>::
check
(
d1
);
MatrixSizeCheck
<
11
,
1
>::
check
(
d2
);
MatrixSizeCheck
<
11
,
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
D1
::
Scalar
,
3
,
1
>
>
dv1
(
&
d1
(
7
)
);
const
typename
D1
::
Scalar
&
dt1
=
d1
(
10
);
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
)
);
const
typename
D2
::
Scalar
&
dt2
=
d2
(
10
);
Map
<
Matrix
<
typename
D3
::
Scalar
,
3
,
1
>
>
diff_p
(
&
d2_minus_d1
(
0
)
);
Map
<
Quaternion
<
typename
D3
::
Scalar
>
>
diff_q
(
&
d2_minus_d1
(
3
)
);
Map
<
Matrix
<
typename
D3
::
Scalar
,
3
,
1
>
>
diff_v
(
&
d2_minus_d1
(
7
)
);
typename
D3
::
Scalar
&
diff_dt
=
d2_minus_d1
(
10
);
between
(
dp1
,
dq1
,
dv1
,
dt1
,
dp2
,
dq2
,
dv2
,
dt2
,
diff_p
,
diff_q
,
diff_v
,
diff_dt
);
}
template
<
typename
D1
,
typename
D2
>
inline
Matrix
<
typename
D1
::
Scalar
,
11
,
1
>
between
(
const
MatrixBase
<
D1
>&
d1
,
const
MatrixBase
<
D2
>&
d2
)
{
Matrix
<
typename
D1
::
Scalar
,
11
,
1
>
diff
;
between
(
d1
,
d2
,
diff
);
return
diff
;
}
template
<
typename
D1
,
typename
D2
,
typename
D3
>
inline
void
composeOverState
(
const
MatrixBase
<
D1
>&
x
,
const
MatrixBase
<
D2
>&
d
,
MatrixBase
<
D3
>&
x_plus_d
)
{
// Same as imu_tools.h
MatrixSizeCheck
<
10
,
1
>::
check
(
x
);
MatrixSizeCheck
<
11
,
1
>::
check
(
d
);
MatrixSizeCheck
<
10
,
1
>::
check
(
x_plus_d
);
Map
<
const
Matrix
<
typename
D1
::
Scalar
,
3
,
1
>
>
p
(
&
x
(
0
)
);
Map
<
const
Quaternion
<
typename
D1
::
Scalar
>
>
q
(
&
x
(
3
)
);
Map
<
const
Matrix
<
typename
D1
::
Scalar
,
3
,
1
>
>
v
(
&
x
(
7
)
);
Map
<
const
Matrix
<
typename
D2
::
Scalar
,
3
,
1
>
>
dp
(
&
d
(
0
)
);
Map
<
const
Quaternion
<
typename
D2
::
Scalar
>
>
dq
(
&
d
(
3
)
);
Map
<
const
Matrix
<
typename
D2
::
Scalar
,
3
,
1
>
>
dv
(
&
d
(
7
)
);
const
typename
D2
::
Scalar
&
dt
=
d
(
10
);
Map
<
Matrix
<
typename
D3
::
Scalar
,
3
,
1
>
>
p_plus_d
(
&
x_plus_d
(
0
)
);
Map
<
Quaternion
<
typename
D3
::
Scalar
>
>
q_plus_d
(
&
x_plus_d
(
3
)
);
Map
<
Matrix
<
typename
D3
::
Scalar
,
3
,
1
>
>
v_plus_d
(
&
x_plus_d
(
7
)
);
p_plus_d
=
p
+
v
*
dt
+
0.5
*
gravity
()
*
dt
*
dt
+
q
*
dp
;
v_plus_d
=
v
+
gravity
()
*
dt
+
q
*
dv
;
q_plus_d
=
q
*
dq
;
// dq here to avoid possible aliasing between x and x_plus_d
}
template
<
typename
D1
,
typename
D2
>
inline
Matrix
<
typename
D1
::
Scalar
,
10
,
1
>
composeOverState
(
const
MatrixBase
<
D1
>&
x
,
const
MatrixBase
<
D2
>&
d
)
{
Matrix
<
typename
D1
::
Scalar
,
10
,
1
>
ret
;
composeOverState
(
x
,
d
,
ret
);
return
ret
;
}
template
<
typename
D1
,
typename
D2
,
typename
D3
,
typename
D4
,
typename
D5
,
typename
D6
,
typename
D7
,
typename
D8
,
typename
D9
,
class
T
>
inline
void
betweenStates
(
const
MatrixBase
<
D1
>&
p1
,
const
QuaternionBase
<
D2
>&
q1
,
const
MatrixBase
<
D3
>&
v1
,
const
MatrixBase
<
D4
>&
p2
,
const
QuaternionBase
<
D5
>&
q2
,
const
MatrixBase
<
D6
>&
v2
,
const
T
dt
,
MatrixBase
<
D7
>&
dp
,
QuaternionBase
<
D8
>&
dq
,
MatrixBase
<
D9
>&
dv
,
T
&
Dt
)
{
// Same as imu_tools.h
MatrixSizeCheck
<
3
,
1
>::
check
(
p1
);
MatrixSizeCheck
<
3
,
1
>::
check
(
v1
);
MatrixSizeCheck
<
3
,
1
>::
check
(
p2
);
MatrixSizeCheck
<
3
,
1
>::
check
(
v2
);
MatrixSizeCheck
<
3
,
1
>::
check
(
dp
);
MatrixSizeCheck
<
3
,
1
>::
check
(
dv
);
dp
=
q1
.
conjugate
()
*
(
p2
-
p1
-
v1
*
dt
-
(
T
)
0.5
*
gravity
().
cast
<
T
>
()
*
(
T
)
dt
*
(
T
)
dt
);
dv
=
q1
.
conjugate
()
*
(
v2
-
v1
-
gravity
().
cast
<
T
>
()
*
(
T
)
dt
);
dq
=
q1
.
conjugate
()
*
q2
;
Dt
=
dt
;
// a bit stupid... to keep things consistant
}
template
<
typename
D1
,
typename
D2
,
typename
D3
,
class
T
>
inline
void
betweenStates
(
const
MatrixBase
<
D1
>&
x1
,
const
MatrixBase
<
D2
>&
x2
,
T
dt
,
MatrixBase
<
D3
>&
x2_minus_x1
)
{
MatrixSizeCheck
<
10
,
1
>::
check
(
x1
);
MatrixSizeCheck
<
10
,
1
>::
check
(
x2
);
MatrixSizeCheck
<
11
,
1
>::
check
(
x2_minus_x1
);
Map
<
const
Matrix
<
typename
D1
::
Scalar
,
3
,
1
>
>
p1
(
&
x1
(
0
)
);
Map
<
const
Quaternion
<
typename
D1
::
Scalar
>
>
q1
(
&
x1
(
3
)
);
Map
<
const
Matrix
<
typename
D1
::
Scalar
,
3
,
1
>
>
v1
(
&
x1
(
7
)
);
Map
<
const
Matrix
<
typename
D2
::
Scalar
,
3
,
1
>
>
p2
(
&
x2
(
0
)
);
Map
<
const
Quaternion
<
typename
D2
::
Scalar
>
>
q2
(
&
x2
(
3
)
);
Map
<
const
Matrix
<
typename
D2
::
Scalar
,
3
,
1
>
>
v2
(
&
x2
(
7
)
);
Map
<
Matrix
<
typename
D3
::
Scalar
,
3
,
1
>
>
dp
(
&
x2_minus_x1
(
0
)
);
Map
<
Quaternion
<
typename
D3
::
Scalar
>
>
dq
(
&
x2_minus_x1
(
3
)
);
Map
<
Matrix
<
typename
D3
::
Scalar
,
3
,
1
>
>
dv
(
&
x2_minus_x1
(
7
)
);
T
&
Dt
=
x2_minus_x1
(
10
);
betweenStates
(
p1
,
q1
,
v1
,
p2
,
q2
,
v2
,
dt
,
dp
,
dq
,
dv
,
Dt
);
}
template
<
typename
D1
,
typename
D2
,
class
T
>
inline
Matrix
<
typename
D1
::
Scalar
,
11
,
1
>
betweenStates
(
const
MatrixBase
<
D1
>&
x1
,
const
MatrixBase
<
D2
>&
x2
,
T
dt
)
{
Matrix
<
typename
D1
::
Scalar
,
11
,
1
>
ret
;
betweenStates
(
x1
,
x2
,
dt
,
ret
);
return
ret
;
}
template
<
typename
D1
,
typename
D2
,
typename
D3
>
inline
void
QandPmat
(
const
MatrixBase
<
D1
>&
th
,
MatrixBase
<
D2
>&
Q
,
MatrixBase
<
D3
>&
P
){
// See equations 29-31 humanoids fourmy 19
// th = omega * dt
MatrixSizeCheck
<
3
,
1
>::
check
(
th
);
MatrixSizeCheck
<
3
,
3
>::
check
(
Q
);
MatrixSizeCheck
<
3
,
3
>::
check
(
P
);
typename
D1
::
Scalar
thn
=
th
.
norm
();
if
(
thn
<
1e-8
){
Q
.
setIdentity
();
P
.
setIdentity
();
P
*=
0.5
;
}
else
{
Matrix
<
typename
D1
::
Scalar
,
3
,
3
>
Id
;
Id
.
setIdentity
();
Matrix
<
typename
D1
::
Scalar
,
3
,
1
>
u
=
th
/
thn
;
Matrix
<
typename
D1
::
Scalar
,
3
,
3
>
ux
=
skew
(
u
);
Q
=
Id
+
+
((
1
-
cos
(
thn
))
/
thn
)
*
ux
+
((
thn
-
sin
(
thn
))
/
(
thn
*
thn
))
*
(
ux
*
ux
);
P
=
0.5
*
Id
+
((
thn
-
sin
(
thn
))
/
(
thn
*
thn
))
*
ux
+
((
cos
(
thn
)
+
0.5
*
thn
*
thn
-
1
)
/
(
thn
*
thn
))
*
(
ux
*
ux
);
}
}
template
<
typename
Derived
>
Matrix
<
typename
Derived
::
Scalar
,
11
,
1
>
exp_IMU
(
const
MatrixBase
<
Derived
>&
d_in
)
{
MatrixSizeCheck
<
10
,
1
>::
check
(
d_in
);
Matrix
<
typename
Derived
::
Scalar
,
11
,
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
<
const
Matrix
<
typename
Derived
::
Scalar
,
3
,
1
>
>
dv_in
(
&
d_in
(
6
)
);
const
typename
Derived
::
Scalar
&
dt_in
=
d_in
(
9
);
Map
<
Matrix
<
typename
Derived
::
Scalar
,
3
,
1
>
>
dp
(
&
ret
(
0
)
);
Map
<
Quaternion
<
typename
Derived
::
Scalar
>
>
dq
(
&
ret
(
3
)
);
Map
<
Matrix
<
typename
Derived
::
Scalar
,
3
,
1
>
>
dv
(
&
ret
(
7
)
);
typename
Derived
::
Scalar
&
dt
=
ret
(
10
);
Matrix
<
typename
Derived
::
Scalar
,
3
,
3
>
Q
;
Matrix
<
typename
Derived
::
Scalar
,
3
,
3
>
P
;
QandPmat
(
do_in
,
Q
,
P
);
std
::
cout
<<
"Q"
<<
std
::
endl
;
std
::
cout
<<
Q
<<
std
::
endl
;
std
::
cout
<<
"P"
<<
std
::
endl
;
std
::
cout
<<
P
<<
std
::
endl
;
dp
=
Q
*
dp_in
+
P
*
dv_in
*
dt_in
;
dv
=
Q
*
dp_in
;
dq
=
exp_q
(
do_in
);
dt
=
dt_in
;
return
ret
;
}
template
<
typename
Derived
>
Matrix
<
typename
Derived
::
Scalar
,
10
,
1
>
log_IMU
(
const
MatrixBase
<
Derived
>&
delta_in
)
{
MatrixSizeCheck
<
11
,
1
>::
check
(
delta_in
);
Matrix
<
typename
Derived
::
Scalar
,
10
,
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
<
const
Matrix
<
typename
Derived
::
Scalar
,
3
,
1
>
>
dv_in
(
&
delta_in
(
7
)
);
const
typename
Derived
::
Scalar
&
dt_in
=
delta_in
(
9
);
Map
<
Matrix
<
typename
Derived
::
Scalar
,
3
,
1
>
>
dp_ret
(
&
ret
(
0
)
);
Map
<
Matrix
<
typename
Derived
::
Scalar
,
3
,
1
>
>
do_ret
(
&
ret
(
3
)
);
Map
<
Matrix
<
typename
Derived
::
Scalar
,
3
,
1
>
>
dv_ret
(
&
ret
(
6
)
);
typename
Derived
::
Scalar
&
dt_ret
=
ret
(
9
);
Matrix
<
typename
Derived
::
Scalar
,
3
,
3
>
Q
;
Matrix
<
typename
Derived
::
Scalar
,
3
,
3
>
Qinv
;
Matrix
<
typename
Derived
::
Scalar
,
3
,
3
>
P
;
do_ret
=
log_q
(
dq_in
);
QandPmat
(
do_ret
,
Q
,
P
);
Qinv
=
Q
.
inverse
();
dp_ret
=
Qinv
*
(
dp_in
-
P
*
Qinv
*
dv_in
*
dt_in
);
dv_ret
=
Qinv
*
dv_in
;
dt_ret
=
dt_in
;
return
ret
;
}
// template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9>
// inline void plus(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
// const MatrixBase<D4>& dp2, const MatrixBase<D5>& do2, const MatrixBase<D6>& dv2,
// MatrixBase<D7>& plus_p, QuaternionBase<D8>& plus_q, MatrixBase<D9>& plus_v )
// {
// plus_p = dp1 + dp2;
// plus_v = dv1 + dv2;
// 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 D1::Scalar, 3, 1> > dv1 ( & d1(7) );
// Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
// Map<const Matrix<typename D2::Scalar, 3, 1> > do2 ( & d2(3) );
// Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(6) );
// Map<Matrix<typename D3::Scalar, 3, 1> > dp_p ( & d_pert(0) );
// Map<Quaternion<typename D3::Scalar> > dq_p ( & d_pert(3) );
// Map<Matrix<typename D3::Scalar, 3, 1> > dv_p ( & d_pert(7) );
// plus(dp1, dq1, dv1, dp2, do2, dv2, dp_p, dq_p, dv_p);
// }
// template<typename D1, typename D2>
// inline Matrix<typename D1::Scalar, 10, 1> plus(const MatrixBase<D1>& d1,
// const MatrixBase<D2>& d2)
// {
// Matrix<typename D1::Scalar, 10, 1> ret;
// plus(d1, d2, ret);
// return ret;
// }
// template<typename D1, typename D2, typename D3, 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<D3>& dv1,
// const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
// MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o, MatrixBase<D9>& diff_v )
// {
// diff_p = dp2 - dp1;
// diff_v = dv2 - dv1;
// diff_o = log_q(dq1.conjugate() * dq2);
// }
// template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11>
// inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
// const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
// MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o, MatrixBase<D9>& diff_v ,
// MatrixBase<D10>& J_do_dq1, MatrixBase<D11>& J_do_dq2)
// {
// diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v);
// 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 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 ( & err(0) );
// Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & err(3) );
// Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & err(6) );
// diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v);
// }
// 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, 9, 9>::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, 9, 1> diff(const MatrixBase<D1>& d1,
// const MatrixBase<D2>& d2)
// {
// Matrix<typename D1::Scalar, 9, 1> ret;
// diff(d1, d2, ret);
// return ret;
// }
// template<typename D1, typename D2, typename D3, typename D4, typename D5>
// inline void body2delta(const MatrixBase<D1>& a,
// const MatrixBase<D2>& w,
// const typename D1::Scalar& dt,
// MatrixBase<D3>& dp,
// QuaternionBase<D4>& dq,
// MatrixBase<D5>& dv)
// {
// MatrixSizeCheck<3,1>::check(a);
// MatrixSizeCheck<3,1>::check(w);
// MatrixSizeCheck<3,1>::check(dp);
// MatrixSizeCheck<3,1>::check(dv);
// dp = 0.5 * a * dt * dt;
// dv = a * dt;
// dq = exp_q(w * dt);
// }
// template<typename D1>
// inline Matrix<typename D1::Scalar, 10, 1> body2delta(const MatrixBase<D1>& body,
// const typename D1::Scalar& dt)
// {
// MatrixSizeCheck<6,1>::check(body);
// typedef typename D1::Scalar T;
// Matrix<T, 10, 1> delta;
// Map< Matrix<T, 3, 1>> dp ( & delta(0) );
// Map< Quaternion<T>> dq ( & delta(3) );
// Map< Matrix<T, 3, 1>> dv ( & delta(7) );
// body2delta(body.block(0,0,3,1), body.block(3,0,3,1), dt, dp, dq, dv);
// return delta;
// }
// template<typename D1, typename D2, typename D3>
// inline void body2delta(const MatrixBase<D1>& body,
// const typename D1::Scalar& dt,
// MatrixBase<D2>& delta,
// MatrixBase<D3>& jac_body)
// {
// MatrixSizeCheck<6,1>::check(body);
// MatrixSizeCheck<9,6>::check(jac_body);
// typedef typename D1::Scalar T;
// delta = body2delta(body, dt);
// Matrix<T, 3, 1> w = body.block(3,0,3,1);
// jac_body.setZero();
// jac_body.block(0,0,3,3) = 0.5 * dt * dt * Matrix<T, 3, 3>::Identity();
// jac_body.block(3,3,3,3) = dt * jac_SO3_right(w * dt);
// jac_body.block(6,0,3,3) = dt * Matrix<T, 3, 3>::Identity();
// }
// template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7>
// void motion2data(const MatrixBase<D1>& a, const MatrixBase<D2>& w, const QuaternionBase<D3>& q, const MatrixBase<D4>& a_b, const MatrixBase<D5>& w_b, MatrixBase<D6>& a_m, MatrixBase<D7>& w_m)
// {
// MatrixSizeCheck<3,1>::check(a);
// MatrixSizeCheck<3,1>::check(w);
// MatrixSizeCheck<3,1>::check(a_b);
// MatrixSizeCheck<3,1>::check(w_b);
// MatrixSizeCheck<3,1>::check(a_m);
// MatrixSizeCheck<3,1>::check(w_m);
// // Note: data = (a_m , w_m)
// a_m = a + a_b - q.conjugate()*gravity();
// w_m = w + w_b;
// }
// /* Create IMU data from body motion
// * Input:
// * motion : [ax, ay, az, wx, wy, wz] the motion in body frame
// * q : the current orientation wrt horizontal
// * bias : the bias of the IMU
// * Output:
// * return : the data vector as created by the IMU (with motion, gravity, and bias)
// */
// template<typename D1, typename D2, typename D3>
// Matrix<typename D1::Scalar, 6, 1> motion2data(const MatrixBase<D1>& motion, const QuaternionBase<D2>& q, const MatrixBase<D3>& bias)
// {
// Matrix<typename D1::Scalar, 6, 1> data;
// Map<Matrix<typename D1::Scalar, 3, 1>> a_m (data.data() );
// Map<Matrix<typename D1::Scalar, 3, 1>> w_m (data.data() + 3);
// motion2data(motion.block(0,0,3,1),
// motion.block(3,0,3,1),
// q,
// bias.block(0,0,3,1),
// bias.block(3,0,3,1),
// a_m,
// w_m );
// return data;
// }
}
// namespace imu
}
// namespace wolf
#endif
/* IMU_TOOLS_LIE_H_ */
This diff is collapsed.
Click to expand it.
test/CMakeLists.txt
+
3
−
0
View file @
40f18270
...
...
@@ -22,6 +22,9 @@ target_link_libraries(gtest_IMU ${PLUGIN_NAME} ${wolf_LIBRARY})
wolf_add_gtest
(
gtest_IMU_tools gtest_IMU_tools.cpp
)
target_link_libraries
(
gtest_IMU_tools
${
PLUGIN_NAME
}
${
wolf_LIBRARY
}
)
wolf_add_gtest
(
gtest_IMU_tools_Lie gtest_IMU_tools_Lie.cpp
)
target_link_libraries
(
gtest_IMU_tools_Lie
${
PLUGIN_NAME
}
${
wolf_LIBRARY
}
)
wolf_add_gtest
(
gtest_processor_IMU_jacobians gtest_processor_IMU_jacobians.cpp
)
target_link_libraries
(
gtest_processor_IMU_jacobians
${
PLUGIN_NAME
}
${
wolf_LIBRARY
}
)
...
...
This diff is collapsed.
Click to expand it.
test/gtest_IMU_tools_Lie.cpp
0 → 100644
+
636
−
0
View file @
40f18270
/*
* gtest_imu_tools_Lie.cpp
*
* Created on: Oct 7, 2019
* Author: mfourmy
*/
#include
"IMU/math/IMU_tools_Lie.h"
#include
<core/utils/utils_gtest.h>
using
namespace
Eigen
;
using
namespace
wolf
;
using
namespace
imu_with_dt
;
// used in IMU_tools_Lie
TEST
(
IMU_tools
,
identity
)
{
VectorXs
id_true
;
id_true
.
setZero
(
11
);
id_true
(
6
)
=
1.0
;
VectorXs
id
=
identity
<
Scalar
>
();
ASSERT_MATRIX_APPROX
(
id
,
id_true
,
1e-10
);
}
TEST
(
IMU_tools
,
identity_split
)
{
VectorXs
p
(
3
),
qv
(
4
),
v
(
3
);
Scalar
dt
;
Quaternions
q
;
identity
(
p
,
q
,
v
,
dt
);
ASSERT_MATRIX_APPROX
(
p
,
Vector3s
::
Zero
(),
1e-10
);
ASSERT_QUATERNION_APPROX
(
q
,
Quaternions
::
Identity
(),
1e-10
);
ASSERT_MATRIX_APPROX
(
v
,
Vector3s
::
Zero
(),
1e-10
);
ASSERT_NEAR
(
dt
,
0.0
,
1e-10
);
identity
(
p
,
qv
,
v
,
dt
);
ASSERT_MATRIX_APPROX
(
p
,
Vector3s
::
Zero
(),
1e-10
);
ASSERT_MATRIX_APPROX
(
qv
,
(
Vector4s
()
<<
0
,
0
,
0
,
1
).
finished
(),
1e-10
);
ASSERT_MATRIX_APPROX
(
v
,
Vector3s
::
Zero
(),
1e-10
);
ASSERT_NEAR
(
dt
,
0.0
,
1e-10
);
}
TEST
(
IMU_tools
,
inverse
)
{
VectorXs
d
(
11
),
id
(
11
),
iid
(
11
),
iiid
(
11
),
I
(
11
);
Vector4s
qv
;
qv
=
(
Vector4s
()
<<
3
,
4
,
5
,
6
).
finished
().
normalized
();
d
<<
0
,
1
,
2
,
qv
,
7
,
8
,
9
,
0.1
;
id
=
inverse
(
d
);
iid
=
inverse
(
id
);
ASSERT_MATRIX_APPROX
(
d
,
iid
,
1e-10
);
compose
(
d
,
id
,
I
);
ASSERT_MATRIX_APPROX
(
I
,
identity
(),
1e-10
);
compose
(
id
,
d
,
I
);
ASSERT_MATRIX_APPROX
(
I
,
identity
(),
1e-10
);
ASSERT_MATRIX_APPROX
(
d
,
iid
,
1e-10
);
iiid
=
inverse
(
iid
);
ASSERT_MATRIX_APPROX
(
id
,
iiid
,
1e-10
);
}
TEST
(
IMU_tools
,
compose_between
)
{
VectorXs
dx1
(
11
),
dx2
(
11
),
dx3
(
11
);
Matrix
<
Scalar
,
11
,
1
>
d1
,
d2
,
d3
;
Vector4s
qv
;
Scalar
dt
=
0.1
;
qv
=
(
Vector4s
()
<<
3
,
4
,
5
,
6
).
finished
().
normalized
();
dx1
<<
0
,
1
,
2
,
qv
,
7
,
8
,
9
,
dt
;
d1
=
dx1
;
qv
=
(
Vector4s
()
<<
6
,
5
,
4
,
3
).
finished
().
normalized
();
dx2
<<
9
,
8
,
7
,
qv
,
2
,
1
,
0
,
dt
;
d2
=
dx2
;
// combinations of templates for sum()
compose
(
dx1
,
dx2
,
dx3
);
compose
(
d1
,
d2
,
d3
);
ASSERT_MATRIX_APPROX
(
d3
,
dx3
,
1e-10
);
// ?? compare results from the same fonction?
compose
(
d1
,
dx2
,
dx3
);
d3
=
compose
(
dx1
,
d2
);
ASSERT_MATRIX_APPROX
(
d3
,
dx3
,
1e-10
);
// minus(d1, d3) should recover delta_2
VectorXs
diffx
(
11
);
Matrix
<
Scalar
,
11
,
1
>
diff
;
between
(
d1
,
d3
,
diff
);
ASSERT_MATRIX_APPROX
(
diff
,
d2
,
1e-10
);
// minus(d3, d1) should recover inverse(d2)
diff
=
between
(
d3
,
d1
);
ASSERT_MATRIX_APPROX
(
diff
,
inverse
(
d2
),
1e-10
);
}
TEST
(
IMU_tools
,
compose_between_with_state
)
{
VectorXs
x
(
10
),
d
(
11
),
x2
(
10
),
x3
(
10
),
d2
(
11
),
d3
(
11
);
Vector4s
qv
;
Scalar
dt
=
0.1
;
qv
=
(
Vector4s
()
<<
3
,
4
,
5
,
6
).
finished
().
normalized
();
x
<<
0
,
1
,
2
,
qv
,
7
,
8
,
9
;
qv
=
(
Vector4s
()
<<
6
,
5
,
4
,
3
).
finished
().
normalized
();
d
<<
9
,
8
,
7
,
qv
,
2
,
1
,
0
,
dt
;
composeOverState
(
x
,
d
,
x2
);
x3
=
composeOverState
(
x
,
d
);
ASSERT_MATRIX_APPROX
(
x3
,
x2
,
1e-10
);
// betweenStates(x, x2) should recover d
betweenStates
(
x
,
x2
,
dt
,
d2
);
d3
=
betweenStates
(
x
,
x2
,
dt
);
ASSERT_MATRIX_APPROX
(
d2
,
d
,
1e-10
);
ASSERT_MATRIX_APPROX
(
d3
,
d
,
1e-10
);
ASSERT_MATRIX_APPROX
(
betweenStates
(
x
,
x2
,
dt
),
d
,
1e-10
);
// x + (x2 - x) = x2
ASSERT_MATRIX_APPROX
(
composeOverState
(
x
,
betweenStates
(
x
,
x2
,
dt
)),
x2
,
1e-10
);
// (x + d) - x = d
ASSERT_MATRIX_APPROX
(
betweenStates
(
x
,
composeOverState
(
x
,
d
),
dt
),
d
,
1e-10
);
}
TEST
(
IMU_tools
,
lift_retract
)
{
VectorXs
d_min
(
10
);
Scalar
dt_min
=
0.1
;
d_min
<<
0
,
1
,
2
,
.3
,
.4
,
.5
,
6
,
7
,
8
,
dt_min
;
// use angles in the ball theta < pi
// transform to delta
VectorXs
delta
=
exp_IMU
(
d_min
);
// expected delta -> reimplement formulas TODO
// Vector3s dp = d_min.head(3);
// Quaternions dq = v2q(d_min.segment<3>(3));
// Vector3s dv = d_min.tail(3);
// Scalar dt = d_min(9);
// VectorXs delta_true(11); delta_true << dp, dq.coeffs(), dv, dt;
// ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10);
// transform to a new tangent -- should be the original tangent
VectorXs
d_from_delta
=
log_IMU
(
delta
);
ASSERT_MATRIX_APPROX
(
d_from_delta
,
d_min
,
1e-10
);
// transform to a new delta -- should be the previous delta
VectorXs
delta_from_d
=
exp_IMU
(
d_from_delta
);
ASSERT_MATRIX_APPROX
(
delta_from_d
,
delta
,
1e-10
);
}
// TEST(IMU_tools, plus)
// {
// VectorXs d1(10), d2(10), d3(10);
// VectorXs err(9);
// Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
// d1 << 0, 1, 2, qv, 7, 8, 9;
// err << 0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07, 0.08, 0.09;
// d3.head(3) = d1.head(3) + err.head(3);
// d3.segment(3,4) = (Quaternions(qv.data()) * exp_q(err.segment(3,3))).coeffs();
// d3.tail(3) = d1.tail(3) + err.tail(3);
// plus(d1, err, d2);
// ASSERT_MATRIX_APPROX(diff(d3, d2), VectorXs::Zero(9), 1e-10);
// }
// TEST(IMU_tools, diff)
// {
// VectorXs d1(10), d2(10);
// Vector4s qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
// d1 << 0, 1, 2, qv, 7, 8, 9;
// d2 = d1;
// VectorXs err(9);
// diff(d1, d2, err);
// ASSERT_MATRIX_APPROX(err, VectorXs::Zero(9), 1e-10);
// ASSERT_MATRIX_APPROX(diff(d1, d2), VectorXs::Zero(9), 1e-10);
// VectorXs d3(10);
// d3.setRandom(); d3.segment(3,4).normalize();
// err.head(3) = d3.head(3) - d1.head(3);
// err.segment(3,3) = log_q(Quaternions(d1.data()+3).conjugate()*Quaternions(d3.data()+3));
// err.tail(3) = d3.tail(3) - d1.tail(3);
// ASSERT_MATRIX_APPROX(err, diff(d1, d3), 1e-10);
// }
// TEST(IMU_tools, compose_jacobians)
// {
// VectorXs d1(10), d2(10), d3(10), d1_pert(10), d2_pert(10), d3_pert(10); // deltas
// VectorXs t1(9), t2(9); // tangents
// Matrix<Scalar, 9, 9> J1_a, J2_a, J1_n, J2_n;
// Vector4s qv1, qv2;
// Scalar dt = 0.1;
// Scalar dx = 1e-6;
// qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
// d1 << 0, 1, 2, qv1, 7, 8, 9;
// qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
// d2 << 9, 8, 7, qv2, 2, 1, 0;
// // analytical jacobians
// compose(d1, d2, dt, d3, J1_a, J2_a);
// // numerical jacobians
// for (unsigned int i = 0; i<9; i++)
// {
// t1 . setZero();
// t1(i) = dx;
// // Jac wrt first delta
// d1_pert = plus(d1, t1); // (d1 + t1)
// d3_pert = compose(d1_pert, d2, dt); // (d1 + t1) + d2
// t2 = diff(d3, d3_pert); // { (d2 + t1) + d2 } - { d1 + d2 }
// J1_n.col(i) = t2/dx; // [ { (d2 + t1) + d2 } - { d1 + d2 } ] / dx
// // Jac wrt second delta
// d2_pert = plus(d2, t1); // (d2 + t1)
// d3_pert = compose(d1, d2_pert, dt); // d1 + (d2 + t1)
// t2 = diff(d3, d3_pert); // { d1 + (d2 + t1) } - { d1 + d2 }
// J2_n.col(i) = t2/dx; // [ { d1 + (d2 + t1) } - { d1 + d2 } ] / dx
// }
// // check that numerical and analytical match
// ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4);
// ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
// }
// TEST(IMU_tools, diff_jacobians)
// {
// VectorXs d1(10), d2(10), d3(9), d1_pert(10), d2_pert(10), d3_pert(9); // deltas
// VectorXs t1(9), t2(9); // tangents
// Matrix<Scalar, 9, 9> J1_a, J2_a, J1_n, J2_n;
// Vector4s qv1, qv2;
// Scalar dx = 1e-6;
// qv1 = (Vector4s() << 3, 4, 5, 6).finished().normalized();
// d1 << 0, 1, 2, qv1, 7, 8, 9;
// qv2 = (Vector4s() << 1, 2, 3, 4).finished().normalized();
// d2 << 9, 8, 7, qv2, 2, 1, 0;
// // analytical jacobians
// diff(d1, d2, d3, J1_a, J2_a);
// // numerical jacobians
// for (unsigned int i = 0; i<9; i++)
// {
// t1 . setZero();
// t1(i) = dx;
// // Jac wrt first delta
// d1_pert = plus(d1, t1); // (d1 + t1)
// d3_pert = diff(d1_pert, d2); // d2 - (d1 + t1)
// t2 = d3_pert - d3; // { d2 - (d1 + t1) } - { d2 - d1 }
// J1_n.col(i) = t2/dx; // [ { d2 - (d1 + t1) } - { d2 - d1 } ] / dx
// // Jac wrt second delta
// d2_pert = plus(d2, t1); // (d2 + t1)
// d3_pert = diff(d1, d2_pert); // (d2 + t1) - d1
// t2 = d3_pert - d3; // { (d2 + t1) - d1 } - { d2 - d1 }
// J2_n.col(i) = t2/dx; // [ { (d2 + t1) - d1 } - { d2 - d1 } ] / dx
// }
// // check that numerical and analytical match
// ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4);
// ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
// }
// TEST(IMU_tools, body2delta_jacobians)
// {
// VectorXs delta(10), delta_pert(10); // deltas
// VectorXs body(6), pert(6);
// VectorXs tang(9); // tangents
// Matrix<Scalar, 9, 6> J_a, J_n; // analytic and numerical jacs
// Vector4s qv;;
// Scalar dt = 0.1;
// Scalar dx = 1e-6;
// qv = (Vector4s() << 3, 4, 5, 6).finished().normalized();
// delta << 0, 1, 2, qv, 7, 8, 9;
// body << 1, 2, 3, 3, 2, 1;
// // analytical jacobians
// body2delta(body, dt, delta, J_a);
// // numerical jacobians
// for (unsigned int i = 0; i<6; i++)
// {
// pert . setZero();
// pert(i) = dx;
// // Jac wrt first delta
// delta_pert = body2delta(body + pert, dt); // delta(body + pert)
// tang = diff(delta, delta_pert); // delta(body + pert) -- delta(body)
// J_n.col(i) = tang/dx; // ( delta(body + pert) -- delta(body) ) / dx
// }
// // check that numerical and analytical match
// ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
// }
// TEST(motion2data, zero)
// {
// Vector6s motion;
// Vector6s bias;
// Quaternions q;
// motion .setZero();
// bias .setZero();
// q .setIdentity();
// Vector6s data = imu::motion2data(motion, q, bias);
// Vector6s data_true; data_true << -gravity(), Vector3s::Zero();
// ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
// }
// TEST(motion2data, motion)
// {
// Vector6s motion, g_extended;
// Vector6s bias;
// Quaternions q;
// g_extended << gravity() , Vector3s::Zero();
// motion << 1,2,3, 4,5,6;
// bias .setZero();
// q .setIdentity();
// Vector6s data = imu::motion2data(motion, q, bias);
// Vector6s data_true; data_true = motion - g_extended;
// ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
// }
// TEST(motion2data, bias)
// {
// Vector6s motion, g_extended;
// Vector6s bias;
// Quaternions q;
// g_extended << gravity() , Vector3s::Zero();
// motion .setZero();
// bias << 1,2,3, 4,5,6;
// q .setIdentity();
// Vector6s data = imu::motion2data(motion, q, bias);
// Vector6s data_true; data_true = bias - g_extended;
// ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
// }
// TEST(motion2data, orientation)
// {
// Vector6s motion, g_extended;
// Vector6s bias;
// Quaternions q;
// g_extended << gravity() , Vector3s::Zero();
// motion .setZero();
// bias .setZero();
// q = v2q(Vector3s(M_PI/2, 0, 0)); // turn 90 deg in X axis
// Vector6s data = imu::motion2data(motion, q, bias);
// Vector6s data_true; data_true << 0,-gravity()(2),0, 0,0,0;
// ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
// }
// TEST(motion2data, AllRandom)
// {
// Vector6s motion, g_extended;
// Vector6s bias;
// Quaternions q;
// motion .setRandom();
// bias .setRandom();
// q.coeffs() .setRandom().normalize();
// g_extended << q.conjugate()*gravity() , Vector3s::Zero();
// Vector6s data = imu::motion2data(motion, q, bias);
// Vector6s data_true; data_true = motion + bias - g_extended;
// ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
// }
// /* Integrate acc and angVel motion, obtain Delta_preintegrated
// * Input:
// * N: number of steps
// * q0: initial orientaiton
// * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
// * bias_real: the real bias of the IMU
// * bias_preint: the bias used for Delta pre-integration
// * Output:
// * return: the preintegrated delta
// */
// VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt)
// {
// VectorXs data(6);
// VectorXs body(6);
// VectorXs delta(10);
// VectorXs Delta(10), Delta_plus(10);
// Delta << 0,0,0, 0,0,0,1, 0,0,0;
// Quaternions q(q0);
// for (int n = 0; n<N; n++)
// {
// data = motion2data(motion, q, bias_real);
// q = q*exp_q(motion.tail(3)*dt);
// body = data - bias_preint;
// delta = body2delta(body, dt);
// Delta_plus = compose(Delta, delta, dt);
// Delta = Delta_plus;
// }
// return Delta;
// }
// /* Integrate acc and angVel motion, obtain Delta_preintegrated
// * Input:
// * N: number of steps
// * q0: initial orientaiton
// * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
// * bias_real: the real bias of the IMU
// * bias_preint: the bias used for Delta pre-integration
// * Output:
// * J_D_b: the Jacobian of the preintegrated delta wrt the bias
// * return: the preintegrated delta
// */
// VectorXs integrateDelta(int N, const Quaternions& q0, const VectorXs& motion, const VectorXs& bias_real, const VectorXs& bias_preint, Scalar dt, Matrix<Scalar, 9, 6>& J_D_b)
// {
// VectorXs data(6);
// VectorXs body(6);
// VectorXs delta(10);
// Matrix<Scalar, 9, 6> J_d_d, J_d_b;
// Matrix<Scalar, 9, 9> J_D_D, J_D_d;
// VectorXs Delta(10), Delta_plus(10);
// Quaternions q;
// Delta << 0,0,0, 0,0,0,1, 0,0,0;
// J_D_b.setZero();
// q = q0;
// for (int n = 0; n<N; n++)
// {
// // Simulate data
// data = motion2data(motion, q, bias_real);
// q = q*exp_q(motion.tail(3)*dt);
// // Motion::integrateOneStep()
// { // IMU::computeCurrentDelta
// body = data - bias_preint;
// body2delta(body, dt, delta, J_d_d);
// J_d_b = - J_d_d;
// }
// { // IMU::deltaPlusDelta
// compose(Delta, delta, dt, Delta_plus, J_D_D, J_D_d);
// }
// // Motion:: jac calib
// J_D_b = J_D_D*J_D_b + J_D_d*J_d_b;
// // Motion:: buffer
// Delta = Delta_plus;
// }
// return Delta;
// }
// TEST(IMU_tools, integral_jacobian_bias)
// {
// VectorXs Delta(10), Delta_pert(10); // deltas
// VectorXs bias_real(6), pert(6), bias_pert(6), bias_preint(6);
// VectorXs body(6), data(6), motion(6);
// VectorXs tang(9); // tangents
// Matrix<Scalar, 9, 6> J_a, J_n; // analytic and numerical jacs
// Scalar dt = 0.001;
// Scalar dx = 1e-4;
// Quaternions q0(3, 4, 5, 6); q0.normalize();
// motion << .1, .2, .3, .3, .2, .1;
// bias_real << .002, .004, .001, .003, .002, .001;
// bias_preint << .004, .005, .006, .001, .002, .003;
// int N = 500; // # steps of integration
// // Analytical Jacobians
// Delta = integrateDelta(N, q0, motion, bias_real, bias_preint, dt, J_a);
// // numerical jacobians
// for (unsigned int i = 0; i<6; i++)
// {
// pert . setZero();
// pert(i) = dx;
// bias_pert = bias_preint + pert;
// Delta_pert = integrateDelta(N, q0, motion, bias_real, bias_pert, dt);
// tang = diff(Delta, Delta_pert); // Delta(bias + pert) -- Delta(bias)
// J_n.col(i) = tang/dx; // ( Delta(bias + pert) -- Delta(bias) ) / dx
// }
// // check that numerical and analytical match
// ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
// }
// TEST(IMU_tools, delta_correction)
// {
// VectorXs Delta(10), Delta_preint(10), Delta_corr; // deltas
// VectorXs bias(6), pert(6), bias_preint(6);
// VectorXs body(6), motion(6);
// VectorXs tang(9); // tangents
// Matrix<Scalar, 9, 6> J_b; // analytic and numerical jacs
// Vector4s qv;;
// Scalar dt = 0.001;
// Quaternions q0(3, 4, 5, 6); q0.normalize();
// motion << 1, 2, 3, 3, 2, 1; motion *= .1;
// int N = 1000; // # steps of integration
// // preintegration with correct bias
// bias << .004, .005, .006, .001, .002, .003;
// Delta = integrateDelta(N, q0, motion, bias, bias, dt);
// // preintegration with wrong bias
// pert << .002, -.001, .003, -.0003, .0002, -.0001;
// bias_preint = bias + pert;
// Delta_preint = integrateDelta(N, q0, motion, bias, bias_preint, dt, J_b);
// // correct perturbated
// Vector9s step = J_b*(bias-bias_preint);
// Delta_corr = plus(Delta_preint, step);
// // Corrected delta should match real delta
// ASSERT_MATRIX_APPROX(Delta, Delta_corr, 1e-5);
// // diff between real and corrected should be zero
// ASSERT_MATRIX_APPROX(diff(Delta, Delta_corr), Vector9s::Zero(), 1e-5);
// // diff between preint and corrected deltas should be the jacobian-computed step
// ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-5);
// }
// TEST(IMU_tools, full_delta_residual)
// {
// VectorXs x1(10), x2(10);
// VectorXs Delta(10), Delta_preint(10), Delta_corr(10);
// VectorXs Delta_real(9), Delta_err(9), Delta_diff(10), Delta_exp(10);
// VectorXs bias(6), pert(6), bias_preint(6), bias_null(6);
// VectorXs body(6), motion(6);
// VectorXs tang(9); // tangents
// Matrix<Scalar, 9, 6> J_b; // analytic and numerical jacs
// Scalar dt = 0.001;
// Quaternions q0; q0.setIdentity();
// x1 << 0,0,0, 0,0,0,1, 0,0,0;
// motion << .3, .2, .1, .1, .2, .3; // acc and gyro
// // motion << .3, .2, .1, .0, .0, .0; // only acc
// // motion << .0, .0, .0, .1, .2, .3; // only gyro
// bias << 0.01, 0.02, 0.003, 0.002, 0.005, 0.01;
// bias_null << 0, 0, 0, 0, 0, 0;
// // bias_preint << 0.003, 0.002, 0.001, 0.001, 0.002, 0.003;
// bias_preint = bias_null;
// int N = 1000; // # steps of integration
// // preintegration with no bias
// Delta_real = integrateDelta(N, q0, motion, bias_null, bias_null, dt);
// // final state
// x2 = composeOverState(x1, Delta_real, 1000*dt);
// // preintegration with the correct bias
// Delta = integrateDelta(N, q0, motion, bias, bias, dt);
// ASSERT_MATRIX_APPROX(Delta, Delta_real, 1e-4);
// // preintegration with wrong bias
// Delta_preint = integrateDelta(N, q0, motion, bias, bias_preint, dt, J_b);
// // compute correction step
// Vector9s step = J_b*(bias-bias_preint);
// // correct preintegrated delta
// Delta_corr = plus(Delta_preint, step);
// // Corrected delta should match real delta
// ASSERT_MATRIX_APPROX(Delta_real, Delta_corr, 1e-3);
// // diff between real and corrected should be zero
// ASSERT_MATRIX_APPROX(diff(Delta_real, Delta_corr), Vector9s::Zero(), 1e-3);
// // expected delta
// Delta_exp = betweenStates(x1, x2, N*dt);
// ASSERT_MATRIX_APPROX(Delta_exp, Delta_corr, 1e-3);
// // compute diff between expected and corrected
// // Matrix<Scalar, 9, 9> J_err_exp, J_err_corr;
// diff(Delta_exp, Delta_corr, Delta_err); //, J_err_exp, J_err_corr);
// ASSERT_MATRIX_APPROX(Delta_err, Vector9s::Zero(), 1e-3);
// // compute error between expected and preintegrated
// Delta_err = diff(Delta_preint, Delta_exp);
// // diff between preint and corrected deltas should be the jacobian-computed step
// ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-3);
// /* This implies:
// * - Since D_corr is expected to be similar to D_exp
// * step ~ diff(Delta_preint, Delta_exp)
// * - the residual can be computed with a regular '-' :
// * residual = diff(D_preint, D_exp) - J_bias * (bias - bias_preint)
// */
// // WOLF_TRACE("Delta real: ", Delta_real.transpose());
// // WOLF_TRACE("x2: ", x2.transpose());
// // WOLF_TRACE("Delta: ", Delta.transpose());
// // WOLF_TRACE("Delta pre: ", Delta_preint.transpose());
// // WOLF_TRACE("Jac bias: \n", J_b);
// // WOLF_TRACE("Delta step: ", step.transpose());
// // WOLF_TRACE("Delta cor: ", Delta_corr.transpose());
// // WOLF_TRACE("Delta exp: ", Delta_exp.transpose());
// // WOLF_TRACE("Delta err: ", Delta_err.transpose());
// // WOLF_TRACE("Delta err exp-pre: ", Delta_err.transpose());
// }
int
main
(
int
argc
,
char
**
argv
)
{
testing
::
InitGoogleTest
(
&
argc
,
argv
);
// ::testing::GTEST_FLAG(filter) = "IMU_tools.delta_correction";
return
RUN_ALL_TESTS
();
}
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment