Skip to content
GitLab
Menu
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
mobile_robotics
wolf_projects
wolf_lib
plugins
bodydynamics
Commits
4f10166a
Commit
4f10166a
authored
Jun 24, 2022
by
Joan Solà Ortega
Browse files
use derived state blocks
parent
78247984
Changes
4
Hide whitespace changes
Inline
Side-by-side
src/capture/capture_inertial_kinematics.cpp
View file @
4f10166a
...
...
@@ -23,6 +23,8 @@
#include
"bodydynamics/capture/capture_inertial_kinematics.h"
#include
"bodydynamics/sensor/sensor_inertial_kinematics.h"
#include
"core/state_block/state_quaternion.h"
#include
"core/state_block/state_block_derived.h"
namespace
wolf
{
...
...
@@ -37,7 +39,7 @@ CaptureInertialKinematics::CaptureInertialKinematics(const TimeStamp& _init_ts,
_sensor_ptr
,
nullptr
,
nullptr
,
std
::
make_shared
<
State
Block
>
(
_bias_pbc
,
false
)),
std
::
make_shared
<
State
Params3
>
(
_bias_pbc
,
false
)),
data_
(
_data
),
B_I_q_
(
_B_I_q
),
B_Lc_m_
(
_B_Lc_m
)
...
...
@@ -56,7 +58,7 @@ CaptureInertialKinematics::CaptureInertialKinematics(const TimeStamp& _init_ts,
_sensor_ptr
,
nullptr
,
nullptr
,
std
::
make_shared
<
State
Block
>
(
3
,
false
)),
std
::
make_shared
<
State
Params3
>
(
Vector3d
::
Zero
()
,
false
)),
data_
(
_data
),
B_I_q_
(
_B_I_q
),
B_Lc_m_
(
_B_Lc_m
)
...
...
src/sensor/sensor_inertial_kinematics.cpp
View file @
4f10166a
...
...
@@ -21,7 +21,7 @@
//--------LICENSE_END--------
#include
"bodydynamics/sensor/sensor_inertial_kinematics.h"
#include
"core/state_block/state_block.h"
#include
"core/state_block/state_block
_derived
.h"
#include
"core/state_block/state_quaternion.h"
namespace
wolf
{
...
...
@@ -30,7 +30,7 @@ SensorInertialKinematics::SensorInertialKinematics(const Eigen::VectorXd& _extri
SensorBase
(
"SensorInertialKinematics"
,
nullptr
,
// no position
nullptr
,
// no orientation
std
::
make_shared
<
State
Block
>
(
Eigen
::
Vector3d
(
0
,
0
,
0
),
false
),
// bias; false = unfixed
std
::
make_shared
<
State
Params3
>
(
Eigen
::
Vector3d
(
0
,
0
,
0
),
false
),
// bias; false = unfixed
(
Eigen
::
Vector6d
()
<<
_params
->
std_pbc
,
_params
->
std_pbc
,
_params
->
std_pbc
,
_params
->
std_vbc
,
_params
->
std_vbc
,
_params
->
std_vbc
).
finished
(),
false
,
false
,
true
),
...
...
test/gtest_factor_force_torque.cpp
View file @
4f10166a
...
...
@@ -58,6 +58,7 @@ FactorInertialKinematics_2KF_1v_bfix,ZeroMvt:
#include
<core/capture/capture_base.h>
#include
<core/feature/feature_base.h>
#include
<core/factor/factor_block_absolute.h>
#include
<core/state_block/state_block_derived.h>
// Imu
// #include "imu/internal/config.h"
...
...
@@ -169,18 +170,26 @@ void perturbateAllIfUnFixed(const FrameBasePtr& KF)
}
}
FrameBasePtr
createKFWithCDLI
(
const
ProblemPtr
&
problem
,
const
TimeStamp
&
t
,
VectorComposite
x_origin
,
Vector3d
c
,
Vector3d
cd
,
Vector3d
Lc
,
Vector6d
bias_imu
)
FrameBasePtr
createKFWithCDLI
(
const
ProblemPtr
&
problem
,
const
TimeStamp
&
t
,
VectorComposite
x_origin
,
Vector3d
c
,
Vector3d
cd
,
Vector3d
Lc
,
Vector6d
bias_imu
)
{
FrameBasePtr
KF
=
FrameBase
::
emplace
<
FrameBase
>
(
problem
->
getTrajectory
(),
t
,
"POV"
,
x_origin
);
StateBlockPtr
sbc
=
make_shared
<
StateBlock
>
(
c
);
KF
->
addStateBlock
(
'C'
,
sbc
,
problem
);
StateBlockPtr
sbd
=
make_shared
<
StateBlock
>
(
cd
);
KF
->
addStateBlock
(
'D'
,
sbd
,
problem
);
StateBlockPtr
sbL
=
make_shared
<
StateBlock
>
(
Lc
);
KF
->
addStateBlock
(
'L'
,
sbL
,
problem
);
StateBlockPtr
sbbimu
=
make_shared
<
StateBlock
>
(
bias_imu
);
KF
->
addStateBlock
(
'I'
,
sbbimu
,
problem
);
// Simulating IMU
FrameBasePtr
KF
=
FrameBase
::
emplace
<
FrameBase
>
(
problem
->
getTrajectory
(),
t
,
"POV"
,
x_origin
);
StateBlockPtr
sbc
=
make_shared
<
StatePoint3d
>
(
c
);
KF
->
addStateBlock
(
'C'
,
sbc
,
problem
);
StateBlockPtr
sbd
=
make_shared
<
StateVector3d
>
(
cd
);
KF
->
addStateBlock
(
'D'
,
sbd
,
problem
);
StateBlockPtr
sbL
=
make_shared
<
StateVector3d
>
(
Lc
);
KF
->
addStateBlock
(
'L'
,
sbL
,
problem
);
StateBlockPtr
sbbimu
=
make_shared
<
StateParams6
>
(
bias_imu
);
KF
->
addStateBlock
(
'I'
,
sbbimu
,
problem
);
// Simulating IMU
return
KF
;
}
class
FactorInertialKinematics_2KF
:
public
testing
::
Test
{
public:
...
...
@@ -247,10 +256,10 @@ class FactorInertialKinematics_2KF : public testing::Test
// ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
bias_p_
=
zero3
;
bias_imu_
=
zero6
;
StateBlockPtr
sbcA
=
make_shared
<
State
Block
>
(
zero3
);
KFA_
->
addStateBlock
(
'C'
,
sbcA
,
problem_
);
StateBlockPtr
sbdA
=
make_shared
<
State
Block
>
(
zero3
);
KFA_
->
addStateBlock
(
'D'
,
sbdA
,
problem_
);
StateBlockPtr
sbLA
=
make_shared
<
State
Block
>
(
zero3
);
KFA_
->
addStateBlock
(
'L'
,
sbLA
,
problem_
);
StateBlockPtr
sbbimuA
=
make_shared
<
State
Block
>
(
bias_imu_
);
KFA_
->
addStateBlock
(
'I'
,
sbbimuA
,
problem_
);
StateBlockPtr
sbcA
=
make_shared
<
State
Point3d
>
(
zero3
);
KFA_
->
addStateBlock
(
'C'
,
sbcA
,
problem_
);
StateBlockPtr
sbdA
=
make_shared
<
State
Vector3d
>
(
zero3
);
KFA_
->
addStateBlock
(
'D'
,
sbdA
,
problem_
);
StateBlockPtr
sbLA
=
make_shared
<
State
Vector3d
>
(
zero3
);
KFA_
->
addStateBlock
(
'L'
,
sbLA
,
problem_
);
StateBlockPtr
sbbimuA
=
make_shared
<
State
Params6
>
(
bias_imu_
);
KFA_
->
addStateBlock
(
'I'
,
sbbimuA
,
problem_
);
// Fix the one we cannot estimate
// KFA_->getP()->fix();
...
...
test/gtest_factor_inertial_kinematics.cpp
View file @
4f10166a
...
...
@@ -58,6 +58,7 @@ FactorInertialKinematics_1KF_1v_bfix,ZeroMvt:
#include
<core/capture/capture_base.h>
#include
<core/feature/feature_base.h>
#include
<core/factor/factor_block_absolute.h>
#include
<core/state_block/state_block_derived.h>
// Imu
//#include "imu/internal/config.h"
...
...
@@ -72,6 +73,7 @@ FactorInertialKinematics_1KF_1v_bfix,ZeroMvt:
#include
"bodydynamics/capture/capture_inertial_kinematics.h"
#include
"bodydynamics/feature/feature_inertial_kinematics.h"
#include
"bodydynamics/factor/factor_inertial_kinematics.h"
#include
<core/state_block/state_block_derived.h>
#include
<Eigen/Eigenvalues>
...
...
@@ -126,11 +128,11 @@ class FactorInertialKinematics_1KF : public testing::Test
// ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
bias_p_
=
ZERO3
;
bias_imu_
=
ZERO6
;
StateBlockPtr
sbc
=
make_shared
<
State
Block
>
(
ZERO3
);
StateBlockPtr
sb
d
=
make_shared
<
State
Block
>
(
ZERO3
);
StateBlockPtr
sb
L
=
make_shared
<
State
Block
>
(
ZERO3
);
StateBlockPtr
sbb
=
make_shared
<
State
Block
>
(
bias_p_
);
sbbimu_
=
make_shared
<
State
Block
>
(
bias_imu_
);
StateBlockPtr
sbc
=
make_shared
<
State
Point3d
>
(
ZERO3
);
StateBlockPtr
sb
L
=
make_shared
<
State
Vector3d
>
(
ZERO3
);
StateBlockPtr
sb
d
=
make_shared
<
State
Vector3d
>
(
ZERO3
);
StateBlockPtr
sbb
=
make_shared
<
State
Params3
>
(
bias_p_
);
sbbimu_
=
make_shared
<
State
Params6
>
(
bias_imu_
);
KF0_
->
addStateBlock
(
'C'
,
sbc
,
problem_
);
KF0_
->
addStateBlock
(
'D'
,
sbd
,
problem_
);
...
...
@@ -328,7 +330,7 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
// Vector6d ZERO6; ZERO6 << ZERO3, ZERO3;
// Vector3d bias_p_ = ZERO3;
// Vector6d bias_imu_ = ZERO6;
// StateBlockPtr sbc = make_shared<State
Block
>(ZERO3);
// StateBlockPtr sbc = make_shared<State
Point3d
>(ZERO3);
// StateBlockPtr sbd = make_shared<StateBlock>(ZERO3);
// StateBlockPtr sbL = make_shared<StateBlock>(ZERO3);
// StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment