Skip to content
Snippets Groups Projects
Commit adcfcbf0 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

adapted to emplace API

parent e8ea17f8
No related branches found
No related tags found
3 merge requests!39release after RAL,!38After 2nd RAL submission,!3Resolve "Adapt to emplace API"
......@@ -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_;
......
......@@ -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;
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment