diff --git a/include/IMU/processor/processor_IMU.h b/include/IMU/processor/processor_IMU.h
index 867a535f887eda226d042e5d39ca98e71bf5e772..6e9586b51c9776208aee1ae6aeb98503dd1653d5 100644
--- a/include/IMU/processor/processor_IMU.h
+++ b/include/IMU/processor/processor_IMU.h
@@ -62,14 +62,17 @@ class ProcessorIMU : public ProcessorMotion{
Motion& _motion,
TimeStamp& _ts) override;
virtual bool voteForKeyFrame() override;
- virtual CaptureMotionPtr createCapture(const TimeStamp& _ts,
- const SensorBasePtr& _sensor,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin) override;
- virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
+ virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXs& _data,
+ const MatrixXs& _data_cov,
+ const VectorXs& _calib,
+ const VectorXs& _calib_preint,
+ const FrameBasePtr& _frame_origin) override;
+ virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override;
+ CaptureBasePtr _capture_origin) override;
protected:
ProcessorParamsIMUPtr params_motion_IMU_;
diff --git a/src/feature/feature_IMU.cpp b/src/feature/feature_IMU.cpp
index ff248ab3ad75e9f9aab0007b83e69e552a3abbb1..ee8d115e2f68b3babb7612b6748132d4ac4d6abc 100644
--- a/src/feature/feature_IMU.cpp
+++ b/src/feature/feature_IMU.cpp
@@ -12,8 +12,6 @@ FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated,
gyro_bias_preint_(_bias.tail<3>()),
jacobian_bias_(_dD_db_jacobians)
{
- if (_cap_imu_ptr)
- this->setCapture(_cap_imu_ptr);
}
FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr):
@@ -22,7 +20,6 @@ FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr):
gyro_bias_preint_(_cap_imu_ptr->getCalibrationPreint().tail<3>()),
jacobian_bias_(_cap_imu_ptr->getJacobianCalib())
{
- this->setCapture(_cap_imu_ptr);
}
FeatureIMU::~FeatureIMU()
diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp
index f3a95512e45ca4bcf8d8544fc535150d21f6ecfa..daa57316c6d4fd986f15c5351957a3c9a407cd08 100644
--- a/src/processor/processor_IMU.cpp
+++ b/src/processor/processor_IMU.cpp
@@ -185,27 +185,34 @@ Motion ProcessorIMU::interpolate(const Motion& _motion_ref, Motion& _motion_seco
}
-CaptureMotionPtr ProcessorIMU::createCapture(const TimeStamp& _ts,
- const SensorBasePtr& _sensor,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const FrameBasePtr& _frame_origin)
+CaptureMotionPtr ProcessorIMU::emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXs& _data,
+ const MatrixXs& _data_cov,
+ const VectorXs& _calib,
+ const VectorXs& _calib_preint,
+ const FrameBasePtr& _frame_origin)
{
- CaptureIMUPtr capture_imu = std::make_shared<CaptureIMU>(_ts,
- _sensor,
- _data,
- _data_cov,
- _frame_origin);
- return capture_imu;
+ auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureIMU>(_frame_own,
+ _ts,
+ _sensor,
+ _data,
+ _data_cov,
+ _frame_origin));
+ cap_motion->setCalibration(_calib);
+ cap_motion->setCalibrationPreint(_calib_preint);
+
+ return cap_motion;
}
-FeatureBasePtr ProcessorIMU::createFeature(CaptureMotionPtr _capture_motion)
+FeatureBasePtr ProcessorIMU::emplaceFeature(CaptureMotionPtr _capture_motion)
{
- FeatureIMUPtr feature = std::make_shared<FeatureIMU>(
- _capture_motion->getBuffer().get().back().delta_integr_,
- _capture_motion->getBuffer().get().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
- _capture_motion->getBuffer().getCalibrationPreint(),
- _capture_motion->getBuffer().get().back().jacobian_calib_);
+ auto feature = FeatureBase::emplace<FeatureIMU>(_capture_motion,
+ _capture_motion->getBuffer().get().back().delta_integr_,
+ _capture_motion->getBuffer().get().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
+ _capture_motion->getBuffer().getCalibrationPreint(),
+ _capture_motion->getBuffer().get().back().jacobian_calib_);
return feature;
}
@@ -213,13 +220,8 @@ FactorBasePtr ProcessorIMU::emplaceFactor(FeatureBasePtr _feature_motion, Captur
{
CaptureIMUPtr cap_imu = std::static_pointer_cast<CaptureIMU>(_capture_origin);
FeatureIMUPtr ftr_imu = std::static_pointer_cast<FeatureIMU>(_feature_motion);
- // FactorIMUPtr fac_imu = std::make_shared<FactorIMU>(ftr_imu, cap_imu, shared_from_this());
- auto fac_imu = FactorBase::emplace<FactorIMU>(_feature_motion, ftr_imu, cap_imu, shared_from_this());
- // link ot wolf tree
- // _feature_motion->addFactor(fac_imu);
- // _capture_origin->addConstrainedBy(fac_imu);
- // _capture_origin->getFrame()->addConstrainedBy(fac_imu);
+ auto fac_imu = FactorBase::emplace<FactorIMU>(_feature_motion, ftr_imu, cap_imu, shared_from_this());
return fac_imu;
}
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
index 04946583e0df53ced27e92e6406bc9c5defe4020..1c6b38afc03f49a6880f6a58e0d788278047ad96 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_IMU.cpp
@@ -58,10 +58,15 @@ class FeatureIMU_test : public testing::Test
MatrixXs P0; P0.setIdentity(9,9);
origin_frame = problem->setPrior(x0, P0, 0.0, 0.01);
- // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
+ // Emplace one capture to store the IMU data arriving from (sensor / callback / file / etc.)
// give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
- imu_ptr = std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero());
- imu_ptr->setFrame(origin_frame); //to get ptr to Frm in processorIMU and then get biases
+ imu_ptr = std::static_pointer_cast<CaptureIMU>(
+ CaptureBase::emplace<CaptureIMU>(origin_frame,
+ t,
+ sensor_ptr,
+ data_,
+ Eigen::Matrix6s::Identity(),
+ Eigen::Vector6s::Zero()) );
//process data
data_ << 2, 0, 9.8, 0, 0, 0;
@@ -74,23 +79,23 @@ class FeatureIMU_test : public testing::Test
// process data in capture
sensor_ptr->process(imu_ptr);
- //create Frame
+ //emplace Frame
ts = problem->getProcessorMotion()->getBuffer().get().back().ts_;
state_vec = problem->getProcessorMotion()->getCurrentState();
last_frame = problem->emplaceFrame(KEY, state_vec, ts);
- //create a feature
+ //emplace a feature
delta_preint = problem->getProcessorMotion()->getMotion().delta_integr_;
delta_preint_cov = problem->getProcessorMotion()->getMotion().delta_integr_cov_ + MatrixXs::Identity(9,9)*1e-08;
VectorXs calib_preint = problem->getProcessorMotion()->getBuffer().getCalibrationPreint();
dD_db_jacobians = problem->getProcessorMotion()->getMotion().jacobian_calib_;
- feat_imu = std::make_shared<FeatureIMU>(delta_preint,
- delta_preint_cov,
- calib_preint,
- dD_db_jacobians,
- imu_ptr);
- feat_imu->setCapture(imu_ptr); //associate the feature to a capture
-
+ feat_imu = std::static_pointer_cast<FeatureIMU>(
+ FeatureBase::emplace<FeatureIMU>(imu_ptr,
+ delta_preint,
+ delta_preint_cov,
+ calib_preint,
+ dD_db_jacobians,
+ imu_ptr) );
}
virtual void TearDown()