Replace Ftr Fac emplacers by emplaceFtrAndFac

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

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_;

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,

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,