diff --git a/include/core/processor/factory_processor.h b/include/core/processor/factory_processor.h index 74f01419ea00bfd01842364b5f79e46df02b525f..fbd3261c29afceed19fbc21666e79dd996b945aa 100644 --- a/include/core/processor/factory_processor.h +++ b/include/core/processor/factory_processor.h @@ -37,6 +37,7 @@ struct ParamsProcessorBase; // wolf #include "core/common/factory.h" +#include "core/utils/params_server.h" // std diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h index 2a00afd27843a41e7b326a4bc32c3875b465d25b..462e5ee22a47f00f874f861dcd4d0ddcd37d8c17 100644 --- a/include/core/processor/processor_diff_drive.h +++ b/include/core/processor/processor_diff_drive.h @@ -79,8 +79,6 @@ class ProcessorDiffDrive : public ProcessorOdom2d 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; diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index 4e883fa4e8541260575c6c61d603e7eceb991ab2..3bf0b996153858d5904181832a463ec4c1854e1c 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -477,16 +477,6 @@ 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 - // */ - // 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 diff --git a/include/core/processor/processor_odom_2d.h b/include/core/processor/processor_odom_2d.h index 1f2f9a2457c6bc7fc1fce7cfbb8472505220f382..1612a4bf5ce75ff875b09017e9624d60d8ae6a3c 100644 --- a/include/core/processor/processor_odom_2d.h +++ b/include/core/processor/processor_odom_2d.h @@ -104,8 +104,6 @@ class ProcessorOdom2d : public ProcessorMotion 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; diff --git a/include/core/processor/processor_odom_3d.h b/include/core/processor/processor_odom_3d.h index c4cef80a76abf5a634b201ee7d3bebdb7e2393f2..fca115c8358bdc22bd60013d1f7f3fbc9f1828d2 100644 --- a/include/core/processor/processor_odom_3d.h +++ b/include/core/processor/processor_odom_3d.h @@ -125,8 +125,6 @@ class ProcessorOdom3d : 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; void emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) override; VectorXd getCalibration(const CaptureBaseConstPtr _capture) const override; diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp index 34326e1d79bcf3715a4d56ac22165a63aef8d8f5..310fb5f107e46cd416a3dff9c7f9a84abd6fe334 100644 --- a/src/processor/processor_diff_drive.cpp +++ b/src/processor/processor_diff_drive.cpp @@ -182,28 +182,6 @@ void ProcessorDiffDrive::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origi params_prc_diff_drv_selfcal_->apply_loss_function); } -// 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_motion.cpp b/src/processor/processor_motion.cpp index 0d34672fd0c47bfb798e78c1939d40cc887786d0..4a798a037c8851591d5367f838de28b43cef13b4 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -484,12 +484,6 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr) auto keyframe = last_ptr_->getFrame(); keyframe ->link(getProblem()); - // // create motion feature and add it to the key_capture - // auto key_feature = emplaceFeature(last_ptr_); - - // // create motion factor and link it to parent feature and other frame (which is origin's frame) - // auto factor = emplaceFactor(key_feature, origin_ptr_); - // create motion feature and add it to the key_capture // create motion factor and link it to parent feature and other frame (which is origin's frame) emplaceFeaturesAndFactors(origin_ptr_, last_ptr_); diff --git a/src/processor/processor_odom_2d.cpp b/src/processor/processor_odom_2d.cpp index 05cc6c6e1eed2f46d547974e6d3f894b655e4b58..69dfe4e6ea31ebea40d9b9205db212e40067a4dc 100644 --- a/src/processor/processor_odom_2d.cpp +++ b/src/processor/processor_odom_2d.cpp @@ -176,30 +176,6 @@ void ProcessorOdom2d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, 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 83e42b4b212734e1045a76404fb4e99fc80df96d..1870bb66b61021cb0c3b4851eaec97ca5b80d80b 100644 --- a/src/processor/processor_odom_3d.cpp +++ b/src/processor/processor_odom_3d.cpp @@ -221,25 +221,6 @@ Eigen::VectorXd ProcessorOdom3d::correctDelta (const Eigen::VectorXd& delta_prei return delta_corrected; } -// 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) {