diff --git a/src/processor_odom_2D.cpp b/src/processor_odom_2D.cpp index a5cc0a1a35e2704280dceb4431d48a03bda59a61..8cd3b12a08956f50c80610e3aa04acc9bf41923f 100644 --- a/src/processor_odom_2D.cpp +++ b/src/processor_odom_2D.cpp @@ -2,18 +2,14 @@ namespace wolf { -ProcessorOdom2D::ProcessorOdom2D(const Scalar& _dist_traveled_th, - const Scalar& _theta_traveled_th, - const Scalar& _cov_det_th, - const Scalar& _elapsed_time_th, - const Scalar& _unmeasured_perturbation_std) : - ProcessorMotion("ODOM 2D", 3, 3, 3, 2), - dist_traveled_th_(_dist_traveled_th), - theta_traveled_th_(_theta_traveled_th), - cov_det_th_(_cov_det_th), - elapsed_time_th_(_elapsed_time_th) +ProcessorOdom2D::ProcessorOdom2D(const ProcessorParamsOdom2D& _params) : + ProcessorMotion("ODOM 2D", 3, 3, 3, 2), + dist_traveled_th_(_params.dist_traveled_th_), + theta_traveled_th_(_params.theta_traveled_th_), + cov_det_th_(_params.cov_det_th_), + elapsed_time_th_(_params.elapsed_time_th_) { - unmeasured_perturbation_cov_ = _unmeasured_perturbation_std * _unmeasured_perturbation_std * Matrix3s::Identity(); + unmeasured_perturbation_cov_ = _params.unmeasured_perturbation_std_ * _params.unmeasured_perturbation_std_ * Matrix3s::Identity(); } ProcessorOdom2D::~ProcessorOdom2D() @@ -180,11 +176,7 @@ ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const { std::shared_ptr<ProcessorParamsOdom2D> params = std::static_pointer_cast<ProcessorParamsOdom2D>(_params); - prc_ptr = std::make_shared<ProcessorOdom2D>(params->dist_traveled_th_, - params->theta_traveled_th_, - params->cov_det_th_, - params->elapsed_time_th_, - params->unmeasured_perturbation_std_); + prc_ptr = std::make_shared<ProcessorOdom2D>(*params); } else { diff --git a/src/processor_odom_2D.h b/src/processor_odom_2D.h index c9b10b1400e7e5cf9a5603ce2f319b7bfd7a7172..4434b2a84f1a0d186c0ad9e6c7041a5cc2f73a08 100644 --- a/src/processor_odom_2D.h +++ b/src/processor_odom_2D.h @@ -21,22 +21,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2D); struct ProcessorParamsOdom2D : public ProcessorParamsBase { - Scalar dist_traveled_th_ = 1.0; - Scalar theta_traveled_th_ = 0.17; - Scalar cov_det_th_ = 1.0; - Scalar elapsed_time_th_ = 1.0; - Scalar unmeasured_perturbation_std_ = 0.001; + Scalar dist_traveled_th_ = 1.0; // 1m + Scalar theta_traveled_th_ = 0.17; // 90 degrees + Scalar cov_det_th_ = 1.0; // 1 rad^2 + Scalar elapsed_time_th_ = 1.0; // 1s + Scalar unmeasured_perturbation_std_ = 0.001; // no particular dimension: the same for displacement and angle }; class ProcessorOdom2D : public ProcessorMotion { public: - ProcessorOdom2D(const Scalar& _traveled_dist_th = 1.0, - const Scalar& _theta_traveled_th = 0.17, - const Scalar& _cov_det_th = 1.0, - const Scalar& _elapsed_time_th = 1.0, - const Scalar& _unmeasured_perturbation_std = 0.001); + ProcessorOdom2D(const ProcessorParamsOdom2D& _params = ProcessorParamsOdom2D()); virtual ~ProcessorOdom2D(); + virtual bool voteForKeyFrame() override; protected: