diff --git a/demos/processor_odom_3D.yaml b/demos/processor_odom_3D.yaml index 82fdecf637f5a14431fc8ff5b66e7e7415ffcfd2..2c04ea1dab811cd4732724db5aa1096c1eba8a6b 100644 --- a/demos/processor_odom_3D.yaml +++ b/demos/processor_odom_3D.yaml @@ -1,11 +1,13 @@ -processor type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error. -processor name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails +type: "ODOM 3D" # This must match the KEY used in the SensorFactory. Otherwise it is an error. +name: "Main odometer" # This is ignored. The name provided to the SensorFactory prevails -time tolerance: 0.01 # seconds +time_tolerance: 0.01 # seconds +unmeasured_perturbation_std: 0.001 + +voting_active: false +voting_aux_active: false +max_time_span: 0.2 # seconds +max_buff_length: 10 # motion deltas +dist_traveled: 0.5 # meters +angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg) -voting_active: false -voting_aux_active: false -k_max_time span: 0.2 # seconds -k_max_buff_length: 10 # motion deltas -k_dist_traveled: 0.5 # meters -k_angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg) \ No newline at end of file diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp index 654e3fbfe4a28a59a1bc7454ece77d2c871fbfd4..ae1d3cc3d0fc2b860a66f7f0a1e454caaadd7b47 100644 --- a/hello_wolf/processor_range_bearing.cpp +++ b/hello_wolf/processor_range_bearing.cpp @@ -14,10 +14,10 @@ namespace wolf { -ProcessorRangeBearing::ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, ProcessorParamsBasePtr _params) : +ProcessorRangeBearing::ProcessorRangeBearing(ProcessorParamsBasePtr _params) : ProcessorBase("RANGE BEARING", _params) { - H_r_s = transform(_sensor_ptr->getP()->getState(), _sensor_ptr->getO()->getState()); + // } void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture) @@ -103,11 +103,10 @@ ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr) { - SensorRangeBearingPtr sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sen_ptr); - ProcessorParamsRangeBearingPtr params = std::static_pointer_cast<ProcessorParamsRangeBearing>(_params); + auto params = std::static_pointer_cast<ProcessorParamsRangeBearing>(_params); // construct processor - ProcessorRangeBearingPtr prc = std::make_shared<ProcessorRangeBearing>(sensor_rb, params); + auto prc = std::make_shared<ProcessorRangeBearing>(params); // setup processor prc->setName(_unique_name); @@ -115,22 +114,6 @@ ProcessorBasePtr ProcessorRangeBearing::create(const std::string& _unique_name, return prc; } -//ProcessorBasePtr ProcessorRangeBearing::createAutoConf(const std::string& _unique_name, -// const ParamsServer& _server, -// const SensorBasePtr _sensor_ptr) -//{ -// SensorRangeBearingPtr sensor_rb = std::static_pointer_cast<SensorRangeBearing>(_sensor_ptr); -// ProcessorParamsRangeBearingPtr params = std::make_shared<ProcessorParamsRangeBearing>(_unique_name, _server); -// -// // construct processor -// ProcessorRangeBearingPtr prc = std::make_shared<ProcessorRangeBearing>(sensor_rb, params); -// -// // setup processor -// prc->setName(_unique_name); -// -// return prc; -//} -// Eigen::Vector2s ProcessorRangeBearing::observe(const Eigen::Vector2s& lmk_w) const { return polar(toSensor(lmk_w)); diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h index 2c7002bdcbf7c4b8a113e4b58d8fbd087bac0efa..3a6a279f62d8cdadc156e8825b338d575c932b8f 100644 --- a/hello_wolf/processor_range_bearing.h +++ b/hello_wolf/processor_range_bearing.h @@ -45,9 +45,9 @@ class ProcessorRangeBearing : public ProcessorBase public: typedef Eigen::Transform<Scalar, 2, Eigen::Isometry> Trf; - ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, ProcessorParamsBasePtr _params); + ProcessorRangeBearing(ProcessorParamsBasePtr _params); virtual ~ProcessorRangeBearing() {/* empty */} - virtual void configure(SensorBasePtr _sensor) override { } + virtual void configure(SensorBasePtr _sensor) override; // Factory method for high level API static ProcessorBasePtr create(const std::string& _unique_name, @@ -85,6 +85,11 @@ class ProcessorRangeBearing : public ProcessorBase Eigen::Vector2s rect (Scalar range, Scalar bearing) const; }; +inline void ProcessorRangeBearing::configure(SensorBasePtr _sensor) +{ + H_r_s = transform(_sensor->getP()->getState(), _sensor->getO()->getState()); +} + } /* namespace wolf */ #endif /* HELLO_WOLF_PROCESSOR_RANGE_BEARING_H_ */ diff --git a/include/core/processor/processor_odom_3D.h b/include/core/processor/processor_odom_3D.h index 759912f27680d5947352d599a889985b0e81d2b3..7eab40c33609139afab9e59301c816400bdea95f 100644 --- a/include/core/processor/processor_odom_3D.h +++ b/include/core/processor/processor_odom_3D.h @@ -60,7 +60,7 @@ WOLF_PTR_TYPEDEFS(ProcessorOdom3D); class ProcessorOdom3D : public ProcessorMotion { public: - ProcessorOdom3D(ProcessorParamsOdom3DPtr _params, SensorOdom3DPtr _sensor_ptr = nullptr); + ProcessorOdom3D(ProcessorParamsOdom3DPtr _params); virtual ~ProcessorOdom3D(); virtual void configure(SensorBasePtr _sensor) override; diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp index 3168276f93aa8da7c5220aaba098503863ff0e68..6d60447c261f6244f4bcf7adf38d469a326ef56d 100644 --- a/src/processor/processor_odom_3D.cpp +++ b/src/processor/processor_odom_3D.cpp @@ -2,18 +2,19 @@ namespace wolf { -ProcessorOdom3D::ProcessorOdom3D(ProcessorParamsOdom3DPtr _params, SensorOdom3DPtr _sensor_ptr) : - ProcessorMotion("ODOM 3D", 7, 7, 6, 6, 0, _params), - params_odom_3D_(_params), - p1_(nullptr), p2_(nullptr), p_out_(nullptr), - q1_(nullptr), q2_(nullptr), q_out_(nullptr) - { - configure(_sensor_ptr); - delta_ = deltaZero(); - delta_integrated_ = deltaZero(); - jacobian_delta_preint_.setZero(delta_cov_size_, delta_cov_size_); - jacobian_delta_.setZero(delta_cov_size_, delta_cov_size_); - } +ProcessorOdom3D::ProcessorOdom3D(ProcessorParamsOdom3DPtr _params) : + ProcessorMotion("ODOM 3D", 7, 7, 6, 6, 0, _params), + params_odom_3D_ (_params), + k_disp_to_disp_ (0), + k_disp_to_rot_ (0), + k_rot_to_rot_ (0), + min_disp_var_ (0.1), // around 10cm error + min_rot_var_ (0.1), // around 6 degrees error + p1_(nullptr), p2_(nullptr), p_out_(nullptr), + q1_(nullptr), q2_(nullptr), q_out_(nullptr) +{ + // +} ProcessorOdom3D::~ProcessorOdom3D() { @@ -21,25 +22,16 @@ ProcessorOdom3D::~ProcessorOdom3D() void ProcessorOdom3D::configure(SensorBasePtr _sensor) { - if (_sensor) - { - SensorOdom3DPtr sen_ptr = std::static_pointer_cast<SensorOdom3D>(_sensor); - // we steal the parameters from the provided odom3D sensor. - k_disp_to_disp_ = sen_ptr->getDispVarToDispNoiseFactor(); - k_disp_to_rot_ = sen_ptr->getDispVarToRotNoiseFactor(); - k_rot_to_rot_ = sen_ptr->getRotVarToRotNoiseFactor(); - min_disp_var_ = sen_ptr->getMinDispVar(); - min_rot_var_ = sen_ptr->getMinRotVar(); - } - else - { - // we put default params. - k_disp_to_disp_ = 0; - k_disp_to_rot_ = 0; - k_rot_to_rot_ = 0; - min_disp_var_ = 0.1; // around 30cm error - min_rot_var_ = 0.1; // around 9 degrees error - } + assert (_sensor && "Trying to configure processor with a sensor but provided sensor is nullptr."); + + SensorOdom3DPtr sen_ptr = std::static_pointer_cast<SensorOdom3D>(_sensor); + + // we steal the parameters from the provided odom3D sensor. + k_disp_to_disp_ = sen_ptr->getDispVarToDispNoiseFactor(); + k_disp_to_rot_ = sen_ptr->getDispVarToRotNoiseFactor(); + k_rot_to_rot_ = sen_ptr->getRotVarToRotNoiseFactor(); + min_disp_var_ = sen_ptr->getMinDispVar(); + min_rot_var_ = sen_ptr->getMinRotVar(); } void ProcessorOdom3D::computeCurrentDelta(const Eigen::VectorXs& _data, @@ -344,12 +336,10 @@ Motion ProcessorOdom3D::interpolate(const Motion& _ref1, ProcessorBasePtr ProcessorOdom3D::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sen_ptr) { // cast inputs to the correct type - std::shared_ptr<ProcessorParamsOdom3D> prc_odo_params = std::static_pointer_cast<ProcessorParamsOdom3D>(_params); - - SensorOdom3DPtr sen_odo =std::static_pointer_cast<SensorOdom3D>(_sen_ptr); + auto params = std::static_pointer_cast<ProcessorParamsOdom3D>(_params); // construct processor - ProcessorOdom3DPtr prc_odo = std::make_shared<ProcessorOdom3D>(prc_odo_params, sen_odo); + auto prc_odo = std::make_shared<ProcessorOdom3D>(params); // setup processor prc_odo->setName(_unique_name);