diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 20ed2330334aea821c7ca8bc6510f7438c555063..2a00afd27843a41e7b326a4bc32c3875b465d25b 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -63,30 +63,32 @@ class ProcessorDiffDrive : public ProcessorOdom2d
protected:
// Motion integration
- void computeCurrentDelta(const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const override;
- CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin) override;
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override;
- VectorXd getCalibration (const CaptureBaseConstPtr _capture = nullptr) const override;
- void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin) override;
+ // FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
+ // FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) override;
+ void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+
+ VectorXd getCalibration(const CaptureBaseConstPtr _capture = nullptr) const override;
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
- ParamsProcessorDiffDrivePtr params_prc_diff_drv_selfcal_;
- double radians_per_tick_;
+ ParamsProcessorDiffDrivePtr params_prc_diff_drv_selfcal_;
+ double radians_per_tick_;
};
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index fd74ef35fd35da344e70755a02924f9c7b094ae9..4e883fa4e8541260575c6c61d603e7eceb991ab2 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -477,18 +477,27 @@ class ProcessorMotion : public ProcessorBase, public MotionProvider
const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin_ptr) = 0;
- /** \brief emplace a feature corresponding to given capture and add the feature to this capture
- * \param _capture_motion: the parent capture
+ // /** \brief emplace a feature corresponding to given capture and add the feature to this capture
+ // * \param _capture_motion: the parent capture
+ // */
+ // virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) = 0;
+
+ // /** \brief emplace a factor and link it in the wolf tree
+ // * \param _feature_motion: the parent feature
+ // * \param _frame_origin: the frame constrained by this motion factor
+ // */
+ // virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0;
+
+ /** \brief emplace the features and factors corresponding to given capture and link them to the capture
+ * \param _capture_own: the parent capture
+ * \param _capture_origin: the capture constrained by this motion factor
+ *
+ * Typical factors to add for a ProcessorMotionDerived can be:
+ * - A preintegrated motion factor -- this is the main factor
+ * - A calibration drift factor -- only for dynamic sensor calibration parameters
+ * - An instantaneous factor for observing non-typical states of the Frame -- useful for complex dynamic robot models
*/
- virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) = 0;
-
- /** \brief emplace a factor and link it in the wolf tree
- * \param _feature_motion: the parent feature
- * \param _frame_origin: the frame constrained by this motion factor
- */
- virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0;
-
- virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) {}; // TODO make pure virtual
+ virtual void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) = 0;
virtual void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) = 0;
diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h
index fea39c4d6808a7a798a83d7b8b99dfbb47abec9d..1f2f9a2457c6bc7fc1fce7cfbb8472505220f382 100644
--- a/include/core/processor/processor_odom_2d.h
+++ b/include/core/processor/processor_odom_2d.h
@@ -71,44 +71,44 @@ class ProcessorOdom2d : public ProcessorMotion
bool voteForKeyFrame() const override;
protected:
- void computeCurrentDelta(const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _Dt2,
- Eigen::VectorXd& _delta1_plus_delta2) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _Dt2,
- Eigen::VectorXd& _delta1_plus_delta2,
- Eigen::MatrixXd& _jacobian1,
- Eigen::MatrixXd& _jacobian2) const override;
- void statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _Dt,
- VectorComposite& _x_plus_delta) const override;
- Eigen::VectorXd deltaZero() const override;
- Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
- const Eigen::VectorXd& delta_step) const override;
-
- CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- const VectorXd& _calib,
- const VectorXd& _calib_preint,
- const CaptureBasePtr& _capture_origin_ptr) override;
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
- CaptureBasePtr _capture_origin) override;
- VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override;
- void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override;
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const override;
+ Eigen::VectorXd deltaZero() const override;
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const override;
+
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin_ptr) override;
+ // FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
+ // FactorBasePtr emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin) override;
+ void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+ VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override;
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
ParamsProcessorOdom2dPtr params_odom_2d_;
diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h
index 02cdbd5ef4480ccd1a0ba28ab0db5c44886fa58c..c4cef80a76abf5a634b201ee7d3bebdb7e2393f2 100644
--- a/include/core/processor/processor_odom_3d.h
+++ b/include/core/processor/processor_odom_3d.h
@@ -90,47 +90,47 @@ class ProcessorOdom3d : public ProcessorMotion
public:
// Motion integration
- void computeCurrentDelta(const Eigen::VectorXd& _data,
- const Eigen::MatrixXd& _data_cov,
- const Eigen::VectorXd& _calib,
- const double _dt,
- Eigen::VectorXd& _delta,
- Eigen::MatrixXd& _delta_cov,
- Eigen::MatrixXd& _jacobian_calib) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _Dt2,
- Eigen::VectorXd& _delta1_plus_delta2) const override;
- void deltaPlusDelta(const Eigen::VectorXd& _delta1,
- const Eigen::VectorXd& _delta2,
- const double _Dt2,
- Eigen::VectorXd& _delta1_plus_delta2,
- Eigen::MatrixXd& _jacobian1,
- Eigen::MatrixXd& _jacobian2) const override;
- void statePlusDelta(const VectorComposite& _x,
- const Eigen::VectorXd& _delta,
- const double _Dt,
- VectorComposite& _x_plus_delta) const override;
- Eigen::VectorXd deltaZero() const override;
- Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
- const Eigen::VectorXd& delta_step) const override;
-
- bool voteForKeyFrame() const override;
- CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
- const SensorBasePtr& _sensor,
- const TimeStamp& _ts,
- const VectorXd& _data,
- const MatrixXd& _data_cov,
- 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;
- VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override;
- void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+ void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
+ const double _dt,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
+ const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override;
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const override;
+ Eigen::VectorXd deltaZero() const override;
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const override;
+
+ bool voteForKeyFrame() const override;
+
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ 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;
+ void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override;
+
+ VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override;
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
ParamsProcessorOdom3dPtr params_odom_3d_;
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index db06d0078261369000aed77e8d8f4d290cd2a355..34326e1d79bcf3715a4d56ac22165a63aef8d8f5 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -166,28 +166,44 @@ CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_o
return cap_motion;
}
-FeatureBasePtr ProcessorDiffDrive::emplaceFeature(CaptureMotionPtr _capture_motion)
+void ProcessorDiffDrive::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
{
- auto key_feature_ptr = FeatureBase::emplace<FeatureDiffDrive>(_capture_motion,
- _capture_motion->getBuffer().back().delta_integr_,
- _capture_motion->getBuffer().back().delta_integr_cov_,
- _capture_motion->getCalibrationPreint(),
- _capture_motion->getBuffer().back().jacobian_calib_);
-
- return key_feature_ptr;
+ auto feature = FeatureBase::emplace<FeatureDiffDrive>(_capture_own,
+ _capture_own->getBuffer().back().delta_integr_,
+ _capture_own->getBuffer().back().delta_integr_cov_,
+ _capture_own->getCalibrationPreint(),
+ _capture_own->getBuffer().back().jacobian_calib_);
+
+ auto ftr_motion = std::static_pointer_cast<FeatureMotion>(feature);
+ FactorBase::emplace<FactorDiffDrive>(ftr_motion,
+ ftr_motion,
+ _capture_origin,
+ shared_from_this(),
+ params_prc_diff_drv_selfcal_->apply_loss_function);
}
-FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
- CaptureBasePtr _capture_origin)
-{
- auto ftr_motion = std::static_pointer_cast<FeatureMotion>(_feature);
- auto fac_odom = FactorBase::emplace<FactorDiffDrive>(ftr_motion,
- ftr_motion,
- _capture_origin,
- shared_from_this(),
- params_prc_diff_drv_selfcal_->apply_loss_function);
- return fac_odom;
-}
+// FeatureBasePtr ProcessorDiffDrive::emplaceFeature(CaptureMotionPtr _capture_motion)
+// {
+// auto key_feature_ptr = FeatureBase::emplace<FeatureDiffDrive>(_capture_motion,
+// _capture_motion->getBuffer().back().delta_integr_,
+// _capture_motion->getBuffer().back().delta_integr_cov_,
+// _capture_motion->getCalibrationPreint(),
+// _capture_motion->getBuffer().back().jacobian_calib_);
+
+// return key_feature_ptr;
+// }
+
+// FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
+// CaptureBasePtr _capture_origin)
+// {
+// auto ftr_motion = std::static_pointer_cast<FeatureMotion>(_feature);
+// auto fac_odom = FactorBase::emplace<FactorDiffDrive>(ftr_motion,
+// ftr_motion,
+// _capture_origin,
+// shared_from_this(),
+// params_prc_diff_drv_selfcal_->apply_loss_function);
+// return fac_odom;
+// }
} /* namespace wolf */
diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp
index 98f55c0f0d6c5ff9af08d627c1ef4d67b351783e..05cc6c6e1eed2f46d547974e6d3f894b655e4b58 100644
--- a/src/processor/processor_odom_2d.cpp
+++ b/src/processor/processor_odom_2d.cpp
@@ -164,29 +164,41 @@ CaptureMotionPtr ProcessorOdom2d::emplaceCapture(const FrameBasePtr& _frame_own,
return cap_motion;
}
-FactorBasePtr ProcessorOdom2d::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
+void ProcessorOdom2d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
{
- auto fac_odom = FactorBase::emplace<FactorRelativePose2d>(_feature,
- _feature,
- _capture_origin->getFrame(),
- shared_from_this(),
- params_->apply_loss_function,
- TOP_MOTION);
- return fac_odom;
-}
-
-FeatureBasePtr ProcessorOdom2d::emplaceFeature(CaptureMotionPtr _capture_motion)
-{
- Eigen::MatrixXd covariance = _capture_motion->getBuffer().back().delta_integr_cov_;
+ Eigen::MatrixXd covariance = _capture_own->getBuffer().back().delta_integr_cov_;
makePosDef(covariance);
- FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
- "ProcessorOdom2d",
- _capture_motion->getBuffer().back().delta_integr_,
- covariance);
- return key_feature_ptr;
+ FeatureBasePtr feature = FeatureBase::emplace<FeatureBase>(
+ _capture_own, "ProcessorOdom2d", _capture_own->getBuffer().back().delta_integr_, covariance);
+
+ FactorBase::emplace<FactorRelativePose2d>(
+ feature, feature, _capture_origin->getFrame(), shared_from_this(), params_->apply_loss_function, TOP_MOTION);
}
+// FeatureBasePtr ProcessorOdom2d::emplaceFeature(CaptureMotionPtr _capture_motion)
+// {
+// Eigen::MatrixXd covariance = _capture_motion->getBuffer().back().delta_integr_cov_;
+// makePosDef(covariance);
+
+// FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
+// "ProcessorOdom2d",
+// _capture_motion->getBuffer().back().delta_integr_,
+// covariance);
+// return key_feature_ptr;
+// }
+
+// FactorBasePtr ProcessorOdom2d::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
+// {
+// auto fac_odom = FactorBase::emplace<FactorRelativePose2d>(_feature,
+// _feature,
+// _capture_origin->getFrame(),
+// shared_from_this(),
+// params_->apply_loss_function,
+// TOP_MOTION);
+// return fac_odom;
+// }
+
} /* namespace wolf */
diff --git a/src/processor/processor_odom_3d.cpp b/src/processor/processor_odom_3d.cpp
index 83c667484034fb7ac7a8fba03c17b6286f17481a..83e42b4b212734e1045a76404fb4e99fc80df96d 100644
--- a/src/processor/processor_odom_3d.cpp
+++ b/src/processor/processor_odom_3d.cpp
@@ -213,15 +213,6 @@ CaptureMotionPtr ProcessorOdom3d::emplaceCapture(const FrameBasePtr& _frame_own,
return cap_motion;
}
-FeatureBasePtr ProcessorOdom3d::emplaceFeature(CaptureMotionPtr _capture_motion)
-{
- FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
- "ProcessorOdom3d",
- _capture_motion->getBuffer().back().delta_integr_,
- _capture_motion->getBuffer().back().delta_integr_cov_);
- return key_feature_ptr;
-}
-
Eigen::VectorXd ProcessorOdom3d::correctDelta (const Eigen::VectorXd& delta_preint,
const Eigen::VectorXd& delta_step) const
{
@@ -230,15 +221,35 @@ Eigen::VectorXd ProcessorOdom3d::correctDelta (const Eigen::VectorXd& delta_prei
return delta_corrected;
}
-FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+// FeatureBasePtr ProcessorOdom3d::emplaceFeature(CaptureMotionPtr _capture_motion)
+// {
+// FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
+// "ProcessorOdom3d",
+// _capture_motion->getBuffer().back().delta_integr_,
+// _capture_motion->getBuffer().back().delta_integr_cov_);
+// return key_feature_ptr;
+// }
+
+// FactorBasePtr ProcessorOdom3d::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+// {
+// auto fac_odom = FactorBase::emplace<FactorRelativePose3d>(_feature_motion,
+// _feature_motion,
+// _capture_origin->getFrame(),
+// shared_from_this(),
+// params_->apply_loss_function,
+// TOP_MOTION);
+// return fac_odom;
+// }
+
+void ProcessorOdom3d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
{
- auto fac_odom = FactorBase::emplace<FactorRelativePose3d>(_feature_motion,
- _feature_motion,
- _capture_origin->getFrame(),
- shared_from_this(),
- params_->apply_loss_function,
- TOP_MOTION);
- return fac_odom;
+ Eigen::MatrixXd covariance = _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_;
+
+ FeatureBasePtr feature = FeatureBase::emplace<FeatureBase>(
+ _capture_own, "ProcessorOdom3d", _capture_own->getBuffer().back().delta_integr_, covariance);
+
+ FactorBase::emplace<FactorRelativePose3d>(
+ feature, feature, _capture_origin->getFrame(), shared_from_this(), params_->apply_loss_function, TOP_MOTION);
}
VectorXd ProcessorOdom3d::getCalibration (const CaptureBaseConstPtr _capture) const
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index c37bed03e30da209b9b644d49b92011afeeb5f93..2cbbc7e4c732a9c42959cac3d3a01cc769154d5a 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -105,16 +105,16 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin);
}
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override
- {
- return Base::emplaceFeature(_capture_own);
- }
-
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override
- {
- return Base::emplaceFactor(_feature_motion, _capture_origin);
- }
+ // FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override
+ // {
+ // return Base::emplaceFeature(_capture_own);
+ // }
+
+ // FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
+ // CaptureBasePtr _capture_origin) override
+ // {
+ // return Base::emplaceFactor(_feature_motion, _capture_origin);
+ // }
ParamsProcessorDiffDrivePtr getParams()
{
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index 0fcf734675b5b4f3f4fa5d3a46e0b6f5d5fa0cc0..9389febe08bac0e5d0e23caff425140a6eacebd2 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -104,16 +104,16 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin);
}
- FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override
- {
- return Base::emplaceFeature(_capture_own);
- }
-
- FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
- CaptureBasePtr _capture_origin) override
- {
- return Base::emplaceFactor(_feature_motion, _capture_origin);
- }
+ // FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override
+ // {
+ // return Base::emplaceFeature(_capture_own);
+ // }
+
+ // FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
+ // CaptureBasePtr _capture_origin) override
+ // {
+ // return Base::emplaceFactor(_feature_motion, _capture_origin);
+ // }
ParamsProcessorDiffDrivePtr getParams()
{