From adcfcbf04f4c1f5b3b115210fafa8ffe6c703b32 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Tue, 25 Jun 2019 15:30:19 +0200 Subject: [PATCH] adapted to emplace API --- include/IMU/processor/processor_IMU.h | 17 ++++++---- src/processor/processor_IMU.cpp | 48 ++++++++++++++------------- 2 files changed, 35 insertions(+), 30 deletions(-) diff --git a/include/IMU/processor/processor_IMU.h b/include/IMU/processor/processor_IMU.h index e90affda5..5795fbd38 100644 --- a/include/IMU/processor/processor_IMU.h +++ b/include/IMU/processor/processor_IMU.h @@ -52,14 +52,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/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp index ac38037cd..5621d2c2d 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; } -- GitLab