diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h index 3166c09bb536e8a4fa0cdb1c8a91f400f718b50d..d82a0afe492f938558eb28c53da70a65fb515b3f 100644 --- a/include/imu/processor/processor_imu.h +++ b/include/imu/processor/processor_imu.h @@ -60,7 +60,7 @@ class ProcessorImu : public ProcessorMotion{ Eigen::VectorXd deltaZero() const override; Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint, const Eigen::VectorXd& delta_step) const override; - VectorXd getCalibration (const CaptureBasePtr _capture) const override; + VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const override; void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override; bool voteForKeyFrame() const override; CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own, diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp index bf1fe72b6241c7b66dca58738d4fe168903c8773..6eb0edced109dec6bc58e2bb3e9a400805c49730 100644 --- a/src/processor/processor_imu.cpp +++ b/src/processor/processor_imu.cpp @@ -87,9 +87,10 @@ FeatureBasePtr ProcessorImu::emplaceFeature(CaptureMotionPtr _capture_motion) VectorXd ProcessorImu::getCalibration (const CaptureBasePtr _capture) const { - assert(_capture && "called with a null capture"); - assert(_capture->getSensorIntrinsic() && "null sensor intrinsics state block"); - return _capture->getSensorIntrinsic()->getState(); + if (_capture) + return _capture->getStateBlock('I')->getState(); + else + return getSensor()->getStateBlockDynamic('I')->getState(); } void ProcessorImu::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration) diff --git a/test/gtest_factor_imu.cpp b/test/gtest_factor_imu.cpp index f53f9af38ff6da00ba0f57d815d4b3a336267e9e..c22ac6eafd00cb4c0cf480f70ffa64c0c8a88676 100644 --- a/test/gtest_factor_imu.cpp +++ b/test/gtest_factor_imu.cpp @@ -915,7 +915,7 @@ class FactorImu_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test TimeStamp t_imu(0.0), t_odo(0.0); double dt_imu(0.001), dt_odo(.01); - capture_imu = std::make_shared<CaptureImu> (t_imu, sensor_imu, data_imu, sensor_imu->getNoiseCov(), sensor_imu->getCalibration(), nullptr); + capture_imu = std::make_shared<CaptureImu> (t_imu, sensor_imu, data_imu, sensor_imu->getNoiseCov(), sensor_imu->getIntrinsic()->getState(), nullptr); capture_odo = std::make_shared<CaptureOdom3d>(t_odo, sensor_odo, data_odo, sensor_odo->getNoiseCov(), nullptr); sensor_odo->process(capture_odo);