From c79f415f974a890c4e44c8ba01a708d743fff731 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Mon, 8 Aug 2022 23:05:29 +0200
Subject: [PATCH] Replace Ftr Fac emplacers by emplaceFtrAndFac
---
include/imu/processor/processor_imu.h | 3 +--
include/imu/processor/processor_imu2d.h | 5 ++--
src/processor/processor_imu.cpp | 23 +++++++-----------
src/processor/processor_imu2d.cpp | 31 +++++++++++--------------
4 files changed, 26 insertions(+), 36 deletions(-)
diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h
index 080fb3dcb..1c03c1b41 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu.h
@@ -122,8 +122,7 @@ class ProcessorImu : public ProcessorMotion{
const VectorXd& _calib,
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin) override;
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) override;
+ virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
public:
/** \brief Enable bootstrapping process
diff --git a/include/imu/processor/processor_imu2d.h b/include/imu/processor/processor_imu2d.h
index 726666f48..44f418140 100644
--- a/include/imu/processor/processor_imu2d.h
+++ b/include/imu/processor/processor_imu2d.h
@@ -92,9 +92,8 @@ class ProcessorImu2d : public ProcessorMotion{
const VectorXd& _calib,
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin) override;
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override;
+ virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+
protected:
ParamsProcessorImu2dPtr params_motion_Imu_;
diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index 2fe42636b..b0b5c5649 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -93,16 +93,6 @@ CaptureMotionPtr ProcessorImu::emplaceCapture(const FrameBasePtr& _frame_own,
return cap_motion;
}
-FeatureBasePtr ProcessorImu::emplaceFeature(CaptureMotionPtr _capture_motion)
-{
- auto feature = FeatureBase::emplace<FeatureImu>(_capture_motion,
- _capture_motion->getBuffer().back().delta_integr_,
- _capture_motion->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
- _capture_motion->getCalibrationPreint(),
- _capture_motion->getBuffer().back().jacobian_calib_);
- return feature;
-}
-
VectorXd ProcessorImu::getCalibration (const CaptureBaseConstPtr _capture) const
{
if (_capture)
@@ -116,12 +106,18 @@ void ProcessorImu::setCalibration (const CaptureBasePtr _capture, const VectorXd
_capture->getSensorIntrinsic()->setState(_calibration);
}
-FactorBasePtr ProcessorImu::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+void ProcessorImu::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
{
+ auto feature = FeatureBase::emplace<FeatureImu>(_capture_own,
+ _capture_own->getBuffer().back().delta_integr_,
+ _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
+ _capture_own->getCalibrationPreint(),
+ _capture_own->getBuffer().back().jacobian_calib_);
+
CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin);
- FeatureImuPtr ftr_imu = std::static_pointer_cast<FeatureImu>(_feature_motion);
+ FeatureImuPtr ftr_imu = std::static_pointer_cast<FeatureImu>(feature);
- auto fac_imu = FactorBase::emplace<FactorImu>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+ auto fac_imu = FactorBase::emplace<FactorImu>(feature, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
if (bootstrapping_)
{
@@ -129,7 +125,6 @@ FactorBasePtr ProcessorImu::emplaceFactor(FeatureBasePtr _feature_motion, Captur
bootstrap_factor_list_.push_back(fac_imu);
}
- return fac_imu;
}
void ProcessorImu::computeCurrentDelta(const Eigen::VectorXd& _data,
diff --git a/src/processor/processor_imu2d.cpp b/src/processor/processor_imu2d.cpp
index dc35d28c0..23fbbd222 100644
--- a/src/processor/processor_imu2d.cpp
+++ b/src/processor/processor_imu2d.cpp
@@ -101,16 +101,6 @@ namespace wolf {
return cap_motion;
}
- FeatureBasePtr ProcessorImu2d::emplaceFeature(CaptureMotionPtr _capture_motion)
- {
- auto feature = FeatureBase::emplace<FeatureImu2d>(_capture_motion,
- _capture_motion->getBuffer().back().delta_integr_,
- _capture_motion->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
- _capture_motion->getCalibrationPreint(),
- _capture_motion->getBuffer().back().jacobian_calib_);
- return feature;
- }
-
VectorXd ProcessorImu2d::getCalibration (const CaptureBaseConstPtr _capture) const
{
if (_capture)
@@ -124,15 +114,22 @@ namespace wolf {
_capture->getSensorIntrinsic()->setState(_calibration);
}
- FactorBasePtr ProcessorImu2d::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+ void ProcessorImu2d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
{
- CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin);
- FeatureImu2dPtr ftr_imu = std::static_pointer_cast<FeatureImu2d>(_feature_motion);
+ auto feature = FeatureBase::emplace<FeatureImu2d>(
+ _capture_own, _capture_own->getBuffer().back().delta_integr_,
+ _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
+ _capture_own->getCalibrationPreint(), _capture_own->getBuffer().back().jacobian_calib_);
- if( std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal() )
- return FactorBase::emplace<FactorImu2d>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
- else
- return FactorBase::emplace<FactorImu2dWithGravity>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+ CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin);
+ FeatureImu2dPtr ftr_imu = std::static_pointer_cast<FeatureImu2d>(feature);
+
+ if (std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal())
+ FactorBase::emplace<FactorImu2d>(ftr_imu, ftr_imu, cap_imu, shared_from_this(),
+ params_->apply_loss_function);
+ else
+ FactorBase::emplace<FactorImu2dWithGravity>(ftr_imu, ftr_imu, cap_imu, shared_from_this(),
+ params_->apply_loss_function);
}
void ProcessorImu2d::computeCurrentDelta(const Eigen::VectorXd& _data,
--
GitLab