diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h
index e0fa72f2b2a4d00978f054579e591d2306a766b9..d82a0afe492f938558eb28c53da70a65fb515b3f 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu.h
@@ -17,7 +17,7 @@ struct ParamsProcessorImu : public ParamsProcessorMotion
{
//
}
- std::string print() const
+ std::string print() const override
{
return "\n" + ParamsProcessorMotion::print();
}
@@ -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/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h
index 6791037b39afca2f73775557baf7a7b72cb9da1b..5ffd061841374ea942f8cdb44793a2a5ce223e9d 100644
--- a/include/imu/sensor/sensor_imu.h
+++ b/include/imu/sensor/sensor_imu.h
@@ -40,7 +40,7 @@ struct ParamsSensorImu : public ParamsSensorBase
ab_rate_stdev = _server.getParam<double>(prefix + _unique_name + "/ab_rate_stdev");
wb_rate_stdev = _server.getParam<double>(prefix + _unique_name + "/wb_rate_stdev");
}
- std::string print() const
+ std::string print() const override
{
return "\n" + ParamsSensorBase::print() + "\n"
+ "w_noise: " + std::to_string(w_noise) + "\n"
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/src/yaml/processor_imu_yaml.cpp b/src/yaml/processor_imu_yaml.cpp
index ce050aed376a428896ac9d81bbd2e18e671e63ee..4d75aa8f13069a7a22404de29fe90462196b394e 100644
--- a/src/yaml/processor_imu_yaml.cpp
+++ b/src/yaml/processor_imu_yaml.cpp
@@ -38,7 +38,6 @@ static ParamsProcessorBasePtr createProcessorImuParams(const std::string & _file
params->dist_traveled = kf_vote["dist_traveled"] .as<double>();
params->angle_turned = kf_vote["angle_turned"] .as<double>();
params->voting_active = kf_vote["voting_active"] .as<bool>();
- params->voting_aux_active = kf_vote["voting_aux_active"] .as<bool>();
return params;
}
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);
diff --git a/test/gtest_feature_imu.cpp b/test/gtest_feature_imu.cpp
index 916dc5c53d7064d1f4ca26bf941bb206fccd3c7a..ef63025f2b0db475bae9ceba850f960755570e08 100644
--- a/test/gtest_feature_imu.cpp
+++ b/test/gtest_feature_imu.cpp
@@ -84,20 +84,19 @@ class FeatureImu_test : public testing::Test
//emplace Frame
ts = problem->getTimeStamp();
state_vec = problem->getState().vector(problem->getFrameStructure());
- last_frame = problem->emplaceKeyFrame(ts, state_vec);
+ last_frame = problem->emplaceFrame(ts, state_vec);
//emplace a feature
delta_preint = processor_motion_ptr_->getMotion().delta_integr_;
delta_preint_cov = processor_motion_ptr_->getMotion().delta_integr_cov_ + MatrixXd::Identity(9,9)*1e-08;
VectorXd calib_preint = processor_motion_ptr_->getLast()->getCalibrationPreint();
dD_db_jacobians = processor_motion_ptr_->getMotion().jacobian_calib_;
- feat_imu = std::static_pointer_cast<FeatureImu>(
- FeatureBase::emplace<FeatureImu>(imu_ptr,
- delta_preint,
- delta_preint_cov,
- calib_preint,
- dD_db_jacobians,
- imu_ptr) );
+ feat_imu = FeatureBase::emplace<FeatureImu>(imu_ptr,
+ delta_preint,
+ delta_preint_cov,
+ calib_preint,
+ dD_db_jacobians,
+ imu_ptr) ;
}
void TearDown() override
@@ -125,9 +124,6 @@ TEST_F(FeatureImu_test, check_frame)
origin_frame->getTimeStamp(ts);
ASSERT_NEAR(t.get(), ts.get(), wolf::Constants::EPS_SMALL) << "t != ts \t t=" << t << "\t ts=" << ts << std::endl;
- ASSERT_TRUE(origin_frame->isKey());
- ASSERT_TRUE(last_frame->isKey());
- ASSERT_TRUE(left_frame->isKey());
wolf::StateBlockPtr origin_pptr, origin_optr, origin_vptr, left_pptr, left_optr, left_vptr;
origin_pptr = origin_frame->getP();
diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp
index 6d51a5619ce1ff0615639be6d75ad3d7caf3089c..161157b8e079a5dd5f3dd4c873e754e021003c6f 100644
--- a/test/gtest_imu.cpp
+++ b/test/gtest_imu.cpp
@@ -502,7 +502,7 @@ class Process_Factor_Imu : public testing::Test
virtual void buildProblem()
{
// ===================================== SET KF in Wolf tree
- FrameBasePtr KF = problem->emplaceKeyFrame(t, x1_exact);
+ FrameBasePtr KF = problem->emplaceFrame(t, x1_exact);
// ===================================== Imu CALLBACK
problem->keyFrameCallback(KF, nullptr, dt/2);