diff --git a/src/sensor/sensor_imu2d.cpp b/src/sensor/sensor_imu2d.cpp index f74b3baa23383ea5265d7d288abc8d97d225a0ff..ca6130465acdac333330b3c97c656ac12a038778 100644 --- a/src/sensor/sensor_imu2d.cpp +++ b/src/sensor/sensor_imu2d.cpp @@ -19,8 +19,9 @@ // along with this program. If not, see <http://www.gnu.org/licenses/>. // //--------LICENSE_END-------- -#include <imu/sensor/sensor_imu2d.h> -#include <core/state_block/state_block.h> +#include "imu/sensor/sensor_imu2d.h" + +#include <core/state_block/state_block_derived.h> #include <core/state_block/state_angle.h> namespace wolf { @@ -31,20 +32,27 @@ SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, ParamsSensorImu2dPt // } -SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu2d& _params) : - SensorBase("SensorImu2d", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), std::make_shared<StateBlock>(3, false, nullptr), (Eigen::Vector3d()<<_params.a_noise,_params.a_noise,_params.w_noise).finished(), false, false, true), - a_noise(_params.a_noise), - w_noise(_params.w_noise), - ab_initial_stdev(_params.ab_initial_stdev), - wb_initial_stdev(_params.wb_initial_stdev), - ab_rate_stdev(_params.ab_rate_stdev), - wb_rate_stdev(_params.wb_rate_stdev), - orthogonal_gravity(_params.orthogonal_gravity) +SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu2d& _params) + : SensorBase("SensorImu2d", + std::make_shared<StatePoint2d>(_extrinsics.head(2), true, false), + std::make_shared<StateAngle>(_extrinsics(2), true, false), + std::make_shared<StateParams3>(Vector3d::Zero(3), false), + (Eigen::Vector3d() << _params.a_noise, _params.a_noise, _params.w_noise).finished(), + false, + false, + true), + a_noise(_params.a_noise), + w_noise(_params.w_noise), + ab_initial_stdev(_params.ab_initial_stdev), + wb_initial_stdev(_params.wb_initial_stdev), + ab_rate_stdev(_params.ab_rate_stdev), + wb_rate_stdev(_params.wb_rate_stdev), + orthogonal_gravity(_params.orthogonal_gravity) { assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2d."); - if(!orthogonal_gravity) + if (!orthogonal_gravity) { - addStateBlock('G', std::make_shared<StateBlock>(2, false), false); + addStateBlock('G', std::make_shared<StateVector2d>(Vector2d::Zero(), false, true), false); } }