Skip to content
Snippets Groups Projects
Commit 6735e1b5 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Use derived state blocks

parent e5d9b860
No related branches found
No related tags found
2 merge requests!54devel->main,!50Resolve "Bootstrap sequence"
......@@ -21,43 +21,51 @@
//--------LICENSE_END--------
#include "imu/capture/capture_imu.h"
#include "imu/sensor/sensor_imu.h"
#include "core/state_block/state_quaternion.h"
#include <core/state_block/state_quaternion.h>
#include <core/state_block/state_block_derived.h>
namespace wolf {
CaptureImu::CaptureImu(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
CaptureImu::CaptureImu(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::Vector6d& _acc_gyro_data,
const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr) :
CaptureMotion("CaptureImu",
_init_ts,
_sensor_ptr,
_acc_gyro_data,
_data_cov,
_capture_origin_ptr,
nullptr,
nullptr,
std::make_shared<StateBlock>((_sensor_ptr->getProblem()->getDim() == 2 ? 3 : 6), false, nullptr, false))
CaptureBasePtr _capture_origin_ptr)
: CaptureMotion(
"CaptureImu",
_init_ts,
_sensor_ptr,
_acc_gyro_data,
_data_cov,
_capture_origin_ptr,
nullptr,
nullptr,
(_sensor_ptr->getProblem()->getDim() == 2)
? std::static_pointer_cast<StateBlock>(std::make_shared<StateParams3>(Vector3d::Zero(), false))
: std::static_pointer_cast<StateBlock>(std::make_shared<StateParams6>(Vector6d::Zero(), false)))
{
//
}
CaptureImu::CaptureImu(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
CaptureImu::CaptureImu(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::Vector6d& _acc_gyro_data,
const Eigen::MatrixXd& _data_cov,
const VectorXd& _bias,
CaptureBasePtr _capture_origin_ptr) :
CaptureMotion("CaptureImu",
_init_ts,
_sensor_ptr,
_acc_gyro_data,
_data_cov,
_capture_origin_ptr,
nullptr,
nullptr,
std::make_shared<StateBlock>(_bias, false, nullptr, false))
const VectorXd& _bias,
CaptureBasePtr _capture_origin_ptr)
: CaptureMotion(
"CaptureImu",
_init_ts,
_sensor_ptr,
_acc_gyro_data,
_data_cov,
_capture_origin_ptr,
nullptr,
nullptr,
(_bias.size() == 2)
? std::static_pointer_cast<StateBlock>(std::make_shared<StateParams3>(Vector3d::Zero(), false))
: std::static_pointer_cast<StateBlock>(std::make_shared<StateParams6>(Vector6d::Zero(), false)))
{
assert((_bias.size() == 3) or (_bias.size() == 6));
}
......
......@@ -20,8 +20,9 @@
//
//--------LICENSE_END--------
#include "imu/sensor/sensor_imu.h"
#include "core/state_block/state_block.h"
#include "core/state_block/state_quaternion.h"
#include <core/state_block/state_quaternion.h>
#include <core/state_block/state_block_derived.h>
namespace wolf {
......@@ -33,9 +34,9 @@ SensorImu::SensorImu(const Eigen::VectorXd& _extrinsics, ParamsSensorImuPtr _par
SensorImu::SensorImu(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu& _params) :
SensorBase("SensorImu",
std::make_shared<StateBlock>(_extrinsics.head(3), true, nullptr, false),
std::make_shared<StatePoint3d>(_extrinsics.head(3), true, false),
std::make_shared<StateQuaternion>(_extrinsics.tail(4), true, false),
std::make_shared<StateBlock>(6, false, nullptr, false),
std::make_shared<StateParams6>(Vector6d::Zero(), false),
(Eigen::Vector6d()<<_params.a_noise,_params.a_noise,_params.a_noise,_params.w_noise,_params.w_noise,_params.w_noise).finished(),
false, false, true),
a_noise(_params.a_noise),
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment