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/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()