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
2696ac9e
Commit
2696ac9e
authored
5 years ago
by
Joan Solà Ortega
Browse files
Options
Downloads
Patches
Plain Diff
Upgrade to new IsMotion API
parent
2ecb4add
No related branches found
Branches containing commit
No related tags found
Tags containing commit
2 merge requests
!39
release after RAL
,
!38
After 2nd RAL submission
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
test/gtest_feature_IMU.cpp
+8
-7
8 additions, 7 deletions
test/gtest_feature_IMU.cpp
test/gtest_processor_IMU.cpp
+18
-16
18 additions, 16 deletions
test/gtest_processor_IMU.cpp
with
26 additions
and
23 deletions
test/gtest_feature_IMU.cpp
+
8
−
7
View file @
2696ac9e
...
@@ -24,6 +24,7 @@ class FeatureIMU_test : public testing::Test
...
@@ -24,6 +24,7 @@ class FeatureIMU_test : public testing::Test
wolf
::
FrameBasePtr
origin_frame
;
wolf
::
FrameBasePtr
origin_frame
;
Eigen
::
MatrixXd
dD_db_jacobians
;
Eigen
::
MatrixXd
dD_db_jacobians
;
wolf
::
ProcessorBasePtr
processor_ptr_
;
wolf
::
ProcessorBasePtr
processor_ptr_
;
wolf
::
ProcessorMotionPtr
processor_motion_ptr_
;
//a new of this will be created for each new test
//a new of this will be created for each new test
virtual
void
SetUp
()
virtual
void
SetUp
()
...
@@ -45,6 +46,7 @@ class FeatureIMU_test : public testing::Test
...
@@ -45,6 +46,7 @@ class FeatureIMU_test : public testing::Test
prc_imu_params
->
dist_traveled
=
5
;
prc_imu_params
->
dist_traveled
=
5
;
prc_imu_params
->
angle_turned
=
0.5
;
prc_imu_params
->
angle_turned
=
0.5
;
processor_ptr_
=
problem
->
installProcessor
(
"ProcessorIMU"
,
"IMU pre-integrator"
,
sensor_ptr
,
prc_imu_params
);
processor_ptr_
=
problem
->
installProcessor
(
"ProcessorIMU"
,
"IMU pre-integrator"
,
sensor_ptr
,
prc_imu_params
);
processor_motion_ptr_
=
std
::
static_pointer_cast
<
ProcessorMotion
>
(
processor_ptr_
);
// Time and data variables
// Time and data variables
TimeStamp
t
;
TimeStamp
t
;
...
@@ -80,16 +82,15 @@ class FeatureIMU_test : public testing::Test
...
@@ -80,16 +82,15 @@ class FeatureIMU_test : public testing::Test
sensor_ptr
->
process
(
imu_ptr
);
sensor_ptr
->
process
(
imu_ptr
);
//emplace Frame
//emplace Frame
ts
=
problem
->
getProcessorMotion
()
->
getBuffer
().
get
().
back
().
ts_
;
problem
->
getCurrentStateAndStamp
(
state_vec
,
ts
);
state_vec
=
problem
->
getProcessorMotion
()
->
getCurrentState
();
last_frame
=
problem
->
emplaceFrame
(
KEY
,
state_vec
,
ts
);
last_frame
=
problem
->
emplaceFrame
(
KEY
,
state_vec
,
ts
);
//emplace a feature
//emplace a feature
delta_preint
=
problem
->
getP
rocessor
M
otion
()
->
getMotion
().
delta_integr_
;
delta_preint
=
p
rocessor
_m
otion
_ptr_
->
getMotion
().
delta_integr_
;
delta_preint_cov
=
problem
->
getP
rocessor
M
otion
()
->
getMotion
().
delta_integr_cov_
+
MatrixXd
::
Identity
(
9
,
9
)
*
1e-08
;
delta_preint_cov
=
p
rocessor
_m
otion
_ptr_
->
getMotion
().
delta_integr_cov_
+
MatrixXd
::
Identity
(
9
,
9
)
*
1e-08
;
VectorXd
calib_preint
=
pro
blem
->
getPro
cessor
M
otion
()
->
getLast
()
->
getCalibrationPreint
();
VectorXd
calib_preint
=
processor
_m
otion
_ptr_
->
getLast
()
->
getCalibrationPreint
();
dD_db_jacobians
=
problem
->
getP
rocessor
M
otion
()
->
getMotion
().
jacobian_calib_
;
dD_db_jacobians
=
p
rocessor
_m
otion
_ptr_
->
getMotion
().
jacobian_calib_
;
feat_imu
=
std
::
static_pointer_cast
<
FeatureIMU
>
(
feat_imu
=
std
::
static_pointer_cast
<
FeatureIMU
>
(
FeatureBase
::
emplace
<
FeatureIMU
>
(
imu_ptr
,
FeatureBase
::
emplace
<
FeatureIMU
>
(
imu_ptr
,
delta_preint
,
delta_preint
,
delta_preint_cov
,
delta_preint_cov
,
...
...
This diff is collapsed.
Click to expand it.
test/gtest_processor_IMU.cpp
+
18
−
16
View file @
2696ac9e
...
@@ -30,6 +30,7 @@ class ProcessorIMUt : public testing::Test
...
@@ -30,6 +30,7 @@ class ProcessorIMUt : public testing::Test
public:
//These can be accessed in fixtures
public:
//These can be accessed in fixtures
wolf
::
ProblemPtr
problem
;
wolf
::
ProblemPtr
problem
;
wolf
::
SensorBasePtr
sensor_ptr
;
wolf
::
SensorBasePtr
sensor_ptr
;
wolf
::
ProcessorMotionPtr
processor_motion
;
wolf
::
TimeStamp
t
;
wolf
::
TimeStamp
t
;
double
mti_clock
,
tmp
;
double
mti_clock
,
tmp
;
double
dt
;
double
dt
;
...
@@ -55,6 +56,7 @@ class ProcessorIMUt : public testing::Test
...
@@ -55,6 +56,7 @@ class ProcessorIMUt : public testing::Test
Vector7d
extrinsics
=
(
Vector7d
()
<<
0
,
0
,
0
,
0
,
0
,
0
,
1
).
finished
();
Vector7d
extrinsics
=
(
Vector7d
()
<<
0
,
0
,
0
,
0
,
0
,
0
,
1
).
finished
();
sensor_ptr
=
problem
->
installSensor
(
"SensorIMU"
,
"Main IMU"
,
extrinsics
,
wolf_root
+
"/demos/sensor_imu.yaml"
);
sensor_ptr
=
problem
->
installSensor
(
"SensorIMU"
,
"Main IMU"
,
extrinsics
,
wolf_root
+
"/demos/sensor_imu.yaml"
);
ProcessorBasePtr
processor_ptr
=
problem
->
installProcessor
(
"ProcessorIMU"
,
"IMU pre-integrator"
,
"Main IMU"
,
wolf_root
+
"/demos/processor_imu.yaml"
);
ProcessorBasePtr
processor_ptr
=
problem
->
installProcessor
(
"ProcessorIMU"
,
"IMU pre-integrator"
,
"Main IMU"
,
wolf_root
+
"/demos/processor_imu.yaml"
);
processor_motion
=
std
::
static_pointer_cast
<
ProcessorMotion
>
(
processor_ptr
);
// Time and data variables
// Time and data variables
data
=
Vector6d
::
Zero
();
data
=
Vector6d
::
Zero
();
...
@@ -221,8 +223,8 @@ TEST_F(ProcessorIMUt, acc_x)
...
@@ -221,8 +223,8 @@ TEST_F(ProcessorIMUt, acc_x)
Vector6d
b
;
b
<<
0
,
0
,
0
,
0
,
0
,
0
;
Vector6d
b
;
b
<<
0
,
0
,
0
,
0
,
0
,
0
;
ASSERT_MATRIX_APPROX
(
problem
->
getCurrentState
().
head
(
10
)
,
x
,
wolf
::
Constants
::
EPS_SMALL
);
ASSERT_MATRIX_APPROX
(
problem
->
getCurrentState
().
head
(
10
)
,
x
,
wolf
::
Constants
::
EPS_SMALL
);
ASSERT_MATRIX_APPROX
(
pro
blem
->
getPro
cessor
M
otion
()
->
getLast
()
->
getCalibration
()
,
b
,
wolf
::
Constants
::
EPS_SMALL
);
ASSERT_MATRIX_APPROX
(
processor
_m
otion
->
getLast
()
->
getCalibration
()
,
b
,
wolf
::
Constants
::
EPS_SMALL
);
ASSERT_MATRIX_APPROX
(
pro
blem
->
getPro
cessor
M
otion
()
->
getLast
()
->
getCalibrationPreint
()
,
b
,
wolf
::
Constants
::
EPS_SMALL
);
ASSERT_MATRIX_APPROX
(
processor
_m
otion
->
getLast
()
->
getCalibrationPreint
()
,
b
,
wolf
::
Constants
::
EPS_SMALL
);
}
}
TEST_F
(
ProcessorIMUt
,
acc_y
)
TEST_F
(
ProcessorIMUt
,
acc_y
)
...
@@ -247,8 +249,8 @@ TEST_F(ProcessorIMUt, acc_y)
...
@@ -247,8 +249,8 @@ TEST_F(ProcessorIMUt, acc_y)
Vector6d
b
;
b
<<
0
,
0
,
0
,
0
,
0
,
0
;
Vector6d
b
;
b
<<
0
,
0
,
0
,
0
,
0
,
0
;
ASSERT_MATRIX_APPROX
(
problem
->
getCurrentState
().
head
(
10
)
,
x
,
wolf
::
Constants
::
EPS_SMALL
);
ASSERT_MATRIX_APPROX
(
problem
->
getCurrentState
().
head
(
10
)
,
x
,
wolf
::
Constants
::
EPS_SMALL
);
ASSERT_MATRIX_APPROX
(
pro
blem
->
getPro
cessor
M
otion
()
->
getLast
()
->
getCalibration
()
,
b
,
wolf
::
Constants
::
EPS_SMALL
);
ASSERT_MATRIX_APPROX
(
processor
_m
otion
->
getLast
()
->
getCalibration
()
,
b
,
wolf
::
Constants
::
EPS_SMALL
);
ASSERT_MATRIX_APPROX
(
pro
blem
->
getPro
cessor
M
otion
()
->
getLast
()
->
getCalibrationPreint
()
,
b
,
wolf
::
Constants
::
EPS_SMALL
);
ASSERT_MATRIX_APPROX
(
processor
_m
otion
->
getLast
()
->
getCalibrationPreint
()
,
b
,
wolf
::
Constants
::
EPS_SMALL
);
//do so for 5s
//do so for 5s
const
unsigned
int
iter
=
1000
;
//how many ms
const
unsigned
int
iter
=
1000
;
//how many ms
...
@@ -261,8 +263,8 @@ TEST_F(ProcessorIMUt, acc_y)
...
@@ -261,8 +263,8 @@ TEST_F(ProcessorIMUt, acc_y)
// advanced at a=20m/s2 during 1s ==> dx = 0.5*20*1^2 = 10; dvx = 20*1 = 20
// advanced at a=20m/s2 during 1s ==> dx = 0.5*20*1^2 = 10; dvx = 20*1 = 20
x
<<
0
,
10
,
0
,
0
,
0
,
0
,
1
,
0
,
20
,
0
;
x
<<
0
,
10
,
0
,
0
,
0
,
0
,
1
,
0
,
20
,
0
;
ASSERT_MATRIX_APPROX
(
problem
->
getCurrentState
().
head
(
10
)
,
x
,
wolf
::
Constants
::
EPS
);
ASSERT_MATRIX_APPROX
(
problem
->
getCurrentState
().
head
(
10
)
,
x
,
wolf
::
Constants
::
EPS
);
ASSERT_MATRIX_APPROX
(
pro
blem
->
getPro
cessor
M
otion
()
->
getLast
()
->
getCalibration
()
,
b
,
wolf
::
Constants
::
EPS
);
ASSERT_MATRIX_APPROX
(
processor
_m
otion
->
getLast
()
->
getCalibration
()
,
b
,
wolf
::
Constants
::
EPS
);
ASSERT_MATRIX_APPROX
(
pro
blem
->
getPro
cessor
M
otion
()
->
getLast
()
->
getCalibrationPreint
()
,
b
,
wolf
::
Constants
::
EPS
);
ASSERT_MATRIX_APPROX
(
processor
_m
otion
->
getLast
()
->
getCalibrationPreint
()
,
b
,
wolf
::
Constants
::
EPS
);
}
}
TEST_F
(
ProcessorIMUt
,
acc_z
)
TEST_F
(
ProcessorIMUt
,
acc_z
)
...
@@ -284,8 +286,8 @@ TEST_F(ProcessorIMUt, acc_z)
...
@@ -284,8 +286,8 @@ TEST_F(ProcessorIMUt, acc_z)
Vector6d
b
;
b
<<
0
,
0
,
0
,
0
,
0
,
0
;
Vector6d
b
;
b
<<
0
,
0
,
0
,
0
,
0
,
0
;
ASSERT_MATRIX_APPROX
(
problem
->
getCurrentState
().
head
(
10
)
,
x
,
wolf
::
Constants
::
EPS_SMALL
);
ASSERT_MATRIX_APPROX
(
problem
->
getCurrentState
().
head
(
10
)
,
x
,
wolf
::
Constants
::
EPS_SMALL
);
ASSERT_MATRIX_APPROX
(
pro
blem
->
getPro
cessor
M
otion
()
->
getLast
()
->
getCalibration
()
,
b
,
wolf
::
Constants
::
EPS_SMALL
);
ASSERT_MATRIX_APPROX
(
processor
_m
otion
->
getLast
()
->
getCalibration
()
,
b
,
wolf
::
Constants
::
EPS_SMALL
);
ASSERT_MATRIX_APPROX
(
pro
blem
->
getPro
cessor
M
otion
()
->
getLast
()
->
getCalibrationPreint
()
,
b
,
wolf
::
Constants
::
EPS_SMALL
);
ASSERT_MATRIX_APPROX
(
processor
_m
otion
->
getLast
()
->
getCalibrationPreint
()
,
b
,
wolf
::
Constants
::
EPS_SMALL
);
//do so for 5s
//do so for 5s
const
unsigned
int
iter
=
50
;
//how 0.1s
const
unsigned
int
iter
=
50
;
//how 0.1s
...
@@ -298,8 +300,8 @@ TEST_F(ProcessorIMUt, acc_z)
...
@@ -298,8 +300,8 @@ TEST_F(ProcessorIMUt, acc_z)
// advanced at a=2m/s2 during 5s ==> dz = 0.5*2*5^2 = 25; dvz = 2*5 = 10
// advanced at a=2m/s2 during 5s ==> dz = 0.5*2*5^2 = 25; dvz = 2*5 = 10
x
<<
0
,
0
,
25
,
0
,
0
,
0
,
1
,
0
,
0
,
10
;
x
<<
0
,
0
,
25
,
0
,
0
,
0
,
1
,
0
,
0
,
10
;
ASSERT_MATRIX_APPROX
(
problem
->
getCurrentState
().
head
(
10
)
,
x
,
wolf
::
Constants
::
EPS
);
ASSERT_MATRIX_APPROX
(
problem
->
getCurrentState
().
head
(
10
)
,
x
,
wolf
::
Constants
::
EPS
);
ASSERT_MATRIX_APPROX
(
pro
blem
->
getPro
cessor
M
otion
()
->
getLast
()
->
getCalibration
()
,
b
,
wolf
::
Constants
::
EPS
);
ASSERT_MATRIX_APPROX
(
processor
_m
otion
->
getLast
()
->
getCalibration
()
,
b
,
wolf
::
Constants
::
EPS
);
ASSERT_MATRIX_APPROX
(
pro
blem
->
getPro
cessor
M
otion
()
->
getLast
()
->
getCalibrationPreint
()
,
b
,
wolf
::
Constants
::
EPS
);
ASSERT_MATRIX_APPROX
(
processor
_m
otion
->
getLast
()
->
getCalibrationPreint
()
,
b
,
wolf
::
Constants
::
EPS
);
}
}
TEST_F
(
ProcessorIMUt
,
check_Covariance
)
TEST_F
(
ProcessorIMUt
,
check_Covariance
)
...
@@ -315,8 +317,8 @@ TEST_F(ProcessorIMUt, check_Covariance)
...
@@ -315,8 +317,8 @@ TEST_F(ProcessorIMUt, check_Covariance)
cap_imu_ptr
->
setTimeStamp
(
0.1
);
cap_imu_ptr
->
setTimeStamp
(
0.1
);
sensor_ptr
->
process
(
cap_imu_ptr
);
sensor_ptr
->
process
(
cap_imu_ptr
);
VectorXd
delta_preint
(
pro
blem
->
getPro
cessor
M
otion
()
->
getMotion
().
delta_integr_
);
VectorXd
delta_preint
(
processor
_m
otion
->
getMotion
().
delta_integr_
);
// Matrix<double,9,9> delta_preint_cov = pro
blem->getPro
cessor
M
otion
()
->getCurrentDeltaPreintCov();
// Matrix<double,9,9> delta_preint_cov = processor
_m
otion->getCurrentDeltaPreintCov();
ASSERT_FALSE
(
delta_preint
.
isMuchSmallerThan
(
1
,
wolf
::
Constants
::
EPS_SMALL
));
ASSERT_FALSE
(
delta_preint
.
isMuchSmallerThan
(
1
,
wolf
::
Constants
::
EPS_SMALL
));
// ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXd::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
// ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXd::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
...
@@ -382,8 +384,8 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
...
@@ -382,8 +384,8 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
// init things
// init things
problem
->
setPrior
(
x0
,
P0
,
t
,
0.01
);
problem
->
setPrior
(
x0
,
P0
,
t
,
0.01
);
pro
blem
->
getPro
cessor
M
otion
()
->
getOrigin
()
->
setCalibration
(
bias
);
processor
_m
otion
->
getOrigin
()
->
setCalibration
(
bias
);
pro
blem
->
getPro
cessor
M
otion
()
->
getLast
()
->
setCalibrationPreint
(
bias
);
processor
_m
otion
->
getLast
()
->
setCalibrationPreint
(
bias
);
// WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
// WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
// data
// data
...
@@ -438,8 +440,8 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
...
@@ -438,8 +440,8 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
Vector6d
bias
;
bias
<<
abx
,
aby
,
0
,
0
,
0
,
0
;
Vector6d
bias
;
bias
<<
abx
,
aby
,
0
,
0
,
0
,
0
;
Vector3d
acc_bias
=
bias
.
head
(
3
);
Vector3d
acc_bias
=
bias
.
head
(
3
);
pro
blem
->
getPro
cessor
M
otion
()
->
getOrigin
()
->
setCalibration
(
bias
);
processor
_m
otion
->
getOrigin
()
->
setCalibration
(
bias
);
pro
blem
->
getPro
cessor
M
otion
()
->
getLast
()
->
setCalibrationPreint
(
bias
);
processor
_m
otion
->
getLast
()
->
setCalibrationPreint
(
bias
);
double
rate_of_turn
=
5
*
M_PI
/
180.0
;
double
rate_of_turn
=
5
*
M_PI
/
180.0
;
// data << 0+abx, 0+aby, 9.806, rate_of_turn, rate_of_turn*1.5, 0; // measure gravity
// data << 0+abx, 0+aby, 9.806, rate_of_turn, rate_of_turn*1.5, 0; // measure gravity
...
...
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