Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
B
bodydynamics
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
bodydynamics
Commits
358ba67e
Commit
358ba67e
authored
4 years ago
by
Médéric Fourmy
Browse files
Options
Downloads
Patches
Plain Diff
Computing running imu bias covariance
parent
5a629fc4
No related branches found
No related tags found
2 merge requests
!18
Release after RAL
,
!17
After 2nd RAL submission
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
demos/solo_mocap_imu.cpp
+42
-34
42 additions, 34 deletions
demos/solo_mocap_imu.cpp
with
42 additions
and
34 deletions
demos/solo_mocap_imu.cpp
+
42
−
34
View file @
358ba67e
...
@@ -61,42 +61,25 @@ typedef pinocchio::ForceTpl<double> Force;
...
@@ -61,42 +61,25 @@ typedef pinocchio::ForceTpl<double> Force;
int
main
(
int
argc
,
char
**
argv
)
{
int
main
(
int
argc
,
char
**
argv
)
{
// retrieve parameters from yaml file
// retrieve parameters from yaml file
YAML
::
Node
config
=
YAML
::
LoadFile
(
"/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml"
);
YAML
::
Node
config
=
YAML
::
LoadFile
(
"/home/mfourmy/Documents/Phd_LAAS/wolf/bodydynamics/demos/solo_real_estimation.yaml"
);
std
::
string
robot_urdf
=
config
[
"robot_urdf"
].
as
<
std
::
string
>
();
int
dimc
=
config
[
"dimc"
].
as
<
int
>
();
int
nb_feet
=
config
[
"nb_feet"
].
as
<
int
>
();
double
dt
=
config
[
"dt"
].
as
<
double
>
();
double
dt
=
config
[
"dt"
].
as
<
double
>
();
double
min_t
=
config
[
"min_t"
].
as
<
double
>
();
double
max_t
=
config
[
"max_t"
].
as
<
double
>
();
double
max_t
=
config
[
"max_t"
].
as
<
double
>
();
double
solve_every_sec
=
config
[
"solve_every_sec"
].
as
<
double
>
();
bool
solve_end
=
config
[
"solve_end"
].
as
<
bool
>
();
bool
solve_end
=
config
[
"solve_end"
].
as
<
bool
>
();
// Ceres setup
// Ceres setup
int
max_num_iterations
=
config
[
"max_num_iterations"
].
as
<
int
>
();
int
max_num_iterations
=
config
[
"max_num_iterations"
].
as
<
int
>
();
// estimator sensor noises
double
std_pbc_est
=
config
[
"std_pbc_est"
].
as
<
double
>
();
double
std_vbc_est
=
config
[
"std_vbc_est"
].
as
<
double
>
();
double
std_f_est
=
config
[
"std_f_est"
].
as
<
double
>
();
double
std_tau_est
=
config
[
"std_tau_est"
].
as
<
double
>
();
double
std_odom3d_est
=
config
[
"std_odom3d_est"
].
as
<
double
>
();
// priors
// priors
double
std_prior_p
=
config
[
"std_prior_p"
].
as
<
double
>
();
double
std_prior_p
=
config
[
"std_prior_p"
].
as
<
double
>
();
double
std_prior_o
=
config
[
"std_prior_o"
].
as
<
double
>
();
double
std_prior_o
=
config
[
"std_prior_o"
].
as
<
double
>
();
double
std_prior_v
=
config
[
"std_prior_v"
].
as
<
double
>
();
double
std_prior_v
=
config
[
"std_prior_v"
].
as
<
double
>
();
double
std_abs_bias_pbc
=
config
[
"std_abs_bias_pbc"
].
as
<
double
>
();
double
std_abs_bias_acc
=
config
[
"std_abs_bias_acc"
].
as
<
double
>
();
double
std_abs_bias_acc
=
config
[
"std_abs_bias_acc"
].
as
<
double
>
();
double
std_abs_bias_gyro
=
config
[
"std_abs_bias_gyro"
].
as
<
double
>
();
double
std_abs_bias_gyro
=
config
[
"std_abs_bias_gyro"
].
as
<
double
>
();
double
std_bp_drift
=
config
[
"std_bp_drift"
].
as
<
double
>
();
std
::
vector
<
double
>
bias_imu_prior_vec
=
config
[
"bias_imu_prior"
].
as
<
std
::
vector
<
double
>>
();
std
::
vector
<
double
>
bias_imu_prior_vec
=
config
[
"bias_imu_prior"
].
as
<
std
::
vector
<
double
>>
();
Eigen
::
Map
<
Vector6d
>
bias_imu_prior
(
bias_imu_prior_vec
.
data
());
Eigen
::
Map
<
Vector6d
>
bias_imu_prior
(
bias_imu_prior_vec
.
data
());
std
::
vector
<
double
>
b_p_bi_vec
=
config
[
"b_p_bi"
].
as
<
std
::
vector
<
double
>>
();
std
::
vector
<
double
>
b_p_bi_vec
=
config
[
"b_p_bi"
].
as
<
std
::
vector
<
double
>>
();
std
::
vector
<
double
>
b_q_i_vec
=
config
[
"b_q_i"
].
as
<
std
::
vector
<
double
>>
();
std
::
vector
<
double
>
b_q_i_vec
=
config
[
"b_q_i"
].
as
<
std
::
vector
<
double
>>
();
std
::
vector
<
double
>
l_p_lg_vec
=
config
[
"l_p_lg"
].
as
<
std
::
vector
<
double
>>
();
Eigen
::
Map
<
Vector3d
>
b_p_bi
(
b_p_bi_vec
.
data
());
Eigen
::
Map
<
Vector3d
>
b_p_bi
(
b_p_bi_vec
.
data
());
Eigen
::
Map
<
Vector4d
>
b_qvec_i
(
b_q_i_vec
.
data
());
Eigen
::
Map
<
Vector4d
>
b_qvec_i
(
b_q_i_vec
.
data
());
Eigen
::
Map
<
Vector3d
>
l_p_lg
(
l_p_lg_vec
.
data
());
double
fz_thresh
=
config
[
"fz_thresh"
].
as
<
double
>
();
// MOCAP
// MOCAP
double
std_pose_p
=
config
[
"std_pose_p"
].
as
<
double
>
();
double
std_pose_p
=
config
[
"std_pose_p"
].
as
<
double
>
();
...
@@ -201,8 +184,8 @@ int main (int argc, char **argv) {
...
@@ -201,8 +184,8 @@ int main (int argc, char **argv) {
// SENSOR + PROCESSOR IMU
// SENSOR + PROCESSOR IMU
SensorBasePtr
sen_imu_base
=
problem
->
installSensor
(
"SensorImu"
,
"SenImu"
,
(
Vector7d
()
<<
0
,
0
,
0
,
0
,
0
,
0
,
1
).
finished
(),
bodydynamics_root_dir
+
"/demos/sensor_imu_solo12.yaml"
);
SensorBasePtr
sen_imu_base
=
problem
->
installSensor
(
"SensorImu"
,
"SenImu"
,
(
Vector7d
()
<<
0
,
0
,
0
,
0
,
0
,
0
,
1
).
finished
(),
bodydynamics_root_dir
+
"/demos/sensor_imu_solo12.yaml"
);
SensorImuPtr
sen_imu
=
std
::
static_pointer_cast
<
SensorImu
>
(
sen_imu_base
);
SensorImuPtr
sen_imu
=
std
::
static_pointer_cast
<
SensorImu
>
(
sen_imu_base
);
ProcessorBasePtr
proc_
ftp
_base
=
problem
->
installProcessor
(
"ProcessorImu"
,
"imu pre-integrator"
,
"SenImu"
,
bodydynamics_root_dir
+
"/demos/processor_imu_solo12.yaml"
);
ProcessorBasePtr
proc_
imu
_base
=
problem
->
installProcessor
(
"ProcessorImu"
,
"imu pre-integrator"
,
"SenImu"
,
bodydynamics_root_dir
+
"/demos/processor_imu_solo12.yaml"
);
ProcessorImuPtr
proc_imu
=
std
::
static_pointer_cast
<
ProcessorImu
>
(
proc_
ftp
_base
);
ProcessorImuPtr
proc_imu
=
std
::
static_pointer_cast
<
ProcessorImu
>
(
proc_
imu
_base
);
// SENSOR + PROCESSOR POSE (for mocap)
// SENSOR + PROCESSOR POSE (for mocap)
// pose sensor and proc (to get extrinsics in the prb)
// pose sensor and proc (to get extrinsics in the prb)
...
@@ -226,10 +209,13 @@ int main (int argc, char **argv) {
...
@@ -226,10 +209,13 @@ int main (int argc, char **argv) {
sensor_pose
->
fixExtrinsics
();
sensor_pose
->
fixExtrinsics
();
}
}
VectorComposite
x_origin
(
"POV"
,
{
w_p_wm
,
w_qvec_wm
,
Vector3d
::
Zero
()});
Matrix3d
w_R_m_init
=
q2R
(
w_qvec_wm
);
Vector3d
w_p_wi_init
=
w_p_wm
+
w_R_m_init
*
b_p_bi
;
VectorComposite
x_origin
(
"POV"
,
{
w_p_wi_init
,
w_qvec_wm
,
Vector3d
::
Zero
()});
VectorComposite
std_origin
(
"POV"
,
{
std_prior_p
*
Vector3d
::
Ones
(),
std_prior_o
*
Vector3d
::
Ones
(),
std_prior_v
*
Vector3d
::
Ones
()});
VectorComposite
std_origin
(
"POV"
,
{
std_prior_p
*
Vector3d
::
Ones
(),
std_prior_o
*
Vector3d
::
Ones
(),
std_prior_v
*
Vector3d
::
Ones
()});
// FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005);
// FrameBasePtr KF1 = problem->setPriorFactor(x_origin, std_origin, t0, 0.0005);
FrameBasePtr
KF1
=
problem
->
setPriorInitialGuess
(
x_origin
,
t0
,
0.0005
);
// if mocap used
FrameBasePtr
KF1
=
problem
->
setPriorInitialGuess
(
x_origin
,
t0
,
time_tolerance_mocap
);
// if mocap used
proc_imu
->
setOrigin
(
KF1
);
proc_imu
->
setOrigin
(
KF1
);
...
@@ -259,6 +245,10 @@ int main (int argc, char **argv) {
...
@@ -259,6 +245,10 @@ int main (int argc, char **argv) {
double
*
imu_bias_fbk_carr
=
new
double
[
6
*
N
];
double
*
imu_bias_fbk_carr
=
new
double
[
6
*
N
];
double
*
extr_mocap_fbk_carr
=
new
double
[
7
*
N
];
double
*
extr_mocap_fbk_carr
=
new
double
[
7
*
N
];
// covariances computed on the fly
std
::
vector
<
Vector6d
>
Qbi_fbk_v
;
Qbi_fbk_v
.
push_back
(
Vector6d
::
Zero
());
//
std
::
vector
<
Vector3d
>
i_omg_oi_v
;
std
::
vector
<
Vector3d
>
i_omg_oi_v
;
//////////////////////////////////////////
//////////////////////////////////////////
...
@@ -266,22 +256,12 @@ int main (int argc, char **argv) {
...
@@ -266,22 +256,12 @@ int main (int argc, char **argv) {
for
(
unsigned
int
i
=
0
;
i
<
N
;
i
++
){
for
(
unsigned
int
i
=
0
;
i
<
N
;
i
++
){
TimeStamp
ts
(
t_arr
[
i
]);
TimeStamp
ts
(
t_arr
[
i
]);
////////////////////////////////////
// Retrieve current state
VectorComposite
curr_state
=
problem
->
getState
();
Vector4d
o_qvec_i_curr
=
curr_state
[
'O'
];
Quaterniond
o_q_i_curr
(
o_qvec_i_curr
);
Vector3d
o_v_oi_curr
=
curr_state
[
'V'
];
////////////////////////////////////
////////////////////////////////////
////////////////////////////////////
////////////////////////////////////
// Get measurements
// Get measurements
// retrieve traj data
// retrieve traj data
Eigen
::
Map
<
Vector3d
>
imu_acc
(
imu_acc_arr
+
3
*
i
);
Eigen
::
Map
<
Vector3d
>
imu_acc
(
imu_acc_arr
+
3
*
i
);
Eigen
::
Map
<
Vector3d
>
i_omg_oi
(
i_omg_oi_arr
+
3
*
i
);
// Hyp: b=i (not the case)
Eigen
::
Map
<
Vector3d
>
i_omg_oi
(
i_omg_oi_arr
+
3
*
i
);
// Hyp: b=i (not the case)
Eigen
::
Map
<
Matrix
<
double
,
12
,
1
>>
tau
(
tau_arr
+
12
*
i
);
Eigen
::
Map
<
Matrix
<
double
,
12
,
1
>>
qa
(
qa_arr
+
12
*
i
);
Eigen
::
Map
<
Matrix
<
double
,
12
,
1
>>
dqa
(
dqa_arr
+
12
*
i
);
Eigen
::
Map
<
Vector3d
>
w_p_wm
(
w_p_wm_arr
+
3
*
i
);
Eigen
::
Map
<
Vector3d
>
w_p_wm
(
w_p_wm_arr
+
3
*
i
);
Eigen
::
Map
<
Vector4d
>
w_qvec_wm
(
w_q_m_arr
+
4
*
i
);
Eigen
::
Map
<
Vector4d
>
w_qvec_wm
(
w_q_m_arr
+
4
*
i
);
w_qvec_wm
.
normalize
();
// not close enough to wolf eps sometimes
w_qvec_wm
.
normalize
();
// not close enough to wolf eps sometimes
...
@@ -311,11 +291,30 @@ int main (int argc, char **argv) {
...
@@ -311,11 +291,30 @@ int main (int argc, char **argv) {
std
::
string
report
=
solver
->
solve
(
SolverManager
::
ReportVerbosity
::
BRIEF
);
std
::
string
report
=
solver
->
solve
(
SolverManager
::
ReportVerbosity
::
BRIEF
);
std
::
cout
<<
ts
<<
" "
;
std
::
cout
<<
ts
<<
" "
;
std
::
cout
<<
report
<<
std
::
endl
;
std
::
cout
<<
report
<<
std
::
endl
;
nb_kf
=
problem
->
getTrajectory
()
->
getFrameMap
().
size
();
// recover covariances at this point
auto
kf_last
=
problem
->
getTrajectory
()
->
getFrameMap
().
rbegin
()
->
second
;
CaptureBasePtr
cap_imu
=
kf_last
->
getCaptureOf
(
sen_imu
);
Eigen
::
MatrixXd
Qbi_fbk
=
Eigen
::
Matrix6d
::
Identity
();
solver
->
computeCovariances
(
cap_imu
->
getStateBlockVec
());
// not computed by ALL and destroys other computed covs -> issue to raise
problem
->
getCovarianceBlock
(
cap_imu
->
getSensorIntrinsic
(),
Qbi_fbk
);
Vector6d
Qbi_fbk_diag
=
Qbi_fbk
.
diagonal
();
Qbi_fbk_v
.
push_back
(
Qbi_fbk_diag
);
// std::cout << "Qbi_fbk: \n" << Qbi_fbk.topLeftCorner<3,3>() << std::endl;
// std::cout << "Qbi_fbk: \n" << Qbi_fbk << std::endl;
nb_kf
++
;
}
}
// Store current state estimation
// Store current state estimation
VectorComposite
state_fbk
=
problem
->
getState
(
ts
);
VectorComposite
state_fbk
=
problem
->
getState
();
// VectorComposite state_fbk = problem->getState(ts);
Vector3d
o_p_oi
=
state_fbk
[
'P'
];
Vector3d
o_p_oi
=
state_fbk
[
'P'
];
Vector4d
o_qvec_i
=
state_fbk
[
'O'
];
Vector4d
o_qvec_i
=
state_fbk
[
'O'
];
Vector3d
o_v_oi
=
state_fbk
[
'V'
];
Vector3d
o_v_oi
=
state_fbk
[
'V'
];
...
@@ -326,11 +325,12 @@ int main (int argc, char **argv) {
...
@@ -326,11 +325,12 @@ int main (int argc, char **argv) {
Matrix3d
o_R_b
=
o_R_i
*
i_R_b
;
Matrix3d
o_R_b
=
o_R_i
*
i_R_b
;
Vector4d
o_qvec_b
=
Quaterniond
(
o_R_b
).
coeffs
();
Vector4d
o_qvec_b
=
Quaterniond
(
o_R_b
).
coeffs
();
Vector3d
o_p_ob
=
o_p_oi
+
o_R_i
*
i_p_ib
;
Vector3d
o_p_ob
=
o_p_oi
+
o_R_i
*
i_p_ib
;
Vector3d
o_v_ob
=
o_v_oi
+
o_R_i
*
i_omg_oi_unbiased
.
cross
(
o_R_i
*
b_p_bi
);
Vector3d
o_v_ob
=
o_v_oi
+
o_R_i
*
i_omg_oi_unbiased
.
cross
(
o_R_i
*
i_R_b
*
b_p_bi
);
// Vector3d o_p_ob = o_p_oi;
// Vector3d o_p_ob = o_p_oi;
// Vector3d o_v_ob = o_v_oi;
// Vector3d o_v_ob = o_v_oi;
imu_bias
=
sen_imu
->
getIntrinsic
()
->
getState
();
imu_bias
=
sen_imu
->
getIntrinsic
()
->
getState
();
// imu_bias = sen_imu->getIntrinsic(ts)->getState();
Vector7d
extr_mocap_est
;
extr_mocap_est
<<
sensor_pose
->
getP
()
->
getState
(),
sensor_pose
->
getO
()
->
getState
();
Vector7d
extr_mocap_est
;
extr_mocap_est
<<
sensor_pose
->
getP
()
->
getState
(),
sensor_pose
->
getO
()
->
getState
();
mempcpy
(
o_p_ob_fbk_carr
+
3
*
i
,
o_p_ob
.
data
(),
3
*
sizeof
(
double
));
mempcpy
(
o_p_ob_fbk_carr
+
3
*
i
,
o_p_ob
.
data
(),
3
*
sizeof
(
double
));
...
@@ -394,6 +394,9 @@ int main (int argc, char **argv) {
...
@@ -394,6 +394,9 @@ int main (int argc, char **argv) {
double
*
Qv_carr
=
new
double
[
3
*
Nkf
];
double
*
Qv_carr
=
new
double
[
3
*
Nkf
];
double
*
Qbi_carr
=
new
double
[
6
*
Nkf
];
double
*
Qbi_carr
=
new
double
[
6
*
Nkf
];
double
*
Qm_carr
=
new
double
[
6
*
Nkf
];
double
*
Qm_carr
=
new
double
[
6
*
Nkf
];
// feedback covariances
double
*
Qbi_fbk_carr
=
new
double
[
6
*
Nkf
];
// factor errors
// factor errors
double
*
fac_imu_err_carr
=
new
double
[
9
*
Nkf
];
double
*
fac_imu_err_carr
=
new
double
[
9
*
Nkf
];
double
*
fac_pose_err_carr
=
new
double
[
6
*
Nkf
];
double
*
fac_pose_err_carr
=
new
double
[
6
*
Nkf
];
...
@@ -440,6 +443,10 @@ int main (int argc, char **argv) {
...
@@ -440,6 +443,10 @@ int main (int argc, char **argv) {
memcpy
(
Qbi_carr
+
6
*
i
,
Qbi_diag
.
data
(),
6
*
sizeof
(
double
));
memcpy
(
Qbi_carr
+
6
*
i
,
Qbi_diag
.
data
(),
6
*
sizeof
(
double
));
memcpy
(
Qm_carr
+
6
*
i
,
Qm_diag
.
data
(),
6
*
sizeof
(
double
));
memcpy
(
Qm_carr
+
6
*
i
,
Qm_diag
.
data
(),
6
*
sizeof
(
double
));
// Recover feedback covariances
memcpy
(
Qbi_fbk_carr
+
6
*
i
,
Qbi_fbk_v
[
i
].
data
(),
6
*
sizeof
(
double
));
// Factor errors
// Factor errors
// std::cout << "Factors " << kf->getTimeStamp().get() << std::endl;
// std::cout << "Factors " << kf->getTimeStamp().get() << std::endl;
FactorBasePtrList
fac_lst
;
FactorBasePtrList
fac_lst
;
...
@@ -507,6 +514,7 @@ int main (int argc, char **argv) {
...
@@ -507,6 +514,7 @@ int main (int argc, char **argv) {
cnpy
::
npz_save
(
out_npz_file_path
,
"Qo"
,
Qo_carr
,
{
Nkf
,
3
},
"a"
);
cnpy
::
npz_save
(
out_npz_file_path
,
"Qo"
,
Qo_carr
,
{
Nkf
,
3
},
"a"
);
cnpy
::
npz_save
(
out_npz_file_path
,
"Qv"
,
Qv_carr
,
{
Nkf
,
3
},
"a"
);
cnpy
::
npz_save
(
out_npz_file_path
,
"Qv"
,
Qv_carr
,
{
Nkf
,
3
},
"a"
);
cnpy
::
npz_save
(
out_npz_file_path
,
"Qbi"
,
Qbi_carr
,
{
Nkf
,
6
},
"a"
);
cnpy
::
npz_save
(
out_npz_file_path
,
"Qbi"
,
Qbi_carr
,
{
Nkf
,
6
},
"a"
);
cnpy
::
npz_save
(
out_npz_file_path
,
"Qbi_fbk"
,
Qbi_fbk_carr
,
{
Nkf
,
6
},
"a"
);
cnpy
::
npz_save
(
out_npz_file_path
,
"Qm"
,
Qm_carr
,
{
Nkf
,
6
},
"a"
);
cnpy
::
npz_save
(
out_npz_file_path
,
"Qm"
,
Qm_carr
,
{
Nkf
,
6
},
"a"
);
cnpy
::
npz_save
(
out_npz_file_path
,
"fac_imu_err"
,
fac_imu_err_carr
,
{
Nkf
,
9
},
"a"
);
cnpy
::
npz_save
(
out_npz_file_path
,
"fac_imu_err"
,
fac_imu_err_carr
,
{
Nkf
,
9
},
"a"
);
cnpy
::
npz_save
(
out_npz_file_path
,
"fac_pose_err"
,
fac_pose_err_carr
,
{
Nkf
,
6
},
"a"
);
cnpy
::
npz_save
(
out_npz_file_path
,
"fac_pose_err"
,
fac_pose_err_carr
,
{
Nkf
,
6
},
"a"
);
...
...
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