diff --git a/src/capture/capture_imu.cpp b/src/capture/capture_imu.cpp index f8a4b0b4118c299fda57c59e77df1b922b2e292a..743caaf80796d7eadaf8d6bff1563f7d2a282d35 100644 --- a/src/capture/capture_imu.cpp +++ b/src/capture/capture_imu.cpp @@ -38,7 +38,7 @@ CaptureImu::CaptureImu(const TimeStamp& _init_ts, _capture_origin_ptr, nullptr, nullptr, - std::make_shared<StateBlock>((_sensor_ptr->getProblem()->getDim() == 2 ? 3 : 6), false)) + std::make_shared<StateBlock>((_sensor_ptr->getProblem()->getDim() == 2 ? 3 : 6), false, nullptr, false)) { // } @@ -57,7 +57,7 @@ CaptureImu::CaptureImu(const TimeStamp& _init_ts, _capture_origin_ptr, nullptr, nullptr, - std::make_shared<StateBlock>(_bias, false)) + std::make_shared<StateBlock>(_bias, false, nullptr, false)) { assert((_bias.size() == 3) or (_bias.size() == 6)); } diff --git a/src/sensor/sensor_imu.cpp b/src/sensor/sensor_imu.cpp index bb6ec35e0061202819385fe04b5b2c58c54a3160..0a0b034b8bea8dd3e03445005b4032180c70faeb 100644 --- a/src/sensor/sensor_imu.cpp +++ b/src/sensor/sensor_imu.cpp @@ -33,9 +33,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), - std::make_shared<StateQuaternion>(_extrinsics.tail(4), true), - std::make_shared<StateBlock>(6, false, nullptr), + std::make_shared<StateBlock>(_extrinsics.head(3), true, nullptr, false), + std::make_shared<StateQuaternion>(_extrinsics.tail(4), true, false), + std::make_shared<StateBlock>(6, false, nullptr, 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),